From 6d3c5c27026f932c25c378ee6db1a8629ea41e57 Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Tue, 19 Dec 2023 08:18:41 +0300
Subject: [PATCH] rbs_bt_executor: add new BT node EnvStarter

---
 rbs_bt_executor/CMakeLists.txt                |  5 ++
 rbs_bt_executor/bt_trees/test_tree.xml        |  2 +-
 .../bt_trees/test_tree_env_manager.xml        | 12 +++++
 rbs_bt_executor/launch/rbs_executor.launch.py |  5 +-
 rbs_bt_executor/package.xml                   |  1 +
 rbs_bt_executor/src/EnvManager.cpp            | 51 +++++++++++++++++++
 6 files changed, 73 insertions(+), 3 deletions(-)
 create mode 100644 rbs_bt_executor/bt_trees/test_tree_env_manager.xml
 create mode 100644 rbs_bt_executor/src/EnvManager.cpp

diff --git a/rbs_bt_executor/CMakeLists.txt b/rbs_bt_executor/CMakeLists.txt
index 26f0109..cfdb430 100644
--- a/rbs_bt_executor/CMakeLists.txt
+++ b/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)
diff --git a/rbs_bt_executor/bt_trees/test_tree.xml b/rbs_bt_executor/bt_trees/test_tree.xml
index b3be4c3..1fb5fd6 100644
--- a/rbs_bt_executor/bt_trees/test_tree.xml
+++ b/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"/>
diff --git a/rbs_bt_executor/bt_trees/test_tree_env_manager.xml b/rbs_bt_executor/bt_trees/test_tree_env_manager.xml
new file mode 100644
index 0000000..9cc366e
--- /dev/null
+++ b/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>
\ No newline at end of file
diff --git a/rbs_bt_executor/launch/rbs_executor.launch.py b/rbs_bt_executor/launch/rbs_executor.launch.py
index 380312f..a968180 100644
--- a/rbs_bt_executor/launch/rbs_executor.launch.py
+++ b/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
diff --git a/rbs_bt_executor/package.xml b/rbs_bt_executor/package.xml
index e96c0d1..44e5d7c 100644
--- a/rbs_bt_executor/package.xml
+++ b/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>
diff --git a/rbs_bt_executor/src/EnvManager.cpp b/rbs_bt_executor/src/EnvManager.cpp
new file mode 100644
index 0000000..85e628b
--- /dev/null
+++ b/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");
+}