Created mechanism node

toid_bt/include/toid_bt/plugins/send_text_action.hpp

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+
+#include "toid_msgs/srv/send_string.hpp"
+
+namespace toid
+{
+
+class SendTextNode : public BT::RosServiceNode<toid_msgs::srv::SendString>
+{
+public:
+  SendTextNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosServiceNode<toid_msgs::srv::SendString>(name, conf, params)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<std::string>("text"),
+    });
+  }
+
+  bool setRequest(typename Request::SharedPtr &req) override { 
+    std::string text = getInput<std::string>("text").value_or("");
+    req->text = text;
+
+    return true;
+  }
+
+  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+};
+
+}  // namespace toid