Question

I gonna synchronize an image by its camera_info. The code is attached, after finishing constructor, it just stays without going to the callback function. I have tried different QoS, made sure topics are published, ...

  OverheadCamNode::OverheadCamNode(rclcpp::NodeOptions options): Node("overhead_camera_node", options){


       message_filters::Subscriber<sensor_msgs::msg::Image> overhead_cam1_sub_(this, "/image", 
       rmw_qos_profile_sensor_data);
      message_filters::Subscriber<sensor_msgs::msg::CameraInfo> camera_info_sub_(this, "/camera_info", 
      rmw_qos_profile_sensor_data);

      typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image , 
      sensor_msgs::msg::CameraInfo> approximate_policy;
      message_filters::Synchronizer<approximate_policy> syncApproximate(approximate_policy(10), 
      overhead_cam1_sub_, camera_info_sub_);

      syncApproximate.registerCallback(std::bind(&OverheadCamNode::synchronized_images_callback, 
      this,std::placeholders::_1, std::placeholders::_2));
      RCLCPP_INFO(this->get_logger(), "Constructor");
            }

void OverheadCamNode::synchronized_images_callback(
                                const sensor_msgs::msg::Image::ConstSharedPtr image_1,
                                const sensor_msgs::msg::CameraInfo::ConstSharedPtr cam_info) {

              RCLCPP_INFO(this->get_logger(), "in CB");
}

Asked by hosh on 2020-12-09 04:12:52 UTC

Answer 1

Haven't compiled it yet, but something like this works for me:

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

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

class OverheadCamNode : public rclcpp::Node{

    public:
        OverheadCamNode() : Node("overhead_camera_node)
        {

            rclcpp::QoS qos(10);
            auto rmw_qos_profile = qos.get_rmw_qos_profile();

            overhead_cam1_sub_.subscribe(this, "/image", rmw_qos_profile );
            camera_info_sub_.subscribe(this, "/camera_info",rmw_qos_profile);

            sync.reset(new Sync(new Sync(MySyncPolicy(10),overhead_cam1_sub_,camera_info_sub_));
            sync->registerCallback(std::bind(&OverheadCamNode::synchronized_images_callback, 
                      this, _1, _2));

        }

    private:
        void synchronized_images_callback(
            const sensor_msgs::msg::Image::ConstSharedPtr& image_1,
            const sensor_msgs::msg::CameraInfo::ConstSharedPtr& cam_info)

        {
            RCLCPP_INFO(this->get_logger(), "in CB");
        }



    message_filters::Subscriber<sensor_msgs::msg::Image> overhead_cam1_sub_;
    message_filters::Subscriber<sensor_msgs::msg::CameraInfo> camera_info_sub_;


    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image , 
    sensor_msgs::msg::CameraInfo> MySyncPolicy;
    typedef message_filters::Synchronizer<MySyncPolicy> Sync;
    std::shared_ptr<Sync> sync;

};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<OverheadCamNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

Asked by __spam__ on 2021-07-27 01:09:02 UTC

message_filters doesn't call the callback function

Comments

Answers

Comments