feat: Enhance pose handling and add place poses functionality

- Modified GetPose service node to accept relative positioning
- Added new GetPlacePoses service node for retrieving place, preplace, and postplace poses

rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp

@@ -1,119 +0,0 @@
-#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 "rbs_skill_interfaces/srv/get_pick_place_poses.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 GraspingService = rbs_skill_interfaces::srv::GetPickPlacePoses;
-using namespace BT;
-
-class Grasping : public RosServiceNode<GraspingService> {
-public:
-  Grasping(const std::string &name, const NodeConfig &conf,
-           const RosNodeParams &params)
-      : RosServiceNode<GraspingService>(name, conf, params) {
-
-    RCLCPP_INFO(this->logger(), "Starting GetGraspPose");
-  }
-
-  static PortsList providedPorts() {
-    return providedBasicPorts(
-        {InputPort<std::string>("object_name"),
-         InputPort<std::string>("action_name"),
-         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
-         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pre_pose"),
-         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("post_pose")});
-  }
-
-  bool setRequest(Request::SharedPtr &request) override {
-    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
-                name().c_str(), this->service_name_.c_str());
-    if (!getInput("action_name", request->action_name)) {
-      RCLCPP_ERROR(this->node_.lock()->get_logger(),
-                   "[%s] Failed to get object_name from input port",
-                   name().c_str());
-      return false;
-    }
-    return true;
-  }
-
-  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
-    if (!response->ok) {
-      RCLCPP_ERROR(this->node_.lock()->get_logger(),
-                   "[%s] Response indicates failure.", name().c_str());
-      return NodeStatus::FAILURE;
-    }
-
-    RCLCPP_INFO(this->node_.lock()->get_logger(),
-                "[%s] Response received successfully.", name().c_str());
-
-    auto logPose = [this](const std::string &pose_name,
-                          const geometry_msgs::msg::Pose &pose) {
-      RCLCPP_INFO(this->node_.lock()->get_logger(),
-                  "[%s] %s:\n"
-                  "  Position: x = %.4f, y = %.4f, z = %.4f\n"
-                  "  Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f",
-                  name().c_str(), 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);
-    };
-    if (!response->grasp.empty()) {
-      RCLCPP_INFO(this->logger(), "[%s] Got Pick Response", name().c_str());
-      std::vector<std::string> poses = {"Pregrasp Pose", "Grasp Pose",
-                                        "Postgrasp Pose"};
-
-      for (size_t n = 0; n < poses.size() && n < response->grasp.size(); ++n) {
-        logPose(poses[n], response->grasp.at(n));
-      }
-
-      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>();
-
-      *pregrasp_pose = response->grasp.at(0);
-      *grasp_pose = response->grasp.at(1);
-      *postgrasp_pose = response->grasp.at(2);
-
-      setOutput("pre_pose", pregrasp_pose);
-      setOutput("pose", grasp_pose);
-      setOutput("post_pose", postgrasp_pose);
-      return NodeStatus::SUCCESS;
-    }
-    if (!response->place.empty()) {
-      RCLCPP_INFO(this->logger(), "[%s] Got Place Response", name().c_str());
-      std::vector<std::string> poses = {"Preplace Pose", "Place Pose",
-                                        "Postplace Pose"};
-
-      for (size_t n = 0; n < poses.size() && n < response->place.size(); ++n) {
-        logPose(poses[n], response->place.at(n));
-      }
-
-      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>();
-
-      *preplace_pose = response->place.at(0);
-      *place_pose = response->place.at(1);
-      *postplace_pose = response->place.at(2);
-
-      setOutput("pre_pose", preplace_pose);
-      setOutput("pose", place_pose);
-      setOutput("post_pose", postplace_pose);
-      return NodeStatus::SUCCESS;
-    }
-
-    RCLCPP_FATAL(this->logger(), "[%s] Could not response grasp pose", name().c_str());
-    return NodeStatus::FAILURE;
-  }
-  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
-};
-
-CreateRosNodePlugin(Grasping, "GetGraspPlacePose");

rbs_mill_assist/bt/plugins/get_grasp_pose.cpp

@@ -0,0 +1,89 @@
+#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 "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
+// #include "rbs_mill_interfaces"
+// #include "rbs_mill_interfaces/srv/get_slot_grasping_pose.hpp"
+#include "rbs_mill_interfaces/srv/get_grasping_pose.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 GraspingService = rbs_mill_interfaces::srv::GetGraspingPose;
+using namespace BT;
+
+class GetGraspPose : public RosServiceNode<GraspingService> {
+public:
+  GetGraspPose(const std::string &name, const NodeConfig &conf,
+               const RosNodeParams &params)
+      : RosServiceNode<GraspingService>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting GetSlotGraspingPose");
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts(
+        {InputPort<std::string>("from_pose"),
+         InputPort<std::string>("relative_to"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("grasp"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pregrasp"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("postgrasp")});
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
+    if (!getInput("from_pose", request->pose_name)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get slot_name from input port",
+                   name().c_str());
+      return false;
+    }
+    if (!getInput("relative_to", request->relative_to)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get relative_to from input port",
+                   name().c_str());
+      return false;
+    }
+    request->action_type = "pick";
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    if (!response->ok) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Response indicates failure.", name().c_str());
+      return NodeStatus::FAILURE;
+    }
+    RCLCPP_INFO(this->node_.lock()->get_logger(),
+                "[%s] Response received successfully.", name().c_str());
+
+    auto grasp =
+        std::make_shared<geometry_msgs::msg::Pose>(response->grasp_poses.pose);
+    auto pregrasp = std::make_shared<geometry_msgs::msg::Pose>(
+        response->grasp_poses.pre_pose);
+    auto postgrasp = std::make_shared<geometry_msgs::msg::Pose>(
+        response->grasp_poses.post_pose);
+
+    if (!grasp || !pregrasp || !postgrasp) {
+      RCLCPP_ERROR(this->logger(), "[%s] Response poses are empty",
+                   name().c_str());
+      return NodeStatus::FAILURE;
+    }
+
+    setOutput("grasp", grasp);
+    setOutput("pregrasp", pregrasp);
+    setOutput("postgrasp", postgrasp);
+
+    return NodeStatus::SUCCESS;
+  }
+  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
+};
+
+CreateRosNodePlugin(GetGraspPose, "GetSlotGraspPoses");

rbs_mill_assist/bt/plugins/get_named_pose.cpp

@@ -4,7 +4,7 @@
 #include <behaviortree_cpp/basic_types.h>
 // #include <geometry_msgs/msg/detail/point__struct.hpp>
 #include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
-#include "rbs_utils_interfaces/srv/get_named_pose.hpp"
+#include "rbs_utils_interfaces/srv/get_relative_named_pose.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>
@@ -12,21 +12,22 @@
 #include <rclcpp/logging.hpp>
 #include <string>
 
-using GetNamedPoseService = rbs_utils_interfaces::srv::GetNamedPose;
+using GetRelativeNamedPose = rbs_utils_interfaces::srv::GetRelativeNamedPose;
 using namespace BT;
 
-class GetNamedPose : public RosServiceNode<GetNamedPoseService> {
+class GetNamedPose : public RosServiceNode<GetRelativeNamedPose> {
 public:
   GetNamedPose(const std::string &name, const NodeConfig &conf,
                const RosNodeParams &params)
-      : RosServiceNode<GetNamedPoseService>(name, conf, params) {
+      : RosServiceNode<GetRelativeNamedPose>(name, conf, params) {
 
-    RCLCPP_INFO(this->logger(), "Starting GetGraspPose");
+    RCLCPP_INFO(this->logger(), "Starting GetNamedPose");
   }
 
   static PortsList providedPorts() {
     return providedBasicPorts(
         {InputPort<std::string>("pose_name"),
+         InputPort<std::string>("relative_to"),
          OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output_pose")});
   }
 
@@ -39,6 +40,19 @@ public:
                    name().c_str());
       return false;
     }
+    if (!getInput("pose_name", request->pose_name)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get pose_name from input port",
+                   name().c_str());
+      return false;
+    }
+
+    if (!getInput("relative_to", request->relative_to)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get pose_name from input port",
+                   name().c_str());
+      return false;
+    }
     return true;
   }
 
@@ -59,7 +73,6 @@ public:
 
     return NodeStatus::SUCCESS;
   }
-  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
 };
 
 CreateRosNodePlugin(GetNamedPose, "GetNamedPose");

rbs_mill_assist/bt/plugins/get_place_pose.cpp

@@ -0,0 +1,90 @@
+#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 "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
+// #include "rbs_mill_interfaces"
+// #include "rbs_mill_interfaces/srv/get_slot_grasping_pose.hpp"
+#include "rbs_mill_interfaces/srv/get_grasping_pose.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 GraspingService = rbs_mill_interfaces::srv::GetGraspingPose;
+using namespace BT;
+
+class GetPlacePoses : public RosServiceNode<GraspingService> {
+public:
+  GetPlacePoses(const std::string &name, const NodeConfig &conf,
+               const RosNodeParams &params)
+      : RosServiceNode<GraspingService>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting GetPlacePose");
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts(
+        {InputPort<std::string>("to_pose"),
+         InputPort<std::string>("relative_to"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("place"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("preplace"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("postplace")});
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
+    if (!getInput("to_pose", request->pose_name)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get slot_name from input port",
+                   name().c_str());
+      return false;
+    }
+    if (!getInput("relative_to", request->relative_to)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get relative_to from input port",
+                   name().c_str());
+      return false;
+    }
+    request->action_type = "place";
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    if (!response->ok) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Response indicates failure.", name().c_str());
+      return NodeStatus::FAILURE;
+    }
+
+    RCLCPP_INFO(this->node_.lock()->get_logger(),
+                "[%s] Response received successfully.", name().c_str());
+
+    auto place =
+        std::make_shared<geometry_msgs::msg::Pose>(response->place_poses.pose);
+    auto preplace = std::make_shared<geometry_msgs::msg::Pose>(
+        response->place_poses.pre_pose);
+    auto postplace = std::make_shared<geometry_msgs::msg::Pose>(
+        response->place_poses.post_pose);
+
+    if (!place || !preplace || !postplace) {
+      RCLCPP_ERROR(this->logger(), "[%s] Response poses are empty",
+                   name().c_str());
+      return NodeStatus::FAILURE;
+    }
+
+    setOutput("place", place);
+    setOutput("preplace", preplace);
+    setOutput("postplace", postplace);
+
+    return NodeStatus::SUCCESS;
+  }
+  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
+};
+
+CreateRosNodePlugin(GetPlacePoses, "GetPlacePoses");