feat(avoidance): output objects of interest (autowarefoundation#5929)

Signed-off-by: satoshi-ota <[email protected]>
StepTurtle · Dec 21, 2023 · f63ad33 · f63ad33
1 parent 18f124e
commit f63ad33
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -1208,6 +1208,12 @@ void AvoidanceModule::updateData()
   // update rtc status.
   updateRTCData();
 
+  // update interest objects data
+  for (const auto & [uuid, data] : debug_data_.collision_check) {
+    const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;
+    setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color);
+  }
+
   safe_ = avoid_data_.safe;
 }