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/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");