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