Added the ability to set postion

mg_bt/src/tree_nodes/set_pos.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "mg_msgs/srv/send_pose2d.hpp"
+
+namespace mg {
+
+    class SendPoseNode : public BT::RosServiceNode<mg_msgs::srv::SendPose2d> {
+      public:
+        SendPoseNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+          BT::RosServiceNode<mg_msgs::srv::SendPose2d>(name, conf, params) { }
+
+        static BT::PortsList providedPorts() {
+            return providedBasicPorts({ BT::InputPort<double>("x"),
+                                        BT::InputPort<double>("y"),
+                                        BT::InputPort<double>("angle"),
+                                        BT::InputPort<bool>("isDegree", "True", {}) });
+        }
+
+        bool setRequest(typename Request::SharedPtr& req) override {
+            req->x     = getInput<double>("x").value();
+            req->y     = getInput<double>("y").value();
+            req->theta = getInput<double>("angle").value();
+            if (getInput<bool>("isDegree").value()) {
+                req->theta *= M_PI / 180;
+            }
+            return true;
+        }
+
+        BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
+            return BT::NodeStatus::SUCCESS;
+        }
+    };
+
+}