Minimum C++ example of using ROS2 message_filter with latching

message_filter_minimum.cpp

// Using ROS Humble
// /author: Kaiyu Zheng
#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <std_msgs/msg/header.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>

using geometry_msgs::msg::PointStamped;
using namespace message_filters;
using namespace message_filters::sync_policies;
using namespace std::chrono_literals;

typedef ApproximateTime<PointStamped, PointStamped> PointSyncPolicy;
typedef Synchronizer<PointSyncPolicy> PointSync;

static const rmw_qos_profile_t rmw_qos_profile_latch =
{
  RMW_QOS_POLICY_HISTORY_KEEP_LAST,
  10,
  RMW_QOS_POLICY_RELIABILITY_RELIABLE,
  RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
  RMW_QOS_DEADLINE_DEFAULT,
  RMW_QOS_LIFESPAN_DEFAULT,
  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
  false
};

class NodeFoo : public rclcpp::Node {
public:
    NodeFoo()
        : Node("foo") {
        auto point_foo = PointStamped();
        point_foo.header.stamp = rclcpp::Clock().now();
        point_foo.point.x = 1.0;
        point_foo.point.y = 0.0;
        point_foo.point.z = 0.0;
        auto qos_latch = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_latch),
                                     rmw_qos_profile_latch);
        pub_ = this->create_publisher<PointStamped>("foo_topic", qos_latch);
        pub_->publish(point_foo);
        RCLCPP_INFO(this->get_logger(), "foo published!");
    }
private:
    rclcpp::Publisher<PointStamped>::SharedPtr pub_;
};


class NodeBar : public rclcpp::Node {
public:
    NodeBar()
        : Node("bar") {
        point_bar_ = PointStamped();
        point_bar_.header.stamp = rclcpp::Clock().now();
        point_bar_.point.x = 0.0;
        point_bar_.point.y = 0.0;
        point_bar_.point.z = 1.0;
        auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default),
                               rmw_qos_profile_default);
        pub_ = this->create_publisher<PointStamped>("bar_topic", qos);
        timer_ = this->create_wall_timer(500ms, std::bind(&NodeBar::timer_callback, this));
    }

    void timer_callback() {
        point_bar_.header.stamp = rclcpp::Clock().now();
        pub_->publish(point_bar_);
        RCLCPP_INFO(this->get_logger(), "bar published!");
    }
private:
    rclcpp::Publisher<PointStamped>::SharedPtr pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    PointStamped point_bar_;
};


class Syncer : public rclcpp::Node {
public:
    Syncer()
        : Node("syncer") {
        sub_foo_ = std::make_shared<message_filters::Subscriber<PointStamped>>(this, "foo_topic", rmw_qos_profile_latch);
        sub_bar_ = std::make_shared<message_filters::Subscriber<PointStamped>>(this, "bar_topic", rmw_qos_profile_default);
        sync_ = std::make_shared<PointSync>(PointSyncPolicy(10),
                                            *sub_foo_, *sub_bar_);
        sync_->registerCallback(&Syncer::cb, this);
    }

    void cb(const PointStamped::SharedPtr msg_foo,
            const PointStamped::SharedPtr msg_bar) {
        RCLCPP_INFO(this->get_logger(), "Received messages!!");
        RCLCPP_INFO(this->get_logger(), "from Foo: %.2f, %.2f, %.2f",
                    msg_foo->point.x, msg_foo->point.y, msg_foo->point.z);
        RCLCPP_INFO(this->get_logger(), "from Bar: %.2f, %.2f, %.2f",
                    msg_bar->point.x, msg_bar->point.y, msg_bar->point.z);
    }
private:
    std::shared_ptr<message_filters::Subscriber<PointStamped>> sub_foo_;
    std::shared_ptr<message_filters::Subscriber<PointStamped>> sub_bar_;
    std::shared_ptr<PointSync> sync_;
};


int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);

    NodeFoo::SharedPtr node_foo = std::make_shared<NodeFoo>();
    NodeBar::SharedPtr node_bar = std::make_shared<NodeBar>();
    Syncer::SharedPtr syncer = std::make_shared<Syncer>();

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node_foo);
    executor.add_node(node_bar);
    executor.add_node(syncer);
    executor.spin();
    rclcpp::shutdown();
}