From 8c2080c8cd4c0cecd52f75cb8fec2523b5e8cc6f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 2 Sep 2022 16:47:29 +0900 Subject: [PATCH] fix(behavior_path_planner): fix stop path for pull_over (#1742) * fix(behavior_path_planner): fix stop path for pull_over Signed-off-by: kosuke55 * use previous stop path when is has been generated Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp Co-authored-by: Fumiya Watanabe Signed-off-by: kosuke55 Co-authored-by: Fumiya Watanabe --- .../pull_over/pull_over_module.hpp | 4 +- .../pull_over/pull_over_module.cpp | 54 +++++++++++-------- 2 files changed, 34 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index f870f4f337733..ec9689841d8d4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -108,12 +108,14 @@ enum PathType { struct PUllOverStatus { PathWithLaneId path; + std::shared_ptr 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; }; @@ -187,7 +189,7 @@ class PullOverModule : public SceneModuleInterface std::unique_ptr last_approved_time_; PathWithLaneId getReferencePath() const; - PathWithLaneId getStopPath() const; + PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPullOverLanes() const; std::pair getSafePath(ShiftParkingPath & safe_path) const; Pose getRefinedGoal() const; diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 4630a259b6d5d..49c72ea815057 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -570,8 +570,18 @@ BehaviorModuleOutput PullOverModule::plan() } BehaviorModuleOutput output; - output.path = status_.is_safe ? std::make_shared(status_.path) - : std::make_shared(getStopPath()); + + // safe: use pull over path + if (status_.is_safe) { + output.path = std::make_shared(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(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) { @@ -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 = @@ -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(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( + 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(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