rbs_bt_executor: add new BT node EnvStarter

rbs_bt_executor/CMakeLists.txt

@@ -20,6 +20,7 @@ find_package(behavior_tree REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 find_package(rcl_interfaces REQUIRED)
+find_package(env_manager_interfaces REQUIRED)
 
 if (NOT CMAKE_CXX_STANDARD)
     set(CMAKE_CXX_STANDARD 17)
@@ -41,6 +42,7 @@ set(dependencies
     control_msgs
     lifecycle_msgs
     rcl_interfaces
+    env_manager_interfaces
 )
 
 include_directories(include)
@@ -66,6 +68,9 @@ list(APPEND plugin_libs rbs_assemble_process_state)
 add_library(rbs_pose_estimation SHARED src/PoseEstimation.cpp)
 list(APPEND plugin_libs rbs_pose_estimation)
 
+add_library(rbs_env_manager_starter SHARED src/EnvManager.cpp)
+list(APPEND plugin_libs rbs_env_manager_starter)
+
 foreach(bt_plugin ${plugin_libs})
     ament_target_dependencies(${bt_plugin} ${dependencies})
     target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

rbs_bt_executor/bt_trees/test_tree.xml

@@ -3,7 +3,7 @@
     <BehaviorTree ID="PoseEstimation">
         <Sequence>
             <Action ID="PoseEstimation" 
-            ObjectName="!/home/shalenikol/robossembler_ws/src/robossembler-ros2/rbs_perception/config/str_param.json"
+            ObjectName="!/rbs_ws/src/robossembler-ros2/rbs_perception/config/str_param.json"
             ReqKind = "calibrate"
             server_name="/pose_estimation/change_state" 
             server_timeout="1000"/>

rbs_bt_executor/bt_trees/test_tree_env_manager.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="EnvManagerBT">
+  <BehaviorTree ID="EnvManagerBT">
+    <Sequence>
+      <Action ID="EnvStarter"
+      env_name="gz_enviroment"
+      env_class="gz_enviroment::GzEnviroment"
+        server_name="/env_manager/start_env"
+        server_timeout="1000" />
+    </Sequence>
+  </BehaviorTree>
+</root>

rbs_bt_executor/launch/rbs_executor.launch.py

@@ -51,13 +51,14 @@ def generate_launch_description():
             executable='bt_engine',
             #prefix=['xterm -e gdb -ex run --args'],
             parameters=[
-                {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_gripper.xml')},
+                {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree_env_manager.xml')},
                 {'plugins':["rbs_skill_move_topose_bt_action_client",
                             "rbs_skill_get_pick_place_pose_service_client",
                             "rbs_skill_gripper_move_bt_action_client",
                             "rbs_skill_move_joint_state",
                             "rbs_add_planning_scene_object",
-                            "rbs_pose_estimation"
+                            "rbs_pose_estimation",
+                            "rbs_env_manager_starter"
                 ]},
                 # gripperPoints,
                 robot_description_semantic

rbs_bt_executor/package.xml

@@ -11,6 +11,7 @@
 
   <depend>rbs_skill_interfaces</depend>
   <depend>component_interfaces</depend>
+  <depend>env_manager_interfaces</depend>
   <depend>behavior_tree</depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

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");
+}