From 7a05ffe0aeb4b5ad33c6e7e8b528b25d7a21509a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 9 Sep 2022 11:17:45 +0900 Subject: [PATCH] feat(behavior_path_planner): pull_over decrease hz (#1819) * feat(behavior_path_planner): pull_over decrease hz Signed-off-by: kosuke55 * generate drivable area at last in pull_out Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../scene_module/pull_out/pull_out_module.hpp | 1 + .../scene_module/pull_over/pull_over_path.hpp | 10 +++---- .../scene_module/pull_out/pull_out_module.cpp | 27 ++++++++++++------- .../pull_over/pull_over_module.cpp | 11 ++++---- .../src/scene_module/pull_over/util.cpp | 23 ++++++++-------- .../utils/geometric_parallel_parking.cpp | 11 -------- 6 files changed, 41 insertions(+), 42 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index fd94ef35cd4ef..cfbb63c02e073 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -52,6 +52,7 @@ struct PullOutStatus PathWithLaneId backward_path; lanelet::ConstLanelets current_lanes; lanelet::ConstLanelets pull_out_lanes; + lanelet::ConstLanelets lanes; std::vector lane_follow_lane_ids; std::vector pull_out_lane_ids; bool is_safe = false; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp index c769a30c0cdaa..5d72e5c87d077 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp @@ -24,14 +24,14 @@ namespace behavior_path_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; struct ShiftParkingPath { - PathWithLaneId path; - PathWithLaneId straight_path; - ShiftedPath shifted_path; - ShiftPoint shift_point; + PathWithLaneId path{}; + PathWithLaneId straight_path{}; + ShiftedPath shifted_path{}; + ShiftPoint shift_point{}; double acceleration{0.0}; double preparation_length{0.0}; double pull_over_length{0.0}; - bool is_safe; + bool is_safe{false}; }; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_PATH_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 31942a0ab61e7..d7f2df9dea01f 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -168,6 +168,9 @@ BehaviorModuleOutput PullOutModule::plan() } else { path = status_.backward_path; } + path.drivable_area = util::generateDrivableArea( + path, status_.lanes, planner_data_->parameters.drivable_area_resolution, + planner_data_->parameters.vehicle_length, planner_data_); output.path = std::make_shared(path); output.turn_signal_info = @@ -235,9 +238,14 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() { BehaviorModuleOutput output; const auto current_lanes = getCurrentLanes(); - const auto shoulder_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); + const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); + auto lanes = current_lanes; + lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end()); - const auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path; + auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path; + candidate_path.drivable_area = util::generateDrivableArea( + candidate_path, lanes, planner_data_->parameters.drivable_area_resolution, + planner_data_->parameters.vehicle_length, planner_data_); auto stop_path = candidate_path; for (auto & p : stop_path.points) { p.point.longitudinal_velocity_mps = 0.0; @@ -354,7 +362,6 @@ void PullOutModule::planWithPriorityOnShortBackDistance( void PullOutModule::updatePullOutStatus() { const auto & route_handler = planner_data_->route_handler; - const auto & common_parameters = planner_data_->parameters; const auto & current_pose = planner_data_->self_pose->pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); @@ -365,6 +372,10 @@ void PullOutModule::updatePullOutStatus() const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); status_.pull_out_lanes = pull_out_lanes; + // combine road and shoulder lanes + status_.lanes = current_lanes; + status_.lanes.insert(status_.lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end()); + // search pull out start candidates backward std::vector start_pose_candidates; { @@ -399,9 +410,6 @@ void PullOutModule::updatePullOutStatus() status_.backward_path = pull_out_utils::getBackwardPath( *route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose, parameters_.backward_velocity); - status_.backward_path.drivable_area = util::generateDrivableArea( - status_.backward_path, pull_out_lanes, common_parameters.drivable_area_resolution, - common_parameters.vehicle_length, planner_data_); } // Update status @@ -432,14 +440,13 @@ lanelet::ConstLanelets PullOutModule::getCurrentLanes() const std::vector PullOutModule::searchBackedPoses() { const auto current_pose = planner_data_->self_pose->pose; - const auto current_lanes = getCurrentLanes(); - const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); // get backward shoulder path - const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose); + const auto arc_position_pose = + lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose); const double check_distance = parameters_.max_back_distance + 30.0; // buffer auto backward_shoulder_path = planner_data_->route_handler->getCenterLinePath( - pull_out_lanes, arc_position_pose.length - check_distance, + status_.pull_out_lanes, arc_position_pose.length - check_distance, arc_position_pose.length + check_distance); // lateral shift to current_pose diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index e9d74f7d6a586..de7143f989ab8 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -624,9 +624,14 @@ BehaviorModuleOutput PullOverModule::plan() -calcMinimumShiftPathDistance(), parameters_.deceleration_interval); } } + // generate drivable area + { + const auto p = planner_data_->parameters; + status_.path.drivable_area = util::generateDrivableArea( + status_.path, status_.lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); + } BehaviorModuleOutput output; - // safe: use pull over path if (status_.is_safe) { output.path = std::make_shared(status_.path); @@ -734,10 +739,6 @@ bool PullOverModule::planShiftPath() return found_safe_path; } - shift_parking_path_.path.drivable_area = util::generateDrivableArea( - shift_parking_path_.path, status_.lanes, common_parameters.drivable_area_resolution, - common_parameters.vehicle_length, planner_data_); - shift_parking_path_.path.header = planner_data_->route_handler->getRouteHeader(); return found_safe_path; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp index f13f753164256..38cd746ee355e 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp @@ -99,11 +99,21 @@ std::vector getShiftParkingPaths( const double jerk_resolution = std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_over_sampling_num; - double distance_to_shoulder_lane_boundary = + const double distance_to_shoulder_lane_boundary = util::getDistanceToShoulderBoundary(route_handler.getShoulderLanelets(), current_pose); - double offset_from_current_pose = + const double offset_from_current_pose = distance_to_shoulder_lane_boundary + common_parameter.vehicle_width / 2 + margin; + // shift end point in shoulder lane + PathPointWithLaneId shift_end_point; + { + const auto arc_position_goal = lanelet::utils::getArcCoordinates(target_lanelets, goal_pose); + const double s_start = arc_position_goal.length - after_pull_over_straight_distance; + const double s_end = s_start + std::numeric_limits::epsilon(); + const auto path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end, true); + shift_end_point = path.points.front(); + } + for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; lateral_jerk += jerk_resolution) { PathShifter path_shifter; @@ -124,15 +134,6 @@ std::vector getShiftParkingPaths( pull_over_distance - arc_position_pose.length; } - // shift end point in shoulder lane - const auto shift_end_point = [&]() { - const auto arc_position_goal = lanelet::utils::getArcCoordinates(target_lanelets, goal_pose); - const double s_start = arc_position_goal.length - after_pull_over_straight_distance; - const double s_end = s_start + std::numeric_limits::epsilon(); - const auto path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end, true); - return path.points.front(); - }(); - PathWithLaneId road_lane_reference_path; { const auto arc_position = lanelet::utils::getArcCoordinates(original_lanelets, current_pose); diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index ec0583215908c..4b726c66f4489 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -347,11 +347,6 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(const Pose & start auto path = planner_data_->route_handler->getCenterLinePath( current_lanes, current_arc_position.length, start_arc_position.length, true); path.header = planner_data_->route_handler->getRouteHeader(); - - path.drivable_area = util::generateDrivableArea( - path, current_lanes, planner_data_->parameters.drivable_area_resolution, - planner_data_->parameters.vehicle_length, planner_data_); - path.points.back().point.longitudinal_velocity_mps = 0; return path; @@ -429,9 +424,6 @@ std::vector GeometricParallelParking::planOneTrial( p.lane_ids.push_back(closest_shoulder_lanelet.id()); } path_turn_left.header = planner_data_->route_handler->getRouteHeader(); - path_turn_left.drivable_area = util::generateDrivableArea( - path_turn_left, lanes, common_params.drivable_area_resolution, common_params.vehicle_length, - planner_data_); PathWithLaneId path_turn_right = generateArcPath( Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, !is_forward, is_forward); @@ -440,9 +432,6 @@ std::vector GeometricParallelParking::planOneTrial( p.lane_ids.push_back(closest_shoulder_lanelet.id()); } path_turn_right.header = planner_data_->route_handler->getRouteHeader(); - path_turn_right.drivable_area = util::generateDrivableArea( - path_turn_right, lanes, common_params.drivable_area_resolution, common_params.vehicle_length, - planner_data_); // Need to add straight path to last right_turning for parking in parallel if (std::abs(end_pose_offset) > 0) {