From 3ae5d0d60ef9ae55079476c41fabbc0d7333c867 Mon Sep 17 00:00:00 2001
From: Berkay Karaman <brkay54@gmail.com>
Date: Thu, 28 Dec 2023 13:21:17 +0300
Subject: [PATCH] update

---
 .../param/reaction_analyzer.param.yaml                 | 10 +++++-----
 tools/reaction_analyzer/src/reaction_analyzer_node.cpp | 10 +++++++---
 2 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml
index 497a2dbe3adfb..b95a22fa6f821 100644
--- a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml
+++ b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml
@@ -1,8 +1,8 @@
 /**:
   ros__parameters:
     chain:
-      obstacle_stop_planner:
-        topic_name: /planning/scenario_planning/lane_driving/trajectory
+      planning_validator:
+        topic_name: /planning/scenario_planning/trajectory
         message_type: autoware_auto_planning_msgs::msg::Trajectory
       scenario_selector:
         topic_name: /planning/scenario_planning/scenario_selector/trajectory
@@ -10,8 +10,8 @@
       motion_velocity_smoother:
         topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory
         message_type: autoware_auto_planning_msgs::msg::Trajectory
-      planning_validator:
-        topic_name: /planning/scenario_planning/trajectory
+      obstacle_stop_planner:
+        topic_name: /planning/scenario_planning/lane_driving/trajectory
         message_type: autoware_auto_planning_msgs::msg::Trajectory
       trajectory_follower:
         topic_name: /control/trajectory_follower/control_cmd
@@ -21,7 +21,7 @@
         message_type: autoware_auto_control_msgs::msg::AckermannControlCommand
     observer:
       timer_period: 0.033 # s
-      spawn_distance_threshold: 15.0
+      spawn_distance_threshold: 25.0
       control_cmd_buffer_time_interval: 1.0
       min_number_descending_order_control_cmd: 8
       min_jerk_for_brake_cmd: 0.3
diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp
index cd2ada570cfb2..21e7f902850b8 100644
--- a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp
+++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp
@@ -295,8 +295,8 @@ void ReactionAnalyzerNode::onTimer()
   }
   if (spawn_cmd_time && !all_topics_reacted_) {
     all_topics_reacted_ = true;
+    const auto current_time = this->now();
     for (const auto & [key, variant] : messageBuffers) {
-      const auto current_time = this->now();
       if (auto * message = std::get_if<ControlBuffer>(&variant)) {
         if (!message->second) {
           all_topics_reacted_ = false;
@@ -321,9 +321,13 @@ void ReactionAnalyzerNode::onTimer()
             message->first.points, nearest_idx, message->first.points.size() - 1);
 
           if (zero_vel_idx) {
+            const auto dist = tier4_autoware_utils::calcDistance3d(
+              message->first.points.at(nearest_idx).pose.position,
+              message->first.points.at(zero_vel_idx.value()).pose.position);
             std::cout << key << " found: nearest_idx: " << nearest_idx
-                      << " zero_vel_idx: " << zero_vel_idx.value()
-                      << " size: " << message->first.points.size() << " stamp: " << current_time.nanoseconds() << std::endl;
+                      << " zero_vel_idx: " << zero_vel_idx.value() << " dist: " << dist
+                      << " size: " << message->first.points.size()
+                      << " stamp: " << current_time.nanoseconds() << std::endl;
             mutex_.lock();
             auto * tmp = std::get_if<PlanningBuffer>(&messageBuffers_[key]);
             if (tmp) tmp->second = message->first;