From 57b1ef1e17034d0e62b3decfc65f3ec5632cfdbf Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Tue, 12 Sep 2023 16:03:35 +0900 Subject: [PATCH] feat(behavior_path_planner): add processOnEntry in start_planner (#4953) * add process on entry in start_planner Signed-off-by: kyoichi-sugahara * delete description Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../goal_planner/goal_planner_module.hpp | 13 +++++---- .../start_planner/start_planner_module.hpp | 4 ++- .../goal_planner/goal_planner_module.cpp | 29 +++++++++++++------ .../start_planner/start_planner_module.cpp | 26 ++++++++++++----- 4 files changed, 49 insertions(+), 23 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..6ba90f54e48fe 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,12 +81,12 @@ 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}; - 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 + 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 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 has_decided_velocity{false}; bool has_requested_approval{false}; @@ -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/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index ba942da69732d..e9a94126d9f0e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -93,7 +93,7 @@ class StartPlannerModule : public SceneModuleInterface BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; - + void processOnEntry() override; void processOnExit() override; void setParameters(const std::shared_ptr & parameters) @@ -125,6 +125,8 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitIdleToRunningState() override { return false; } + void initializeSafetyCheckParameters(); + std::shared_ptr parameters_; mutable std::shared_ptr ego_predicted_path_params_; mutable std::shared_ptr objects_filtering_params_; 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 d55b3c173cc83..54a052c91f180 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 @@ -252,12 +252,27 @@ 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(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); + } } void GoalPlannerModule::processOnExit() @@ -354,11 +369,6 @@ 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; @@ -690,7 +700,6 @@ 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.reference_path = getPreviousModuleOutput().reference_path; status_.prev_stop_path = output.path; // set stop path as pull over path mutex_.lock(); @@ -705,10 +714,10 @@ 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::setDrivableAreaInfo(BehaviorModuleOutput & output) const @@ -1097,6 +1106,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{}; } @@ -1521,8 +1534,6 @@ bool GoalPlannerModule::isSafePath() const utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - goal_planner_data_.collision_check); bool is_safe_dynamic_objects = true; // Check for collisions with each predicted path of the object 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 9cc75a384192b..29e16fd570720 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 @@ -98,6 +98,16 @@ BehaviorModuleOutput StartPlannerModule::run() return plan(); } +void StartPlannerModule::processOnEntry() +{ + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + start_planner_data_.collision_check); + } +} + void StartPlannerModule::processOnExit() { resetPathCandidate(); @@ -149,11 +159,6 @@ bool StartPlannerModule::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; @@ -272,6 +277,15 @@ CandidateOutput StartPlannerModule::planCandidate() const return CandidateOutput{}; } +void StartPlannerModule::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_); +} + PathWithLaneId StartPlannerModule::getFullPath() const { // combine partial pull out path @@ -982,8 +996,6 @@ bool StartPlannerModule::isSafePath() const utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - start_planner_data_.collision_check); bool is_safe_dynamic_objects = true; // Check for collisions with each predicted path of the object