feat(start_planner): output objects of interest (autowarefoundation#6078

)

* feat(start_planner): output objects of interest

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* style(pre-commit): autofix

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -254,7 +254,7 @@ class StartPlannerModule : public SceneModuleInterface
   void onFreespacePlannerTimer();
   bool planFreespacePath();
 
-  void setDebugData() const;
+  void setDebugData();
   void logPullOutStatus(rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const;
 };
 }  // namespace behavior_path_planner

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -1325,7 +1325,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons
   }
 }
 
-void StartPlannerModule::setDebugData() const
+void StartPlannerModule::setDebugData()
 {
   using marker_utils::addFootprintMarker;
   using marker_utils::createFootprintMarkerArray;
@@ -1432,6 +1432,13 @@ void StartPlannerModule::setDebugData() const
     add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info"));
     add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path"));
     add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation"));
+
+    // set objects of interest
+    for (const auto & [uuid, data] : start_planner_data_.collision_check) {
+      const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;
+      setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color);
+    }
+
     initializeCollisionCheckDebugMap(start_planner_data_.collision_check);
   }