feat: enhance pick-and-place functionality with behavior tree support for place operations

- Added `GetPlacePose` skill client library and associated service node to handle place pose retrieval, including preplace and postplace poses.
- Created new behavior trees (`pick_and_place.xml`, `place_object.xml`) for handling pick-and-place operations with enhanced modularity.
- Updated `AssemblyConfig` to include `place_poses` alongside `grasp_poses`, improving flexibility in assembly workflows.
- Refactored YAML parsing and codebase to differentiate between `grasp_poses` and `place_poses` for better clarity and maintainability.
- Updated related XML behavior tree nodes, scripts, and skill structures to integrate `GetPlacePose` functionality.
- Adjusted gripper command maximum effort to `3.0` for improved control.
- Added a new message definition for `GetPlacePose` service in the `rbs_utils_interfaces` package.
- Updated CMakeLists.txt to include the new service and ensure proper build configuration.

rbs_bt_executor/CMakeLists.txt

@@ -64,8 +64,12 @@ list(APPEND plugin_libs rbs_skill_move_topose_bt_action_client)
 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/GetGraspPose.cpp)
-list(APPEND plugin_libs rbs_skill_get_pick_place_pose_service_client)
+add_library(rbs_get_grasp_pose_skill_client SHARED src/GetGraspPose.cpp)
+list(APPEND plugin_libs rbs_get_grasp_pose_skill_client)
+
+
+add_library(rbs_get_place_pose_skill_client SHARED src/GetPlacePose.cpp)
+list(APPEND plugin_libs rbs_get_place_pose_skill_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

@@ -4,15 +4,15 @@
     <Sequence>
 
 
-      <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'" />
+      <!-- <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_poses'" /> -->
+      <!-- <Script code="move_action:='/mtp_jtc'" /> -->
+      <!-- <Script code="grpper_action:='/gripper_controller/gripper_cmd'" /> -->
 
       <Action ID="GetGraspPose"
         object_name="box"
@@ -25,13 +25,13 @@
         action_name="{move_action}" />
 
       <Action ID="GripperCommand" open_limit="{open_gripper_limit}" close_limit="{close_gripper_limit}" state="open"
-        action_name="{grpper_action}" />
+        action_name="{gripper_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_name="{gripper_action}" />
 
       <Action ID="MoveToPose" pose="{postgrasp_pose}" duration="2.0" robot_name="{robot_name}"
         action_name="{move_action}" />

rbs_bt_executor/bt_trees/pick_and_place.xml

@@ -0,0 +1,17 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="PickAndPlace">
+    <Sequence>
+
+      <SubTree ID="GraspObject" open_gripper_limit="-0.014" close_gripper_limit="0.0"
+        robot_name="ar4" object_name="box" grasp_service="/get_grasp_poses" move_action="/mtp_jtc"
+        gripper_action="/gripper_controller/gripper_cmd" />
+
+      <SubTree ID="PlaceObject" open_gripper_limit="-0.014" close_gripper_limit="0.0"
+        robot_name="ar4" place_pose_name="box_2mm_hole" grasp_service="/get_place_poses"
+        move_action="/mtp_jtc"
+        gripper_action="/gripper_controller/gripper_cmd" />
+
+    </Sequence>
+  </BehaviorTree>
+</root>

rbs_bt_executor/bt_trees/place_object.xml

@@ -0,0 +1,64 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="PlaceObject">
+    <Sequence>
+
+
+      <!-- <Script code="open_gripper_limit:='-0.014'" /> -->
+      <!-- <Script code="close_gripper_limit:='0.0'" /> -->
+      <!---->
+      <!-- <Script code="robot_name:='rbs_arm'" /> -->
+      <!-- <Script code="place_pose_name:='box_2mm_hole'" /> -->
+      <!---->
+      <!-- <Script code="grasp_service:='/get_place_poses'" /> -->
+      <!-- <Script code="move_action:='/mtp_jtc'" /> -->
+      <!-- <Script code="grpper_action:='/gripper_controller/gripper_cmd'" /> -->
+
+      <Action ID="GetPlacePose"
+        place_pose_name="{place_pose_name}"
+        service_name="{grasp_service}"
+        place_pose="{place_pose}"
+        preplace_pose="{preplace_pose}"
+        postplace_pose="{postplace_pose}" />
+
+      <Action ID="MoveToPose" pose="{preplace_pose}" duration="5.0" robot_name="{robot_name}"
+        action_name="{move_action}" />
+
+      <Action ID="MoveToPose" pose="{place_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="open"
+        action_name="{gripper_action}" />
+
+      <Action ID="MoveToPose" pose="{postplace_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="{gripper_action}" />
+
+    </Sequence>
+  </BehaviorTree>
+
+  <TreeNodesModel>
+    <Action ID="GetGraspPose">
+      <input_port name="object_name" />
+      <input_port name="service_name" />
+      <output_port name="place_pose" />
+      <output_port name="preplace_pose" />
+      <output_port name="postplace_pose" />
+    </Action>
+
+    <Action ID="MoveToPose">
+      <input_port name="pose" />
+      <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/src/GetGraspPose.cpp

@@ -1,7 +1,6 @@
 #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>
@@ -64,8 +63,6 @@ public:
                   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);

rbs_bt_executor/src/GetPlacePose.cpp

@@ -0,0 +1,87 @@
+#include "behaviortree_ros2/bt_service_node.hpp"
+
+#include "behaviortree_ros2/plugins.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_place_pose__struct.hpp>
+#include <rclcpp/logging.hpp>
+#include <string>
+
+using GetPlacePoseService = rbs_utils_interfaces::srv::GetPlacePose;
+using namespace BT;
+
+class GetPlacePose : public RosServiceNode<GetPlacePoseService> {
+public:
+  GetPlacePose(const std::string &name, const NodeConfig &conf,
+               const RosNodeParams &params)
+      : RosServiceNode<GetPlacePoseService>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting GetPlacePose");
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts(
+        {InputPort<std::string>("place_pose_name"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("place_pose"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("preplace_pose"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>(
+             "postplace_pose")});
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    RCLCPP_INFO(this->logger(), "Starting send request");
+    if (!getInput("place_pose_name", request->place_pose_name)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "Failed to get place_pose_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);
+    };
+
+    logPose("Place Pose", response->place_pose);
+    logPose("Pre-place Pose", response->preplace_pose);
+    logPose("Post-place Pose", response->postplace_pose);
+
+    auto place_pose = std::make_shared<geometry_msgs::msg::Pose>();
+    auto preplace_pose = std::make_shared<geometry_msgs::msg::Pose>();
+    auto postplace_pose = std::make_shared<geometry_msgs::msg::Pose>();
+
+    *place_pose = response->place_pose;
+    *preplace_pose = response->preplace_pose;
+    *postplace_pose = response->postplace_pose;
+
+    setOutput("place_pose", place_pose);
+    setOutput("preplace_pose", preplace_pose);
+    setOutput("postplace_pose", postplace_pose);
+
+    return NodeStatus::SUCCESS;
+  }
+  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
+};
+
+CreateRosNodePlugin(GetPlacePose, "GetPlacePose");

rbs_bt_executor/src/GripperCommand.cpp

@@ -43,7 +43,7 @@ public:
                               << state << "'. Expected 'open' or 'close'.");
       return false;
     }
-    goal.command.max_effort = 1.0;
+    goal.command.max_effort = 3.0;
     return true;
   }