While using message_filters to do the time synchronization,but it cannot enter into the callback function
I am using sync_policies::ApproximateTime in message_filter to do the PointCloud2 and LaserScan time synchronization, but the problem is it cannot enter into the callback function ofsync.registerCallback(boost::bind(&Message_Filter::callback, this, _1, _2));
. At first, I think that it is because the publishing frequency of the two topics are different which make the code cannot enter into the callback function, but when I write another publish node which publishes two topic almost at the same time, it still not work, so I really don't know where the problem is. Does anyone knows about this? Thanks a lot.
Here is my codes:
#include "message_filter_node.h"
Message_Filter::Message_Filter()
{
message_filters::Subscriber<sensor_msgs::PointCloud2> Velodyne_sub(nh, "/velodyne_points", 1);
message_filters::Subscriber<sensor_msgs::LaserScan> Hokuyo_sub(nh,"/scan" , 1);
typedef sync_policies::ApproximateTime<PointCloud2, LaserScan> MySyncPolicy;
// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
Synchronizer<MySyncPolicy> sync(MySyncPolicy(100), Velodyne_sub, Hokuyo_sub);
sync.registerCallback(boost::bind(&Message_Filter::callback, this, _1, _2));
Velodyne_pub=nh.advertise<PointCloud2>("message_filter/velodyne_points",1);
Hokuyo_pub=nh.advertise<LaserScan>("message_filter/scan",1);
}
void Message_Filter::callback(const PointCloud2::ConstPtr& point_cloud2, const LaserScan::ConstPtr& laser_scan)
{
ROS_ERROR("Enter Publish");
Velodyne_pub.publish(point_cloud2);
Hokuyo_pub.publish(laser_scan);
}
Here is my header file:
#ifndef MESSAGE_FILTER_NODE_H
#define MESSAGE_FILTER_NODE_H
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include "image_transport/image_transport.h"
using namespace sensor_msgs;
using namespace message_filters;
class Message_Filter
{
public:
Message_Filter();
void callback(const PointCloud2::ConstPtr& point_cloud2, const LaserScan::ConstPtr& laser_scan);
public:
ros::Publisher Velodyne_pub;
ros::Publisher Hokuyo_pub;
ros::NodeHandle nh;
};
#endif // MESSAGE_FILTER_NODE_H
Thanks Dimitri Schachmann , the problem was solved. And my codes are:
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include "image_transport/image_transport.h"
#include "ros/ros.h"
//ros::Publisher Velodyne_pub;
void callback(const sensor_msgs:: PointCloud2::ConstPtr& point_cloud2, const
sensor_msgs::LaserScan::ConstPtr& laser_scan)
{
ROS_ERROR("Enter Publish");
// Velodyne_pub.publish(point_cloud2);
// Hokuyo_pub.publish(laser_scan);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "message_filter_node");
ros::Time::init();
ros::NodeHandle nh;
ROS_INFO("start message filter");
message_filters::Subscriber<sensor_msgs::PointCloud2> Velodyne_sub(nh, "/velodyne_points", 1);
message_filters::Subscriber<sensor_msgs::LaserScan> Hokuyo_sub(nh,"/scan" , 1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,
sensor_msgs::LaserScan> MySyncPolicy;
// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), Velodyne_sub, Hokuyo_sub);
sync.registerCallback(boost::bind(&callback, _1, _2));
ros::spin();
return 0;
}