Revision history [back]

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