0
$\begingroup$

Rosanswers logo

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?


Originally posted by DinnerHowe on ROS Answers with karma: 188 on 2018-01-25

Post score: 1

$\endgroup$

2 Answers 2

1
$\begingroup$

Rosanswers logo

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...


Originally posted by mgruhler with karma: 12390 on 2018-01-25

This answer was NOT ACCEPTED on the original site

Post score: 3

$\endgroup$
0
$\begingroup$

Rosanswers logo

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/blob/406ff011234762f34027649da7e834d0a0ada6ea/imgui_ros/src/viz3d.cpp#L384

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();
  ...

Originally posted by lucasw with karma: 8729 on 2018-12-10

This answer was NOT ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.