no data message_filter
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;
}
Asked by dinesh on 2021-05-30 17:30:30 UTC
Answers
It is worth to make sure the qos from the publisher & subscriber is correct and matched with each other.
Asked by BCJ on 2022-12-29 00:23:31 UTC
Comments
Hey dinesh, i'm having the same problem as you had, did you solve it ? I'm trying to merge two pointclouds, one i subscribe and one i create from an image but no data published on the topics, i would be very grateful if you share with me your solution.
Asked by marcelomm103 on 2022-08-02 15:23:18 UTC