How to efficiently get TransformStamped in Eloquent?
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?