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)
+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)
+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" 
+      <Script code="open_gripper_limit:='-0.014'" />
-        object_name="{object_name}" 
+      <Script code="close_gripper_limit:='0.0'" />
-        service_name="{get_workspace}" 
+
-        grasp_pose="{grasp_pose}" 
+      <Script code="robot_name:='rbs_arm'" />
-        pregrasp_pose="{pregrasp_pose}" 
+      <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="{pregrasp_pose}" duration="5.0" robot_name="{robot_name}"
-      <Action ID="MoveToPose" pose="{grasp_pose}"     robot_name="{robot_name}" action_name="{action}" />
+        action_name="{move_action}" />
-      <Action ID="MoveToPose" pose="{postgrasp_pose}" robot_name="{robot_name}" action_name="{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(
+    return providedBasicPorts({
-        {InputPort<std::string>("type"),
+        InputPort<std::string>("type"),
-         OutputPort<std::shared_ptr<geometry_msgs::msg::PoseArray>>(
+        OutputPort<std::shared_ptr<geometry_msgs::msg::PoseArray>>("workspace")
-             "workspace")});
+    });
   }
 
   bool setRequest(Request::SharedPtr &request) override {
@@ -29,36 +29,37 @@ public:
     return true;
   }
   NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
-    if (!response->ok) {
+      if (!response->ok) {
-      return NodeStatus::FAILURE;
+          return NodeStatus::FAILURE;
-    }
+      }
 
-    auto workspace = response->workspace;
+      auto workspace = response->workspace;
-    auto workspace_arr = std::make_shared<geometry_msgs::msg::PoseArray>();
+      auto workspace_arr = std::make_shared<geometry_msgs::msg::PoseArray>();
-    auto quat = std::make_shared<geometry_msgs::msg::Quaternion>();
+      auto quat = std::make_shared<geometry_msgs::msg::Quaternion>();
 
-    quat->w = 0.0;
+      quat->w = 0.0;
-    quat->x = 0.0;
+      quat->x = 0.0;
-    quat->y = 1.0;
+      quat->y = 1.0;
-    quat->z = 0.0;
+      quat->z = 0.0;
 
-    workspace_arr->poses.resize(workspace.size());
+      workspace_arr->poses.resize(workspace.size());
 
-    size_t i = 0;
+      size_t i = 0;
-    for (auto &point : workspace) {
+      for (auto& point : workspace) {
-      point.z += 0.35;
+          point.z += 0.35;
 
-      geometry_msgs::msg::Pose pose;
+          geometry_msgs::msg::Pose pose;
-      pose.position = point;
+          pose.position = point;
-      pose.orientation = *quat;
+          pose.orientation = *quat;
 
-      workspace_arr->poses[i++] = pose;
+          workspace_arr->poses[i++] = pose;
-    }
+      }
 
-    setOutput("workspace", workspace_arr);
+      setOutput("workspace", workspace_arr);
-    return NodeStatus::SUCCESS;
+      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);
+    getInput("pose", m_target_pose);
-    goal.duration = 2.0;
+    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");

rbs_utils/rbs_utils/scripts/assembly_config_service.py

@@ -9,6 +9,8 @@ from rbs_utils_interfaces.msg import NamedPose, RelativeNamedPose, AssemblyConfi
 from rbs_utils_interfaces.srv import GetGraspPose, GetWorkspace
 from rbs_assets_library import get_asm_config
 from env_manager.utils import Tf2Broadcaster
+from env_manager.utils import Tf2Listener
+import re
 
 
 class AssemblyConfigService(Node):
@@ -17,16 +19,21 @@ class AssemblyConfigService(Node):
 
         # Initialize parameters
         self.declare_parameter("assembly_config_name", "board_pick_and_place")
-        config_name = self.get_parameter("assembly_config_name").get_parameter_value().string_value
+        self.declare_parameter("base_link", "world")
+        # self.declare_parameter("gripper_offset", 0.055)
+        config_name = (
+            self.get_parameter("assembly_config_name")
+            .get_parameter_value()
+            .string_value
+        )
         if config_name:
             self.get_logger().info(f"Loading assembly config with name: {config_name}")
             asm_config_filepath = get_asm_config(config_name)
-            yaml_file = os.path.join(
+            yaml_file = os.path.join(asm_config_filepath)
-                asm_config_filepath
-            )
             self.assembly_config = parse_yaml(yaml_file)
 
             tf2_broadcaster = Tf2Broadcaster(self)
+            self.tf2_listner = Tf2Listener(self)
             for relative_part in self.assembly_config.relative_part:
                 tf2_broadcaster.broadcast_tf(
                     relative_part.relative_at,
@@ -40,10 +47,10 @@ class AssemblyConfigService(Node):
                         relative_part.pose.orientation.x,
                         relative_part.pose.orientation.y,
                         relative_part.pose.orientation.z,
-                        relative_part.pose.orientation.w
+                        relative_part.pose.orientation.w,
-                    )
+                    ),
                 )
-            
+
             for grasp_pose in self.assembly_config.grasp_pose:
                 tf2_broadcaster.broadcast_tf(
                     grasp_pose.relative_at,
@@ -58,10 +65,9 @@ class AssemblyConfigService(Node):
                         grasp_pose.pose.orientation.y,
                         grasp_pose.pose.orientation.z,
                         grasp_pose.pose.orientation.w,
-                    )
+                    ),
                 )
 
-
             # Services
             self.workspace_service = self.create_service(
                 GetWorkspace, "get_workspace", self.get_workspace_callback
@@ -77,10 +83,121 @@ class AssemblyConfigService(Node):
         return response
 
     def get_grasp_pose_callback(self, request, response):
-        response.grasp_pose = self.assembly_config.grasp_pose
+        # Create regular expressions for grasp poses
+        grasp_pose_pattern = re.compile(
+            rf"^{re.escape(request.model_name)}_grasp_pose$"
+        )
+        pregrasp_pose_pattern = re.compile(
+            rf"^{re.escape(request.model_name)}_pregrasp_pose$"
+        )
+        postgrasp_pose_pattern = re.compile(
+            rf"^{re.escape(request.model_name)}_postgrasp_pose$"
+        )
+        prepostgrasp_pose_pattern = re.compile(
+            rf"^{re.escape(request.model_name)}_prepostgrasp_pose$"
+        )
+
+        # Find poses using generators
+        grasp_pose_ = next(
+            (
+                pose
+                for pose in self.assembly_config.grasp_pose
+                if grasp_pose_pattern.fullmatch(pose.name)
+            ),
+            None,
+        )
+        pregrasp_pose_ = next(
+            (
+                pose
+                for pose in self.assembly_config.grasp_pose
+                if pregrasp_pose_pattern.fullmatch(pose.name)
+            ),
+            None,
+        )
+        postgrasp_pose_ = next(
+            (
+                pose
+                for pose in self.assembly_config.grasp_pose
+                if postgrasp_pose_pattern.fullmatch(pose.name)
+            ),
+            None,
+        )
+        prepostgrasp_pose_ = next(
+            (
+                pose
+                for pose in self.assembly_config.grasp_pose
+                if prepostgrasp_pose_pattern.fullmatch(pose.name)
+            ),
+            None,
+        )
+
+        # If grasp_pose is not found, raise an error
+        if grasp_pose_ is None:
+            response.ok = False
+            raise RuntimeError(f"Grasp pose not found for model {request.model_name}")
+
+        grasp_pose_msg = self._create_pose_msg_from_name(grasp_pose_)
+
+        # Create messages for pregrasp and postgrasp, considering prepostgrasp
+        if prepostgrasp_pose_:
+            pregrasp_pose_msg = self._create_pose_msg_from_name(prepostgrasp_pose_)
+            postgrasp_pose_msg = pregrasp_pose_msg
+        else:
+            pregrasp_pose_msg = self._create_pose_msg_from_name(pregrasp_pose_)
+            postgrasp_pose_msg = self._create_pose_msg_from_name(postgrasp_pose_)
+
+        # Ensure pregrasp and postgrasp have values
+        if pregrasp_pose_msg is None:
+            pregrasp_pose_msg = (
+                Pose()
+            )  # Fill with an empty Pose object if pregrasp is missing
+
+        if postgrasp_pose_msg is None:
+            postgrasp_pose_msg = (
+                pregrasp_pose_msg  # Use pregrasp if postgrasp is missing
+            )
+
+        # Fill the response
+        response.grasp_pose = grasp_pose_msg
+        response.pregrasp_pose = pregrasp_pose_msg
+        response.postgrasp_pose = postgrasp_pose_msg
         response.ok = True
         return response
 
+    def _create_pose_msg(self, transform):
+        # Convert the transform to a Pose message
+        if transform is None:
+            raise RuntimeError("Pose transform not found")
+
+        # gripper_offset = self.get_parameter("gripper_offset").get_parameter_value().double_value
+
+        pose_msg = Pose()
+        pose_msg.position.x = transform.translation.x
+        pose_msg.position.y = -transform.translation.y
+        pose_msg.position.z = transform.translation.z #+ gripper_offset
+
+        pose_msg.orientation.x = transform.rotation.x
+        pose_msg.orientation.y = transform.rotation.y
+        pose_msg.orientation.z = transform.rotation.z
+        pose_msg.orientation.w = transform.rotation.w
+
+        return pose_msg
+
+    def _create_pose_msg_from_name(self, pose):
+        # Convert RelativeNamedPose to Pose (stub, depends on your implementation)
+        if pose is None:
+            return None
+
+        # Assumes that pose contains data that can be used to form a Pose
+        transform = self.tf2_listner.lookup_transform_sync(
+            target_frame=pose.name,
+            source_frame=self.get_parameter("base_link")
+            .get_parameter_value()
+            .string_value,
+        )
+        return self._create_pose_msg(transform)
+
+
 def parse_yaml(yaml_file):
     with open(yaml_file, "r") as file:
         data = yaml.safe_load(file)
@@ -139,6 +256,7 @@ def parse_yaml(yaml_file):
 
     return assembly_config
 
+
 def main():
     rclpy.init()
 
@@ -150,5 +268,6 @@ def main():
     except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
         node.destroy_node()
 
+
 if __name__ == "__main__":
     main()

rbs_utils/rbs_utils_interfaces/srv/GetGraspPose.srv

@@ -1,4 +1,6 @@
 string model_name
 ---
-rbs_utils_interfaces/RelativeNamedPose[] grasp_pose
+geometry_msgs/Pose grasp_pose
+geometry_msgs/Pose pregrasp_pose
+geometry_msgs/Pose postgrasp_pose
 bool ok