plot first_last collision itr

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_velocity_intersection_module/scripts/ttc.py

@@ -64,8 +64,12 @@ def __init__(self, data: list[float]):
         self.collision_start_dist = data[10]
         self.collision_end_time = data[11]
         self.collision_end_dist = data[12]
-        self.pred_x = data[13:53:2]
-        self.pred_y = data[14:53:2]
+        self.first_collision_x = data[13]
+        self.first_collision_y = data[14]
+        self.last_collision_x = data[15]
+        self.last_collision_y = data[16]
+        self.pred_x = data[17:58:2]
+        self.pred_y = data[18:58:2]
 
 
 class TTCVisualizer(Node):
@@ -196,6 +200,12 @@ def plot_world(self):
             Rth = np.array([[math.cos(th), -math.sin(th)], [math.sin(th), math.cos(th)]])
             bbox_rot = Rth @ bbox
             self.world_ax.fill(bbox_rot[0, :] + x, bbox_rot[1, :] + y, color, alpha=0.5)
+            self.world_ax.plot(
+                [npc.first_collision_x, npc.last_collision_x],
+                [npc.first_collision_y, npc.last_collision_y],
+                c=color,
+                linewidth=3.0,
+            )
             if npc.dangerous:
                 self.world_ax.plot(npc.pred_x, npc.pred_y, c=color, linewidth=1.5)
             else:
@@ -255,7 +265,7 @@ def on_object_ttc(self, msg):
     parser.add_argument(
         "--range",
         type=float,
-        default=100,
+        default=60,
         help="detect range for drawing",
     )
     parser.add_argument("-s", "--save", action="store_true", help="flag to save gif")

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1572,8 +1572,9 @@ bool IntersectionModule::checkCollision(
   object_ttc_time_array.layout.dim.at(1).label =
     "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, "
     "start_time, start_dist, "
-    "end_time, end_dist, prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]";
-  object_ttc_time_array.layout.dim.at(1).size = 53;
+    "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, last_collision_y, "
+    "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]";
+  object_ttc_time_array.layout.dim.at(1).size = 57;
   bool collision_detected = false;
   for (const auto & target_object : target_objects->all_attention_objects) {
     const auto & object = target_object.object;
@@ -1682,7 +1683,8 @@ bool IntersectionModule::checkCollision(
          object.kinematics.initial_twist_with_covariance.twist.linear.x,
          1.0 * static_cast<int>(collision_detected), ref_object_enter_time, ref_object_exit_time,
          start_time_distance_itr->first, start_time_distance_itr->second,
-         end_time_distance_itr->first, end_time_distance_itr->second});
+         end_time_distance_itr->first, end_time_distance_itr->second, first_itr->position.x,
+         first_itr->position.y, last_itr->position.x, last_itr->position.y});
       for (unsigned i = 0; i < 20; i++) {
         const auto & pos =
           predicted_path.path.at(std::min<size_t>(i, predicted_path.path.size() - 1)).position;