diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 3fc64736731f5..26be2af8590af 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1454,7 +1454,7 @@ bool NormalLaneChange::getLaneChangePaths( RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); for (const auto & lateral_acc : sample_lat_acc) { - const auto debug_print = [&](const auto & s) { + const auto debug_print_lat = [&](const auto & s) { RCLCPP_DEBUG_STREAM( logger_, " - " << s << " : prep_time = " << prepare_duration << ", lon_acc = " << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc); @@ -1476,7 +1476,7 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, current_velocity, terminal_lane_changing_velocity); if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - debug_print("Reject: length of lane changing path is longer than length to goal!!"); + debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); continue; } @@ -1495,7 +1495,7 @@ bool NormalLaneChange::getLaneChangePaths( s_start + lane_changing_length + finish_judge_buffer + backward_buffer + next_lane_change_buffer > s_goal) { - debug_print("Reject: length of lane changing path is longer than length to goal!!"); + debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); continue; } } @@ -1505,7 +1505,7 @@ bool NormalLaneChange::getLaneChangePaths( initial_lane_changing_velocity, next_lane_change_buffer); if (target_segment.points.empty()) { - debug_print("Reject: target segment is empty!! something wrong..."); + debug_print_lat("Reject: target segment is empty!! something wrong..."); continue; } @@ -1536,7 +1536,7 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; if (!is_valid_start_point) { - debug_print( + debug_print_lat( "Reject: lane changing points are not inside of the target preferred lanes or its " "neighbors"); continue; @@ -1550,7 +1550,7 @@ bool NormalLaneChange::getLaneChangePaths( next_lane_change_buffer); if (target_lane_reference_path.points.empty()) { - debug_print("Reject: target_lane_reference_path is empty!!"); + debug_print_lat("Reject: target_lane_reference_path is empty!!"); continue; } @@ -1562,12 +1562,12 @@ bool NormalLaneChange::getLaneChangePaths( sorted_lane_ids); if (!candidate_path) { - debug_print("Reject: failed to generate candidate path!!"); + debug_print_lat("Reject: failed to generate candidate path!!"); continue; } if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - debug_print("Reject: invalid candidate path!!"); + debug_print_lat("Reject: invalid candidate path!!"); continue; } @@ -1575,7 +1575,7 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_parameters_->regulate_on_crosswalk && !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) { if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - debug_print("Reject: including crosswalk!!"); + debug_print_lat("Reject: including crosswalk!!"); continue; } RCLCPP_INFO_THROTTLE( @@ -1586,7 +1586,7 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_parameters_->regulate_on_intersection && !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) { if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - debug_print("Reject: including intersection!!"); + debug_print_lat("Reject: including intersection!!"); continue; } RCLCPP_WARN_STREAM( @@ -1596,14 +1596,14 @@ bool NormalLaneChange::getLaneChangePaths( if ( lane_change_parameters_->regulate_on_traffic_light && !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes)) { - debug_print("Reject: regulate on traffic light!!"); + debug_print_lat("Reject: regulate on traffic light!!"); continue; } if (utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( lane_change_info.current_lanes, candidate_path.value().path, planner_data_, lane_change_info.length.sum())) { - debug_print("Ego is stopping near traffic light. Do not allow lane change"); + debug_print_lat("Ego is stopping near traffic light. Do not allow lane change"); continue; } candidate_paths->push_back(*candidate_path); @@ -1613,14 +1613,14 @@ bool NormalLaneChange::getLaneChangePaths( route_handler, *candidate_path, filtered_objects.target_lane, lane_change_buffer, is_goal_in_route, *lane_change_parameters_, lane_change_debug_.collision_check_objects)) { - debug_print( + debug_print_lat( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); return false; } if (!check_safety) { - debug_print("ACCEPT!!!: it is valid (and safety check is skipped)."); + debug_print_lat("ACCEPT!!!: it is valid (and safety check is skipped)."); return false; } @@ -1628,11 +1628,11 @@ bool NormalLaneChange::getLaneChangePaths( *candidate_path, target_objects, rss_params, lane_change_debug_.collision_check_objects); if (is_safe) { - debug_print("ACCEPT!!!: it is valid and safe!"); + debug_print_lat("ACCEPT!!!: it is valid and safe!"); return true; } - debug_print("Reject: sampled path is not safe."); + debug_print_lat("Reject: sampled path is not safe."); } } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index d989a1e6d301d..9568f803773c4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -888,7 +888,7 @@ bool isParkedObject( const auto most_left_lanelet_candidates = route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute sub_type = + const lanelet::Attribute most_left_sub_type = most_left_lanelet.attribute(lanelet::AttributeName::Subtype); for (const auto & ll : most_left_lanelet_candidates) { @@ -898,7 +898,7 @@ bool isParkedObject( } } bound = most_left_lanelet.leftBound2d().basicLineString(); - if (sub_type.value() != "road_shoulder") { + if (most_left_sub_type.value() != "road_shoulder") { center_to_bound_buffer = object_check_min_road_shoulder_width; } } else { @@ -909,7 +909,7 @@ bool isParkedObject( most_right_road_lanelet.rightBound()); lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute sub_type = + const lanelet::Attribute most_right_sub_type = most_right_lanelet.attribute(lanelet::AttributeName::Subtype); for (const auto & ll : most_right_lanelet_candidates) { @@ -919,7 +919,7 @@ bool isParkedObject( } } bound = most_right_lanelet.rightBound2d().basicLineString(); - if (sub_type.value() != "road_shoulder") { + if (most_right_sub_type.value() != "road_shoulder") { center_to_bound_buffer = object_check_min_road_shoulder_width; } }