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 70ea047f2a19f..65663607618d2 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; @@ -175,7 +176,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 cf6f2fc01773a..c8dbba21c7094 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 @@ -1130,12 +1130,12 @@ bool NormalLaneChange::getLaneChangePaths( logger_, "Stop time is over threshold. Allow lane change in intersection."); } + candidate_paths->push_back(*candidate_path); 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); if (!check_safety) { return false; 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 269eb194988e1..900b4c018e0b9 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -946,7 +946,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; @@ -969,6 +970,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()) { @@ -992,6 +994,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; }