no data message_filter

asked 2021-05-30 17:30:30 -0500

dinesh gravatar image

updated 2021-05-30 17:39:32 -0500

include "rclcpp/rclcpp.hpp"

 #include "sensor_msgs/msg/image.hpp"
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
 #include <message_filters/sync_policies/approximate_time.h>
 #include <message_filters/time_synchronizer.h>
 #include "boost/bind/bind.hpp"

    #include <message_filters/sync_policies/exact_time.h>

using std::placeholders::_1;
using std::placeholders::_2;

void callback(const sensor_msgs::msg::Image::ConstPtr& left, const sensor_msgs::msg::Image::ConstPtr& right) {
    std::cout<<"hell ya"<<std::endl;
    std::cout<<left->header.stamp.sec<<"\t"<<right->header.stamp.sec<<std::endl;
    }

    int main(int argc, char** argv){
        rclcpp::init(argc, argv);
        auto node = rclcpp::Node::make_shared("my_node");

        message_filters::Subscriber<sensor_msgs::msg::Image> image_1(node, "/image");
        message_filters::Subscriber<sensor_msgs::msg::Image> image_2(node, "/image");

    message_filters::TimeSynchronizer<sensor_msgs::msg::Image, sensor_msgs::msg::Image> sync(image_1, image_2, 10);
    sync.registerCallback(boost::bind(&callback, _1, _2));

    // typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image> MySyncPolicy;
    // message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_1, image_2);
    // sync.registerCallback(boost::bind(&callback, boost::placeholders::_1, boost::placeholders::_2));

    rclcpp::spin(node);
    return 0;
}

Above is the ros2 subscriber node which i'm trying to use for subscribing to two ros2 topics. Here i have made the both topic same for testing purpose. Here this program buids succesfully. but when i run this program i can't see the output from the callback function as expected.

I'm using ros2 foxy, ubuntu 20.

I also tried this code but still same problem no data:

#include "rclcpp/rclcpp.hpp"
#include <sensor_msgs/msg/point_cloud2.hpp>
#include "sensor_msgs/msg/image.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 MultiSubscriber : public rclcpp::Node
{
  public:

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

    MultiSubscriber(const std::string& name) 
    : Node(name)
    {
      image_sub_.subscribe(this, "/camera/color/image_raw");
      cloud_sub_.subscribe(this, "/camera/depth/color/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(&MultiSubscriber::disparityCb,this);
    }
    private:
    void disparityCb(const sensor_msgs::msg::Image::ConstPtr cloud, const sensor_msgs::msg::PointCloud2::ConstPtr image){
        std::cout<<cloud->header.stamp.sec<<"\t"<<image->header.stamp.sec<<std::endl;
    }
};

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