From 1e7be6118f6d3d93c319443c51595f4b7c938ff8 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Tue, 21 Jan 2025 10:45:59 +0100
Subject: [PATCH] geometry helpers: add get_pose_* and get_pose_fields actions
to retrieve pose x,y,z,roll,pitch,yaw,frame_id fields
---
.../iri_base_bt_client/geometry_bt_helpers.h | 37 +++-
src/geometry_bt_helpers.cpp | 191 +++++++++++++++++-
2 files changed, 221 insertions(+), 7 deletions(-)
diff --git a/include/iri_base_bt_client/geometry_bt_helpers.h b/include/iri_base_bt_client/geometry_bt_helpers.h
index 71dae89..2d0ebc2 100644
--- a/include/iri_base_bt_client/geometry_bt_helpers.h
+++ b/include/iri_base_bt_client/geometry_bt_helpers.h
@@ -61,9 +61,9 @@ class GeometryBTHelpers
*
* distance (double): Distance.
*
- * \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.
+ * \return BT:NodeStatus::SUCCESS if pose can be transformed
+ * Otherwise returns BT:NodeStatus::FAILURE.
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
*/
BT::NodeStatus compute_distance(BT::TreeNode& self);
@@ -98,11 +98,36 @@ class GeometryBTHelpers
*
* 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.
+ * \return BT:NodeStatus::SUCCESS if pose can be returned. Otherwise returns BT:NodeStatus::FAILURE.
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
*/
BT::NodeStatus get_pose(BT::TreeNode& self);
+
+ /**
+ * \brief Function to get a PoseStamped
+ *
+ * It has the following MANDATORY input ports:
+ *
+ * pose (geometry_msgs::PoseStamped): The pose to get the frame id from
+ *
+ * It has the following output ports:
+ *
+ * frame_id (std::string): the frame_id of the pose
+ *
+ * \return BT:NodeStatus::SUCCESS if frame_id can be returned.
+ * If inputs are missing or incorrect it returns BT:NodeStatus::FAILURE.
+ */
+ BT::NodeStatus get_pose_frame_id(BT::TreeNode& self);
+
+
+
+ BT::NodeStatus get_pose_x(BT::TreeNode& self);
+ BT::NodeStatus get_pose_y(BT::TreeNode& self);
+ BT::NodeStatus get_pose_z(BT::TreeNode& self);
+ BT::NodeStatus get_pose_roll(BT::TreeNode& self);
+ BT::NodeStatus get_pose_pitch(BT::TreeNode& self);
+ BT::NodeStatus get_pose_yaw(BT::TreeNode& self);
+ BT::NodeStatus get_pose_fields(BT::TreeNode& self);
};
#endif
diff --git a/src/geometry_bt_helpers.cpp b/src/geometry_bt_helpers.cpp
index affa0f1..6848223 100644
--- a/src/geometry_bt_helpers.cpp
+++ b/src/geometry_bt_helpers.cpp
@@ -18,6 +18,24 @@ void GeometryBTHelpers::init(IriBehaviorTreeFactory &factory)
BT::InputPort<double>("pitch"),
BT::InputPort<double>("yaw"),
BT::OutputPort<geometry_msgs::PoseStamped>("pose")};
+ BT::PortsList get_pose_frame_id_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::OutputPort<std::string>("frame_id")};
+ BT::PortsList get_pose_x_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::OutputPort<double>("x")};
+ BT::PortsList get_pose_y_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::OutputPort<double>("y")};
+ BT::PortsList get_pose_z_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::OutputPort<double>("z")};
+ BT::PortsList get_pose_roll_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::OutputPort<double>("roll")};
+ BT::PortsList get_pose_pitch_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::OutputPort<double>("pitch")};
+ BT::PortsList get_pose_yaw_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::OutputPort<double>("yaw")};
+
+ BT::PortsList get_pose_fields_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose"),
+ BT::OutputPort<std::string>("frame_id"),
+ BT::OutputPort<double>("x"),
+ BT::OutputPort<double>("y"),
+ BT::OutputPort<double>("z"),
+ BT::OutputPort<double>("roll"),
+ BT::OutputPort<double>("pitch"),
+ BT::OutputPort<double>("yaw")};
+
+
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);
@@ -25,6 +43,15 @@ void GeometryBTHelpers::init(IriBehaviorTreeFactory &factory)
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);
+ factory.registerSimpleAction("get_pose_frame_id", std::bind(&GeometryBTHelpers::get_pose_frame_id, this, std::placeholders::_1),get_pose_frame_id_ports);
+ factory.registerSimpleAction("get_pose_x", std::bind(&GeometryBTHelpers::get_pose_x, this, std::placeholders::_1),get_pose_x_ports);
+ factory.registerSimpleAction("get_pose_y", std::bind(&GeometryBTHelpers::get_pose_y, this, std::placeholders::_1),get_pose_y_ports);
+ factory.registerSimpleAction("get_pose_z", std::bind(&GeometryBTHelpers::get_pose_z, this, std::placeholders::_1),get_pose_z_ports);
+ factory.registerSimpleAction("get_pose_roll", std::bind(&GeometryBTHelpers::get_pose_roll, this, std::placeholders::_1),get_pose_roll_ports);
+ factory.registerSimpleAction("get_pose_pitch", std::bind(&GeometryBTHelpers::get_pose_pitch, this, std::placeholders::_1),get_pose_pitch_ports);
+ factory.registerSimpleAction("get_pose_yaw", std::bind(&GeometryBTHelpers::get_pose_yaw, this, std::placeholders::_1),get_pose_yaw_ports);
+
+ factory.registerSimpleAction("get_pose_fields", std::bind(&GeometryBTHelpers::get_pose_fields, this, std::placeholders::_1),get_pose_fields_ports);
}
BT::NodeStatus GeometryBTHelpers::transform_pose(BT::TreeNode& self)
@@ -57,8 +84,12 @@ BT::NodeStatus GeometryBTHelpers::transform_pose(BT::TreeNode& self)
return BT::NodeStatus::SUCCESS;
}
else
+ {
+ ROS_ERROR("GeometryBTHelpers::transform_pose (%s) -> transform between frames %s<-->%s does not exist)", self.name().c_str(), pose_in_value.header.frame_id.c_str(), target_frame_value.c_str());
return BT::NodeStatus::FAILURE;
+ }
}catch(tf2::TransformException &ex){
+ ROS_ERROR("GeometryBTHelpers::transform_pose (%s) -> tf2 exception: %s)", self.name().c_str(), ex.what());
return BT::NodeStatus::FAILURE;
}
}
@@ -237,4 +268,162 @@ BT::NodeStatus GeometryBTHelpers::get_pose(BT::TreeNode& self)
self.setOutput("pose",pose);
return BT::NodeStatus::SUCCESS;
-}
\ No newline at end of file
+}
+
+BT::NodeStatus GeometryBTHelpers::get_pose_frame_id(BT::TreeNode& self)
+{
+ BT::Optional<geometry_msgs::PoseStamped> pose_port = self.getInput<geometry_msgs::PoseStamped>("pose");
+ if(!pose_port)
+ {
+ ROS_ERROR_STREAM("GeometryBTHelpers::get_pose_frame_id (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose (geometry_msgs::PoseStamped)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ geometry_msgs::PoseStamped pose = pose_port.value();
+
+ self.setOutput("frame_id", pose.header.frame_id);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus GeometryBTHelpers::get_pose_x(BT::TreeNode& self)
+{
+ BT::Optional<geometry_msgs::PoseStamped> pose_port = self.getInput<geometry_msgs::PoseStamped>("pose");
+ if(!pose_port)
+ {
+ ROS_ERROR_STREAM("GeometryBTHelpers::get_pose_x (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose (geometry_msgs::PoseStamped)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ geometry_msgs::PoseStamped pose = pose_port.value();
+
+ self.setOutput("x", pose.pose.position.x);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus GeometryBTHelpers::get_pose_y(BT::TreeNode& self)
+{
+ BT::Optional<geometry_msgs::PoseStamped> pose_port = self.getInput<geometry_msgs::PoseStamped>("pose");
+ if(!pose_port)
+ {
+ ROS_ERROR_STREAM("GeometryBTHelpers::get_pose_y (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose (geometry_msgs::PoseStamped)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ geometry_msgs::PoseStamped pose = pose_port.value();
+
+ self.setOutput("y", pose.pose.position.y);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus GeometryBTHelpers::get_pose_z(BT::TreeNode& self)
+{
+ BT::Optional<geometry_msgs::PoseStamped> pose_port = self.getInput<geometry_msgs::PoseStamped>("pose");
+ if(!pose_port)
+ {
+ ROS_ERROR_STREAM("GeometryBTHelpers::get_pose_z (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose (geometry_msgs::PoseStamped)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ geometry_msgs::PoseStamped pose = pose_port.value();
+
+ self.setOutput("z", pose.pose.position.z);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus GeometryBTHelpers::get_pose_roll(BT::TreeNode& self)
+{
+ BT::Optional<geometry_msgs::PoseStamped> pose_port = self.getInput<geometry_msgs::PoseStamped>("pose");
+ if(!pose_port)
+ {
+ ROS_ERROR_STREAM("GeometryBTHelpers::get_pose_roll (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose (geometry_msgs::PoseStamped)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ geometry_msgs::PoseStamped pose = pose_port.value();
+
+ tf2::Quaternion q(pose.pose.orientation.x,
+ pose.pose.orientation.y,
+ pose.pose.orientation.z,
+ pose.pose.orientation.w);
+ tf2::Matrix3x3 m(q);
+ double roll, pitch, yaw;
+ m.getRPY(roll, pitch, yaw);
+
+ self.setOutput("roll", roll);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus GeometryBTHelpers::get_pose_pitch(BT::TreeNode& self)
+{
+ BT::Optional<geometry_msgs::PoseStamped> pose_port = self.getInput<geometry_msgs::PoseStamped>("pose");
+ if(!pose_port)
+ {
+ ROS_ERROR_STREAM("GeometryBTHelpers::get_pose_pitch (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose (geometry_msgs::PoseStamped)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ geometry_msgs::PoseStamped pose = pose_port.value();
+
+ tf2::Quaternion q(pose.pose.orientation.x,
+ pose.pose.orientation.y,
+ pose.pose.orientation.z,
+ pose.pose.orientation.w);
+ tf2::Matrix3x3 m(q);
+ double roll, pitch, yaw;
+ m.getRPY(roll, pitch, yaw);
+
+ self.setOutput("pitch", pitch);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus GeometryBTHelpers::get_pose_yaw(BT::TreeNode& self)
+{
+ BT::Optional<geometry_msgs::PoseStamped> pose_port = self.getInput<geometry_msgs::PoseStamped>("pose");
+ if(!pose_port)
+ {
+ ROS_ERROR_STREAM("GeometryBTHelpers::get_pose_yaw (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose (geometry_msgs::PoseStamped)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ geometry_msgs::PoseStamped pose = pose_port.value();
+
+ tf2::Quaternion q(pose.pose.orientation.x,
+ pose.pose.orientation.y,
+ pose.pose.orientation.z,
+ pose.pose.orientation.w);
+ tf2::Matrix3x3 m(q);
+ double roll, pitch, yaw;
+ m.getRPY(roll, pitch, yaw);
+
+ self.setOutput("yaw", yaw);
+ return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus GeometryBTHelpers::get_pose_fields(BT::TreeNode& self)
+{
+ BT::Optional<geometry_msgs::PoseStamped> pose_port = self.getInput<geometry_msgs::PoseStamped>("pose");
+ if(!pose_port)
+ {
+ ROS_ERROR_STREAM("GeometryBTHelpers::get_pose_fields (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose (geometry_msgs::PoseStamped)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ geometry_msgs::PoseStamped pose = pose_port.value();
+
+ tf2::Quaternion q(pose.pose.orientation.x,
+ pose.pose.orientation.y,
+ pose.pose.orientation.z,
+ pose.pose.orientation.w);
+ tf2::Matrix3x3 m(q);
+ double roll, pitch, yaw;
+ m.getRPY(roll, pitch, yaw);
+
+ self.setOutput("x", pose.pose.position.x);
+ self.setOutput("y", pose.pose.position.y);
+ self.setOutput("z", pose.pose.position.z);
+ self.setOutput("roll", roll);
+ self.setOutput("pitch", pitch);
+ self.setOutput("yaw", yaw);
+ return BT::NodeStatus::SUCCESS;
+}
+
--
GitLab