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 (#5529)

* refactor(start_planner): refactor backward path calculation in StartPlannerModule

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]>

---------

Signed-off-by: kyoichi-sugahara <[email protected]>
Co-authored-by: Kosuke Takeuchi <[email protected]>
  • Loading branch information
@kyoichi-sugahara @kosuke55
kyoichi-sugahara and kosuke55 authored Nov 10, 2023
1 parent ff255eb commit 08b905a
Show file tree
Hide file tree
Showing 3 changed files with 55 additions and 58 deletions.
2 changes: 1 addition & 1 deletion planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -252,7 +252,7 @@ If a safe path cannot be generated from the current position, search backwards f
| max_back_distance | [m] | double | maximum back distance | 30.0 |
| backward_search_resolution | [m] | double | distance interval for searching backward pull out start point | 2.0 |
| backward_path_update_duration | [s] | double | time interval for searching backward pull out start point. this prevents chattering between back driving and pull_out | 3.0 |
| ignore_distance_from_lane_end | [m] | double | distance from end of pull out lanes for ignoring start candidates | 15.0 |
| ignore_distance_from_lane_end | [m] | double | If distance from shift start pose to end of shoulder lane is less than this value, this start pose candidate is ignored | 15.0 |

### **freespace pull out**

Expand Down
10 changes: 5 additions & 5 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,8 +176,9 @@ class StartPlannerModule : public SceneModuleInterface
std::mutex mutex_;

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

std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;

Expand All @@ -194,9 +195,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
101 changes: 49 additions & 52 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 @@ -718,15 +718,15 @@ 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;

// search pull out start candidates backward
const std::vector<Pose> start_pose_candidates = std::invoke([&]() -> std::vector<Pose> {
if (parameters_->enable_back) {
return searchPullOutStartPoses(start_pose_candidates_path);
return searchPullOutStartPoseCandidates(start_pose_candidates_path);
}
return {*refined_start_pose};
});
Expand All @@ -739,6 +739,7 @@ void StartPlannerModule::updatePullOutStatus()

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,71 +761,66 @@ void StartPlannerModule::updateStatusAfterBackwardDriving()
}
}

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

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;
}

std::vector<Pose> StartPlannerModule::searchPullOutStartPoses(
const PathWithLaneId & start_pose_candidates) const
std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
const PathWithLaneId & back_path_from_start_pose) const
{
const Pose & current_pose = planner_data_->self_odometry->pose.pose;

std::vector<Pose> pull_out_start_pose{};

std::vector<Pose> pull_out_start_pose_candidates{};
const auto & start_pose = planner_data_->route_handler->getOriginalStartPose();
const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_);
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 max_back_distance = std::clamp(
s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance);
const double current_arc_length =
lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length;
const double allowed_backward_distance = std::clamp(
current_arc_length - 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;
for (double back_distance = 0.0; back_distance <= allowed_backward_distance;
back_distance += parameters_->backward_search_resolution) {
const auto backed_pose = calcLongitudinalOffsetPose(
start_pose_candidates.points, current_pose.position, -back_distance);
back_path_from_start_pose.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 =
const double backed_pose_arc_length =
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); });
const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose;
const double distance_from_lane_end = length_to_lane_end - backed_pose_arc_length;
if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) {
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000,
Expand All @@ -833,27 +829,28 @@ 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.
}

pull_out_start_pose.push_back(*backed_pose);
pull_out_start_pose_candidates.push_back(*backed_pose);
}
return pull_out_start_pose;
return pull_out_start_pose_candidates;
}

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;
}
}
const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
*planner_data_->dynamic_object, velocity_threshold);

return false;
// filter for objects located in pull_out_lanes and moving at a speed below the threshold
const auto [stop_objects_in_pull_out_lanes, others] =
utils::path_safety_checker::separateObjectsByLanelets(
stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);

return stop_objects_in_pull_out_lanes;
}

bool StartPlannerModule::hasFinishedPullOut() const
Expand Down

0 comments on commit 08b905a

Please sign in to comment.