编写的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,但也是一样无法进入回调函数内。
同样的代码我在另外一套程序中订阅可以顺利进入回调,但是在该程序中迟迟无法进入回调,这个问题我无法解决,请有懂得的大佬帮我分析下是哪个环节出现了问题呢?