Template compilation error with multi-stage message_filters node

asked 2023-03-13 19:22:29 -0500

Nilaos gravatar image

Hi! I'm working with message_filters on ROS2 Galactic (EOL, I know, but we haven't upgraded to 22.04 to run Humble quite yet). I'm having template matching issues when compiling a larger version of the class below which I believe to be related to the message_filters library I'm using. From the error message I suspect it's to do with either how I've instantiated the function, or how I've declared it's callback, but I'm not quite sure how to address this issue.

namespace ImageSynch {

class SynchroniserNode : public rclcpp::Node {

public: 
    explicit SynchroniserNode(const rclcpp::NodeOptions & options);

protected:
    using Image = sensor_msgs::msg::Image;
    using CameraInfo = sensor_msgs::msg::CameraInfo;
    using ExactPolicy = message_filters::sync_policies::ExactTime<Image, CameraInfo>;
    using ApproximatePolicy = message_filters::sync_policies::ApproximateTime<
        Image, CameraInfo, 
        Image, CameraInfo>;
    using ExactSync = message_filters::Synchronizer<ExactPolicy>;
    using ApproximateSync = message_filters::Synchronizer<ApproximatePolicy>;

    image_transport::SubscriberFilter sub_l_image_, sub_r_image_;
    message_filters::Subscriber<CameraInfo> sub_l_info_, sub_r_info_;

    // Message Filters
    std::shared_ptr<ExactSync> exact_sync_l_;
    std::shared_ptr<ExactSync> exact_sync_r_;
    std::shared_ptr<ApproximateSync> approximate_sync_;

    // Publishers go here

    void synchCb(
        const Image::ConstSharedPtr &l_img_msg,
        const CameraInfo::ConstSharedPtr &l_info_msg,
        const Image::ConstSharedPtr &r_img_msg,
        const CameraInfo::ConstSharedPtr &r_info_msg);
};

SynchroniserNode::SynchroniserNode(const rclcpp::NodeOptions & options)
: rclcpp::Node("SynchroniserNode", options)
{
    using namespace std::placeholders;

    // Declare/read parameters
    int queue_size = this->declare_parameter("queue_size", 5);
    this->declare_parameter("use_system_default_qos", false);

    // Setup synch subscribers
    exact_sync_l_.reset(new ExactSync(ExactPolicy(queue_size), sub_l_image_, sub_l_info_));
    exact_sync_r_.reset(new ExactSync(ExactPolicy(queue_size), sub_r_image_, sub_r_info_));

    approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(queue_size), *exact_sync_l_, *exact_sync_r_));
    // approximate_sync_->connectInput(exact_sync_l_, exact_sync_r_);
    approximate_sync_->registerCallback(
        std::bind(&SynchroniserNode::synchCb, this, _1, _2, _3, _4) );
}

void SynchroniserNode::synchCb(
    const sensor_msgs::msg::Image::ConstSharedPtr &l_img_msg,
    const sensor_msgs::msg::CameraInfo::ConstSharedPtr &l_info_msg,
    const sensor_msgs::msg::Image::ConstSharedPtr &r_img_msg,
    const sensor_msgs::msg::CameraInfo::ConstSharedPtr &r_info_msg) 
{
    // Do Things Here
}
} // namespace ImageSynch

The errors on compilation are:

--- stderr: image_synch                               
In file included from /opt/ros/galactic/include/message_filters/synchronizer.h:47,
                 from /home/simsn1/repos/thesis/ros2_ws/src/image_synch/src/image_synch/image_synch.cpp:13:
/opt/ros/galactic/include/message_filters/signal9.h: In instantiation of ‘message_filters::Connection message_filters::Signal9<M0, M1, M2, M3, M4, M5, M6, M7, M8>::addCallback(C&) [with C = const std::function<void(const message_filters::MessageEvent<const sensor_msgs::msg::Image_<std::allocator<void> > >&)>; M0 = sensor_msgs::msg::Image_<std::allocator<void> >; M1 = sensor_msgs::msg::CameraInfo_<std::allocator<void> >; M2 = message_filters::NullType; M3 = message_filters::NullType; M4 = message_filters::NullType; M5 = message_filters::NullType; M6 = message_filters::NullType; M7 = message_filters::NullType; M8 = message_filters::NullType]’:
/opt/ros/galactic/include/message_filters/synchronizer.h:298:40:   required from ‘message_filters::Connection message_filters::Synchronizer<Policy>::registerCallback(const C&) [with C = std::function<void(const message_filters::MessageEvent<const sensor_msgs::msg::Image_<std::allocator<void> > >&)>; Policy = message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image_<std::allocator<void> >, sensor_msgs::msg::CameraInfo_<std::allocator<void> > >]’
/opt/ros/galactic/include/message_filters/synchronizer.h:278:27:   required from ‘void message_filters::Synchronizer<Policy>::connectInput(F0&, F1&, F2&, F3&, F4&, F5&, F6&, F7&, F8&) [with F0 = message_filters::Synchronizer<message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image_<std::allocator<void> >, sensor_msgs::msg::CameraInfo_<std::allocator<void> > > >; F1 = message_filters::Synchronizer<message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image_<std::allocator<void> >, sensor_msgs::msg::CameraInfo_<std::allocator<void> > > >; F2 = message_filters::NullFilter<sensor_msgs::msg::Image_<std::allocator<void> > >; F3 = message_filters::NullFilter<sensor_msgs::msg::CameraInfo_<std::allocator<void> > > ...
(more)
edit retag flag offensive close merge delete

Comments

1

I believe what you show is only a partial error message. The actual cause is not included (the compiler typically at least gives a hint, and you only include the lines which state where something is going wrong).

I'd suggest copy-pasting the complete error message verbatim.

gvdhoorn gravatar image gvdhoorn  ( 2023-03-14 03:04:40 -0500 )edit

Noted, I'll do that in future. In the meanwhile I solved this by splitting this task into two smaller python nodes and defining a custom message type to group the Image and CameraInfo from each camera together exactly; before doing an approximate sync on those 'bundle' messages.

Nilaos gravatar image Nilaos  ( 2023-06-09 00:37:11 -0500 )edit