ROS2消息主题时间戳处理和同步？

Question

我用 C++ 成功构建并运行了一个 ROS2 简陋节点。我订阅了两个主题，即相机图像（/camera/image）和imu（/drone/imu）。然后我调用 ORB_SLAM3 方法并能够发布 /orb_pose 主题的姿势。问题在于姿势主题中有一个谜语（没有发布任何值）。这是发生这种情况的函数。

void ImageGrabber::SyncWithImu()
{
    // Define a rate control to limit loop frequency
    rclcpp::Rate loop_rate(30);  // Adjust the frequency as needed

    while (rclcpp::ok())
    {
        // Fetch IMU data with lock and minimal work within lock
        std::vector<ORB_SLAM3::IMU::Point> vImuMeas;
        {
            std::lock_guard<std::mutex> lock(mpImuGb->mBufMutex);
            while (!mpImuGb->imuBuf.empty())
            {
                auto imu_msg = mpImuGb->imuBuf.front();
                double t = imu_msg->header.stamp.nanosec * 0.17;
                //double t = imu_msg->header.stamp.nanosec * 0.17;

                cv::Point3f gyr(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z);
                cv::Point3f acc(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z);

                vImuMeas.push_back(ORB_SLAM3::IMU::Point(gyr, acc, t));
                mpImuGb->imuBuf.pop();
            }
        }

        // Process image with minimal work within lock
        cv::Mat im;
        double tIm = 0;
        {
            std::lock_guard<std::mutex> lock(mBufMutex);
            if (!img0Buf.empty())
            {
                tIm = img0Buf.front()->header.stamp.sec * 1e-9;
                //tIm = img0Buf.front()->header.stamp.sec*1e2 + img0Buf.front()->header.stamp.nanosec * 1e-9;
                im = GetImage(img0Buf.front());
                img0Buf.pop();
            }
        }

        if (!vImuMeas.empty() && !im.empty())
        {
            // Perform image processing outside the lock
            if (mbClahe)
                mClahe->apply(im, im);

            // Perform SLAM operations
            Sophus::SE3f Tcw_se3 = mpSLAM->TrackMonocular(im, tIm, vImuMeas);

            // Publish pose data
            Eigen::Matrix4f Tcw_eigen = Tcw_se3.matrix().cast<float>();
            Eigen::Matrix3f R_eigen = Tcw_eigen.block<3, 3>(0, 0);
            Eigen::Vector3f t_eigen = Tcw_eigen.block<3, 1>(0, 3);

            geometry_msgs::msg::PoseStamped pose_msg;
            pose_msg.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now();
            pose_msg.header.frame_id = "/world";
            pose_msg.pose.position.x = t_eigen(0);
            pose_msg.pose.position.y = t_eigen(1);
            pose_msg.pose.position.z = t_eigen(2);

            Eigen::Quaterniond quat(R_eigen.cast<double>());
            pose_msg.pose.orientation.x = quat.x();
            pose_msg.pose.orientation.y = quat.y();
            pose_msg.pose.orientation.z = quat.z();
            pose_msg.pose.orientation.w = quat.w();

            orb_pub_->publish(pose_msg);
        }

        // Control loop frequency
        loop_rate.sleep();
    }
}

这里是img0Buf的声明

class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM3::System* pSLAM, ImuGrabber *pImuGb, const bool bClahe, rclcpp::Logger logger)
        : mpSLAM(pSLAM), mpImuGb(pImuGb), mbClahe(bClahe), logger_(logger) {}

    void GrabImage(const sensor_msgs::msg::Image::SharedPtr img_msg);
    cv::Mat GetImage(const sensor_msgs::msg::Image::SharedPtr img_msg);
    void SyncWithImu();
    void SetPub(rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub);

    queue<sensor_msgs::msg::Image::SharedPtr> img0Buf;

以及使用它的功能

void ImageGrabber::GrabImage(const sensor_msgs::msg::Image::SharedPtr img_msg)
{
    mBufMutex.lock();
    if (!img0Buf.empty())
        img0Buf.pop();
    img0Buf.push(img_msg);
    mBufMutex.unlock();
    //RCLCPP_INFO(logger_, "ImageGrabber: New image data grabbed.");
}

相机/图像主题回显标题是这样的

header:
  stamp:
    sec: 1656
    nanosec: 192730000
  frame_id: ''
height: 720
width: 1280
encoding: bgr8
is_bigendian: 0
step: 3840
data:
- 67

imu主题标题是

header:
  stamp:
    sec: 1692350187
    nanosec: 261417829
  frame_id: /world
orientation:
  x: 0.00347900390625
  y: -0.03326416015625
  z: 0.18280029296875
  w: 0.9825439453125
orientation_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
linear_acceleration:
  x: 0.0
  y: 0.0
  z: 0.0
linear_acceleration_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0

发布的姿势是

--
header:
  stamp:
    sec: 1692351274
    nanosec: 545066597
  frame_id: /world
pose:
  position:
    x: -0.04395366832613945
    y: 0.14896374940872192
    z: -0.015092355199158192
  orientation:
    x: -0.002570459378912546
    y: 0.0052243227063072765
    z: 0.009787312273148031
    w: 0.9999351480435963

我认为问题出在这两行（时间戳计算）

double t = imu_msg->header.stamp.nanosec * 0.17
tIm = img0Buf.front()->header.stamp.sec * 1e-9;

使用此值时

tIm = img0Buf.front()->header.stamp.sec + img0Buf.front()->header.stamp.nanosec;

它崩溃并在 Sohpys 中出现分段错误错误（一些 nan 值）。有什么解决方案可以让它平稳连续地工作而没有任何空闲阶段吗？

Answer 1

您的时间戳预计以秒为单位，因此这一行：

tIm = img0Buf.front()->header.stamp.sec * 1e-9;

为您提供了无效的秒值（因为这将是一个小十进制）。

相反，您应该提供从消息标题中获取的纯秒时间，例如

tIm = img0Buf.front()->header.stamp.sec;