Callback not being called using Message Filters

asked 2022-08-03 14:04:59 -0500

marcelomm103 gravatar image

Hi Everyone!

I'm having some issues trying to implement message filters in my code. Using this simple example everything compiles but no callback function is called when it runs. Does anybody knows why this is happening? I already checked the QoS of both topics and they match so i dont know why the callback function isn't being called. I'm running ROS2 Galactic on Ubuntu 20.04. Any help will be appreciated!

#include "rclcpp/rclcpp.hpp" 
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

//using std::placeholders::_1;

class MinimalSubscriber : public rclcpp::Node
{
public:

message_filters::Subscriber<sensor_msgs::msg::Image> image_sub_;
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloud_sub_;

MinimalSubscriber()
: Node("minimal_subscriber_left")
{

  image_sub_.subscribe(this, "/left/image_raw");
  cloud_sub_.subscribe(this, "/model/prius_hybrid/laserscan/points");

  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::PointCloud2> approximate_policy;
  message_filters::Synchronizer<approximate_policy>syncApproximate(approximate_policy(10), image_sub_, cloud_sub_);
  syncApproximate.registerCallback(&MinimalSubscriber::topic_callback, this); 

};

public:
void topic_callback(const sensor_msgs::msg::Image::SharedPtr image, const sensor_msgs::msg::PointCloud2::SharedPtr cloud2)
{ 
  std::cout<<"Hello messages are being received";
  RCLCPP_INFO(this->get_logger(), "Publishing");    
}; 
};

int main(int argc, char * argv[]) 
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalSubscriber>());
  rclcpp::shutdown();
  return 0;
}
edit retag flag offensive close merge delete

Comments

In the main each time you create a shared pointer, you reintialize the subscribers everytime. But instead you should intialise the subscribers and syncronise them only once. and call the callback in the spin. I would move the callback "syncApproximate.registerCallback(&MinimalSubscriber::topic_callback, this); " outside the constructor

sumanthrao1997 gravatar image sumanthrao1997  ( 2023-04-17 09:02:15 -0500 )edit