Ask Your Question
1

transform geometry_msgs::TransformStamped to tf2::Transform transform

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

DinnerHowe gravatar image

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

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:

  tf2::Transform

so that I can get OpenGL SubMatrix by

     double mv[12];
     *.getBasis().getOpenGLSubMatrix(mv);

How can I deal with this problem?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
3

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

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
1

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

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);
    #endif

    glm::dmat4 model_matrix_double;
    stamped_transform.getOpenGLMatrix(&model_matrix_double[0][0]);
    // 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) {
    ...
  }

https://github.com/lucasw/imgui_ros/b...

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.

geometry_msgs::msg::TransformStamped 
  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

3 followers

Stats

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

Seen: 3,520 times

Last updated: Dec 10 '18