Skip to content
Snippets Groups Projects
Commit 1e7be611 authored by Fernando Herrero's avatar Fernando Herrero
Browse files

geometry helpers: add get_pose_* and get_pose_fields actions to retrieve pose...

geometry helpers: add get_pose_* and get_pose_fields actions to retrieve pose x,y,z,roll,pitch,yaw,frame_id fields
parent efb4118e
No related branches found
No related tags found
No related merge requests found
...@@ -61,9 +61,9 @@ class GeometryBTHelpers ...@@ -61,9 +61,9 @@ class GeometryBTHelpers
* *
* distance (double): Distance. * distance (double): Distance.
* *
* \return BT:NodeStatus::SUCCESS if distance can be computed. * \return BT:NodeStatus::SUCCESS if pose can be transformed
* Otherwise returns BT:NodeStatus::FAILURE. If inputs are missing or incorrect it also * Otherwise returns BT:NodeStatus::FAILURE.
* returns BT:NodeStatus::FAILURE. * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
*/ */
BT::NodeStatus compute_distance(BT::TreeNode& self); BT::NodeStatus compute_distance(BT::TreeNode& self);
...@@ -98,11 +98,36 @@ class GeometryBTHelpers ...@@ -98,11 +98,36 @@ class GeometryBTHelpers
* *
* pose (geometry_msgs::PoseStamped): the generated stamped pose * pose (geometry_msgs::PoseStamped): the generated stamped pose
* *
* \return BT:NodeStatus::SUCCESS if distance can be computed. * \return BT:NodeStatus::SUCCESS if pose can be returned. Otherwise returns BT:NodeStatus::FAILURE.
* Otherwise returns BT:NodeStatus::FAILURE. If inputs are missing or incorrect it also * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
* returns BT:NodeStatus::FAILURE.
*/ */
BT::NodeStatus get_pose(BT::TreeNode& self); 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 #endif
...@@ -18,6 +18,24 @@ void GeometryBTHelpers::init(IriBehaviorTreeFactory &factory) ...@@ -18,6 +18,24 @@ void GeometryBTHelpers::init(IriBehaviorTreeFactory &factory)
BT::InputPort<double>("pitch"), BT::InputPort<double>("pitch"),
BT::InputPort<double>("yaw"), BT::InputPort<double>("yaw"),
BT::OutputPort<geometry_msgs::PoseStamped>("pose")}; 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("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_distance", std::bind(&GeometryBTHelpers::compute_distance, this, std::placeholders::_1),compute_distance_ports);
...@@ -25,6 +43,15 @@ void GeometryBTHelpers::init(IriBehaviorTreeFactory &factory) ...@@ -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_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("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", 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) BT::NodeStatus GeometryBTHelpers::transform_pose(BT::TreeNode& self)
...@@ -57,8 +84,12 @@ BT::NodeStatus GeometryBTHelpers::transform_pose(BT::TreeNode& self) ...@@ -57,8 +84,12 @@ BT::NodeStatus GeometryBTHelpers::transform_pose(BT::TreeNode& self)
return BT::NodeStatus::SUCCESS; return BT::NodeStatus::SUCCESS;
} }
else 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; return BT::NodeStatus::FAILURE;
}
}catch(tf2::TransformException &ex){ }catch(tf2::TransformException &ex){
ROS_ERROR("GeometryBTHelpers::transform_pose (%s) -> tf2 exception: %s)", self.name().c_str(), ex.what());
return BT::NodeStatus::FAILURE; return BT::NodeStatus::FAILURE;
} }
} }
...@@ -237,4 +268,162 @@ BT::NodeStatus GeometryBTHelpers::get_pose(BT::TreeNode& self) ...@@ -237,4 +268,162 @@ BT::NodeStatus GeometryBTHelpers::get_pose(BT::TreeNode& self)
self.setOutput("pose",pose); self.setOutput("pose",pose);
return BT::NodeStatus::SUCCESS; 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;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment