Ask Your Question

transform geometry_msgs::TransformStamped to tf2::Transform transform

asked 2018-01-25 00:37:17 -0500

DinnerHowe gravatar image

updated 2018-01-25 00:55:58 -0500

hi, there:

I use tf2 get a transform between two frames, with type of geometry_msgs::TransformStamped.

  transformStamped = tf2_listener.lookupTransform(target_frame,  input.header.frame_id, ros::Time(0), ros::Duration(1.0));

Now I wanna transfer the transformStamped to follow data type:


so that I can get OpenGL SubMatrix by

     double mv[12];

How can I deal with this problem?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted

answered 2018-01-25 01:33:18 -0500

mgruhler gravatar image

There are the toMsg and fromMsg conversion functions in tf2_geometry_msgs (API Doc). Note that you'll end up with a Stamped transform than. But the rest is trivial...

edit flag offensive delete link more

answered 2018-12-10 09:08:38 -0500

lucasw gravatar image

This is for ros2 bouncy but mostly similar to older versions, also using glm instead of raw arrays:

 glm::mat4 model_matrix = glm::mat4(1.0f);
 try {
    geometry_msgs::msg::TransformStamped tf;
    tf = tf_buffer_->lookupTransform(frame_id_, child_frame_id, tf2::TimePointZero);
    tf2::Stamped<tf2::Transform> stamped_transform;
    tf2::fromMsg(tf, stamped_transform);
    #if 0
    tf2::TimePoint time_out;
    // this is private, so doesn't work 
    tf_buffer_->lookupTransformImpl(frame_id_, child_frame_id,
        tf2::TimePointZero, transform, time_out);

    glm::dmat4 model_matrix_double;
    // TODO(lucasw) is there a glm double to float conversion function?
    for (size_t i = 0; i < 4; ++i)
      for (size_t j = 0; j < 4; ++j)
        model_matrix[i][j] = model_matrix_double[i][j];
  } catch (tf2::TransformException& ex) {

Inside the code for lookupTransform() is the private lookupTransform which uses a tf2::Stamped, which is then converted to a geometry_msgs::msg::TransformStamped (manually, it isn't using toMsg itself) which then has to be converted immediately back in user code because API (I think tf1 exposed a tf::transform lookup, it wasn't private then)- seems inefficient but probably not too bad unless millions of these are being done in tight loop.

  BufferCore::lookupTransform(const std::string& target_frame, const std::string& source_frame,
      const TimePoint& time) const
  tf2::Transform transform;
  TimePoint time_out;
  lookupTransformImpl(target_frame, source_frame, time, transform, time_out);
  geometry_msgs::msg::TransformStamped msg;
  msg.transform.translation.x = transform.getOrigin().x();
  msg.transform.translation.y = transform.getOrigin().y();
  msg.transform.translation.z = transform.getOrigin().z();
  msg.transform.rotation.x = transform.getRotation().x();
  msg.transform.rotation.y = transform.getRotation().y();
  msg.transform.rotation.z = transform.getRotation().z();
  msg.transform.rotation.w = transform.getRotation().w();
edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools



Asked: 2018-01-25 00:37:17 -0500

Seen: 8,083 times

Last updated: Dec 10 '18