From 6550b85ca328d4a60b767e7d01ef55a2261a3528 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 16 Jan 2024 10:28:36 +0900 Subject: [PATCH] feat(goal_planner): output objects of interest (#6077) Signed-off-by: kosuke55 --- .../src/goal_planner_module.cpp | 7 +++++++ .../utils/path_safety_checker/safety_check.cpp | 15 +++++++-------- 2 files changed, 14 insertions(+), 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 index 7bd97b3775bad..a13efd543d61c 100644 --- 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 index bf93e71ab3591..e209e8dba36be 100644 --- 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); } } }