Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(behavior_path_planner): fix stop path for pull_over #1742

Merged
merged 3 commits into from
Sep 2, 2022
Merged

fix(behavior_path_planner): fix stop path for pull_over #1742

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9af9511
fix(behavior_path_planner): fix stop path for pull_over
kosuke55 Aug 31, 2022
217a558
use previous stop path when is has been generated
kosuke55 Sep 1, 2022
7e85b3f
Update planning/behavior_path_planner/src/scene_module/pull_over/pull…
kosuke55 Sep 2, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion ...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,12 +108,14 @@ enum PathType {
struct PUllOverStatus
{
PathWithLaneId path;
std::shared_ptr<PathWithLaneId> prev_stop_path = nullptr;
lanelet::ConstLanelets current_lanes;
lanelet::ConstLanelets pull_over_lanes;
lanelet::ConstLanelets lanes; // current + pull_over
bool has_decided_path = false;
int path_type = PathType::NONE;
bool is_safe = false;
bool prev_is_safe = false;
bool has_decided_velocity = false;
bool has_requested_approval_ = false;
};
Expand Down Expand Up @@ -187,7 +189,7 @@ class PullOverModule : public SceneModuleInterface
std::unique_ptr<rclcpp::Time> last_approved_time_;

PathWithLaneId getReferencePath() const;
PathWithLaneId getStopPath() const;
PathWithLaneId generateStopPath() const;
lanelet::ConstLanelets getPullOverLanes() const;
std::pair<bool, bool> getSafePath(ShiftParkingPath & safe_path) const;
Pose getRefinedGoal() const;
Expand Down
54 changes: 31 additions & 23 deletions planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -570,8 +570,18 @@ BehaviorModuleOutput PullOverModule::plan()
}

BehaviorModuleOutput output;
output.path = status_.is_safe ? std::make_shared<PathWithLaneId>(status_.path)
: std::make_shared<PathWithLaneId>(getStopPath());

// safe: use pull over path
if (status_.is_safe) {
output.path = std::make_shared<PathWithLaneId>(status_.path);
} else if (status_.prev_is_safe || status_.prev_stop_path == nullptr) {
// safe -> not_safe or no prev_stop_path: generate new stop_path
output.path = std::make_shared<PathWithLaneId>(generateStopPath());
status_.prev_stop_path = output.path;
} else { // not_safe -> not_safe: use previous stop path
output.path = status_.prev_stop_path;
}
status_.prev_is_safe = status_.is_safe;

// set hazard and turn signal
if (status_.has_decided_path) {
Expand Down Expand Up @@ -696,7 +706,7 @@ PathWithLaneId PullOverModule::getReferencePath() const

// stop pose is behind current pose
if (s_forward < s_backward) {
return getStopPath();
return generateStopPath();
}

PathWithLaneId reference_path =
Expand All @@ -721,45 +731,43 @@ PathWithLaneId PullOverModule::getReferencePath() const
return reference_path;
}

PathWithLaneId PullOverModule::getStopPath() const
PathWithLaneId PullOverModule::generateStopPath() const
{
PathWithLaneId reference_path;

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 double current_to_stop_distance =
std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2;
const auto arclength_current_pose =
const double arclength_current_pose =
lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length;

reference_path = util::getCenterLinePath(
*route_handler, status_.current_lanes, current_pose, common_parameters.backward_path_length,
common_parameters.forward_path_length, common_parameters);

for (auto & point : reference_path.points) {
const auto arclength =
for (auto & point : stop_path.points) {
const double arclength =
lanelet::utils::getArcCoordinates(status_.current_lanes, point.point.pose).length;
const auto arclength_current_to_point = arclength - arclength_current_pose;
if (0 < arclength_current_to_point) {
point.point.longitudinal_velocity_mps = std::min(
point.point.longitudinal_velocity_mps,
static_cast<float>(std::max(
0.0, current_vel * (current_to_stop_distance - arclength_current_to_point) /
current_to_stop_distance)));
const double arclength_current_to_point = arclength - arclength_current_pose;
if (0.0 < arclength_current_to_point) {
point.point.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);
} else {
point.point.longitudinal_velocity_mps =
std::min(point.point.longitudinal_velocity_mps, static_cast<float>(current_vel));
}
}

reference_path.drivable_area = util::generateDrivableArea(
reference_path, status_.current_lanes, common_parameters.drivable_area_resolution,
stop_path.drivable_area = util::generateDrivableArea(
stop_path, status_.current_lanes, common_parameters.drivable_area_resolution,
common_parameters.vehicle_length, planner_data_);

return reference_path;
return stop_path;
}

lanelet::ConstLanelets PullOverModule::getPullOverLanes() const
Expand Down