Added mg_bt

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<tf2_ros::Buffer>(node()->get_clock());
+        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+        const std::function<std::pair<glm::vec2, double>()> func = std::bind(&MgTreeExecutor::position, this);
+    }
+
+    void MgTreeExecutor::onTreeCreated(BT::Tree& tree) { logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree); }
+
+    void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {
+        factory.registerNodeType<mg::MovePointNode>("MovePoint", node());
+        factory.registerNodeType<mg::RotateNode>("RotateNode", node());
+        factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
+        factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
+    }
+
+    std::pair<glm::vec2, double> 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<std::string> 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;
+    }
+}