From 0c6b01bbfe75dbe9460a4bb61468e85d36df1947 Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Mon, 9 Dec 2024 18:25:32 +0300
Subject: [PATCH] feat: improve behavior tree executor and dependency
 management

- **CMakeLists.txt**:
  - Commented out the unused `rbs_env_manager_starter` library.
- **Behavior Trees**:
  - Added new `grasp_object.xml` behavior tree for grasping operations.
  - Modified `test_tree.xml` to update `robot_name` in the `WorkspaceMovement` subtree.
- **Source Code Updates**:
  - Removed `EnvManager.cpp`, deprecating its functionality.
  - Formatting `GetWorkspace.cpp`
  - Formatting `MoveGripper.cpp`
  - Formatting `MoveToJointStates.cpp`
  - Formatting `PoseEstimation.cpp` and `TreeRunner.cpp`
- **Assembly Config Service**:
  - Added broadcasting for grasp pose transformations in `assembly_config_service.py`.

These updates enhance modularity, maintainability, and the overall functionality of the behavior tree executor.
---
 rbs_bt_executor/CMakeLists.txt                |  4 +-
 rbs_bt_executor/bt_trees/grasp_object.xml     | 36 ++++++++++++
 rbs_bt_executor/bt_trees/test_tree.xml        |  2 +-
 rbs_bt_executor/src/EnvManager.cpp            | 43 ---------------
 rbs_bt_executor/src/GetWorkspace.cpp          | 55 +++++++++----------
 rbs_bt_executor/src/MoveGripper.cpp           | 30 +++++-----
 rbs_bt_executor/src/MoveToJointStates.cpp     |  7 +--
 rbs_bt_executor/src/MoveToPose.cpp            |  7 +--
 rbs_bt_executor/src/PoseEstimation.cpp        |  5 +-
 rbs_bt_executor/src/TreeRunner.cpp            |  2 +-
 .../scripts/assembly_config_service.py        | 17 ++++++
 11 files changed, 110 insertions(+), 98 deletions(-)
 create mode 100644 rbs_bt_executor/bt_trees/grasp_object.xml
 delete mode 100644 rbs_bt_executor/src/EnvManager.cpp

diff --git a/rbs_bt_executor/CMakeLists.txt b/rbs_bt_executor/CMakeLists.txt
index b1efb12..78910e9 100644
--- a/rbs_bt_executor/CMakeLists.txt
+++ b/rbs_bt_executor/CMakeLists.txt
@@ -80,8 +80,8 @@ list(APPEND plugin_libs rbs_skill_move_joint_state)
 add_library(rbs_object_detection SHARED src/ObjectDetection.cpp)
 list(APPEND plugin_libs rbs_object_detection)
 
-add_library(rbs_env_manager_starter SHARED src/EnvManager.cpp)
-list(APPEND plugin_libs rbs_env_manager_starter)
+# add_library(rbs_env_manager_starter SHARED src/EnvManager.cpp)
+# list(APPEND plugin_libs rbs_env_manager_starter)
 
 add_library(rbs_skill_move_topose_array_bt_action_client SHARED src/MoveToPoseArray.cpp)
 list(APPEND plugin_libs rbs_skill_move_topose_array_bt_action_client)
diff --git a/rbs_bt_executor/bt_trees/grasp_object.xml b/rbs_bt_executor/bt_trees/grasp_object.xml
new file mode 100644
index 0000000..3a64022
--- /dev/null
+++ b/rbs_bt_executor/bt_trees/grasp_object.xml
@@ -0,0 +1,36 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="GraspObject">
+    <Sequence>
+
+      <Script code="action:='/cartesian_move_to_pose'" />
+
+      <Action ID="GetWorkspace" 
+        object_name="{object_name}" 
+        service_name="{get_workspace}" 
+        grasp_pose="{grasp_pose}" 
+        pregrasp_pose="{pregrasp_pose}" 
+        postgrasp_pose="{postgrasp_pose}" />
+
+      <Action ID="MoveToPose" pose="{pregrasp_pose}"  robot_name="{robot_name}" action_name="{action}" />
+      <Action ID="MoveToPose" pose="{grasp_pose}"     robot_name="{robot_name}" action_name="{action}" />
+      <Action ID="MoveToPose" pose="{postgrasp_pose}" robot_name="{robot_name}" action_name="{action}" />
+    </Sequence>
+  </BehaviorTree>
+
+  <TreeNodesModel>
+    <Action ID="GetWorkspace">
+      <input_port name="object_name" />
+      <input_port name="service_name" />
+      <output_port name="grasp_pose" />
+      <output_port name="pregrasp_pose" />
+      <output_port name="postgrasp_pose" />
+    </Action>
+
+    <Action ID="MoveToPose">
+      <input_port name="pose" />
+      <input_port name="robot_name" />
+      <input_port name="action_name" />
+    </Action>
+  </TreeNodesModel>
+</root>
diff --git a/rbs_bt_executor/bt_trees/test_tree.xml b/rbs_bt_executor/bt_trees/test_tree.xml
index 889382f..02c2066 100644
--- a/rbs_bt_executor/bt_trees/test_tree.xml
+++ b/rbs_bt_executor/bt_trees/test_tree.xml
@@ -4,7 +4,7 @@
         <Sequence>
       <!-- <Action ID="EnvManager" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"  -->
         <!-- service_name="/env_manager/start_env" /> -->
-          <SubTree ID="WorkspaceMovement" robot_name="arm1" />
+          <SubTree ID="WorkspaceMovement" robot_name="rbs_arm" />
         </Sequence>
     </BehaviorTree>
 
diff --git a/rbs_bt_executor/src/EnvManager.cpp b/rbs_bt_executor/src/EnvManager.cpp
deleted file mode 100644
index 5e69775..0000000
--- a/rbs_bt_executor/src/EnvManager.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-
-#include "behaviortree_ros2/bt_service_node.hpp"
-#include "env_manager_interfaces/srv/start_env.hpp"
-#include "env_manager_interfaces/srv/unload_env.hpp"
-#include "geometry_msgs/msg/pose_array.hpp"
-
-#include "rbs_utils/rbs_utils.hpp"
-#include <behaviortree_cpp/basic_types.h>
-#include "behaviortree_ros2/plugins.hpp"
-#include <string>
-
-using EnvManagerService = env_manager_interfaces::srv::StartEnv;
-using namespace BT;
-
-class EnvManager : public RosServiceNode<EnvManagerService> {
-public:
-  EnvManager(const std::string &name, const NodeConfig &conf,
-                    const RosNodeParams &params)
-      : RosServiceNode<EnvManagerService>(name, conf, params) {}
-
-  static PortsList providedPorts() {
-    return providedBasicPorts({
-        InputPort<std::string>("env_name"),
-        InputPort<std::string>("env_class"),
-    });
-  }
-
-  bool setRequest(Request::SharedPtr &request) override {
-    getInput("env_name", request->name);
-    getInput("env_class", request->type);
-    return true;
-  }
-  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
-    if (!response->ok) {
-      return NodeStatus::FAILURE;
-    }
-    return NodeStatus::SUCCESS;
-  }
-  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
-};
-
-CreateRosNodePlugin(EnvManager, "EnvManager");
-
diff --git a/rbs_bt_executor/src/GetWorkspace.cpp b/rbs_bt_executor/src/GetWorkspace.cpp
index 2aaae68..cdf93b8 100644
--- a/rbs_bt_executor/src/GetWorkspace.cpp
+++ b/rbs_bt_executor/src/GetWorkspace.cpp
@@ -1,8 +1,8 @@
 #include "behaviortree_ros2/bt_service_node.hpp"
 
+#include "behaviortree_ros2/plugins.hpp"
 #include "rbs_utils_interfaces/srv/get_workspace.hpp"
 #include <behaviortree_cpp/basic_types.h>
-#include "behaviortree_ros2/plugins.hpp"
 #include <geometry_msgs/msg/detail/point__struct.hpp>
 #include <geometry_msgs/msg/detail/pose_array__struct.hpp>
 #include <geometry_msgs/msg/detail/quaternion__struct.hpp>
@@ -14,14 +14,14 @@ using namespace BT;
 class GetWorkspace : public RosServiceNode<GetWorkspaceService> {
 public:
   GetWorkspace(const std::string &name, const NodeConfig &conf,
-                    const RosNodeParams &params)
+               const RosNodeParams &params)
       : RosServiceNode<GetWorkspaceService>(name, conf, params) {}
 
   static PortsList providedPorts() {
-    return providedBasicPorts({
-        InputPort<std::string>("type"),
-        OutputPort<std::shared_ptr<geometry_msgs::msg::PoseArray>>("workspace")
-    });
+    return providedBasicPorts(
+        {InputPort<std::string>("type"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::PoseArray>>(
+             "workspace")});
   }
 
   bool setRequest(Request::SharedPtr &request) override {
@@ -29,37 +29,36 @@ public:
     return true;
   }
   NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
-      if (!response->ok) {
-          return NodeStatus::FAILURE;
-      }
+    if (!response->ok) {
+      return NodeStatus::FAILURE;
+    }
 
-      auto workspace = response->workspace;
-      auto workspace_arr = std::make_shared<geometry_msgs::msg::PoseArray>();
-      auto quat = std::make_shared<geometry_msgs::msg::Quaternion>();
+    auto workspace = response->workspace;
+    auto workspace_arr = std::make_shared<geometry_msgs::msg::PoseArray>();
+    auto quat = std::make_shared<geometry_msgs::msg::Quaternion>();
 
-      quat->w = 0.0;
-      quat->x = 0.0;
-      quat->y = 1.0;
-      quat->z = 0.0;
+    quat->w = 0.0;
+    quat->x = 0.0;
+    quat->y = 1.0;
+    quat->z = 0.0;
 
-      workspace_arr->poses.resize(workspace.size());
+    workspace_arr->poses.resize(workspace.size());
 
-      size_t i = 0;
-      for (auto& point : workspace) {
-          point.z += 0.35;
+    size_t i = 0;
+    for (auto &point : workspace) {
+      point.z += 0.35;
 
-          geometry_msgs::msg::Pose pose;
-          pose.position = point;
-          pose.orientation = *quat;
+      geometry_msgs::msg::Pose pose;
+      pose.position = point;
+      pose.orientation = *quat;
 
-          workspace_arr->poses[i++] = pose;
-      }
+      workspace_arr->poses[i++] = pose;
+    }
 
-      setOutput("workspace", workspace_arr);
-      return NodeStatus::SUCCESS;
+    setOutput("workspace", workspace_arr);
+    return NodeStatus::SUCCESS;
   }
   // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
 };
 
 CreateRosNodePlugin(GetWorkspace, "GetWorkspace");
-
diff --git a/rbs_bt_executor/src/MoveGripper.cpp b/rbs_bt_executor/src/MoveGripper.cpp
index 6e020d0..88eb3b4 100644
--- a/rbs_bt_executor/src/MoveGripper.cpp
+++ b/rbs_bt_executor/src/MoveGripper.cpp
@@ -1,10 +1,11 @@
 #include "behaviortree_ros2/bt_action_node.hpp"
-#include "rbs_skill_interfaces/action/gripper_command.hpp"
+#include <behaviortree_cpp/basic_types.h>
 #include <behaviortree_cpp/tree_node.h>
 #include <behaviortree_ros2/plugins.hpp>
 #include <behaviortree_ros2/ros_node_params.hpp>
+#include <control_msgs/action/detail/gripper_command__struct.hpp>
 
-using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
+using GripperCommand = control_msgs::action::GripperCommand;
 using namespace BT;
 
 class GripperControl : public RosActionNode<GripperCommand> {
@@ -12,28 +13,31 @@ public:
   GripperControl(const std::string &name, const NodeConfig &conf,
                  const RosNodeParams &params)
       : RosActionNode<GripperCommand>(name, conf, params) {
-    if (!node_.lock()->has_parameter("open")) {
-      node_.lock()->declare_parameter("open", position.open);
-    }
-    if (!node_.lock()->has_parameter("close")) {
-      node_.lock()->declare_parameter("close", position.close);
-    }
+
+    auto state = getInput<std::string>("state").value();
+    auto open_limit = getInput<double>("open_limit").value();
+    auto close_limit = getInput<double>("close_limit").value();
+
+    position.open = open_limit;
+    position.close = close_limit;
   }
 
   static PortsList providedPorts() {
     return providedBasicPorts({
-        InputPort<std::string>("pose"),
+        InputPort<std::string>("state"),
+        InputPort<double>("open_limit"),
+        InputPort<double>("close_limit"),
     });
   }
 
   bool setGoal(RosActionNode::Goal &goal) override {
-    getInput("pose", pose);
-    goal.position = node_.lock()->get_parameter(pose).as_double();
+    goal.command.position = position.open;
+    goal.command.max_effort = 0.0;
     return true;
   }
 
-  NodeStatus onResultReceived(const WrappedResult& wr) override {
-    if (!wr.result->success) {
+  NodeStatus onResultReceived(const WrappedResult &wr) override {
+    if (!wr.result->reached_goal) {
       return NodeStatus::FAILURE;
     }
     return NodeStatus::SUCCESS;
diff --git a/rbs_bt_executor/src/MoveToJointStates.cpp b/rbs_bt_executor/src/MoveToJointStates.cpp
index 90fc558..893ab4f 100644
--- a/rbs_bt_executor/src/MoveToJointStates.cpp
+++ b/rbs_bt_executor/src/MoveToJointStates.cpp
@@ -1,10 +1,10 @@
 #include "behaviortree_ros2/bt_action_node.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
 #include <behaviortree_cpp/basic_types.h>
 #include <behaviortree_cpp/tree_node.h>
 #include <behaviortree_ros2/plugins.hpp>
 #include <behaviortree_ros2/ros_node_params.hpp>
-#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
 
 using namespace BT;
 using MoveToJointStateAction =
@@ -21,14 +21,13 @@ public:
                                InputPort<std::vector<double>>("JointState")});
   }
 
-
-  bool setGoal(RosActionNode::Goal& goal) override {
+  bool setGoal(RosActionNode::Goal &goal) override {
     getInput("robot_name", goal.robot_name);
     getInput("JointState", goal.joint_values);
     return true;
   }
 
-  NodeStatus onResultReceived(const WrappedResult& wr) override {
+  NodeStatus onResultReceived(const WrappedResult &wr) override {
 
     if (!wr.result->success) {
       return NodeStatus::FAILURE;
diff --git a/rbs_bt_executor/src/MoveToPose.cpp b/rbs_bt_executor/src/MoveToPose.cpp
index 624862f..0bc336d 100644
--- a/rbs_bt_executor/src/MoveToPose.cpp
+++ b/rbs_bt_executor/src/MoveToPose.cpp
@@ -10,7 +10,7 @@ using MoveitSendPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
 class MoveToPose : public RosActionNode<MoveitSendPoseAction> {
 public:
   MoveToPose(const std::string &name, const NodeConfig &conf,
-                 const RosNodeParams &params)
+             const RosNodeParams &params)
       : RosActionNode<MoveitSendPoseAction>(name, conf, params) {}
 
   static BT::PortsList providedPorts() {
@@ -19,20 +19,19 @@ public:
          BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose")});
   }
 
-  bool setGoal(RosActionNode::Goal& goal) override {
+  bool setGoal(RosActionNode::Goal &goal) override {
     getInput("robot_name", goal.robot_name);
     getInput("pose", goal.target_pose);
     goal.duration = 2.0;
     return true;
   }
 
-  NodeStatus onResultReceived(const WrappedResult& wr) override {
+  NodeStatus onResultReceived(const WrappedResult &wr) override {
     if (!wr.result->success) {
       return NodeStatus::FAILURE;
     }
     return NodeStatus::SUCCESS;
   }
-    
 };
 
 CreateRosNodePlugin(MoveToPose, "MoveToPose");
diff --git a/rbs_bt_executor/src/PoseEstimation.cpp b/rbs_bt_executor/src/PoseEstimation.cpp
index 939e0d6..df63330 100644
--- a/rbs_bt_executor/src/PoseEstimation.cpp
+++ b/rbs_bt_executor/src/PoseEstimation.cpp
@@ -72,8 +72,9 @@ private:
     // ParamSetter param_setter(m_params_client);
     if (req_type == "configure") {
       set_mesh_param();
-      // auto str_mesh_param = std::make_shared<SetParamShareDirectoryStrategy>("model_name", m_model_name);
-      // param_setter.setStrategy(str_mesh_param);
+      // auto str_mesh_param =
+      // std::make_shared<SetParamShareDirectoryStrategy>("model_name",
+      // m_model_name); param_setter.setStrategy(str_mesh_param);
       // param_setter.setParam()
 
       m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
diff --git a/rbs_bt_executor/src/TreeRunner.cpp b/rbs_bt_executor/src/TreeRunner.cpp
index 0fc5b38..317e4e8 100644
--- a/rbs_bt_executor/src/TreeRunner.cpp
+++ b/rbs_bt_executor/src/TreeRunner.cpp
@@ -1,5 +1,5 @@
-#include <behaviortree_ros2/tree_execution_server.hpp>
 #include <behaviortree_cpp/loggers/bt_cout_logger.h>
+#include <behaviortree_ros2/tree_execution_server.hpp>
 
 // #include <rclcpp/logging.hpp>
 // #include <std_msgs/msg/float32.hpp>
diff --git a/rbs_utils/rbs_utils/scripts/assembly_config_service.py b/rbs_utils/rbs_utils/scripts/assembly_config_service.py
index 594e244..1b67153 100755
--- a/rbs_utils/rbs_utils/scripts/assembly_config_service.py
+++ b/rbs_utils/rbs_utils/scripts/assembly_config_service.py
@@ -42,6 +42,23 @@ class AssemblyConfigService(Node):
                     relative_part.pose.orientation.w
                 )
             )
+        
+        for grasp_pose in self.assembly_config.grasp_pose:
+            tf2_broadcaster.broadcast_tf(
+                grasp_pose.relative_at,
+                grasp_pose.name,
+                translation=(
+                    grasp_pose.pose.position.x,
+                    grasp_pose.pose.position.y,
+                    grasp_pose.pose.position.z,
+                ),
+                rotation=(
+                    grasp_pose.pose.orientation.x,
+                    grasp_pose.pose.orientation.y,
+                    grasp_pose.pose.orientation.z,
+                    grasp_pose.pose.orientation.w,
+                )
+            )
 
 
         # Services