Skip to content

Commit

Permalink
fix(behavior_path_planner): pull over stop path (autowarefoundation#2207
Browse files Browse the repository at this point in the history 
)

* fix(behavior_path_planner): pull over stop path

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

* fix getExtendedCurrentLanes

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

* remove unused variables and fix deceleration

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

* fix typo

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

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
@kosuke55
kosuke55 committed Nov 10, 2022
1 parent a6381b5 commit 4ec2613
Show file tree
Hide file tree
Showing 4 changed files with 72 additions and 96 deletions.
6 changes: 0 additions & 6 deletions planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -309,12 +309,6 @@ PathWithLaneId setDecelerationVelocity(
const lanelet::ConstLanelets & lanelet_sequence, const double lane_change_prepare_duration,
const double lane_change_buffer);

PathWithLaneId setDecelerationVelocity(
const RouteHandler & route_handler, const PathWithLaneId & input,
const lanelet::ConstLanelets & lanelet_sequence, const double distance_after_pullover,
const double pullover_distance_min, const double distance_before_pull_over,
const double deceleration_interval, Pose goal_pose);

bool checkLaneIsInIntersection(
const RouteHandler & route_handler, const PathWithLaneId & ref,
const lanelet::ConstLanelets & lanelet_sequence, const BehaviorPathPlannerParameters & parameters,
Expand Down
102 changes: 60 additions & 42 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 @@ -33,6 +33,8 @@
#include <vector>

using motion_utils::calcSignedArcLength;
using motion_utils::findFirstNearestIndexWithSoftConstraints;
using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints;
using nav_msgs::msg::OccupancyGrid;
using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::calcOffsetPose;
Expand Down Expand Up @@ -451,6 +453,11 @@ BehaviorModuleOutput PullOverModule::plan()
if (status_.prev_is_safe || status_.prev_stop_path == nullptr) {
output.path = std::make_shared<PathWithLaneId>(generateStopPath());
status_.prev_stop_path = output.path;
// set stop path as pull over path
PullOverPath pull_over_path{};
status_.pull_over_path = pull_over_path;
status_.pull_over_path.path = *output.path;
status_.pull_over_path.partial_paths.push_back(*output.path);
} else { // not_safe -> not_safe: use previous stop path
output.path = status_.prev_stop_path;
}
Expand Down Expand Up @@ -501,10 +508,9 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval()
{
updateOccupancyGrid();
BehaviorModuleOutput out;
const auto path = *(plan().path);
out.path_candidate = std::make_shared<PathWithLaneId>(path);
plan(); // update status_
out.path = std::make_shared<PathWithLaneId>(getReferencePath());
out.path_candidate = std::make_shared<PathWithLaneId>(getFullPath());
out.path_candidate = status_.is_safe ? std::make_shared<PathWithLaneId>(getFullPath()) : out.path;

const double distance_to_path_change = calcDistanceToPathChange();
updateRTCStatus(distance_to_path_change);
Expand Down Expand Up @@ -538,41 +544,45 @@ PathWithLaneId PullOverModule::getReferencePath() const
if (status_.current_lanes.empty()) {
return PathWithLaneId{};
}
const auto arc_coordinates =

// generate reference path
const auto s_current =
lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length;
const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length);
const double s_end = s_current + common_parameters.forward_path_length;
auto reference_path =
route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true);

// if not approved, stop parking start position or goal search start position.
const auto refined_goal_arc_coordinates =
lanelet::utils::getArcCoordinates(status_.current_lanes, refined_goal_pose_);
const Pose search_start_pose = calcOffsetPose(
refined_goal_pose_, -parameters_.backward_goal_search_length, -arc_coordinates.distance, 0);
// if not approved, stop parking start position or goal search start position.
refined_goal_pose_, -parameters_.backward_goal_search_length,
-refined_goal_arc_coordinates.distance, 0);
const Pose stop_pose = status_.is_safe ? status_.pull_over_path.start_pose : search_start_pose;

// generate center line path to stop_pose
const auto arc_position_stop_pose =
lanelet::utils::getArcCoordinates(status_.current_lanes, stop_pose);
const double s_forward = arc_position_stop_pose.length;
const auto arc_position_current_pose =
lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose);
const double s_backward =
std::max(0.0, arc_position_current_pose.length - common_parameters.backward_path_length);

// stop pose is behind current pose
if (s_forward < s_backward) {
// if stop pose is behind current pose, stop as soon as possible
const size_t ego_idx = findFirstNearestIndexWithSoftConstraints(
reference_path.points, planner_data_->self_pose->pose,
planner_data_->parameters.ego_nearest_dist_threshold,
planner_data_->parameters.ego_nearest_yaw_threshold);
const size_t stop_idx = findFirstNearestSegmentIndexWithSoftConstraints(
reference_path.points, stop_pose, common_parameters.ego_nearest_dist_threshold,
common_parameters.ego_nearest_yaw_threshold);
const double ego_to_stop_distance = calcSignedArcLength(
reference_path.points, current_pose.position, ego_idx, stop_pose.position, stop_idx);
if (ego_to_stop_distance < 0.0) {
return generateStopPath();
}

PathWithLaneId reference_path =
route_handler->getCenterLinePath(status_.current_lanes, s_backward, s_forward, true);
reference_path.header = route_handler->getRouteHeader();

// slow down for turn signal, insert stop point to stop_pose
reference_path = util::setDecelerationVelocityForTurnSignal(
reference_path, stop_pose, planner_data_->parameters.turn_light_on_threshold_time);

// slow down before the search area.
if (stop_pose != search_start_pose) {
reference_path = util::setDecelerationVelocity(
reference_path, parameters_.pull_over_velocity, search_start_pose,
-calcMinimumShiftPathDistance(), parameters_.deceleration_interval);
}
reference_path = util::setDecelerationVelocity(
reference_path, parameters_.pull_over_velocity, search_start_pose,
-calcMinimumShiftPathDistance(), parameters_.deceleration_interval);

reference_path.drivable_area = util::generateDrivableArea(
reference_path, status_.current_lanes, common_parameters.drivable_area_resolution,
Expand All @@ -586,30 +596,38 @@ PathWithLaneId PullOverModule::generateStopPath() const
const auto & route_handler = planner_data_->route_handler;
const auto & current_pose = planner_data_->self_pose->pose;
const auto & common_parameters = planner_data_->parameters;

const double current_vel = planner_data_->self_odometry->twist.twist.linear.x;
auto stop_path = util::getCenterLinePath(
*route_handler, status_.current_lanes, current_pose, common_parameters.backward_path_length,
common_parameters.forward_path_length, common_parameters);

const auto s_current =
lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length;
const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length);
const double s_end = s_current + common_parameters.forward_path_length;
auto stop_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true);

// set deceleration velocity
const size_t ego_idx = findFirstNearestIndexWithSoftConstraints(
stop_path.points, planner_data_->self_pose->pose,
planner_data_->parameters.ego_nearest_dist_threshold,
planner_data_->parameters.ego_nearest_yaw_threshold);
const double current_to_stop_distance =
std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2;
const double arclength_current_pose =
lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length;
motion_utils::insertStopPoint(current_pose, current_to_stop_distance, stop_path.points);

for (auto & point : stop_path.points) {
const double arclength =
lanelet::utils::getArcCoordinates(status_.current_lanes, point.point.pose).length;
const double arclength_current_to_point = arclength - arclength_current_pose;
if (0.0 < arclength_current_to_point) {
point.point.longitudinal_velocity_mps = std::clamp(
auto & p = point.point;
const size_t target_idx = findFirstNearestSegmentIndexWithSoftConstraints(
stop_path.points, p.pose, common_parameters.ego_nearest_dist_threshold,
common_parameters.ego_nearest_yaw_threshold);
const double distance_to_target = calcSignedArcLength(
stop_path.points, current_pose.position, ego_idx, p.pose.position, target_idx);
if (0.0 < distance_to_target) {
p.longitudinal_velocity_mps = std::clamp(
static_cast<float>(
current_vel * (current_to_stop_distance - arclength_current_to_point) /
current_to_stop_distance),
0.0f, point.point.longitudinal_velocity_mps);
current_vel * (current_to_stop_distance - distance_to_target) / current_to_stop_distance),
0.0f, p.longitudinal_velocity_mps);
} else {
point.point.longitudinal_velocity_mps =
std::min(point.point.longitudinal_velocity_mps, static_cast<float>(current_vel));
p.longitudinal_velocity_mps =
std::min(p.longitudinal_velocity_mps, static_cast<float>(current_vel));
}
}

Expand Down
3 changes: 2 additions & 1 deletion planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,8 @@ PathWithLaneId ShiftPullOver::generateRoadLaneReferencePath(
// (or keep original velocity if it is lower than pull over velocity)
for (auto & point : road_lane_reference_path.points) {
const auto arclength = lanelet::utils::getArcCoordinates(road_lanes, point.point.pose).length;
const double distance_to_pull_over_start = std::max(0.0, s_end - arclength);
const double distance_to_pull_over_start =
std::clamp(s_end - arclength, 0.0, deceleration_interval);
const auto decelerated_velocity = static_cast<float>(
distance_to_pull_over_start / deceleration_interval *
(point.point.longitudinal_velocity_mps - pull_over_velocity) +
Expand Down
57 changes: 10 additions & 47 deletions planning/behavior_path_planner/src/utilities.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1828,44 +1828,8 @@ PathWithLaneId setDecelerationVelocity(
return reference_path;
}

PathWithLaneId setDecelerationVelocity(
const RouteHandler & route_handler, const PathWithLaneId & input,
const lanelet::ConstLanelets & lanelet_sequence, const double distance_after_pullover,
const double pullover_distance_min, const double distance_before_pull_over,
const double deceleration_interval, Pose goal_pose)
{
auto reference_path = input;
const auto pullover_buffer =
distance_after_pullover + pullover_distance_min + distance_before_pull_over;
const auto arclength_goal_pose =
lanelet::utils::getArcCoordinates(lanelet_sequence, goal_pose).length;
const auto arclength_pull_over_start = arclength_goal_pose - pullover_buffer;
const auto arclength_path_front =
lanelet::utils::getArcCoordinates(lanelet_sequence, reference_path.points.front().point.pose)
.length;

if (
route_handler.isDeadEndLanelet(lanelet_sequence.back()) &&
pullover_distance_min > std::numeric_limits<double>::epsilon()) {
for (auto & point : reference_path.points) {
const auto arclength =
lanelet::utils::getArcCoordinates(lanelet_sequence, point.point.pose).length;
const double distance_to_pull_over_start =
std::max(0.0, arclength_pull_over_start - arclength);
point.point.longitudinal_velocity_mps = std::min(
point.point.longitudinal_velocity_mps,
static_cast<float>(distance_to_pull_over_start / deceleration_interval) *
point.point.longitudinal_velocity_mps);
}
}

double distance_to_pull_over_start =
std::max(0.0, arclength_pull_over_start - arclength_path_front);
const auto stop_point = util::insertStopPoint(distance_to_pull_over_start, &reference_path);

return reference_path;
}

// TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex inside the
// function
PathWithLaneId setDecelerationVelocity(
const PathWithLaneId & input, const double target_velocity, const Pose target_pose,
const double buffer, const double deceleration_interval)
Expand All @@ -1888,7 +1852,8 @@ PathWithLaneId setDecelerationVelocity(

const auto stop_point_length =
motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + buffer;
if (target_velocity == 0.0 && stop_point_length > 0) {
constexpr double eps{0.01};
if (std::abs(target_velocity) < eps && stop_point_length > 0.0) {
const auto stop_point = util::insertStopPoint(stop_point_length, &reference_path);
}

Expand Down Expand Up @@ -1971,15 +1936,13 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
current_lane, current_pose, common_parameters.backward_path_length,
common_parameters.forward_path_length);

// Add next_lanes
for (const auto & next_lane : route_handler->getNextLanelets(current_lanes.back())) {
current_lanes.push_back(next_lane);
}
// Add next lane
const auto next_lanes = route_handler->getNextLanelets(current_lanes.back());
current_lanes.push_back(next_lanes.front());

// Add previous lanes
for (const auto & prev_lane : route_handler->getPreviousLanelets(current_lanes.front())) {
current_lanes.insert(current_lanes.begin(), prev_lane);
}
// Add previous lane
const auto prev_lanes = route_handler->getPreviousLanelets(current_lanes.front());
current_lanes.insert(current_lanes.begin(), prev_lanes.front());

return current_lanes;
}
Expand Down

0 comments on commit 4ec2613

Please sign in to comment.