From a22ab3d21534a8c7296eaf93d84cc64ead1781c7 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 12 Oct 2023 21:29:06 +0900 Subject: [PATCH] fix(lane_change): add visualization for passParkedObject Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner/utils/lane_change/utils.hpp | 4 +++- .../src/scene_module/lane_change/normal.cpp | 2 +- .../behavior_path_planner/src/utils/lane_change/utils.cpp | 6 +++++- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index b2592bc1b899b..cd8f293b2e610 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -52,6 +52,7 @@ using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using path_safety_checker::CollisionCheckDebugMap; using route_handler::Direction; using tier4_autoware_utils::Polygon2d; @@ -170,7 +171,8 @@ bool isParkedObject( bool passParkedObject( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters); + const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, + CollisionCheckDebugMap & object_debug); boost::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 913ddcacf08bc..b66b097cfc467 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1224,7 +1224,7 @@ bool NormalLaneChange::getLaneChangePaths( if (utils::lane_change::passParkedObject( route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_)) { + is_goal_in_route, *lane_change_parameters_, object_debug_)) { return false; } candidate_paths->push_back(*candidate_path); diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 038cf9488345f..17adb54ad6ff5 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -930,7 +930,8 @@ bool isParkedObject( bool passParkedObject( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters) + const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, + CollisionCheckDebugMap & object_debug) { const auto & object_check_min_road_shoulder_width = lane_change_parameters.object_check_min_road_shoulder_width; @@ -953,6 +954,7 @@ bool passParkedObject( } const auto & leading_obj = objects.at(*leading_obj_idx); + auto debug = marker_utils::createObjectDebug(leading_obj); const auto leading_obj_poly = tier4_autoware_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { @@ -976,6 +978,8 @@ bool passParkedObject( // If there are still enough length after the target object, we delay the lane change if (min_dist_to_end_of_current_lane > minimum_lane_change_length) { + debug.second.unsafe_reason = "delay lane change"; + marker_utils::updateCollisionCheckDebugMap(object_debug, debug, false); return true; }