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