diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index bfb0173874784..08e0b8508c991 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -50,7 +50,7 @@ class ShiftPullOver : public PullOverPlannerBase const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const; + const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const; static double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); static std::vector splineTwoPoints( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index c299957d16a66..e1184c49e40ba 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -132,7 +132,7 @@ std::optional ShiftPullOver::generatePullOverPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const + const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; const double after_shift_straight_distance = parameters_.after_shift_straight_distance; @@ -224,7 +224,7 @@ std::optional ShiftPullOver::generatePullOverPath( p.point.longitudinal_velocity_mps = 0.0; p.point.pose = goal_pose; p.lane_ids = shifted_path.path.points.back().lane_ids; - for (const auto & lane : shoulder_lanes) { + for (const auto & lane : pull_over_lanes) { p.lane_ids.push_back(lane.id()); } shifted_path.path.points.push_back(p); @@ -246,7 +246,7 @@ std::optional ShiftPullOver::generatePullOverPath( } } // add shoulder lane_id if not found - for (const auto & lane : shoulder_lanes) { + for (const auto & lane : pull_over_lanes) { if ( std::find(point.lane_ids.begin(), point.lane_ids.end(), lane.id()) == point.lane_ids.end()) { @@ -294,7 +294,7 @@ std::optional ShiftPullOver::generatePullOverPath( }); const bool is_in_lanes = std::invoke([&]() -> bool { const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_over_lanes); const auto & dp = planner_data->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 518d1108f0ee1..9cc9bec8e963a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -70,11 +70,11 @@ class GeometricParallelParking bool isParking() const; bool planPullOver( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, + const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward, const bool left_side_parking); bool planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start, + const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start, const std::shared_ptr autoware_lane_departure_checker); void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; } @@ -117,7 +117,7 @@ class GeometricParallelParking void clearPaths(); std::vector planOneTrial( const Pose & start_pose, const Pose & goal_pose, const double R_E_far, - const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, const double lane_departure_margin, const double arc_path_interval, const std::shared_ptr @@ -134,7 +134,7 @@ class GeometricParallelParking const bool left_side_parking); std::vector generatePullOverPaths( const Pose & start_pose, const Pose & goal_pose, const double R_E_far, - const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, const double velocity); PathWithLaneId generateStraightPath( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index e2218e37a771b..b18858e9dad04 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -105,7 +105,7 @@ void GeometricParallelParking::setVelocityToArcPaths( std::vector GeometricParallelParking::generatePullOverPaths( const Pose & start_pose, const Pose & goal_pose, const double R_E_far, - const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, const double velocity) { @@ -115,7 +115,7 @@ std::vector GeometricParallelParking::generatePullOverPaths( const double arc_path_interval = is_forward ? parameters_.forward_parking_path_interval : parameters_.backward_parking_path_interval; auto arc_paths = planOneTrial( - start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking, + start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking, end_pose_offset, lane_departure_margin, arc_path_interval, {}); if (arc_paths.empty()) { return std::vector{}; @@ -156,7 +156,7 @@ void GeometricParallelParking::clearPaths() bool GeometricParallelParking::planPullOver( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, + const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward, const bool left_side_parking) { const auto & common_params = planner_data_->parameters; @@ -186,7 +186,7 @@ bool GeometricParallelParking::planPullOver( } const auto paths = generatePullOverPaths( - *start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking, + *start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking, end_pose_offset, parameters_.forward_parking_velocity); if (!paths.empty()) { paths_ = paths; @@ -208,8 +208,8 @@ bool GeometricParallelParking::planPullOver( } const auto paths = generatePullOverPaths( - *start_pose, goal_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_parking, - end_pose_offset, parameters_.backward_parking_velocity); + *start_pose, goal_pose, R_E_min_, road_lanes, pull_over_lanes, is_forward, + left_side_parking, end_pose_offset, parameters_.backward_parking_velocity); if (!paths.empty()) { paths_ = paths; return true; @@ -222,7 +222,7 @@ bool GeometricParallelParking::planPullOver( bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start, + const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start, const std::shared_ptr lane_departure_checker) { @@ -242,7 +242,7 @@ bool GeometricParallelParking::planPullOut( // plan reverse path of parking. end_pose <-> start_pose auto arc_paths = planOneTrial( - *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_start, + *end_pose, start_pose, R_E_min_, road_lanes, pull_over_lanes, is_forward, left_side_start, start_pose_offset, parameters_.pull_out_lane_departure_margin, parameters_.pull_out_arc_path_interval, lane_departure_checker); if (arc_paths.empty()) { @@ -374,7 +374,7 @@ PathWithLaneId GeometricParallelParking::generateStraightPath( std::vector GeometricParallelParking::planOneTrial( const Pose & start_pose, const Pose & goal_pose, const double R_E_far, - const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, const double lane_departure_margin, const double arc_path_interval, const std::shared_ptr @@ -419,7 +419,7 @@ std::vector GeometricParallelParking::planOneTrial( } lanes.push_back(lane); } - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); // If start_pose is parallel to goal_pose, we can know lateral deviation of edges of vehicle, // and detect lane departure. @@ -427,7 +427,7 @@ std::vector GeometricParallelParking::planOneTrial( const double R_front_near = std::hypot(R_E_far + common_params.vehicle_width / 2, common_params.base_link2front); const double distance_to_near_bound = - utils::getSignedDistanceFromBoundary(shoulder_lanes, arc_end_pose, left_side_parking); + utils::getSignedDistanceFromBoundary(pull_over_lanes, arc_end_pose, left_side_parking); const double near_deviation = R_front_near - R_E_far; if (std::abs(distance_to_near_bound) - near_deviation < lane_departure_margin) { return std::vector{};