ROS在我玩罗斯巴格时没有发布关于某个主题的消息



我正在尝试使用如下代码从我自己的图像(RGB-D(和IMU数据创建一个rosbag:

sensor_msgs::ImagePtr ros_rgb_msg;
ros_rgb_msg = imgrgb.toImageMsg();
ros_rgb_msg->header.seq = iFrame;
ros_rgb_msg->header.stamp = stamp;
ros_rgb_msg->header.frame_id = iFrame;
bag.write("cam0/image_raw", stamp,ros_rgb_msg);
sensor_msgs::ImagePtr ros_depth_msg;
ros_depth_msg = imgdepth.toImageMsg();
ros_depth_msg->header.seq = iFrame;
ros_depth_msg->header.stamp = stamp;
ros_depth_msg->header.frame_id = iFrame;
bag.write("cam0/depth", stamp, ros_depth_msg);
sensor_msgs::Imu imu_msg;
imu_msg.header.stamp = stamp;
imu_msg.header.seq = iFrame;
imu_msg.header.frame_id = "imu";
imu_msg.angular_velocity.x = tmpgyros.data[0];
imu_msg.angular_velocity.y = tmpgyros.data[1];
imu_msg.angular_velocity.z = tmpgyros.data[2];
imu_msg.linear_acceleration.x = tmpacc.data[0];
imu_msg.linear_acceleration.y = tmpacc.data[1];
imu_msg.linear_acceleration.z = tmpacc.data[2];
imu_msg.angular_velocity_covariance[0] = 0.001f;
imu_msg.angular_velocity_covariance[1] = 0.0f;
imu_msg.angular_velocity_covariance[2] = 0.0f;
imu_msg.angular_velocity_covariance[3] = 0.0f;
imu_msg.angular_velocity_covariance[4] = 0.001f;
imu_msg.angular_velocity_covariance[5] = 0.0f;
imu_msg.angular_velocity_covariance[6] = 0.0f;
imu_msg.angular_velocity_covariance[7] = 0.0f;
imu_msg.angular_velocity_covariance[8] = 0.001f;
imu_msg.linear_acceleration_covariance = imu_msg.orientation_covariance =imu_msg.angular_velocity_covariance;
bag.write("/imu0", stamp, imu_msg);

其中 iFrame 是一个int,表示 iFrame-th 图像正在写入包中,代码的开头六行将 RGB 部分写入包中并发布到cam0/image_raw上,接下来的六行将深度部分写入包中并发布到cam0/depth

然后我尝试使用生成的包作为 VINS 的输入,在feature_tracker_node.cpp中,我修改了订阅者如下

ros::Subscriber sub_img = n.subscribe("cam0/image_raw", 100, img_callback);

但是,当我第一次运行roslaunch vins_estimator euroc.launch然后在两个不同的终端中运行rosbag play mytest.bag时,我使用rostopic hz cam0/image_raw来监视节点上的消息,但它显示当两个终端都运行时,此节点上没有发布任何消息。而feature_tracker_node.cpp中的回调函数,即,void img_callback(const sensor_msgs::ImageConstPtr &img_msg)永远不会被调用。

但是当我观看imu0时,它运行良好,发布了许多有关此主题的消息。

我也用了

cv_bridge::CvImageConstPtr ptr1;
ptr1 = cv_bridge::toCvCopy(ros_rgb_msg, sensor_msgs::image_encodings::RGB8);
cv::imshow("test bag1",ptr1->image);
cv::waitKey(0);

在我将其写入包中之前检查图像是否损坏,但是,显示的图像没有损坏

有谁知道可能导致问题的原因?

对我来说,主题名称似乎有问题。查看 ROS 文档以了解名称。特别是绝对和相对名称。

您正在相对于您的节点进行订阅:

ros::Subscriber sub_img = n.subscribe("cam0/image_raw", 100, img_callback);

这意味着,结果将是订阅像/your_node_name/cam0/image_raw这样的主题,这不是 rosbag 发布的主题。要订阅绝对,您需要在主题前面添加一个/,例如:

ros::Subscriber sub_img = n.subscribe("/cam0/image_raw", 100, img_callback);

提示:要检查可用主题,请输入

rostopic list

在开始重播后的终端中。

仔细检查数据集后,我发现它的 IMU 和 RGB-D 时间戳不匹配,所以,问题是我忘记编写有关时间戳更正的代码。

最新更新