Skip to content

Commit

Permalink
refactor(start_planner): refactor backward path calculation in StartP…
Browse files Browse the repository at this point in the history 
…lannerModule

The method "calcStartPoseCandidatesBackwardPath" has been renamed to "calcBackwardPathFromStartPose" to better reflect its purpose. The method now calculates the backward path by shifting the original start pose coordinates to align with the pull out lanes. The stop objects in the pull out lanes are filtered by velocity, using the new "filterStopObjectsInPullOutLanes" method. Additionally, the redundant "isOverlappedWithLane" method has been removed.

Signed-off-by: kyoichi-sugahara <[email protected]>
  • Loading branch information
@kyoichi-sugahara
kyoichi-sugahara committed Nov 9, 2023
1 parent 0e471b3 commit e326693
Show file tree
Hide file tree
Showing 2 changed files with 44 additions and 51 deletions.
7 changes: 3 additions & 4 deletions ...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -176,7 +176,7 @@ class StartPlannerModule : public SceneModuleInterface
std::mutex mutex_;

PathWithLaneId getFullPath() const;
PathWithLaneId calcStartPoseCandidatesBackwardPath() const;
PathWithLaneId calcBackwardPathFromStartPose() const;
std::vector<Pose> searchPullOutStartPoses(const PathWithLaneId & start_pose_candidates) const;

std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
Expand All @@ -194,9 +194,8 @@ class StartPlannerModule : public SceneModuleInterface
std::vector<DrivableLanes> generateDrivableLanes(const PathWithLaneId & path) const;
void updatePullOutStatus();
void updateStatusAfterBackwardDriving();
static bool isOverlappedWithLane(
const lanelet::ConstLanelet & candidate_lanelet,
const tier4_autoware_utils::LinearRing2d & vehicle_footprint);
PredictedObjects filterStopObjectsInPullOutLanes(
const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const;
bool hasFinishedPullOut() const;
bool isBackwardDrivingComplete() const;
bool isStopped();
Expand Down
88 changes: 41 additions & 47 deletions planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -619,7 +619,7 @@ PathWithLaneId StartPlannerModule::generateStopPath() const
PathPointWithLaneId p{};
p.point.pose = pose;
p.point.longitudinal_velocity_mps = 0.0;
const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
const auto & pull_out_lanes = start_planner_utils::getPullOutLanes(
planner_data_,
planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
lanelet::Lanelet closest_lanelet;
Expand Down Expand Up @@ -667,7 +667,7 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId
std::vector<DrivableLanes> StartPlannerModule::generateDrivableLanes(
const PathWithLaneId & path) const
{
const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
const auto & pull_out_lanes = start_planner_utils::getPullOutLanes(
planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);

const auto path_road_lanes = getPathRoadLanes(path);
Expand Down Expand Up @@ -718,7 +718,7 @@ void StartPlannerModule::updatePullOutStatus()
// refine start pose with pull out lanes.
// 1) backward driving is not allowed: use refined pose just as start pose.
// 2) backward driving is allowed: use refined pose to check if backward driving is needed.
const PathWithLaneId start_pose_candidates_path = calcStartPoseCandidatesBackwardPath();
const PathWithLaneId start_pose_candidates_path = calcBackwardPathFromStartPose();
const auto refined_start_pose = calcLongitudinalOffsetPose(
start_pose_candidates_path.points, planner_data_->self_odometry->pose.pose.position, 0.0);
if (!refined_start_pose) return;
Expand All @@ -734,11 +734,12 @@ void StartPlannerModule::updatePullOutStatus()
planWithPriority(
start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority);

const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
const auto & pull_out_lanes = start_planner_utils::getPullOutLanes(
planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);

if (isBackwardDrivingComplete()) {
updateStatusAfterBackwardDriving();
// should be moved to transition state
current_state_ = ModuleStatus::SUCCESS; // for breaking loop
} else {
status_.backward_path = start_planner_utils::getBackwardPath(
Expand All @@ -760,24 +761,27 @@ void StartPlannerModule::updateStatusAfterBackwardDriving()
}
}

PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const
PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const
{
const Pose & current_pose = planner_data_->self_odometry->pose.pose;

const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose();
const auto & pull_out_lanes = start_planner_utils::getPullOutLanes(
planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);

// get backward shoulder path
const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose);
const double check_distance = parameters_->max_back_distance + 30.0; // buffer
auto path = planner_data_->route_handler->getCenterLinePath(
pull_out_lanes, arc_position_pose.length - check_distance,
arc_position_pose.length + check_distance);

// lateral shift to current_pose
const double distance_from_center_line = arc_position_pose.distance;
for (auto & p : path.points) {
p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0);
const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose);

// common buffer distance for both front and back
static constexpr double buffer = 30.0;
const double check_distance = parameters_->max_back_distance + buffer;

const double start_distance = arc_position_pose.length - check_distance;
const double end_distance = arc_position_pose.length + buffer;

auto path =
planner_data_->route_handler->getCenterLinePath(pull_out_lanes, start_distance, end_distance);

// shift all path points laterally to align with the start pose
for (auto & path_point : path.points) {
path_point.point.pose = calcOffsetPose(path_point.point.pose, 0, arc_position_pose.distance, 0);
}

return path;
Expand All @@ -786,41 +790,30 @@ PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const
std::vector<Pose> StartPlannerModule::searchPullOutStartPoses(
const PathWithLaneId & start_pose_candidates) const
{
const Pose & current_pose = planner_data_->self_odometry->pose.pose;

std::vector<Pose> pull_out_start_pose{};
const auto & start_pose = planner_data_->route_handler->getOriginalStartPose();

const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
const auto & pull_out_lanes = start_planner_utils::getPullOutLanes(
planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);

// filter pull out lanes stop objects
const auto [pull_out_lane_objects, others] =
utils::path_safety_checker::separateObjectsByLanelets(
*planner_data_->dynamic_object, pull_out_lanes,
utils::path_safety_checker::isPolygonOverlapLanelet);
const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
pull_out_lane_objects, parameters_->th_moving_object_velocity);
const auto stop_objects_in_pull_out_lanes =
filterStopObjectsInPullOutLanes(pull_out_lanes, parameters_->th_moving_object_velocity);

// Set the maximum backward distance less than the distance from the vehicle's base_link to the
// lane's rearmost point to prevent lane departure.
const double s_current = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose).length;
const double s_current = lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length;
const double max_back_distance = std::clamp(
s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance);

// check collision between footprint and object at the backed pose
const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_);
for (double back_distance = 0.0; back_distance <= max_back_distance;
back_distance += parameters_->backward_search_resolution) {
const auto backed_pose = calcLongitudinalOffsetPose(
start_pose_candidates.points, current_pose.position, -back_distance);
const auto backed_pose =
calcLongitudinalOffsetPose(start_pose_candidates.points, start_pose.position, -back_distance);
if (!backed_pose) {
continue;
}

// check the back pose is near the lane end
const double length_to_backed_pose =
lanelet::utils::getArcCoordinates(pull_out_lanes, *backed_pose).length;

const double length_to_lane_end = std::accumulate(
std::begin(pull_out_lanes), std::end(pull_out_lanes), 0.0,
[](double acc, const auto & lane) { return acc + lanelet::utils::getLaneletLength2d(lane); });
Expand All @@ -833,7 +826,7 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoses(
}

if (utils::checkCollisionBetweenFootprintAndObjects(
local_vehicle_footprint, *backed_pose, pull_out_lane_stop_objects,
local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes,
parameters_->collision_check_margin)) {
break; // poses behind this has a collision, so break.
}
Expand All @@ -843,17 +836,18 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoses(
return pull_out_start_pose;
}

bool StartPlannerModule::isOverlappedWithLane(
const lanelet::ConstLanelet & candidate_lanelet,
const tier4_autoware_utils::LinearRing2d & vehicle_footprint)
PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes(
const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const
{
for (const auto & point : vehicle_footprint) {
if (boost::geometry::within(point, candidate_lanelet.polygon2d().basicPolygon())) {
return true;
}
}
// filter for objects located in pull_out_lanes and moving at a speed below the threshold
const auto [objects_in_pull_out_lanes, others] =
utils::path_safety_checker::separateObjectsByLanelets(
*planner_data_->dynamic_object, pull_out_lanes,
utils::path_safety_checker::isPolygonOverlapLanelet);
const auto stop_objects_in_pull_out_lanes = utils::path_safety_checker::filterObjectsByVelocity(
objects_in_pull_out_lanes, velocity_threshold);

return false;
return stop_objects_in_pull_out_lanes;
}

bool StartPlannerModule::hasFinishedPullOut() const
Expand Down

0 comments on commit e326693

Please sign in to comment.