How to do tf2 transform and pose publish with ROS2 and humble?

Question

I like to publish a pose using IMU and Monocular Image from ORB_SLAM3 in ROS2 and humble. I made a function. But some how not familiar with tf2 in ROS2 . So I got some errors. I include the tf2 header and also declared in CMakeList.txt dependencies. But still not working

Here the code

...
void ImageGrabber::SyncWithImu()
{
  //while(1)
  while (rclcpp::ok())
  {
    cv::Mat im;
    double tIm = 0;
    if (!img0Buf.empty()&&!mpImuGb->imuBuf.empty())
    {
      tIm = img0Buf.front()->header.stamp.sec + img0Buf.front()->header.stamp.nanosec * 1e-9;
      if (tIm > mpImuGb->imuBuf.back()->header.stamp.sec + mpImuGb->imuBuf.back()->header.stamp.nanosec * 1e-9)
          continue;
      {
      this->mBufMutex.lock();
      im = GetImage(img0Buf.front());
      img0Buf.pop();
      this->mBufMutex.unlock();
      }

      vector<ORB_SLAM3::IMU::Point> vImuMeas;
      mpImuGb->mBufMutex.lock();
      if(!mpImuGb->imuBuf.empty())
      {
        // Load imu measurements from buffer
        vImuMeas.clear();
        while (!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.sec +
                        mpImuGb->imuBuf.front()->header.stamp.nanosec * 1e-9 <= tIm)
        {
          double t = mpImuGb->imuBuf.front()->header.stamp.sec + mpImuGb->imuBuf.front()->header.stamp.nanosec * 1e-9;          
          cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);
          cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);
          vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t));
          mpImuGb->imuBuf.pop();
        }
      }
      mpImuGb->mBufMutex.unlock();
      if(mbClahe)
        mClahe->apply(im,im);

      cv::Mat T_, R_, t_ ;

      //stores return variable of TrackMonocular
      mpSLAM->TrackMonocular(im, tIm, vImuMeas);

      if (pub_tf || pub_pose)
      {    
        if (!(T_.empty())) {

          cv::Size s = T_.size();
          if ((s.height >= 3) && (s.width >= 3)) {
            R_ = T_.rowRange(0,3).colRange(0,3).t();
            t_ = -R_*T_.rowRange(0,3).col(3);
            std::vector<float> q = ORB_SLAM3::Converter::toQuaternion(R_);
            float scale_factor=1.0;
            //tf2::Transform transform;
            geometry_msgs::msg::TransformStamped tf_msg;
            tf_msg.header.stamp = rclcpp::Time(tIm);
            tf_msg.header.frame_id = "world";
            tf_msg.child_frame_id = "ORB_SLAM3_MONO_INERTIAL";
            tf2::Transform tf_msg;
            tf_msg.transform.translation.x = t_.at<float>(0, 0) * scale_factor;
            tf_msg.transform.translation.y = t_.at<float>(0, 1) * scale_factor;
            tf_msg.transform.translation.z = t_.at<float>(0, 2) * scale_factor;
            tf_msg.transform.rotation.x = q[0];
            tf_msg.transform.rotation.y = q[1];
            tf_msg.transform.rotation.z = q[2];
            tf_msg.transform.rotation.w = q[3];
            //transform = tf2::Transform(tf_msg.transform);
            //tf_broadcaster_->sendTransform(tf_msg.transform);
          /*
          if (pub_tf)
            {
              static tf::TransformBroadcaster br_;
              br_.sendTransform(tf::StampedTransform(transform, ros::Time(tIm), "world", "ORB_SLAM3_MONO_INERTIAL"));
            }

          */

          if (pub_pose)
            {
              geometry_msgs::msg::PoseStamped pose;
              //pose.header.stamp = img0Buf.front()->header.stamp;
              pose.header.frame_id ="ORB_SLAM3_MONO_INERTIAL";
              tf_msg::poseTFToMsg(transform, pose.pose);
              orb_pub->publish(pose);
            }

          }
        }
      }
    }

    std::chrono::milliseconds tSleep(1);
    std::this_thread::sleep_for(tSleep);
  }
}

..

So i got errors in

tf2::Transform tf_msg;

and

tf_msg::poseTFToMsg(transform, pose.pose);

The errors are:

error: ‘tf2’ has not been declared
    245 | tf2::Transform tf_msg;

    error: ‘tf_msg’ is not a class, namespace, or enumeration 269 |
    tf_msg::poseTFToMsg(transform, pose.pose);

Any help? Thanks

Answer 1

In ROS2, the tf2 library is slightly different from ROS1, and it seems like you are mixing up the usage of tf2 with tf1 (ROS1). To use tf2 in ROS2, you need to include the appropriate headers and use the correct types and functions.

Modify your code to use tf2 in ROS2:

1. Include the necessary headers for tf2:

#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

2. Replace the line with the error "tf2::Transform tf_msg;" with:

tf2::Transform tf_transform;

3. Modify the publishing part to use the correct functions for tf2:

// Instead of "tf_msg::poseTFToMsg(transform, pose.pose);"
// Use the following to convert the tf2::Transform to geometry_msgs::msg::Pose
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = "ORB_SLAM3_MONO_INERTIAL";
tf2::toMsg(tf_transform, pose.pose);
orb_pub->publish(pose);

Also, since you have already declared the tf2_geometry_msgs dependency in your CMakeLists.txt, make sure to build your ROS2 package to apply the changes.

With this, you should be able to use tf2 for publishing the pose in ROS2. Remember that tf2 is slightly different from tf1, so the function names and types are not interchangeable between the two.