From 017ca0497a76252aefb6613766836f367ccc4873 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 8 Sep 2023 03:03:59 +0900 Subject: [PATCH 1/9] add feature of stop after approval for start planner Signed-off-by: kyoichi-sugahara --- .../start_planner/start_planner_module.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 90078a50e6ac9..a29cf3d11c27b 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -188,6 +188,24 @@ BehaviorModuleOutput StartPlannerModule::plan() return output; } + status_.is_safe_dynamic_objects = isSafePath(); + + if (status_.is_safe_dynamic_objects) { + const auto distance_to_stop_point = utils::start_goal_planner_common::calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop, + 0); + if (distance_to_stop_point) { + const auto output = generateStopOutput(); + RCLCPP_ERROR_THROTTLE( + getLogger(), *clock_, 5000, "dangerous against dynamic object. publish stop path"); + return output; + } else { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + "Failed to generate feasible stop point. So don't stop but path is not safe."); + } + } + if (isWaitingApproval()) { clearWaitingApproval(); resetPathCandidate(); From a0746450d93d019ed36068b7668c955821eddd1e Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 8 Sep 2023 12:07:01 +0900 Subject: [PATCH 2/9] initialize safetycheck param Signed-off-by: kyoichi-sugahara --- .../goal_planner/goal_planner_module.hpp | 1 + .../goal_planner/goal_planner_module.cpp | 23 ++++++++++--- .../start_planner/start_planner_module.cpp | 33 +++++++++---------- 3 files changed, 35 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index ab6f77b1f53d6..83a5cc9e6e417 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -299,6 +299,7 @@ class GoalPlannerModule : public SceneModuleInterface std::pair calcDistanceToPathChange() const; // safety check + void initializeSafetyCheckParameters(); SafetyCheckParams createSafetyCheckParams() const; void updateSafetyCheckTargetObjectsData( const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index cfdc968656353..d6c196d00c862 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -253,12 +253,25 @@ void GoalPlannerModule::initializeOccupancyGridMap() occupancy_grid_map_->setParam(occupancy_grid_map_param); } +void GoalPlannerModule::initializeSafetyCheckParameters() +{ + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); +} + void GoalPlannerModule::processOnEntry() { // Initialize occupancy grid map if (parameters_->use_occupancy_grid) { initializeOccupancyGridMap(); } + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + } } void GoalPlannerModule::processOnExit() @@ -355,15 +368,11 @@ bool GoalPlannerModule::isExecutionReady() const } if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { - utils::start_goal_planner_common::updateEgoPredictedPathParams( - ego_predicted_path_params_, parameters_); - utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); - utils::start_goal_planner_common::updateObjectsFilteringParams( - objects_filtering_params_, parameters_); if (!isSafePath()) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); return false; } + std::cerr << "path is safe against dynamic objects" << std::endl; } return true; } @@ -1098,6 +1107,10 @@ bool GoalPlannerModule::incrementPathIndex() PathWithLaneId GoalPlannerModule::getCurrentPath() const { + if (status_.pull_over_path == nullptr) { + return PathWithLaneId{}; + } + if (status_.pull_over_path->partial_paths.size() <= status_.current_path_idx) { return PathWithLaneId{}; } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index f21eb0938dbd2..e20c5a12437f8 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -188,23 +188,22 @@ BehaviorModuleOutput StartPlannerModule::plan() return output; } - status_.is_safe_dynamic_objects = isSafePath(); - - if (status_.is_safe_dynamic_objects) { - const auto distance_to_stop_point = utils::start_goal_planner_common::calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop, - 0); - if (distance_to_stop_point) { - const auto output = generateStopOutput(); - RCLCPP_ERROR_THROTTLE( - getLogger(), *clock_, 5000, "dangerous against dynamic object. publish stop path"); - return output; - } else { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, - "Failed to generate feasible stop point. So don't stop but path is not safe."); - } - } + // if (!isSafePath()) { + // const auto distance_to_stop_point = + // utils::start_goal_planner_common::calcFeasibleDecelDistance( + // planner_data_, parameters_->maximum_deceleration_for_stop, + // parameters_->maximum_jerk_for_stop, 0.0); + // if (distance_to_stop_point) { + // const auto output = generateStopOutput(); + // RCLCPP_ERROR_THROTTLE( + // getLogger(), *clock_, 5000, "dangerous against dynamic object. publish stop path"); + // return output; + // } else { + // RCLCPP_WARN_THROTTLE( + // getLogger(), *clock_, 5000, + // "Failed to generate feasible stop point. So don't stop but path is not safe."); + // } + // } if (isWaitingApproval()) { clearWaitingApproval(); From 8768b35b964cd9431da9de7ce677d88b9727d144 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 8 Sep 2023 15:10:19 +0900 Subject: [PATCH 3/9] insert stop point after approval Signed-off-by: kyoichi-sugahara --- .../goal_planner/goal_planner_module.hpp | 4 + .../goal_planner/goal_planner_module.cpp | 80 ++++++++++++++++--- 2 files changed, 74 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 83a5cc9e6e417..7ac47037aa9d8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -81,6 +81,7 @@ struct PullOverStatus size_t current_path_idx{0}; bool require_increment_{true}; // if false, keep current path idx. std::shared_ptr prev_stop_path{nullptr}; + std::shared_ptr prev_stop_path_after_approval{nullptr}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets pull_over_lanes{}; std::vector lanes{}; // current + pull_over @@ -88,6 +89,7 @@ struct PullOverStatus bool is_safe_static_objects{false}; // current path is safe against static objects bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects bool prev_is_safe{false}; + bool prev_is_safe_dynamic_objects{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; bool is_ready{false}; @@ -230,6 +232,7 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); + PathWithLaneId insertStopPointInCurrentPath(); void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; @@ -277,6 +280,7 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output); void setStopPath(BehaviorModuleOutput & output); + void setStopPathInCurrentPath(BehaviorModuleOutput & output); void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) const; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index d6c196d00c862..09aca00e0ea84 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -664,18 +664,24 @@ void GoalPlannerModule::setLanes() void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { if (status_.is_safe_static_objects) { - // clear stop pose when the path is safe and activated - if (isActivated()) { - resetWallPoses(); - } + if (isSafePath()) { + // clear stop pose when the path is safe against static objects and activated + if (isActivated()) { + resetWallPoses(); + } + // keep stop if not enough time passed, + // because it takes time for the trajectory to be reflected + auto current_path = getCurrentPath(); + keepStoppedWithCurrentPath(current_path); - // keep stop if not enough time passed, - // because it takes time for the trajectory to be reflected - auto current_path = getCurrentPath(); - keepStoppedWithCurrentPath(current_path); + output.path = std::make_shared(current_path); + output.reference_path = getPreviousModuleOutput().reference_path; + } else { + // situation : not safe against dynamic objects after approval + // insert stop point in original path + setStopPathInCurrentPath(output); + } - output.path = std::make_shared(current_path); - output.reference_path = getPreviousModuleOutput().reference_path; } else { // not safe: use stop_path setStopPath(output); @@ -693,6 +699,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. status_.prev_is_safe = status_.is_safe_static_objects; + status_.prev_is_safe_dynamic_objects = status_.is_safe_dynamic_objects; } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) @@ -721,6 +728,29 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) } } +void GoalPlannerModule::setStopPathInCurrentPath(BehaviorModuleOutput & output) +{ + if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) { + // safe -> not_safe or no prev_stop_path: generate new stop_path + // generate 感だす。 + output.path = std::make_shared(insertStopPointInCurrentPath()); + output.reference_path = getPreviousModuleOutput().reference_path; + status_.prev_stop_path_after_approval = output.path; + // set stop path as pull over path + mutex_.lock(); + last_path_update_time_ = std::make_unique(clock_->now()); + mutex_.unlock(); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Found approved pull_over path is not safe, generate stop path"); + } else { + // not_safe -> not_safe: use previous stop path + output.path = status_.prev_stop_path_after_approval; + output.reference_path = getPreviousModuleOutput().reference_path; + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Found approved pull_over path is not safe, generate stop path"); + } +} + void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { @@ -1080,6 +1110,36 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() return stop_path; } +PathWithLaneId GoalPlannerModule::insertStopPointInCurrentPath() +{ + const auto & current_pose = planner_data_->self_odometry->pose.pose; + + if (status_.current_lanes.empty()) { + return PathWithLaneId{}; + } + + // get current path + auto current_path = getCurrentPath(); + + // try to insert stop point in current_path after approval + // but if can't stop with constraints(maximum deceleration, maximum jerk), don't insert stop point + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + if (!min_stop_distance) { + return current_path; + } + + // set stop point + const auto stop_idx = + motion_utils::insertStopPoint(current_pose, *min_stop_distance, current_path.points); + + if (stop_idx) { + stop_pose_ = current_path.points.at(*stop_idx).point.pose; + } + + return current_path; +} + void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { if (isActivated() && last_approved_time_ != nullptr) { From ba5ce258737ad84190cc9af57d23f486ff16737f Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 8 Sep 2023 15:21:34 +0900 Subject: [PATCH 4/9] add description Signed-off-by: kyoichi-sugahara --- .../goal_planner/goal_planner_module.hpp | 23 ++++++++++++++++++- .../goal_planner/goal_planner_module.cpp | 11 ++++----- .../start_planner/start_planner_module.cpp | 17 -------------- 3 files changed, 27 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 7ac47037aa9d8..874cdec3f1378 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -232,7 +232,18 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); - PathWithLaneId insertStopPointInCurrentPath(); + + /** + * @brief Generate a stop point in the current path based on the vehicle's pose and constraints. + * + * This function generates a stop point in the current path. If no lanes are available or if + * stopping is not feasible due to constraints (maximum deceleration, maximum jerk), no stop point + * is inserted into the path. + * + * @return PathWithLaneId The modified path with the stop point inserted. If no stop point is + * inserted, returns the original path. + */ + PathWithLaneId generateStopPointInCurrentPath(); void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; @@ -280,6 +291,16 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output); void setStopPath(BehaviorModuleOutput & output); + + /** + * @brief Sets a stop path in the current path based on safety conditions and previous paths. + * + * This function sets a stop path in the current path. Depending on whether the previous safety + * judgement against dynamic objects were safe or if a previous stop path existed, it either + * generates a new stop path or uses the previous stop path. + * + * @param output BehaviorModuleOutput + */ void setStopPathInCurrentPath(BehaviorModuleOutput & output); void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) const; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 09aca00e0ea84..e8a2488a1a27f 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -372,7 +372,6 @@ bool GoalPlannerModule::isExecutionReady() const RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); return false; } - std::cerr << "path is safe against dynamic objects" << std::endl; } return true; } @@ -665,7 +664,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { if (status_.is_safe_static_objects) { if (isSafePath()) { - // clear stop pose when the path is safe against static objects and activated + // clear stop pose when the path is safe against static/dynamic objects and activated if (isActivated()) { resetWallPoses(); } @@ -678,7 +677,8 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) output.reference_path = getPreviousModuleOutput().reference_path; } else { // situation : not safe against dynamic objects after approval - // insert stop point in original path + // insert stop point in current path if ego is able to stop with acceleration and jerk + // constraints setStopPathInCurrentPath(output); } @@ -732,8 +732,7 @@ void GoalPlannerModule::setStopPathInCurrentPath(BehaviorModuleOutput & output) { if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) { // safe -> not_safe or no prev_stop_path: generate new stop_path - // generate 感だす。 - output.path = std::make_shared(insertStopPointInCurrentPath()); + output.path = std::make_shared(generateStopPointInCurrentPath()); output.reference_path = getPreviousModuleOutput().reference_path; status_.prev_stop_path_after_approval = output.path; // set stop path as pull over path @@ -1110,7 +1109,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() return stop_path; } -PathWithLaneId GoalPlannerModule::insertStopPointInCurrentPath() +PathWithLaneId GoalPlannerModule::generateStopPointInCurrentPath() { const auto & current_pose = planner_data_->self_odometry->pose.pose; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index e20c5a12437f8..e8acc77113959 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -188,23 +188,6 @@ BehaviorModuleOutput StartPlannerModule::plan() return output; } - // if (!isSafePath()) { - // const auto distance_to_stop_point = - // utils::start_goal_planner_common::calcFeasibleDecelDistance( - // planner_data_, parameters_->maximum_deceleration_for_stop, - // parameters_->maximum_jerk_for_stop, 0.0); - // if (distance_to_stop_point) { - // const auto output = generateStopOutput(); - // RCLCPP_ERROR_THROTTLE( - // getLogger(), *clock_, 5000, "dangerous against dynamic object. publish stop path"); - // return output; - // } else { - // RCLCPP_WARN_THROTTLE( - // getLogger(), *clock_, 5000, - // "Failed to generate feasible stop point. So don't stop but path is not safe."); - // } - // } - if (isWaitingApproval()) { clearWaitingApproval(); resetPathCandidate(); From dbc6b460638d80ae3c6c596ce2e45b6fca964be1 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Sat, 9 Sep 2023 01:06:22 +0900 Subject: [PATCH 5/9] comment-aligned changes Signed-off-by: kyoichi-sugahara --- .../goal_planner/goal_planner_module.hpp | 2 +- .../goal_planner/goal_planner_module.cpp | 17 ++++++++--------- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 874cdec3f1378..3cddfe8fcef1a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -243,7 +243,7 @@ class GoalPlannerModule : public SceneModuleInterface * @return PathWithLaneId The modified path with the stop point inserted. If no stop point is * inserted, returns the original path. */ - PathWithLaneId generateStopPointInCurrentPath(); + PathWithLaneId generateStopInsertedCurrentPath(); void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index e8a2488a1a27f..ef1d8e199919a 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -706,7 +706,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) { 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()); + output.path = std::make_shared(generateStopInsertedCurrentPath()); output.reference_path = getPreviousModuleOutput().reference_path; status_.prev_stop_path = output.path; // set stop path as pull over path @@ -732,22 +732,21 @@ void GoalPlannerModule::setStopPathInCurrentPath(BehaviorModuleOutput & output) { if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) { // safe -> not_safe or no prev_stop_path: generate new stop_path - output.path = std::make_shared(generateStopPointInCurrentPath()); + output.path = std::make_shared(generateStopInsertedCurrentPath()); output.reference_path = getPreviousModuleOutput().reference_path; status_.prev_stop_path_after_approval = output.path; // set stop path as pull over path - mutex_.lock(); - last_path_update_time_ = std::make_unique(clock_->now()); - mutex_.unlock(); - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Found approved pull_over path is not safe, generate stop path"); + { + std::lock_guard lock(mutex_); + last_path_update_time_ = std::make_unique(clock_->now()); + } } else { // not_safe -> not_safe: use previous stop path output.path = status_.prev_stop_path_after_approval; output.reference_path = getPreviousModuleOutput().reference_path; - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Found approved pull_over path is not safe, generate stop path"); } + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Found approved pull_over path is not safe, generate stop path"); } void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const From 938ac7910189c1cbd2cf1d122f415b6f94f251f5 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Sat, 9 Sep 2023 01:24:01 +0900 Subject: [PATCH 6/9] comment-aligned changes Signed-off-by: kyoichi-sugahara --- .../scene_module/goal_planner/goal_planner_module.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index ef1d8e199919a..eb66ffddb8653 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -675,7 +675,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) output.path = std::make_shared(current_path); output.reference_path = getPreviousModuleOutput().reference_path; - } else { + } else if (status_.has_decided_path && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints @@ -733,7 +733,6 @@ void GoalPlannerModule::setStopPathInCurrentPath(BehaviorModuleOutput & output) if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopInsertedCurrentPath()); - output.reference_path = getPreviousModuleOutput().reference_path; status_.prev_stop_path_after_approval = output.path; // set stop path as pull over path { @@ -743,8 +742,8 @@ void GoalPlannerModule::setStopPathInCurrentPath(BehaviorModuleOutput & output) } else { // not_safe -> not_safe: use previous stop path output.path = status_.prev_stop_path_after_approval; - output.reference_path = getPreviousModuleOutput().reference_path; } + output.reference_path = getPreviousModuleOutput().reference_path; RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Found approved pull_over path is not safe, generate stop path"); } @@ -1111,14 +1110,12 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() PathWithLaneId GoalPlannerModule::generateStopPointInCurrentPath() { const auto & current_pose = planner_data_->self_odometry->pose.pose; + auto current_path = getCurrentPath(); - if (status_.current_lanes.empty()) { + if (status_.current_lanes.empty() || current_path.points.empty()) { return PathWithLaneId{}; } - // get current path - auto current_path = getCurrentPath(); - // try to insert stop point in current_path after approval // but if can't stop with constraints(maximum deceleration, maximum jerk), don't insert stop point const auto min_stop_distance = calcFeasibleDecelDistance( From 36806e53bd04346676a889c48674f0fbcbdda964 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Sat, 9 Sep 2023 18:55:05 +0900 Subject: [PATCH 7/9] refactor Signed-off-by: kyoichi-sugahara --- .../goal_planner/goal_planner_module.hpp | 19 ++++---- .../goal_planner/goal_planner_module.cpp | 48 ++++++++++--------- 2 files changed, 35 insertions(+), 32 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 3cddfe8fcef1a..12a2d8dc71983 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -82,12 +82,13 @@ struct PullOverStatus bool require_increment_{true}; // if false, keep current path idx. std::shared_ptr prev_stop_path{nullptr}; std::shared_ptr prev_stop_path_after_approval{nullptr}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets pull_over_lanes{}; - std::vector lanes{}; // current + pull_over - bool has_decided_path{false}; - bool is_safe_static_objects{false}; // current path is safe against static objects - bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects + // stop path after approval, stop path is not updated until safety is confirmed + lanelet::ConstLanelets current_lanes{}; // TODO(someone): explain + lanelet::ConstLanelets pull_over_lanes{}; // TODO(someone): explain + std::vector lanes{}; // current + pull_over + bool has_decided_path{false}; // if true, the path has is decided and safe against static objects + bool is_safe_static_objects{false}; // current path is safe against *static* objects + bool is_safe_dynamic_objects{false}; // current path is safe against *dynamic* objects bool prev_is_safe{false}; bool prev_is_safe_dynamic_objects{false}; bool has_decided_velocity{false}; @@ -240,10 +241,10 @@ class GoalPlannerModule : public SceneModuleInterface * stopping is not feasible due to constraints (maximum deceleration, maximum jerk), no stop point * is inserted into the path. * - * @return PathWithLaneId The modified path with the stop point inserted. If no stop point is - * inserted, returns the original path. + * @return the modified path with the stop point inserted. If no feasible stop point can be + * determined, returns an empty optional. */ - PathWithLaneId generateStopInsertedCurrentPath(); + std::optional generateStopInsertedCurrentPath(); void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index eb66ffddb8653..f949a652679dd 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -706,8 +706,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) { 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(generateStopInsertedCurrentPath()); - output.reference_path = getPreviousModuleOutput().reference_path; + output.path = std::make_shared(generateStopPath()); status_.prev_stop_path = output.path; // set stop path as pull over path mutex_.lock(); @@ -722,30 +721,34 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) } else { // not_safe -> not_safe: use previous stop path output.path = status_.prev_stop_path; - output.reference_path = getPreviousModuleOutput().reference_path; RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } + output.reference_path = getPreviousModuleOutput().reference_path; } void GoalPlannerModule::setStopPathInCurrentPath(BehaviorModuleOutput & output) { + // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) { - // safe -> not_safe or no prev_stop_path: generate new stop_path - output.path = std::make_shared(generateStopInsertedCurrentPath()); - status_.prev_stop_path_after_approval = output.path; - // set stop path as pull over path - { - std::lock_guard lock(mutex_); - last_path_update_time_ = std::make_unique(clock_->now()); + if (const auto stop_inserted_path = generateStopInsertedCurrentPath()) { + output.path = std::make_shared(*stop_inserted_path); + status_.prev_stop_path_after_approval = output.path; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); + } else { + output.path = std::make_shared(getCurrentPath()); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + "Collision detected, no feasible stop path found, cannot stop."); } + std::lock_guard lock(mutex_); + last_path_update_time_ = std::make_unique(clock_->now()); } else { - // not_safe -> not_safe: use previous stop path + // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path output.path = status_.prev_stop_path_after_approval; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); } output.reference_path = getPreviousModuleOutput().reference_path; - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Found approved pull_over path is not safe, generate stop path"); } void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const @@ -1107,13 +1110,10 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() return stop_path; } -PathWithLaneId GoalPlannerModule::generateStopPointInCurrentPath() +std::optional GoalPlannerModule::generateStopInsertedCurrentPath() { - const auto & current_pose = planner_data_->self_odometry->pose.pose; - auto current_path = getCurrentPath(); - - if (status_.current_lanes.empty() || current_path.points.empty()) { - return PathWithLaneId{}; + if (getCurrentPath().points.empty()) { + return {}; } // try to insert stop point in current_path after approval @@ -1121,14 +1121,16 @@ PathWithLaneId GoalPlannerModule::generateStopPointInCurrentPath() const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return current_path; + return {}; } // set stop point - const auto stop_idx = - motion_utils::insertStopPoint(current_pose, *min_stop_distance, current_path.points); + const auto stop_idx = motion_utils::insertStopPoint( + planner_data_->self_odometry->pose.pose, *min_stop_distance, current_path.points); - if (stop_idx) { + if (!stop_idx) { + return {}; + } else { stop_pose_ = current_path.points.at(*stop_idx).point.pose; } From afd6013d1403335713fc1c3e21b5ad7f62d8f62f Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Sat, 9 Sep 2023 23:56:22 +0900 Subject: [PATCH 8/9] fix build error Signed-off-by: kyoichi-sugahara --- .../src/scene_module/goal_planner/goal_planner_module.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f949a652679dd..07736f5bcbadc 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1112,7 +1112,8 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() std::optional GoalPlannerModule::generateStopInsertedCurrentPath() { - if (getCurrentPath().points.empty()) { + auto current_path = getCurrentPath(); + if (current_path.points.empty()) { return {}; } From c288508a6ab7c92c67b5fbc37d8411a7516a4580 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 12 Sep 2023 13:20:23 +0900 Subject: [PATCH 9/9] modify function name Signed-off-by: kyoichi-sugahara --- .../scene_module/goal_planner/goal_planner_module.hpp | 2 +- .../src/scene_module/goal_planner/goal_planner_module.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 12a2d8dc71983..a6f9565c08aae 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -302,7 +302,7 @@ class GoalPlannerModule : public SceneModuleInterface * * @param output BehaviorModuleOutput */ - void setStopPathInCurrentPath(BehaviorModuleOutput & output); + void setStopPathFromCurrentPath(BehaviorModuleOutput & output); void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) const; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 612d9c3ec3bcc..d4d9555182441 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -678,7 +678,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints - setStopPathInCurrentPath(output); + setStopPathFromCurrentPath(output); } } else { @@ -726,7 +726,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) output.reference_path = getPreviousModuleOutput().reference_path; } -void GoalPlannerModule::setStopPathInCurrentPath(BehaviorModuleOutput & output) +void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) { // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) {