MessageFilter ApproximateTime doesn't call callback function
Hello, I'm trying to implement MessageFilter in a class. I'm using ROS1 Melodic on Ubuntu 18.04. The code compiles without any errors but when I play my bag file, the callback function aren't called. I checked some other questions but I couldn't find a solution. Here is my code :
Main:
#include <ros/ros.h>
#include "object_localization/CloudPainter.hpp"
int main(int argc, char** argv)
{
ros::init(argc, argv, "cloud_painter");
ros::NodeHandle node_handle("~");
object_localization::CloudPainter cloud_painter(node_handle);
ros::spin();
return 0;
}
CloudPainter.hpp:
#pragma once
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
typedef message_filters::Subscriber<sensor_msgs::PointCloud2> PointCloudSubscriber;
typedef message_filters::Subscriber<sensor_msgs::Image> ImageSubscriber;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,
sensor_msgs::Image>
MySyncPolicy;
namespace object_localization
{
class CloudPainter
{
public:
CloudPainter(ros::NodeHandle& t_node_handle);
private:
bool readParameters();
void messageFilterCallback(const sensor_msgs::PointCloud2ConstPtr& t_point_cloud,
const sensor_msgs::ImageConstPtr& t_image);
ros::NodeHandle& m_node_handle;
std::string m_lidar_topic;
std::string m_camera_topic;
PointCloudSubscriber m_point_cloud_subscriber;
ImageSubscriber m_image_subscriber;
message_filters::Synchronizer<MySyncPolicy> m_synchronizer;
};
} // namespace object_localization
CloudPainter.cpp:
#include "object_localization/CloudPainter.hpp"
namespace object_localization
{
CloudPainter::CloudPainter(ros::NodeHandle& t_node_handle)
: m_node_handle(t_node_handle)
, m_synchronizer(MySyncPolicy(10), m_point_cloud_subscriber, m_image_subscriber)
{
if (!readParameters())
{
ROS_ERROR("Could not read parameters.");
ros::requestShutdown();
}
ROS_INFO("Lidar topic: : %s", m_lidar_topic.c_str());
ROS_INFO("Camera topic: : %s", m_camera_topic.c_str());
m_point_cloud_subscriber.subscribe(m_node_handle, m_lidar_topic, 1);
m_image_subscriber.subscribe(m_node_handle, m_camera_topic, 1);
m_synchronizer.registerCallback(
boost::bind(&CloudPainter::messageFilterCallback, this, _1, _2));
ROS_INFO("Successfully launched node.");
}
bool CloudPainter::readParameters()
{
if (!m_node_handle.getParam("lidar_topic", m_lidar_topic))
{
return false;
}
if (!m_node_handle.getParam("camera_topic", m_camera_topic))
{
return false;
}
return true;
}
void CloudPainter::messageFilterCallback(const sensor_msgs::PointCloud2ConstPtr& t_point_cloud,
const sensor_msgs::ImageConstPtr& t_image)
{
ROS_INFO("Callback.");
}
} // namespace object_localization
Output:
[ INFO] [1625585829.773481527]: Lidar topic: : velodyne_points
[ INFO] [1625585829.774161339]: Camera topic: : pylon_camera_node/image_rect
[ INFO] [1625585829.777213271]: Successfully launched node.