From 213179adbd0c411a99274792d5bfe8199b0e97d3 Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Wed, 18 Dec 2024 14:14:01 +0300
Subject: [PATCH] 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                |   8 +-
 rbs_bt_executor/bt_trees/grasp_object.xml     |  22 +--
 rbs_bt_executor/bt_trees/pick_and_place.xml   |  17 ++
 rbs_bt_executor/bt_trees/place_object.xml     |  64 +++++++
 rbs_bt_executor/src/GetGraspPose.cpp          |   3 -
 rbs_bt_executor/src/GetPlacePose.cpp          |  87 +++++++++
 rbs_bt_executor/src/GripperCommand.cpp        |   2 +-
 .../scripts/assembly_config_service.py        | 180 +++++++++++++++---
 rbs_utils/rbs_utils/src/rbs_utils.cpp         |   6 +-
 rbs_utils/rbs_utils_interfaces/CMakeLists.txt |   1 +
 .../msg/AssemblyConfig.msg                    |   3 +-
 .../rbs_utils_interfaces/srv/GetPlacePose.srv |   7 +
 12 files changed, 356 insertions(+), 44 deletions(-)
 create mode 100644 rbs_bt_executor/bt_trees/pick_and_place.xml
 create mode 100644 rbs_bt_executor/bt_trees/place_object.xml
 create mode 100644 rbs_bt_executor/src/GetPlacePose.cpp
 create mode 100644 rbs_utils/rbs_utils_interfaces/srv/GetPlacePose.srv

diff --git a/rbs_bt_executor/CMakeLists.txt b/rbs_bt_executor/CMakeLists.txt
index bb5147b..0bb5ed0 100644
--- a/rbs_bt_executor/CMakeLists.txt
+++ b/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)
diff --git a/rbs_bt_executor/bt_trees/grasp_object.xml b/rbs_bt_executor/bt_trees/grasp_object.xml
index 223245c..9e1ac91 100644
--- a/rbs_bt_executor/bt_trees/grasp_object.xml
+++ b/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}" />
diff --git a/rbs_bt_executor/bt_trees/pick_and_place.xml b/rbs_bt_executor/bt_trees/pick_and_place.xml
new file mode 100644
index 0000000..d530af7
--- /dev/null
+++ b/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>
diff --git a/rbs_bt_executor/bt_trees/place_object.xml b/rbs_bt_executor/bt_trees/place_object.xml
new file mode 100644
index 0000000..6c03b6c
--- /dev/null
+++ b/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>
diff --git a/rbs_bt_executor/src/GetGraspPose.cpp b/rbs_bt_executor/src/GetGraspPose.cpp
index 0af6114..2d2bb08 100644
--- a/rbs_bt_executor/src/GetGraspPose.cpp
+++ b/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);
diff --git a/rbs_bt_executor/src/GetPlacePose.cpp b/rbs_bt_executor/src/GetPlacePose.cpp
new file mode 100644
index 0000000..98cde81
--- /dev/null
+++ b/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");
diff --git a/rbs_bt_executor/src/GripperCommand.cpp b/rbs_bt_executor/src/GripperCommand.cpp
index fa7e236..ab9a3a7 100644
--- a/rbs_bt_executor/src/GripperCommand.cpp
+++ b/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;
   }
 
diff --git a/rbs_utils/rbs_utils/scripts/assembly_config_service.py b/rbs_utils/rbs_utils/scripts/assembly_config_service.py
index 696a474..ce92375 100755
--- a/rbs_utils/rbs_utils/scripts/assembly_config_service.py
+++ b/rbs_utils/rbs_utils/scripts/assembly_config_service.py
@@ -6,7 +6,7 @@ from rclpy.node import Node
 import yaml
 from geometry_msgs.msg import Point, Pose, Quaternion
 from rbs_utils_interfaces.msg import NamedPose, RelativeNamedPose, AssemblyConfig
-from rbs_utils_interfaces.srv import GetGraspPose, GetWorkspace
+from rbs_utils_interfaces.srv import GetGraspPose, GetWorkspace, GetPlacePose
 from rbs_assets_library import get_asm_config
 from env_manager.utils import Tf2Broadcaster
 from env_manager.utils import Tf2Listener
@@ -30,7 +30,7 @@ class AssemblyConfigService(Node):
             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(asm_config_filepath)
-            self.assembly_config = parse_yaml(yaml_file)
+            self.assembly_config = parse_assembly_config(yaml_file)
 
             tf2_broadcaster = Tf2Broadcaster(self)
             self.tf2_listner = Tf2Listener(self)
@@ -51,20 +51,37 @@ class AssemblyConfigService(Node):
                     ),
                 )
 
-            for grasp_pose in self.assembly_config.grasp_pose:
+            for grasp_poses in self.assembly_config.grasp_poses:
                 tf2_broadcaster.broadcast_tf(
-                    grasp_pose.relative_at,
-                    grasp_pose.name,
+                    grasp_poses.relative_at,
+                    grasp_poses.name,
                     translation=(
-                        grasp_pose.pose.position.x,
-                        grasp_pose.pose.position.y,
-                        grasp_pose.pose.position.z,
+                        grasp_poses.pose.position.x,
+                        grasp_poses.pose.position.y,
+                        grasp_poses.pose.position.z,
                     ),
                     rotation=(
-                        grasp_pose.pose.orientation.x,
-                        grasp_pose.pose.orientation.y,
-                        grasp_pose.pose.orientation.z,
-                        grasp_pose.pose.orientation.w,
+                        grasp_poses.pose.orientation.x,
+                        grasp_poses.pose.orientation.y,
+                        grasp_poses.pose.orientation.z,
+                        grasp_poses.pose.orientation.w,
+                    ),
+                )
+
+            for place_poses in self.assembly_config.place_poses:
+                tf2_broadcaster.broadcast_tf(
+                    place_poses.relative_at,
+                    place_poses.name,
+                    translation=(
+                        place_poses.pose.position.x,
+                        place_poses.pose.position.y,
+                        place_poses.pose.position.z,
+                    ),
+                    rotation=(
+                        place_poses.pose.orientation.x,
+                        place_poses.pose.orientation.y,
+                        place_poses.pose.orientation.z,
+                        place_poses.pose.orientation.w,
                     ),
                 )
 
@@ -72,8 +89,11 @@ class AssemblyConfigService(Node):
             self.workspace_service = self.create_service(
                 GetWorkspace, "get_workspace", self.get_workspace_callback
             )
-            self.grasp_pose_service = self.create_service(
-                GetGraspPose, "get_grasp_pose", self.get_grasp_pose_callback
+            self.grasp_poses_service = self.create_service(
+                GetGraspPose, "get_grasp_poses", self.get_grasp_poses_callback
+            )
+            self.place_pose_service = self.create_service(
+                GetPlacePose, "get_place_poses", self.get_place_poses_callback
             )
 
     def get_workspace_callback(self, request, response):
@@ -82,7 +102,96 @@ class AssemblyConfigService(Node):
         response.ok = True
         return response
 
-    def get_grasp_pose_callback(self, request, response):
+    def get_place_poses_callback(self, request: GetPlacePose.Request, response):
+        # Compile regex patterns for the various place poses
+
+        place_pose_pattern = re.compile(
+            rf"^{re.escape(request.place_pose_name)}_place$"
+        )
+        preplace_pose_pattern = re.compile(
+            rf"^{re.escape(request.place_pose_name)}_preplace$"
+        )
+        postplace_pose_pattern = re.compile(
+            rf"^{re.escape(request.place_pose_name)}_postplace$"
+        )
+        prepostplace_pose_pattern = re.compile(
+            rf"^{re.escape(request.place_pose_name)}_prepostplace$"
+        )
+
+        # Find poses in the assembly config
+        place_pose_ = next(
+            (
+                pose
+                for pose in self.assembly_config.place_poses
+                if place_pose_pattern.fullmatch(pose.name)
+            ),
+            None,
+        )
+        preplace_pose_ = next(
+            (
+                pose
+                for pose in self.assembly_config.place_poses
+                if preplace_pose_pattern.fullmatch(pose.name)
+            ),
+            None,
+        )
+        postplace_pose_ = next(
+            (
+                pose
+                for pose in self.assembly_config.place_poses
+                if postplace_pose_pattern.fullmatch(pose.name)
+            ),
+            None,
+        )
+        prepostplace_pose_ = next(
+            (
+                pose
+                for pose in self.assembly_config.place_poses
+                if prepostplace_pose_pattern.fullmatch(pose.name)
+            ),
+            None,
+        )
+
+        # If place_pose is not found, raise an error
+        if place_pose_ is None:
+            response.ok = False
+            self.get_logger().error(
+                f"Place pose not found for name {request.place_pose_name}"
+            )
+            return response
+
+        place_pose_msg = self._create_pose_msg_from_name(place_pose_)
+
+        # Create messages for preplace and postplace, considering prepostplace
+        if prepostplace_pose_:
+            preplace_pose_msg = self._create_pose_msg_from_name(prepostplace_pose_)
+            postplace_pose_msg = preplace_pose_msg
+        else:
+            preplace_pose_msg = self._create_pose_msg_from_name(preplace_pose_)
+            postplace_pose_msg = self._create_pose_msg_from_name(postplace_pose_)
+
+        # Ensure preplace and postplace have values
+        if preplace_pose_msg is None:
+            preplace_pose_msg = (
+                Pose()
+            )  # Fill with an empty Pose object if preplace is missing
+
+        if postplace_pose_msg is None:
+            postplace_pose_msg = (
+                preplace_pose_msg  # Use preplace if postplace is missing
+            )
+
+        # Fill the response
+        response.place_pose = place_pose_msg
+        response.preplace_pose = preplace_pose_msg
+        response.postplace_pose = postplace_pose_msg
+        response.ok = True
+        self.get_logger().info(
+            f"Place pose response prepared for {request.place_pose_name}"
+        )
+        return response
+
+    def get_grasp_poses_callback(self, request, response):
         # Create regular expressions for grasp poses
         grasp_pose_pattern = re.compile(
             rf"^{re.escape(request.model_name)}_grasp_pose$"
@@ -101,7 +210,7 @@ class AssemblyConfigService(Node):
         grasp_pose_ = next(
             (
                 pose
-                for pose in self.assembly_config.grasp_pose
+                for pose in self.assembly_config.grasp_poses
                 if grasp_pose_pattern.fullmatch(pose.name)
             ),
             None,
@@ -109,7 +218,7 @@ class AssemblyConfigService(Node):
         pregrasp_pose_ = next(
             (
                 pose
-                for pose in self.assembly_config.grasp_pose
+                for pose in self.assembly_config.grasp_poses
                 if pregrasp_pose_pattern.fullmatch(pose.name)
             ),
             None,
@@ -117,7 +226,7 @@ class AssemblyConfigService(Node):
         postgrasp_pose_ = next(
             (
                 pose
-                for pose in self.assembly_config.grasp_pose
+                for pose in self.assembly_config.grasp_poses
                 if postgrasp_pose_pattern.fullmatch(pose.name)
             ),
             None,
@@ -125,7 +234,7 @@ class AssemblyConfigService(Node):
         prepostgrasp_pose_ = next(
             (
                 pose
-                for pose in self.assembly_config.grasp_pose
+                for pose in self.assembly_config.grasp_poses
                 if prepostgrasp_pose_pattern.fullmatch(pose.name)
             ),
             None,
@@ -174,7 +283,7 @@ class AssemblyConfigService(Node):
         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.position.z = transform.translation.z  # + gripper_offset
 
         pose_msg.orientation.x = transform.rotation.x
         pose_msg.orientation.y = transform.rotation.y
@@ -198,7 +307,7 @@ class AssemblyConfigService(Node):
         return self._create_pose_msg(transform)
 
 
-def parse_yaml(yaml_file):
+def parse_assembly_config(yaml_file):
     with open(yaml_file, "r") as file:
         data = yaml.safe_load(file)
 
@@ -229,7 +338,7 @@ def parse_yaml(yaml_file):
         )
 
     grasp_pose = []
-    for pose in data.get("grasp_pose", []):
+    for pose in data.get("grasp_poses", []):
         position = pose["pose"]["position"]
         orientation = pose["pose"].get(
             "orientation", {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
@@ -244,14 +353,39 @@ def parse_yaml(yaml_file):
         )
 
     extra_poses = []
+    for pose in data.get("extra_poses", []):
+        position = pose["pose"]["position"]
+        orientation = pose["pose"].get(
+            "orientation", {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
+        )
+        pose_obj = Pose(
+            position=Point(**position), orientation=Quaternion(**orientation)
+        )
+        extra_poses.append(NamedPose(name=pose["name"], pose=pose_obj))
+
+    place_poses = []
+    for pose in data.get("place_poses", []):
+        position = pose["pose"]["position"]
+        orientation = pose["pose"].get(
+            "orientation", {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
+        )
+        pose_obj = Pose(
+            position=Point(**position), orientation=Quaternion(**orientation)
+        )
+        place_poses.append(
+            RelativeNamedPose(
+                name=pose["name"], relative_at=pose["relative_at"], pose=pose_obj
+            )
+        )
 
     assembly_config = AssemblyConfig(
         assets_db=assets_db,
         workspace=workspace,
         absolute_part=absolute_part,
         relative_part=relative_part,
-        grasp_pose=grasp_pose,
+        grasp_poses=grasp_pose,
         extra_poses=extra_poses,
+        place_poses=place_poses,
     )
 
     return assembly_config
diff --git a/rbs_utils/rbs_utils/src/rbs_utils.cpp b/rbs_utils/rbs_utils/src/rbs_utils.cpp
index eadad3c..5d84828 100644
--- a/rbs_utils/rbs_utils/src/rbs_utils.cpp
+++ b/rbs_utils/rbs_utils/src/rbs_utils.cpp
@@ -103,7 +103,7 @@ tf2_msgs::msg::TFMessage AssemblyConfigLoader::getAllPossibleTfData() {
     RCLCPP_INFO(m_logger, "Model name [%s]", relative_part.name.c_str());
   }
   // Get grasp poses
-  for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
+  for (const auto &grasp_pose : m_assembly_config.grasp_poses) {
     geometry_msgs::msg::TransformStamped tmp;
     tmp.transform = createTransform(grasp_pose.pose);
     tmp.child_frame_id = grasp_pose.name;
@@ -134,7 +134,7 @@ AssemblyConfigLoader::getGraspTfData(const std::string &model_name) {
                  m_assembly_config.relative_part.size());
   }
 
-  for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
+  for (const auto &grasp_pose : m_assembly_config.grasp_poses) {
     if (grasp_pose.relative_at == model_name) {
       geometry_msgs::msg::TransformStamped tmp;
       tmp.transform = createTransform(grasp_pose.pose);
@@ -204,7 +204,7 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) {
                  m_assembly_config.relative_part.size());
   }
 
-  for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
+  for (const auto &grasp_pose : m_assembly_config.grasp_poses) {
     if (grasp_pose.relative_at == model_name) {
       geometry_msgs::msg::TransformStamped tmp;
       tmp.transform = createTransform(grasp_pose.pose);
diff --git a/rbs_utils/rbs_utils_interfaces/CMakeLists.txt b/rbs_utils/rbs_utils_interfaces/CMakeLists.txt
index a1b393d..2c90a56 100644
--- a/rbs_utils/rbs_utils_interfaces/CMakeLists.txt
+++ b/rbs_utils/rbs_utils_interfaces/CMakeLists.txt
@@ -16,6 +16,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "msg/NamedPose.msg"
     "msg/RelativeNamedPose.msg"
     "srv/GetGraspPose.srv"
+    "srv/GetPlacePose.srv"
     "srv/GetWorkspace.srv"
     DEPENDENCIES std_msgs geometry_msgs
 )
diff --git a/rbs_utils/rbs_utils_interfaces/msg/AssemblyConfig.msg b/rbs_utils/rbs_utils_interfaces/msg/AssemblyConfig.msg
index 5172c78..d218e3c 100644
--- a/rbs_utils/rbs_utils_interfaces/msg/AssemblyConfig.msg
+++ b/rbs_utils/rbs_utils_interfaces/msg/AssemblyConfig.msg
@@ -3,5 +3,6 @@ string assets_db
 geometry_msgs/Point[] workspace
 rbs_utils_interfaces/NamedPose[] absolute_part
 rbs_utils_interfaces/RelativeNamedPose[] relative_part
-rbs_utils_interfaces/RelativeNamedPose[] grasp_pose
+rbs_utils_interfaces/RelativeNamedPose[] grasp_poses
+rbs_utils_interfaces/RelativeNamedPose[] place_poses
 rbs_utils_interfaces/NamedPose[] extra_poses
diff --git a/rbs_utils/rbs_utils_interfaces/srv/GetPlacePose.srv b/rbs_utils/rbs_utils_interfaces/srv/GetPlacePose.srv
new file mode 100644
index 0000000..b2e4911
--- /dev/null
+++ b/rbs_utils/rbs_utils_interfaces/srv/GetPlacePose.srv
@@ -0,0 +1,7 @@
+string model_name
+string place_pose_name
+---
+geometry_msgs/Pose place_pose
+geometry_msgs/Pose preplace_pose
+geometry_msgs/Pose postplace_pose
+bool ok