diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000..042089a
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,3 @@
+[submodule "ext/BehaviorTreeRos2"]
+ path = ext/BehaviorTreeRos2
+ url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git
diff --git a/ext/BehaviorTreeRos2 b/ext/BehaviorTreeRos2
new file mode 160000
index 0000000..7a6ace6
--- /dev/null
+++ b/ext/BehaviorTreeRos2
@@ -0,0 +1 @@
+Subproject commit 7a6ace6424adaa81737b95d20d81e1e8c0782ed7
diff --git a/mg_bt/CMakeLists.txt b/mg_bt/CMakeLists.txt
new file mode 100644
index 0000000..837a473
--- /dev/null
+++ b/mg_bt/CMakeLists.txt
@@ -0,0 +1,67 @@
+cmake_minimum_required(VERSION 3.12)
+project(mg_bt)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(PACKAGE_DEPS
+ rclcpp
+ behaviortree_cpp
+ behaviortree_ros2
+ btcpp_ros2_interfaces
+ tf2
+ tf2_ros
+ tf2_geometry_msgs
+ mg_msgs
+ std_msgs
+ std_srvs
+ )
+
+find_package(ament_cmake REQUIRED)
+
+include(FindPkgConfig)
+pkg_search_module(LIBGLM REQUIRED glm)
+
+foreach(PACKAGE ${PACKAGE_DEPS})
+ find_package(${PACKAGE} REQUIRED)
+endforeach()
+
+set(SOURCES
+ src/mg_bt.cpp
+ src/mg_tree_executor.cpp
+)
+
+
+add_executable(mg_bt_executor ${SOURCES})
+
+target_include_directories(
+ mg_bt_executor
+ PRIVATE
+ ${LIBGLM_INCLUDE_DIRS}
+ include
+)
+
+ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS})
+
+install( TARGETS
+ mg_bt_executor
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY
+ behavior_trees
+ config
+ launch
+ DESTINATION share/${PROJECT_NAME}/
+)
+
+target_compile_features(mg_bt_executor PUBLIC
+ c_std_99
+ cxx_std_17
+) # Require C99 and C++17
+
+ament_package()
\ No newline at end of file
diff --git a/mg_bt/behavior_trees/calib_bt.xml b/mg_bt/behavior_trees/calib_bt.xml
new file mode 100644
index 0000000..0259d06
--- /dev/null
+++ b/mg_bt/behavior_trees/calib_bt.xml
@@ -0,0 +1,118 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Service name
+
+
+ Action server name
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Action server name
+
+
+ Service name
+
+
+
+
diff --git a/mg_bt/behavior_trees/mg_bt.btproj b/mg_bt/behavior_trees/mg_bt.btproj
new file mode 100644
index 0000000..80c4e2f
--- /dev/null
+++ b/mg_bt/behavior_trees/mg_bt.btproj
@@ -0,0 +1,47 @@
+
+
+
+
+
+
+
+
+
+ Service name
+
+
+ Action server name
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Action server name
+
+
+ Service name
+
+
+
diff --git a/mg_bt/config/mg_bt_executor.yaml b/mg_bt/config/mg_bt_executor.yaml
new file mode 100644
index 0000000..5839d6a
--- /dev/null
+++ b/mg_bt/config/mg_bt_executor.yaml
@@ -0,0 +1,14 @@
+bt_action_server:
+ ros__parameters:
+ action_name: "mg_bt_action_server"
+ tick_frequency: 100
+ groot2_port: 8420
+ ros_plugins_timeout: 1000
+
+ plugins:
+ - btcpp_ros2_samples/bt_plugins
+
+ behavior_trees:
+ - mg_bt/behavior_trees
+
+
diff --git a/mg_bt/launch/launch.py b/mg_bt/launch/launch.py
new file mode 100644
index 0000000..6abfce9
--- /dev/null
+++ b/mg_bt/launch/launch.py
@@ -0,0 +1,22 @@
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from pathlib import Path
+
+def generate_launch_description():
+
+ basedir = FindPackageShare("mg_bt")
+
+ bt_exec_config = PathJoinSubstitution([basedir, "config/mg_bt_executor.yaml"])
+
+ return LaunchDescription([
+ Node(
+ package='mg_bt',
+ executable='mg_bt_executor',
+ output="screen",
+ parameters=[bt_exec_config]
+ ),
+ ])
+
\ No newline at end of file
diff --git a/mg_bt/package.xml b/mg_bt/package.xml
new file mode 100644
index 0000000..0c34014
--- /dev/null
+++ b/mg_bt/package.xml
@@ -0,0 +1,24 @@
+
+
+
+ mg_bt
+ 0.0.0
+ Behavior for MagRob
+ Pimpest
+ Apache 2.0
+
+ ament_cmake
+
+ rclcpp
+ behaviortree_cpp
+ behaviortree_ros2
+ btcpp_ros2_interfaces
+ mg_msgs
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
\ No newline at end of file
diff --git a/mg_bt/src/mg_bt.cpp b/mg_bt/src/mg_bt.cpp
new file mode 100644
index 0000000..19e32b3
--- /dev/null
+++ b/mg_bt/src/mg_bt.cpp
@@ -0,0 +1,23 @@
+#include
+
+#include "mg_tree_executor.hpp"
+
+int main(const int argc, const char** argv) {
+ rclcpp::init(argc, argv);
+
+ std::cout << "Starting up Behavior Tree Executor" << std::endl;
+
+ rclcpp::NodeOptions options;
+
+ auto bt_exec_server = std::make_shared(options);
+
+ rclcpp::executors::MultiThreadedExecutor executor(
+ rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
+
+ executor.add_node(bt_exec_server->node());
+ executor.spin();
+ executor.remove_node(bt_exec_server->node());
+
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/mg_bt/src/mg_tree_executor.cpp b/mg_bt/src/mg_tree_executor.cpp
new file mode 100644
index 0000000..95edb52
--- /dev/null
+++ b/mg_bt/src/mg_tree_executor.cpp
@@ -0,0 +1,59 @@
+#include "mg_tree_executor.hpp"
+
+#include "behaviortree_cpp/xml_parsing.h"
+#include "tree_nodes/calib.hpp"
+#include "tree_nodes/move_point.hpp"
+#include "tree_nodes/rotate.hpp"
+#include "tree_nodes/zero.hpp"
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2/LinearMath/Transform.h"
+#include "tf2/convert.h"
+
+namespace mg {
+ MgTreeExecutor::MgTreeExecutor(const rclcpp::NodeOptions opts) : BT::TreeExecutionServer(opts) {
+ executeRegistration();
+ std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
+ tf_buffer_ = std::make_shared(node()->get_clock());
+ tf_listener_ = std::make_shared(*tf_buffer_);
+
+ const std::function()> func = std::bind(&MgTreeExecutor::position, this);
+ }
+
+ void MgTreeExecutor::onTreeCreated(BT::Tree& tree) { logger_cout_ = std::make_shared(tree); }
+
+ void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {
+ factory.registerNodeType("MovePoint", node());
+ factory.registerNodeType("RotateNode", node());
+ factory.registerNodeType("ZeroNode", node());
+ factory.registerNodeType("CalibWidth", node(), [this]() { return this->position(); });
+ }
+
+ std::pair MgTreeExecutor::position() {
+ double x = NAN;
+ double y = NAN;
+ double theta;
+ glm::vec2 pos;
+
+ auto tmsg = tf_buffer_->lookupTransform("odom", "base-link", tf2::TimePointZero);
+
+ tf2::Transform t;
+ tf2::convert(tmsg.transform, t);
+ t.getBasis().getRPY(x, y, theta);
+ auto vec3 = tmsg.transform.translation;
+
+ pos = glm::dvec2(vec3.x, vec3.y);
+ return { pos, theta };
+ }
+
+ std::optional MgTreeExecutor::onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) {
+ (void)status;
+ logger_cout_.reset();
+
+ if (was_cancelled) {
+ std::cout << "Behavior tree was cancled" << std::endl;
+ }
+
+ return std::nullopt;
+ }
+}
\ No newline at end of file
diff --git a/mg_bt/src/mg_tree_executor.hpp b/mg_bt/src/mg_tree_executor.hpp
new file mode 100644
index 0000000..fc64c7f
--- /dev/null
+++ b/mg_bt/src/mg_tree_executor.hpp
@@ -0,0 +1,25 @@
+#pragma once
+
+#include "behaviortree_ros2/tree_execution_server.hpp"
+#include "behaviortree_cpp/loggers/bt_cout_logger.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include "glm/glm.hpp"
+
+namespace mg {
+ class MgTreeExecutor : public BT::TreeExecutionServer {
+ public:
+ MgTreeExecutor(const rclcpp::NodeOptions opts);
+ void onTreeCreated(BT::Tree& tree) override;
+ void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override;
+
+ std::optional onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override;
+
+ std::pair position();
+
+ private:
+ std::shared_ptr logger_cout_;
+ tf2_ros::Buffer::SharedPtr tf_buffer_;
+ std::shared_ptr tf_listener_;
+ };
+}
\ No newline at end of file
diff --git a/mg_bt/src/tree_nodes/calib.hpp b/mg_bt/src/tree_nodes/calib.hpp
new file mode 100644
index 0000000..cd6511d
--- /dev/null
+++ b/mg_bt/src/tree_nodes/calib.hpp
@@ -0,0 +1,44 @@
+#pragma once
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "mg_msgs/srv/send_double.hpp"
+#include "glm/glm.hpp"
+
+namespace mg {
+
+ using PosFuncSig = std::function()>;
+
+ class CalibWidthNode : public BT::RosServiceNode {
+ public:
+ CalibWidthNode(const std::string& name,
+ const BT::NodeConfig& conf,
+ const BT::RosNodeParams& params,
+ const PosFuncSig pos_query) :
+ BT::RosServiceNode(name, conf, params), pos_query_(pos_query) { }
+
+ static BT::PortsList providedPorts() {
+ return providedBasicPorts({ BT::InputPort("PreviousWidth"),
+ BT::InputPort("Count", 1, {}),
+ BT::OutputPort("NewWidth") });
+ }
+
+ bool setRequest(typename Request::SharedPtr& request) override {
+ auto [pos, theta] = pos_query_();
+ double width = getInput("PreviousWidth").value();
+ int count = getInput("Count").value();
+ double new_width = width * (1 + (theta / (2 * M_PI * count)));
+
+ RCLCPP_INFO(logger(), "Setting width to: %lf", new_width);
+
+ request->set__data(count);
+ setOutput("Count", new_width);
+ return true;
+ }
+
+ BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ private:
+ const PosFuncSig pos_query_;
+ };
+}
\ No newline at end of file
diff --git a/mg_bt/src/tree_nodes/move_point.hpp b/mg_bt/src/tree_nodes/move_point.hpp
new file mode 100644
index 0000000..2bf0cc4
--- /dev/null
+++ b/mg_bt/src/tree_nodes/move_point.hpp
@@ -0,0 +1,70 @@
+#pragma once
+
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "mg_msgs/action/move_point.hpp"
+
+namespace mg {
+
+ class MovePointNode : public BT::RosActionNode {
+ public:
+ MovePointNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+ BT::RosActionNode(name, conf, params) { }
+
+ static BT::PortsList providedPorts() {
+ return providedBasicPorts({ BT::InputPort("x_goal"),
+ BT::InputPort("y_goal"),
+ BT::InputPort("tolerance"),
+ BT::InputPort("max_wheel_speed"),
+ BT::InputPort("max_angular"),
+ BT::InputPort("max_vel"),
+ BT::InputPort("pos_mult"),
+ BT::InputPort("ornt_mult"),
+ BT::InputPort("t_ornt_mult"),
+ BT::InputPort("obst_mult_a"),
+ BT::InputPort("obst_mult_b"),
+ BT::InputPort("obst_mult_c"),
+ BT::InputPort("IgnoreList", "", {}) });
+ }
+
+ bool setGoal(Goal& goal) override {
+ auto x_goal = getInput("x_goal");
+ auto y_goal = getInput("y_goal");
+ auto tolerance = getInput("tolerance");
+ auto max_wheel_speed = getInput("max_wheel_speed");
+ auto max_angular = getInput("max_angular");
+ auto max_vel = getInput("max_vel");
+ auto pos_mult = getInput("pos_mult");
+ auto ornt_mult = getInput("ornt_mult");
+ auto t_ornt_mult = getInput("t_ornt_mult");
+ auto obst_mult_a = getInput("obst_mult_a");
+ auto obst_mult_b = getInput("obst_mult_b");
+ auto obst_mult_c = getInput("obst_mult_c");
+ goal.x = x_goal.value();
+ goal.y = y_goal.value();
+ goal.tolerance = tolerance.value_or(goal.tolerance);
+ goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
+ goal.max_angular = max_angular.value_or(goal.max_angular);
+ goal.max_vel = max_vel.value_or(goal.max_vel);
+ goal.pos_mult = pos_mult.value_or(goal.pos_mult);
+ goal.ornt_mult = ornt_mult.value_or(goal.ornt_mult);
+ goal.t_ornt_mult = t_ornt_mult.value_or(goal.t_ornt_mult);
+ goal.obst_mult_a = obst_mult_a.value_or(goal.obst_mult_a);
+ goal.obst_mult_b = obst_mult_b.value_or(goal.obst_mult_b);
+ goal.obst_mult_c = obst_mult_c.value_or(goal.obst_mult_c);
+ return true;
+ }
+
+ void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
+
+ BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
+ return (wr.result->error == ActionType::Result::SUCCESS) ? BT::NodeStatus::SUCCESS :
+ BT::NodeStatus::FAILURE;
+ }
+
+ BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
+ RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+ return BT::NodeStatus::FAILURE;
+ }
+ };
+
+}
\ No newline at end of file
diff --git a/mg_bt/src/tree_nodes/rotate.hpp b/mg_bt/src/tree_nodes/rotate.hpp
new file mode 100644
index 0000000..377b844
--- /dev/null
+++ b/mg_bt/src/tree_nodes/rotate.hpp
@@ -0,0 +1,67 @@
+#pragma once
+
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "mg_msgs/action/rotate.hpp"
+
+namespace mg {
+
+ class RotateNode : public BT::RosActionNode {
+ public:
+ RotateNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+ BT::RosActionNode(name, conf, params) { }
+
+ static BT::PortsList providedPorts() {
+ return providedBasicPorts({ BT::InputPort("angle"),
+ BT::InputPort("tolerance"),
+ BT::InputPort("max_wheel_speed"),
+ BT::InputPort("max_angular"),
+ BT::InputPort("max_vel"),
+ BT::InputPort("pos_mult"),
+ BT::InputPort("ornt_mult"),
+ BT::InputPort("t_ornt_mult"),
+ BT::InputPort("obst_mult_a"),
+ BT::InputPort("obst_mult_b"),
+ BT::InputPort("obst_mult_c"),
+ BT::InputPort("IgnoreList", "", {}) });
+ }
+
+ bool setGoal(Goal& goal) override {
+ auto angle = getInput("angle");
+ auto tolerance = getInput("tolerance");
+ auto max_wheel_speed = getInput("max_wheel_speed");
+ auto max_angular = getInput("max_angular");
+ auto max_vel = getInput("max_vel");
+ auto pos_mult = getInput("pos_mult");
+ auto ornt_mult = getInput("ornt_mult");
+ auto t_ornt_mult = getInput("t_ornt_mult");
+ auto obst_mult_a = getInput("obst_mult_a");
+ auto obst_mult_b = getInput("obst_mult_b");
+ auto obst_mult_c = getInput("obst_mult_c");
+ goal.angle = angle.value();
+ goal.tolerance = tolerance.value_or(goal.tolerance);
+ goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
+ goal.max_angular = max_angular.value_or(goal.max_angular);
+ goal.max_vel = max_vel.value_or(goal.max_vel);
+ goal.pos_mult = pos_mult.value_or(goal.pos_mult);
+ goal.ornt_mult = ornt_mult.value_or(goal.ornt_mult);
+ goal.t_ornt_mult = t_ornt_mult.value_or(goal.t_ornt_mult);
+ goal.obst_mult_a = obst_mult_a.value_or(goal.obst_mult_a);
+ goal.obst_mult_b = obst_mult_b.value_or(goal.obst_mult_b);
+ goal.obst_mult_c = obst_mult_c.value_or(goal.obst_mult_c);
+ return true;
+ }
+
+ void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
+
+ BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
+ return (wr.result->error == ActionType::Result::SUCCESS) ? BT::NodeStatus::SUCCESS :
+ BT::NodeStatus::FAILURE;
+ }
+
+ BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
+ RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+ return BT::NodeStatus::FAILURE;
+ }
+ };
+
+}
\ No newline at end of file
diff --git a/mg_bt/src/tree_nodes/zero.hpp b/mg_bt/src/tree_nodes/zero.hpp
new file mode 100644
index 0000000..594922b
--- /dev/null
+++ b/mg_bt/src/tree_nodes/zero.hpp
@@ -0,0 +1,20 @@
+#pragma once
+
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "std_srvs/srv/empty.hpp"
+
+namespace mg {
+
+ class ZeroNode : public BT::RosServiceNode {
+ public:
+ ZeroNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+ BT::RosServiceNode(name, conf, params) { }
+
+ bool setRequest(typename Request::SharedPtr&) override { return true; }
+
+ BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
+ return BT::NodeStatus::SUCCESS;
+ }
+ };
+
+}
\ No newline at end of file