Enhance grasping task handling and behavior tree configurations

- Improved `get_grasp_pose_callback` with regex for grasp pose matching and added fallback mechanisms for `pregrasp_pose` and `postgrasp_pose`.
- Introduced `_create_pose_msg` and `_create_pose_msg_from_name` helper functions for dynamic transform-to-pose conversion.
- Updated `GetGraspPose.srv` to include `pregrasp_pose` and `postgrasp_pose` in the service response structure.
- Enhanced TF listener integration in `assembly_config_service.py` for dynamic pose retrieval and parameter management.
- Added `rbs_utils_interfaces` dependency in `CMakeLists.txt` and introduced new `GetGraspPose` node for behavior tree integration.
- Replaced `MoveGripper` with `GripperCommand` to improve gripper state transitions.
- Refactored `MoveToPose` and `GetWorkspace` nodes for enhanced logging and parameter handling.
- Updated `workspace_movement.xml` and `grasp_object.xml` with improved actions and configurations.

rbs_bt_executor/CMakeLists.txt

@@ -27,6 +27,7 @@ find_package(env_manager_interfaces REQUIRED)
 find_package(rbs_utils REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(rbs_utils_interfaces REQUIRED)
 
 if (NOT CMAKE_CXX_STANDARD)
     set(CMAKE_CXX_STANDARD 17)
@@ -52,6 +53,7 @@ set(dependencies
     rbs_utils
     tf2_ros
     tf2_eigen
+    rbs_utils_interfaces
 )
 
 include_directories(include)
@@ -59,11 +61,11 @@ include_directories(include)
 add_library(rbs_skill_move_topose_bt_action_client SHARED src/MoveToPose.cpp)
 list(APPEND plugin_libs rbs_skill_move_topose_bt_action_client)
 
-add_library(rbs_skill_gripper_move_bt_action_client SHARED src/MoveGripper.cpp)
+add_library(rbs_skill_gripper_move_bt_action_client SHARED src/GripperCommand.cpp)
 list(APPEND plugin_libs rbs_skill_gripper_move_bt_action_client)
 
-# add_library(rbs_skill_get_pick_place_pose_service_client SHARED src/GetPickPlacePoses.cpp)
-# list(APPEND plugin_libs rbs_skill_get_pick_place_pose_service_client)
+add_library(rbs_skill_get_pick_place_pose_service_client SHARED src/GetGraspPose.cpp)
+list(APPEND plugin_libs rbs_skill_get_pick_place_pose_service_client)
 
 add_library(rbs_skill_move_joint_state SHARED src/MoveToJointStates.cpp)
 list(APPEND plugin_libs rbs_skill_move_joint_state)

rbs_bt_executor/bt_trees/grasp_object.xml

@@ -3,23 +3,43 @@
   <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}" 
+      <Script code="open_gripper_limit:='-0.014'" />
+      <Script code="close_gripper_limit:='0.0'" />
+
+      <Script code="robot_name:='rbs_arm'" />
+      <Script code="object_name:='box'" />
+
+      <Script code="grasp_service:='/get_grasp_pose'" />
+      <Script code="move_action:='/mtp_jtc'" />
+      <Script code="grpper_action:='/gripper_controller/gripper_cmd'" />
+
+      <Action ID="GetGraspPose"
+        object_name="box"
+        service_name="{grasp_service}"
+        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}" />
+      <Action ID="MoveToPose" pose="{pregrasp_pose}" duration="5.0" robot_name="{robot_name}"
+        action_name="{move_action}" />
+
+      <Action ID="GripperCommand" open_limit="{open_gripper_limit}" close_limit="{close_gripper_limit}" state="open"
+        action_name="{grpper_action}" />
+
+      <Action ID="MoveToPose" pose="{grasp_pose}" duration="2.0" robot_name="{robot_name}"
+        action_name="{move_action}" />
+
+      <Action ID="GripperCommand" open_limit="{open_gripper_limit}" close_limit="{close_gripper_limit}" state="close"
+        action_name="{grpper_action}" />
+
+      <Action ID="MoveToPose" pose="{postgrasp_pose}" duration="2.0" robot_name="{robot_name}"
+        action_name="{move_action}" />
     </Sequence>
   </BehaviorTree>
 
   <TreeNodesModel>
-    <Action ID="GetWorkspace">
+    <Action ID="GetGraspPose">
       <input_port name="object_name" />
       <input_port name="service_name" />
       <output_port name="grasp_pose" />
@@ -32,5 +52,12 @@
       <input_port name="robot_name" />
       <input_port name="action_name" />
     </Action>
+
+    <Action ID="GripperCommand">
+      <input_port name="open_limit" />
+      <input_port name="close_limit" />
+      <input_port name="state" />
+      <input_port name="action_name"/>
+    </Action>
   </TreeNodesModel>
 </root>

rbs_bt_executor/bt_trees/workspace_movement.xml

@@ -6,7 +6,7 @@
       <!-- <Script code="action:='/'+robot_name+'/cartesian_move_to_pose'" /> -->
       <!-- <Script code="get_workspace:='/'+robot_name+'/get_workspace'"/> -->
 
-      <Script code="action:='/cartesian_move_to_pose'"/>
+      <Script code="action:='/mtp_jtc'"/>
       <Script code="get_workspace:='/get_workspace'"/>
 
       <Action ID="GetWorkspace" type="box" service_name="{get_workspace}" workspace="{workspace}"/>

rbs_bt_executor/src/GetGraspPose.cpp

@@ -0,0 +1,90 @@
+#include "behaviortree_ros2/bt_service_node.hpp"
+
+#include "behaviortree_ros2/plugins.hpp"
+#include "rbs_utils_interfaces/srv/get_grasp_pose.hpp"
+#include <behaviortree_cpp/basic_types.h>
+#include <geometry_msgs/msg/detail/point__struct.hpp>
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
+#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
+#include <memory>
+#include <rbs_utils_interfaces/srv/detail/get_grasp_pose__struct.hpp>
+#include <rclcpp/logging.hpp>
+#include <string>
+
+using GetGraspPoseService = rbs_utils_interfaces::srv::GetGraspPose;
+using namespace BT;
+
+class GetGraspPose : public RosServiceNode<GetGraspPoseService> {
+public:
+  GetGraspPose(const std::string &name, const NodeConfig &conf,
+               const RosNodeParams &params)
+      : RosServiceNode<GetGraspPoseService>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting GetGraspPose");
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts(
+        {InputPort<std::string>("object_name"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("grasp_pose"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pregrasp_pose"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>(
+             "postgrasp_pose")});
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    RCLCPP_INFO(this->logger(), "Starting send request");
+    if (!getInput("object_name", request->model_name)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "Failed to get object_name from input port");
+      return false;
+    }
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    if (!response->ok) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "Response indicates failure.");
+      return NodeStatus::FAILURE;
+    }
+
+    RCLCPP_INFO(this->node_.lock()->get_logger(),
+                "Response received successfully.");
+
+    auto logPose = [this](const std::string &pose_name,
+                          const geometry_msgs::msg::Pose &pose) {
+      RCLCPP_INFO(this->node_.lock()->get_logger(),
+                  "%s:\n"
+                  "  Position: x = %.4f, y = %.4f, z = %.4f\n"
+                  "  Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f",
+                  pose_name.c_str(), pose.position.x, pose.position.y,
+                  pose.position.z, pose.orientation.x, pose.orientation.y,
+                  pose.orientation.z, pose.orientation.w);
+    };
+
+    auto ppose = std::make_shared<geometry_msgs::msg::Pose>(response->grasp_pose);
+
+    logPose("Grasp Pose", response->grasp_pose);
+    logPose("Pre-grasp Pose", response->pregrasp_pose);
+    logPose("Post-grasp Pose", response->postgrasp_pose);
+
+    auto grasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
+    auto pregrasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
+    auto postgrasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
+
+    *grasp_pose = response->grasp_pose;
+    *pregrasp_pose = response->pregrasp_pose;
+    *postgrasp_pose = response->postgrasp_pose;
+
+    setOutput("grasp_pose", grasp_pose);
+    setOutput("pregrasp_pose", pregrasp_pose);
+    setOutput("postgrasp_pose", postgrasp_pose);
+
+    return NodeStatus::SUCCESS;
+  }
+  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
+};
+
+CreateRosNodePlugin(GetGraspPose, "GetGraspPose");

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,36 +29,37 @@ 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");
+

rbs_bt_executor/src/GripperCommand.cpp

@@ -4,6 +4,7 @@
 #include <behaviortree_ros2/plugins.hpp>
 #include <behaviortree_ros2/ros_node_params.hpp>
 #include <control_msgs/action/detail/gripper_command__struct.hpp>
+#include <rclcpp/logging.hpp>
 
 using GripperCommand = control_msgs::action::GripperCommand;
 using namespace BT;
@@ -14,12 +15,6 @@ public:
                  const RosNodeParams &params)
       : RosActionNode<GripperCommand>(name, conf, params) {
 
-    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() {
@@ -31,8 +26,24 @@ public:
   }
 
   bool setGoal(RosActionNode::Goal &goal) override {
-    goal.command.position = position.open;
-    goal.command.max_effort = 0.0;
+
+    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;
+    if (state == "open") {
+      goal.command.position = position.open;
+    } else if (state == "close") {
+      goal.command.position = position.close;
+    } else {
+      RCLCPP_ERROR_STREAM(node_.lock()->get_logger(),
+                          "Error: Invalid state '"
+                              << state << "'. Expected 'open' or 'close'.");
+      return false;
+    }
+    goal.command.max_effort = 1.0;
     return true;
   }
 
@@ -48,8 +59,6 @@ private:
     double open = 0.008;
     double close = 0.000;
   } position;
-
-  std::string pose;
 };
 
-CreateRosNodePlugin(GripperControl, "MoveGripper");
+CreateRosNodePlugin(GripperControl, "GripperCommand");

rbs_bt_executor/src/MoveToPose.cpp

@@ -11,27 +11,37 @@ class MoveToPose : public RosActionNode<MoveitSendPoseAction> {
 public:
   MoveToPose(const std::string &name, const NodeConfig &conf,
              const RosNodeParams &params)
-      : RosActionNode<MoveitSendPoseAction>(name, conf, params) {}
+      : RosActionNode<MoveitSendPoseAction>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting MoveToPose");
+  }
 
   static BT::PortsList providedPorts() {
     return providedBasicPorts(
         {BT::InputPort<std::string>("robot_name"),
-         BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose")});
+         BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
+         BT::InputPort<double>("duration")});
   }
 
   bool setGoal(RosActionNode::Goal &goal) override {
+    RCLCPP_INFO(this->logger(), "Starting send request");
     getInput("robot_name", goal.robot_name);
-    getInput("pose", goal.target_pose);
-    goal.duration = 2.0;
+    getInput("pose", m_target_pose);
+    getInput("duration", goal.duration);
+    goal.target_pose = *m_target_pose;
     return true;
   }
 
   NodeStatus onResultReceived(const WrappedResult &wr) override {
+
+    RCLCPP_INFO(this->logger(), "Starting get response");
     if (!wr.result->success) {
       return NodeStatus::FAILURE;
     }
     return NodeStatus::SUCCESS;
   }
+private:
+  std::shared_ptr<geometry_msgs::msg::Pose> m_target_pose;
 };
 
 CreateRosNodePlugin(MoveToPose, "MoveToPose");