From 9f11a8b28dc4d4f7a16092ec079c18da458d6022 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Thu, 17 Oct 2024 11:11:46 +0200
Subject: [PATCH] geometry helpers: adding get_pose action

---
 .../iri_base_bt_client/geometry_bt_helpers.h  | 31 +++++++++
 src/geometry_bt_helpers.cpp                   | 65 ++++++++++++++++++-
 2 files changed, 95 insertions(+), 1 deletion(-)

diff --git a/include/iri_base_bt_client/geometry_bt_helpers.h b/include/iri_base_bt_client/geometry_bt_helpers.h
index e032e0b..71dae89 100644
--- a/include/iri_base_bt_client/geometry_bt_helpers.h
+++ b/include/iri_base_bt_client/geometry_bt_helpers.h
@@ -72,6 +72,37 @@ class GeometryBTHelpers
     BT::NodeStatus compute_y_distance(BT::TreeNode& self);
 
     BT::NodeStatus compute_z_distance(BT::TreeNode& self);
+
+    /**
+      * \brief Function to get a PoseStamped
+      * 
+      * It has the following MANDATORY input ports:
+      *
+      *   frame_id (std::string): The frame_id of the PoseStamped.
+      * 
+      * It has the following OPTIONAL input ports. If not defained, it's value defaults to 0.0
+      * 
+      *   x (double): the position x coordinate value in meters
+      * 
+      *   y (double): the position y coordinate value in meters
+      * 
+      *   z (double): the position z coordinate value in meters
+      * 
+      *   roll (double):  the orientation roll angle in radians
+      * 
+      *   pitch (double): the orientation pitch angle in radians
+      * 
+      *   yaw (double):   the orientation yaw angle in radians
+      * 
+      * It also has the following output ports:
+      * 
+      *   pose (geometry_msgs::PoseStamped): the generated stamped pose
+      *
+      * \return BT:NodeStatus::SUCCESS if distance can be computed.
+      * Otherwise returns BT:NodeStatus::FAILURE. If inputs are missing or incorrect it also 
+      * returns BT:NodeStatus::FAILURE.
+      */
+    BT::NodeStatus get_pose(BT::TreeNode& self);
     };
 
 #endif
diff --git a/src/geometry_bt_helpers.cpp b/src/geometry_bt_helpers.cpp
index 18fb52e..affa0f1 100644
--- a/src/geometry_bt_helpers.cpp
+++ b/src/geometry_bt_helpers.cpp
@@ -10,12 +10,21 @@ void GeometryBTHelpers::init(IriBehaviorTreeFactory &factory)
 {
   BT::PortsList transform_pose_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose_in"),BT::InputPort<std::string>("target_frame"),BT::OutputPort<geometry_msgs::PoseStamped>("pose_out")};
   BT::PortsList compute_distance_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose1"),BT::InputPort<geometry_msgs::PoseStamped>("pose2"),BT::OutputPort<double>("distance")};
+  BT::PortsList get_pose_ports = {BT::InputPort<std::string>("frame_id"),
+                                  BT::InputPort<double>("x"),
+                                  BT::InputPort<double>("y"),
+                                  BT::InputPort<double>("z"),
+                                  BT::InputPort<double>("roll"),
+                                  BT::InputPort<double>("pitch"),
+                                  BT::InputPort<double>("yaw"),
+                                  BT::OutputPort<geometry_msgs::PoseStamped>("pose")};
 
   factory.registerSimpleAction("transform_pose", std::bind(&GeometryBTHelpers::transform_pose, this, std::placeholders::_1),transform_pose_ports);
   factory.registerSimpleAction("compute_distance", std::bind(&GeometryBTHelpers::compute_distance, this, std::placeholders::_1),compute_distance_ports);
   factory.registerSimpleAction("compute_x_distance", std::bind(&GeometryBTHelpers::compute_x_distance, this, std::placeholders::_1),compute_distance_ports);
   factory.registerSimpleAction("compute_y_distance", std::bind(&GeometryBTHelpers::compute_y_distance, this, std::placeholders::_1),compute_distance_ports);
   factory.registerSimpleAction("compute_z_distance", std::bind(&GeometryBTHelpers::compute_z_distance, this, std::placeholders::_1),compute_distance_ports);
+  factory.registerSimpleAction("get_pose", std::bind(&GeometryBTHelpers::get_pose, this, std::placeholders::_1),get_pose_ports);
 }
 
 BT::NodeStatus GeometryBTHelpers::transform_pose(BT::TreeNode& self)
@@ -167,7 +176,7 @@ BT::NodeStatus GeometryBTHelpers::compute_z_distance(BT::TreeNode& self)
 
   if (pose1_value.header.frame_id != pose1_value.header.frame_id)
   {
-    ROS_ERROR_STREAM("GeometryBTHelpers::compute_y_distance (" << self.name() << ") -> Points with different frame ids: pose1=" << pose1_value.header.frame_id << "; pose2=" << pose1_value.header.frame_id);
+    ROS_ERROR_STREAM("GeometryBTHelpers::compute_z_distance (" << self.name() << ") -> Points with different frame ids: pose1=" << pose1_value.header.frame_id << "; pose2=" << pose1_value.header.frame_id);
     return BT::NodeStatus::FAILURE;
   }
 
@@ -175,3 +184,57 @@ BT::NodeStatus GeometryBTHelpers::compute_z_distance(BT::TreeNode& self)
   self.setOutput("distance",distance);
   return BT::NodeStatus::SUCCESS;
 }
+
+BT::NodeStatus GeometryBTHelpers::get_pose(BT::TreeNode& self)
+{
+  ROS_DEBUG("GeometryBTHelpers::get_pose");
+  BT::Optional<std::string> frame_id_port = self.getInput<std::string>("frame_id");
+  BT::Optional<double> x_port = self.getInput<double>("x");
+  BT::Optional<double> y_port = self.getInput<double>("y");
+  BT::Optional<double> z_port = self.getInput<double>("z");
+  BT::Optional<double> roll_port  = self.getInput<double>("roll");
+  BT::Optional<double> pitch_port = self.getInput<double>("pitch");
+  BT::Optional<double> yaw_port   = self.getInput<double>("yaw");
+
+  if((!frame_id_port))
+  {
+    ROS_ERROR_STREAM("GeometryBTHelpers::get_pose (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: frame_id (std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  double x=0.0;
+  double y=0.0;
+  double z=0.0;
+  double roll=0.0;
+  double pitch=0.0;
+  double yaw=0.0;
+
+  std::string frame_id_value=frame_id_port.value();
+  if(x_port)
+    x = x_port.value();
+  if(y_port)
+    y = y_port.value();
+  if(z_port)
+    z = z_port.value();
+  if(roll_port)
+    roll = roll_port.value();
+  if(pitch_port)
+    pitch = pitch_port.value();
+  if(yaw_port)
+    yaw = yaw_port.value();
+
+  geometry_msgs::PoseStamped pose;
+  pose.header.frame_id = frame_id_value;
+
+  pose.pose.position.x=x;
+  pose.pose.position.y=y;
+  pose.pose.position.z=z;
+
+  tf2::Quaternion quat_tf;
+  quat_tf.setRPY(roll,pitch,yaw);
+  pose.pose.orientation = tf2::toMsg(quat_tf);
+
+  self.setOutput("pose",pose);
+
+  return BT::NodeStatus::SUCCESS;
+}
\ No newline at end of file
-- 
GitLab