zkytony · April 9, 2023 16:57 · Apr 9, 2023 · Apr 9, 2023
diff --git a/message_filter_minimum.cpp b/message_filter_minimum.cpp
@@ -1,4 +1,5 @@
 // Using ROS Humble
+// /author: Kaiyu Zheng
 #include <chrono>
 #include <rclcpp/rclcpp.hpp>
 #include <message_filters/subscriber.h>

diff --git a/message_filter_minimum.cpp b/message_filter_minimum.cpp
@@ -0,0 +1,117 @@
+// Using ROS Humble
+#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();
+}
No results found