feat(intersection): resurrect debug ego-object ttc visualizer (autowa…

…refoundation#6829) Signed-off-by: Mamoru Sobue <[email protected]>
tier4 · Oct 23, 2024 · 3f24def · 3f24def
1 parent dbb2b2f
commit 3f24def
Show file tree

Hide file tree

Showing 4 changed files with 107 additions and 278 deletions.
diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_intersection_module/scripts/ttc.py
@@ -21,6 +21,7 @@
 from threading import Lock
 import time
 
+from PIL import Image
 import imageio
 import matplotlib
 import matplotlib.pyplot as plt
@@ -218,7 +219,19 @@ def plot_world(self):
     def cleanup(self):
         if self.args.save:
             kwargs_write = {"fps": self.args.fps, "quantizer": "nq"}
-            imageio.mimsave("./" + self.args.gif + ".gif", self.images, **kwargs_write)
+            max_size_total = 0
+            max_size = None
+            for image in self.images:
+                (w, h) = image.size
+                if w * h > max_size_total:
+                    max_size = image.size
+                    max_size_total = w * h
+            reshaped = []
+            for image in self.images:
+                reshaped.append(image.resize(max_size))
+
+            imageio.mimsave("./" + self.args.gif + ".gif", reshaped, **kwargs_write)
+            print("saved fig")
         rclpy.shutdown()
 
     def on_plot_timer(self):
@@ -241,6 +254,7 @@ def on_plot_timer(self):
             if self.args.save:
                 image = np.frombuffer(self.fig.canvas.tostring_rgb(), dtype="uint8")
                 image = image.reshape(self.fig.canvas.get_width_height()[::-1] + (3,))
+                image = Image.fromarray(image.astype(np.uint8))
                 self.images.append(image)
 
     def on_ego_ttc(self, msg):

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -222,9 +222,20 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   // passed each pass judge line for the first time, save current collision status for late
   // diagnosis
   // ==========================================================================================
+  tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array;
   updateObjectInfoManagerCollision(
     path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line,
-    safely_passed_2nd_judge_line);
+    safely_passed_2nd_judge_line, &object_ttc_time_array);
+  {
+    const auto & debug = planner_param_.debug.ttc;
+    if (
+      std::find(debug.begin(), debug.end(), lane_id_) != debug.end() ||
+      std::find(debug.begin(), debug.end(), -1) != debug.end()) {
+      ego_ttc_pub_->publish(ego_ttc_time_array);
+      object_ttc_pub_->publish(object_ttc_time_array);
+    }
+  }
+
   for (const auto & object_info : object_info_manager_.attentionObjects()) {
     if (!object_info->unsafe_info()) {
       continue;

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -693,7 +693,8 @@ class IntersectionModule : public SceneModuleInterface
   void updateObjectInfoManagerCollision(
     const intersection::PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array,
     const TrafficPrioritizedLevel & traffic_prioritized_level,
-    const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time);
+    const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time,
+    tier4_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array);
 
   void cutPredictPathWithinDuration(
     const builtin_interfaces::msg::Time & object_stamp, const double time_thr,