ROS订阅多个话题,但是始终无法进入回调函数内
-
编写的C++程序,订阅了两个话题,通过
message_filters::TimeSynchronizer
进行了同步然后传入回调函数处理,但是话题一直无法传入回调函数内,订阅的相关话题也均有内容发布并不是未发布或者错误话题。在编译和运行的过程中没有出现警告和错误。
原始代码如下:#include <iostream> #include <boost/bind.hpp> #include <opencv2/opencv.hpp> #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/imgproc.hpp> #include <cv_bridge/cv_bridge.h> #include <ros/ros.h> #include <message_filters/subscriber.h> #include <message_filters/time_synchronizer.h> #include <message_filters/sync_policies/approximate_time.h> #include <sensor_msgs/Image.h> #include "depthai_ros_msgs/SpatialDetection.h" #include "depthai_ros_msgs/SpatialDetectionArray.h" #include "detectmsg/pose2D.h" #include "detectmsg/multi_pedestrian_pose.h" void Callback(const depthai_ros_msgs::SpatialDetectionArray::ConstPtr& , const sensor_msgs::Image::ConstPtr& image) { ROS_INFO("In!"); cv::Mat img; cv_bridge::CvImagePtr cv_ptr; cv_ptr = cv_bridge::toCvCopy(image, "bgr8");//"bgr8"、"rgb8"、"mono8" cv_ptr->image.copyTo(img); cv::imshow("YOLO", img); cv::waitKey(3); } int main(int argc, char **argv){ ros::init(argc, argv, "obj_publish_node"); ros::NodeHandle nh; message_filters::Subscriber<depthai_ros_msgs::SpatialDetectionArray> obj_sub_(nh, "/yolov4_publisher/color/yolov4_Spatial_detections", 1); message_filters::Subscriber<sensor_msgs::Image> image_sub_(nh, "/yolov4_publisher/color/image", 1); message_filters::TimeSynchronizer<depthai_ros_msgs::SpatialDetectionArray, sensor_msgs::Image> sync(obj_sub_, image_sub_, 1000); sync.registerCallback(boost::bind(&Callback, _1, _2)); ros::spin(); return 0; }
两个话题的发布情况如下:
so@so-QiTianM437-A449:~/v5ros$ rostopic hz /yolov4_publisher/color/image subscribed to [/yolov4_publisher/color/image] average rate: 7.071 min: 0.011s max: 0.331s std dev: 0.12130s window: 6 average rate: 7.210 min: 0.008s max: 0.331s std dev: 0.10411s window: 14 average rate: 7.740 min: 0.005s max: 0.331s std dev: 0.08521s window: 23 average rate: 7.706 min: 0.005s max: 0.331s std dev: 0.08848s window: 30 average rate: 7.739 min: 0.001s max: 0.331s std dev: 0.08873s window: 37 average rate: 7.737 min: 0.001s max: 0.331s std dev: 0.08291s window: 46 average rate: 7.799 min: 0.001s max: 0.331s std dev: 0.07954s window: 50
so@so-QiTianM437-A449:~/v5ros$ rostopic hz /yolov4_publisher/color/yolov4_Spatial_detections subscribed to [/yolov4_publisher/color/yolov4_Spatial_detections] average rate: 6.753 min: 0.001s max: 0.270s std dev: 0.08529s window: 7 average rate: 7.090 min: 0.001s max: 0.301s std dev: 0.09249s window: 14 average rate: 7.763 min: 0.001s max: 0.301s std dev: 0.08885s window: 23 average rate: 7.554 min: 0.001s max: 0.301s std dev: 0.09299s window: 29 average rate: 7.384 min: 0.001s max: 0.339s std dev: 0.09336s window: 37 average rate: 7.631 min: 0.001s max: 0.339s std dev: 0.09092s window: 45 average rate: 7.625 min: 0.001s max: 0.339s std dev: 0.09341s window: 50
两个话题均是同一个节点所发,不存在频率差异过大等问题。也尝试将c++的代码改写为python,但也是一样无法进入回调函数内。 同样的代码我在另外一套程序中订阅可以顺利进入回调,但是在该程序中迟迟无法进入回调,这个问题我无法解决,请有懂得的大佬帮我分析下是哪个环节出现了问题呢?
-
@2229364719 核对下消息类型和时间戳,深度和彩色图像大小应该不同,传输速度不同,虽然频率接近,但可能存在时检差
-
@2229364719 要不用Synchronizer ApproximateTime试一下
https://blog.csdn.net/weixin_42803820/article/details/127113959