add sync pointcloud publish

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

Author: brkay54

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -294,6 +294,10 @@ class ReactionAnalyzerNode : public rclcpp::Node
   // TEMP
   void genericMessagePublisherTimer(const std::string & topic_name);
   std::unordered_map<std::string, PublisherVariablesVariant> topic_publisher_map_;
+  std::vector<PublisherVariables<PointCloud2>> sync_pointcloud_publishers_;
+  bool publish_pointcloud_sync_{true};
+  void publishAllPointClouds();
+  rclcpp::TimerBase::SharedPtr sync_pointcloud_publish_timer_;
 
   // Initialization Variables
   geometry_msgs::msg::Pose entity_pose_;

tools/reaction_analyzer/src/reaction_analyzer_node.cpp

@@ -612,7 +612,6 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
           }
         }
       }
-      std::cout << "2" << std::endl;
 
       // set frequencies of the publishers
       // After collecting all timestamps for each topic
@@ -953,15 +952,41 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
         },
         variant);
 
-      // Ensure conjugate_pointclouds or the relevant struct is accessible and appropriately scoped
+      // Conditionally create the timer based on the message type and publish_pointcloud_sync_
       std::visit(
         [&](auto & var) {
-          var.timer = rclcpp::create_timer(this, get_clock(), period_ns, [this, topic_name]() {
-            this->genericMessagePublisherTimer(topic_name);
-          });
+          using MessageType = typename decltype(var.empty_area_message)::element_type;
+
+          if constexpr (std::is_same_v<MessageType, sensor_msgs::msg::PointCloud2>) {
+            if (!publish_pointcloud_sync_) {
+              // do not create timer if the pointcloud messages will be published synchronously
+              var.timer = rclcpp::create_timer(this, get_clock(), period_ns, [this, topic_name]() {
+                this->genericMessagePublisherTimer(topic_name);
+              });
+            } else {
+              // It is Pointcloud2 message and it will be published synchronously
+              sync_pointcloud_publishers_.push_back(var);
+            }
+          } else {
+            var.timer = rclcpp::create_timer(this, get_clock(), period_ns, [this, topic_name]() {
+              this->genericMessagePublisherTimer(topic_name);
+            });
+          }
         },
         variant);
     }
+
+    if (publish_pointcloud_sync_) {
+      if (sync_pointcloud_publishers_.empty()) {
+        RCLCPP_ERROR(get_logger(), "No pointcloud publishers found for synchronous publishing");
+        return;
+      }
+      const auto period_ns =
+        std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(0.1));
+      sync_pointcloud_publish_timer_ = rclcpp::create_timer(
+        this, get_clock(), period_ns,
+        std::bind(&ReactionAnalyzerNode::publishAllPointClouds, this));
+    }
   }
 
   const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
@@ -970,6 +995,26 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
     this, get_clock(), period_ns, std::bind(&ReactionAnalyzerNode::onTimer, this));
 }
 
+void ReactionAnalyzerNode::publishAllPointClouds()
+{
+  mutex_.lock();
+  const auto is_spawned = spawn_cmd_time_;
+  mutex_.unlock();
+  const auto current_time = this->now();
+
+  for (auto & publisher_var : sync_pointcloud_publishers_) {
+    std::unique_ptr<sensor_msgs::msg::PointCloud2> msg_cloud_pub =
+      std::make_unique<sensor_msgs::msg::PointCloud2>();
+    if (!is_spawned) {
+      *msg_cloud_pub = *publisher_var.empty_area_message;
+    } else {
+      *msg_cloud_pub = *publisher_var.object_spawned_message;
+    }
+    msg_cloud_pub->header.stamp = current_time;
+    publisher_var.publisher->publish(std::move(msg_cloud_pub));
+  }
+}
+
 void ReactionAnalyzerNode::genericMessagePublisherTimer(const std::string & topic_name)
 {
   PublisherVarAccessor accessor;