update

autowarefoundation · Dec 28, 2023 · 3ae5d0d · 3ae5d0d
1 parent c4f419d
commit 3ae5d0d
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 8 deletions.
diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml
@@ -1,17 +1,17 @@
 /**:
   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
         message_type: autoware_auto_planning_msgs::msg::Trajectory
       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
@@ -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;