rbs_bt_executor: add new BT node EnvStarter

rbs_bt_executor/src/EnvManager.cpp

@@ -0,0 +1,51 @@
+#include <behavior_tree/BtService.hpp>
+
+#include <behaviortree_cpp_v3/basic_types.h>
+#include <env_manager_interfaces/srv/detail/start_env__struct.hpp>
+#include <rclcpp/logging.hpp>
+#include <string>
+
+#include "env_manager_interfaces/srv/start_env.hpp"
+#include "env_manager_interfaces/srv/unload_env.hpp"
+
+using EnvStarterService = env_manager_interfaces::srv::StartEnv;
+
+class EnvManagerStarter : public BtService<EnvStarterService> {
+public:
+  EnvManagerStarter(const std::string &name,
+                    const BT::NodeConfiguration &config)
+      : BtService<EnvStarterService>(name, config) {
+    RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
+  }
+
+  EnvStarterService::Request::SharedPtr populate_request() override {
+    auto request = std::make_shared<EnvStarterService::Request>();
+    std::string env_name = getInput<std::string>("env_name").value();
+    std::string env_class = getInput<std::string>("env_class").value();
+    request->name = env_name;
+    request->type = env_class;
+
+    return request;
+  }
+
+  BT::NodeStatus handle_response(
+      const EnvStarterService::Response::SharedPtr response) override {
+    if (response->ok) {
+      return BT::NodeStatus::SUCCESS;
+    }
+    return BT::NodeStatus::FAILURE;
+  }
+
+  static BT::PortsList providedPorts() {
+    return providedBasicPorts({
+        BT::InputPort<std::string>("env_name"),
+        BT::InputPort<std::string>("env_class"),
+    });
+  }
+};
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+BT_REGISTER_NODES(factory) {
+  factory.registerNodeType<EnvManagerStarter>("EnvStarter");
+}