Ask Your Question
0

How to efficiently get TransformStamped in Eloquent?

asked 2020-03-26 05:29:46 -0500

bluemoon93 gravatar image

I'm trying to build a geometry_msgs::msg::TransformStamped message, to send through a TransformBroadcaster.

void sendTransform(tf2:Transform myTf2Transform,  rclcpp::Time myTime, std::string global_frame, std::string child_frame)
{
    geometry_msgs::msg::TransformStamped tmp_tf_stamped;
    // ...
    tfb_->sendTransform(tmp_tf_stamped);
}

I tried to use tf2::convert to convert a tf2::Stamped<tf2::Transform> into geometry_msgs::msg::TransformStamped:

tf2::TimePoint myTimepoint; // TODO get tf2::TimePoint from rclcpp::Time
tf2::Stamped <tf2::Transform> test_stamped(myTf2Transform, myTimepoint, global_frame);
tf2::convert(test_stamped, tmp_tf_stamped);    
// /opt/ros/eloquent/include/tf2/impl/convert.h:65:12: error: no matching function for call to ‘toMsg(const tf2::Stamped<tf2::Transform>&)’
tmp_tf_stamped.child_frame_id = child_frame;

I then tried to create the message and just fill in its fields, converting the tf2:Transform into the geometry_msgs::msg::Transform type of the message field:

tmp_tf_stamped.child_frame_id = child_frame;
tmp_tf_stamped.header.frame_id = global_frame;
tmp_tf_stamped.header.stamp = myTime;

geometry_msgs::msg::Transform test_transform;
tf2::convert(latest_tf_.inverse(), test_transform); 
// /opt/ros/eloquent/include/tf2/impl/convert.h:65:12: error: no matching function for call to ‘toMsg(const tf2::Transform&)’
tmp_tf_stamped.transform = test_transform;

And finally I got it working by inputting each field by hand.

tmp_tf_stamped.child_frame_id = child_frame;
tmp_tf_stamped.header.frame_id = global_frame;
tmp_tf_stamped.header.stamp = myTime;
tmp_tf_stamped.transform.translation.x = myTf2Transform.getOrigin().x();
tmp_tf_stamped.transform.translation.y = myTf2Transform.getOrigin().y();
tmp_tf_stamped.transform.translation.z = myTf2Transform.getOrigin().z();
tmp_tf_stamped.transform.rotation.x = myTf2Transform.getRotation().x();
tmp_tf_stamped.transform.rotation.y = myTf2Transform.getRotation().y();
tmp_tf_stamped.transform.rotation.z = myTf2Transform.getRotation().z();
tmp_tf_stamped.transform.rotation.w = myTf2Transform.getRotation().w();

This feels quite inefficient, though. What is the recommended approach to do this?

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2020-03-26 12:46:33 -0500

tfoote gravatar image

updated 2020-03-26 12:48:47 -0500

Did you include the header tf2_geometry_msgs/tf2_geometry_msgs.h that implements the templated version of those conversions?

https://github.com/ros/geometry2/blob...

geometry_msgs::TransformStamped toMsg(const tf2::Stamped<tf2::Transform>& in)

Sorry that's meldodic, it's also in ROS 2 at: https://github.com/ros2/geometry2/blo...

edit flag offensive delete link more

Comments

Yes, that works. I just have to find a way to convert rclcpp::Time to tf2:TimePoint

bluemoon93 gravatar image bluemoon93  ( 2020-03-27 08:28:22 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-03-26 05:14:40 -0500

Seen: 44 times

Last updated: Mar 26