Skip to content

Commit

Permalink
fix(behavior_path_planner): pull_over shift parking (autowarefoundati…
Browse files Browse the repository at this point in the history 
…on#1652)

* fix(behavior_path_planner): pull_over shift parking

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* check lane_depature for each shift path

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* change pull_over_velocity to 3.0

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
@kosuke55 @1222-takeshi
kosuke55 authored and 1222-takeshi committed Oct 18, 2022
1 parent fa514d2 commit 919194f
Show file tree
Hide file tree
Showing 7 changed files with 62 additions and 62 deletions.
7 changes: 3 additions & 4 deletions control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,9 +102,8 @@ class LaneDepartureChecker
vehicle_info_ptr_ = std::make_shared<vehicle_info_util::VehicleInfo>(vehicle_info);
}

bool checkPathWillLeaveLane(const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path);

vehicle_info_util::VehicleInfo vehicle_info_public_;
bool checkPathWillLeaveLane(
const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const;

private:
Param param_;
Expand All @@ -122,7 +121,7 @@ class LaneDepartureChecker
std::vector<LinearRing2d> createVehicleFootprints(
const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory,
const Param & param);
std::vector<LinearRing2d> createVehicleFootprints(const PathWithLaneId & path);
std::vector<LinearRing2d> createVehicleFootprints(const PathWithLaneId & path) const;

static std::vector<LinearRing2d> createVehiclePassingAreas(
const std::vector<LinearRing2d> & vehicle_footprints);
Expand Down
5 changes: 3 additions & 2 deletions control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ Output LaneDepartureChecker::update(const Input & input)
}

bool LaneDepartureChecker::checkPathWillLeaveLane(
const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path)
const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const
{
std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints);
Expand Down Expand Up @@ -242,7 +242,8 @@ std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(
return vehicle_footprints;
}

std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(const PathWithLaneId & path)
std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(
const PathWithLaneId & path) const
{
// Create vehicle footprint in base_link coordinate
const auto local_vehicle_footprint = createVehicleFootprint(*vehicle_info_ptr_);
Expand Down
2 changes: 1 addition & 1 deletion ...nning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
th_arrived_distance_m: 1.0
th_stopped_velocity_mps: 0.01
th_stopped_time_sec: 2.0 # It must be greater than the state_machine's.
pull_over_velocity: 2.0
pull_over_velocity: 3.0
pull_over_minimum_velocity: 0.3
margin_from_boundary: 0.5
decide_path_distance: 10.0
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_path_planner/config/pull_over/pull_over.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
th_arrived_distance_m: 1.0
th_stopped_velocity_mps: 0.01
th_stopped_time_sec: 2.0 # It must be greater than the state_machine's.
pull_over_velocity: 2.0
pull_over_velocity: 3.0
pull_over_minimum_velocity: 0.3
margin_from_boundary: 0.5
decide_path_distance: 10.0
Expand Down
5 changes: 4 additions & 1 deletion planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,8 @@
#include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp"
#include "behavior_path_planner/utilities.hpp"

#include <lane_departure_checker/lane_departure_checker.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
Expand Down Expand Up @@ -54,7 +56,8 @@ std::vector<ShiftParkingPath> selectValidPaths(
const std::vector<ShiftParkingPath> & paths, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes,
const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose,
const bool isInGoalRouteSection, const Pose & goal_pose);
const bool isInGoalRouteSection, const Pose & goal_pose,
const lane_departure_checker::LaneDepartureChecker & lane_departure_checker);
bool selectSafePath(
const std::vector<ShiftParkingPath> & paths,
const OccupancyGridBasedCollisionDetector & occupancy_grid_map, ShiftParkingPath & selected_path);
Expand Down
20 changes: 9 additions & 11 deletions planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -342,12 +342,9 @@ bool PullOverModule::planWithEfficientPath()
for (const auto goal_candidate : goal_candidates_) {
modified_goal_pose_ = goal_candidate.goal_pose;
if (
planShiftPath() &&
isLongEnoughToParkingStart(
shift_parking_path_.path, shift_parking_path_.shift_point.start) &&
!lane_departure_checker_->checkPathWillLeaveLane(
status_.lanes, shift_parking_path_.shifted_path.path)) {
// shift parking path already confirm safe in it's own function.
planShiftPath() && isLongEnoughToParkingStart(
shift_parking_path_.path, shift_parking_path_.shift_point.start)) {
// shift parking plan already confirms safety and no lane departure in it's own function.
status_.path = shift_parking_path_.path;
status_.path_type = PathType::SHIFT;
status_.is_safe = true;
Expand Down Expand Up @@ -410,10 +407,8 @@ bool PullOverModule::planWithCloseGoal()
// Generate arc shift path.
if (
parameters_.enable_shift_parking && planShiftPath() &&
isLongEnoughToParkingStart(shift_parking_path_.path, shift_parking_path_.shift_point.start) &&
!lane_departure_checker_->checkPathWillLeaveLane(
status_.lanes, shift_parking_path_.shifted_path.path)) {
// shift parking path already confirm safe in it's own function.
isLongEnoughToParkingStart(shift_parking_path_.path, shift_parking_path_.shift_point.start)) {
// shift parking plan already confirms safety and no lane departure in it's own function.
status_.path = shift_parking_path_.path;
status_.path_type = PathType::SHIFT;
status_.is_safe = true;
Expand Down Expand Up @@ -652,6 +647,9 @@ bool PullOverModule::planShiftPath()
// Find pull_over path
bool found_valid_path, found_safe_path;
std::tie(found_valid_path, found_safe_path) = getSafePath(shift_parking_path_);
if (!found_safe_path) {
return found_safe_path;
}

shift_parking_path_.path.drivable_area = util::generateDrivableArea(
shift_parking_path_.path, status_.lanes, common_parameters.drivable_area_resolution,
Expand Down Expand Up @@ -815,7 +813,7 @@ std::pair<bool, bool> PullOverModule::getSafePath(ShiftParkingPath & safe_path)
valid_paths = pull_over_utils::selectValidPaths(
pull_over_paths, status_.current_lanes, check_lanes, *route_handler->getOverallGraphPtr(),
current_pose, route_handler->isInGoalRouteSection(status_.current_lanes.back()),
modified_goal_pose_);
modified_goal_pose_, *lane_departure_checker_);

if (valid_paths.empty()) {
return std::make_pair(false, false);
Expand Down
83 changes: 41 additions & 42 deletions planning/behavior_path_planner/src/scene_module/pull_over/util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,7 +195,7 @@ std::vector<ShiftParkingPath> getShiftParkingPaths(
p.position.x -= std::sin(yaw) * offset;
p.position.y += std::cos(yaw) * offset;
}
path_shifter.setPath(target_lane_reference_path);
path_shifter.setPath(util::resamplePathWithSpline(target_lane_reference_path, 1.0));
}
}
ShiftPoint shift_point;
Expand All @@ -222,61 +222,50 @@ std::vector<ShiftParkingPath> getShiftParkingPaths(

const auto shift_end_idx =
motion_utils::findNearestIndex(shifted_path.path.points, shift_end_point.point.pose);

const auto goal_idx = motion_utils::findNearestIndex(shifted_path.path.points, goal_pose);

if (shift_end_idx && goal_idx) {
// get target shoulder lane
lanelet::ConstLanelet target_shoulder_lanelet;
lanelet::utils::query::getClosestLanelet(
target_lanelets, shifted_path.path.points.back().point.pose, &target_shoulder_lanelet);

for (size_t i = 0; i < shifted_path.path.points.size(); ++i) {
auto & point = shifted_path.path.points.at(i);

// add road lane_ids if not found
for (const auto id : road_lane_reference_path.points.back().lane_ids) {
if (std::find(point.lane_ids.begin(), point.lane_ids.end(), id) == point.lane_ids.end()) {
point.lane_ids.push_back(id);
}
}

// add shoulder lane_id if not found
if (
std::find(point.lane_ids.begin(), point.lane_ids.end(), target_shoulder_lanelet.id()) ==
point.lane_ids.end()) {
point.lane_ids.push_back(target_shoulder_lanelet.id());
}

// set velocity
if (i < *shift_end_idx) {
// set velocity during shift
point.point.longitudinal_velocity_mps = std::min(
point.point.longitudinal_velocity_mps,
road_lane_reference_path.points.back().point.longitudinal_velocity_mps);
continue;
} else if (i > *goal_idx) {
} else if (i >= *goal_idx) {
// set velocity after goal
point.point.longitudinal_velocity_mps = 0.0;
continue;
}
point.point.longitudinal_velocity_mps = pull_over_velocity;

// add closest shoulder lane id
lanelet::Lanelet closest_shoulder_lanelet;
lanelet::utils::query::getClosestLanelet(
route_handler.getShoulderLanelets(), point.point.pose, &closest_shoulder_lanelet);
point.lane_ids.clear();
point.lane_ids.push_back(closest_shoulder_lanelet.id());
}

candidate_path.straight_path = road_lane_reference_path;
// resample is needed for adding orientation to path points for collision check
candidate_path.path = util::resamplePathWithSpline(
combineReferencePath(road_lane_reference_path, shifted_path.path), 1.0);

// add goal pose because resampling removes it
PathPointWithLaneId goal_path_point{};
goal_path_point.point.pose = goal_pose;
// z of goal_pose can not be used
// https://github.com/autowarefoundation/autoware.universe/issues/711
goal_path_point.point.pose.position.z =
candidate_path.path.points.back().point.pose.position.z;
goal_path_point.point.longitudinal_velocity_mps = 0.0;
goal_path_point.lane_ids = shifted_path.path.points.back().lane_ids;
candidate_path.path.points.push_back(goal_path_point);

const auto shift_start_idx =
motion_utils::findNearestIndex(candidate_path.path.points, shift_point.start.position);
for (size_t i = shift_start_idx; i < candidate_path.path.points.size(); i++) {
candidate_path.shifted_path.path.points.push_back(candidate_path.path.points.at(i));
}

shift_point.start_idx = motion_utils::findNearestIndex(
candidate_path.shifted_path.path.points, shift_point.start.position);
shift_point.end_idx = motion_utils::findNearestIndex(
candidate_path.shifted_path.path.points, shift_point.end.position);
// todo: shift_length size dose not match path size due to resample,
// so sume fuctions (like getTurnInfo()) can not be used with this shifted_point.
candidate_path.path = combineReferencePath(road_lane_reference_path, shifted_path.path);
candidate_path.shifted_path = shifted_path;
shift_point.start_idx = path_shifter.getShiftPoints().front().start_idx;
shift_point.end_idx = path_shifter.getShiftPoints().front().end_idx;
candidate_path.shifted_path.shift_length = shifted_path.shift_length;
candidate_path.shift_point = shift_point;
// candidate_path.acceleration = acceleration;
Expand All @@ -299,16 +288,26 @@ std::vector<ShiftParkingPath> selectValidPaths(
const std::vector<ShiftParkingPath> & paths, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes,
const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose,
const bool isInGoalRouteSection, const Pose & goal_pose)
const bool isInGoalRouteSection, const Pose & goal_pose,
const lane_departure_checker::LaneDepartureChecker & lane_departure_checker)
{
std::vector<ShiftParkingPath> available_paths;
// combine road and shoulder lanes
lanelet::ConstLanelets lanes = current_lanes;
lanes.insert(lanes.end(), target_lanes.begin(), target_lanes.end());

std::vector<ShiftParkingPath> available_paths;
for (const auto & path : paths) {
if (hasEnoughDistance(
if (!hasEnoughDistance(
path, current_lanes, target_lanes, current_pose, isInGoalRouteSection, goal_pose,
overall_graphs)) {
available_paths.push_back(path);
continue;
}

if (lane_departure_checker.checkPathWillLeaveLane(lanes, path.shifted_path.path)) {
continue;
}

available_paths.push_back(path);
}

return available_paths;
Expand Down

0 comments on commit 919194f

Please sign in to comment.