From 08833c61848ffeaa4b7a5af463825d61f3b5e6d6 Mon Sep 17 00:00:00 2001
From: Berkay Karaman <berkay@leodrive.ai>
Date: Tue, 14 May 2024 15:48:18 +0300
Subject: [PATCH] fix: container die problem

Signed-off-by: Berkay Karaman <berkay@leodrive.ai>
---
 tools/reaction_analyzer/include/reaction_analyzer_node.hpp | 1 +
 tools/reaction_analyzer/include/topic_publisher.hpp        | 4 +++-
 tools/reaction_analyzer/src/reaction_analyzer_node.cpp     | 5 ++++-
 tools/reaction_analyzer/src/topic_publisher.cpp            | 6 +++++-
 4 files changed, 13 insertions(+), 3 deletions(-)

diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp
index 631233cef8a7d..cce073a7f3367 100644
--- a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp
+++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp
@@ -100,6 +100,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
   std::optional<rclcpp::Time> test_environment_init_time_;
   std::optional<rclcpp::Time> spawn_cmd_time_;
   std::atomic<bool> spawn_object_cmd_{false};
+  std::atomic<bool> ego_initialized_{false};
   bool is_initialization_requested{false};
   bool is_route_set_{false};
   size_t test_iteration_count_{0};
diff --git a/tools/reaction_analyzer/include/topic_publisher.hpp b/tools/reaction_analyzer/include/topic_publisher.hpp
index e2e9ea072970e..c6d3a90650884 100644
--- a/tools/reaction_analyzer/include/topic_publisher.hpp
+++ b/tools/reaction_analyzer/include/topic_publisher.hpp
@@ -179,7 +179,7 @@ class TopicPublisher
 {
 public:
   explicit TopicPublisher(
-    rclcpp::Node * node, std::atomic<bool> & spawn_object_cmd,
+    rclcpp::Node * node, std::atomic<bool> & spawn_object_cmd, std::atomic<bool> & ego_initialized,
     std::optional<rclcpp::Time> & spawn_cmd_time, const RunningMode & node_running_mode,
     const EntityParams & entity_params);
 
@@ -192,6 +192,8 @@ class TopicPublisher
   rclcpp::Node * node_;
   RunningMode node_running_mode_;
   std::atomic<bool> & spawn_object_cmd_;
+  std::atomic<bool> & ego_initialized_;  // used for planning_control mode because if pointcloud is
+                                         // published before ego is initialized, it causes crash
   EntityParams entity_params_;
   std::optional<rclcpp::Time> & spawn_cmd_time_;  // Set by a publisher function when the
                                                   // spawn_object_cmd_ is true
diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp
index e7d01ecef6842..2e47c19ae0ab2 100644
--- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp
+++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp
@@ -133,7 +133,8 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions node_options)
   }
 
   topic_publisher_ptr_ = std::make_unique<topic_publisher::TopicPublisher>(
-    this, spawn_object_cmd_, spawn_cmd_time_, node_running_mode_, node_params_.entity_params);
+    this, spawn_object_cmd_, ego_initialized_, spawn_cmd_time_, node_running_mode_,
+    node_params_.entity_params);
 
   // initialize the odometry before init the subscriber
   odometry_ptr_ = std::make_shared<Odometry>();
@@ -299,6 +300,7 @@ void ReactionAnalyzerNode::init_test_env(
         call_operation_mode_service_without_response();
       }
     }
+    ego_initialized_ = true;
 
     const bool is_ready =
       (is_initialization_requested && is_route_set_ &&
@@ -419,6 +421,7 @@ void ReactionAnalyzerNode::reset()
   std::lock_guard<std::mutex> lock(mutex_);
   spawn_cmd_time_ = std::nullopt;
   subscriber_ptr_->reset();
+  ego_initialized_ = false;
   RCLCPP_INFO(this->get_logger(), "Test - %zu is done, resetting..", test_iteration_count_);
 }
 }  // namespace reaction_analyzer
diff --git a/tools/reaction_analyzer/src/topic_publisher.cpp b/tools/reaction_analyzer/src/topic_publisher.cpp
index 9816f1970debd..8a66bf9e33911 100644
--- a/tools/reaction_analyzer/src/topic_publisher.cpp
+++ b/tools/reaction_analyzer/src/topic_publisher.cpp
@@ -21,12 +21,13 @@ namespace reaction_analyzer::topic_publisher
 {
 
 TopicPublisher::TopicPublisher(
-  rclcpp::Node * node, std::atomic<bool> & spawn_object_cmd,
+  rclcpp::Node * node, std::atomic<bool> & spawn_object_cmd, std::atomic<bool> & ego_initialized,
   std::optional<rclcpp::Time> & spawn_cmd_time, const RunningMode & node_running_mode,
   const EntityParams & entity_params)
 : node_(node),
   node_running_mode_(node_running_mode),
   spawn_object_cmd_(spawn_object_cmd),
+  ego_initialized_(ego_initialized),
   entity_params_(entity_params),
   spawn_cmd_time_(spawn_cmd_time)
 {
@@ -192,6 +193,9 @@ void TopicPublisher::generic_message_publisher(const std::string & topic_name)
 
 void TopicPublisher::dummy_perception_publisher()
 {
+  if (!ego_initialized_) {
+    return;  // do not publish anything if ego is not initialized
+  }
   if (!spawn_object_cmd_) {
     // do not spawn it, send empty pointcloud
     pcl::PointCloud<pcl::PointXYZ> pcl_empty;