feat(goal_planner): output objects of interest (autowarefoundation#6077)

Signed-off-by: kosuke55 <[email protected]>
Interplai · May 28, 2024 · 6550b85 · 6550b85
1 parent 1eabc41
commit 6550b85
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 8 deletions.
diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -1957,6 +1957,13 @@ void GoalPlannerModule::setDebugData()
     }
     add(showPredictedPath(goal_planner_data_.collision_check, "ego_predicted_path"));
     add(showPolygon(goal_planner_data_.collision_check, "ego_and_target_polygon_relation"));
+
+    // set objects of interest
+    for (const auto & [uuid, data] : goal_planner_data_.collision_check) {
+      const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;
+      setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color);
+    }
+
     utils::parking_departure::initializeCollisionCheckDebugMap(goal_planner_data_.collision_check);
 
     // visualize safety status maker

diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp
@@ -453,16 +453,15 @@ bool checkSafetyWithIntegralPredictedPolygon(
     for (const auto & path : object.predicted_paths) {
       for (const auto & pose_with_poly : path.path) {
         if (boost::geometry::overlaps(ego_integral_polygon, pose_with_poly.poly)) {
-          {
-            debug_pair.second.ego_predicted_path = ego_predicted_path;  // raw path
-            debug_pair.second.obj_predicted_path = path.path;           // raw path
-            debug_pair.second.extended_obj_polygon = pose_with_poly.poly;
-            debug_pair.second.extended_ego_polygon =
-              ego_integral_polygon;  // time filtered extended polygon
-            updateCollisionCheckDebugMap(debug_map, debug_pair, false);
-          }
+          debug_pair.second.ego_predicted_path = ego_predicted_path;  // raw path
+          debug_pair.second.obj_predicted_path = path.path;           // raw path
+          debug_pair.second.extended_obj_polygon = pose_with_poly.poly;
+          debug_pair.second.extended_ego_polygon =
+            ego_integral_polygon;  // time filtered extended polygon
+          updateCollisionCheckDebugMap(debug_map, debug_pair, /*is_safe=*/false);
           return false;
         }
+        updateCollisionCheckDebugMap(debug_map, debug_pair, /*is_safe=*/true);
       }
     }
   }