From cef67759866e3763cf46b3acd75f574e86a1cf8e Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 31 Jan 2025 11:59:41 +0900 Subject: [PATCH] feat(goal_planner): do not use isActivated() in deciding state transition Signed-off-by: Mamoru Sobue --- .../decision_state.hpp | 12 +++----- .../src/decision_state.cpp | 29 ++++++++----------- .../src/goal_planner_module.cpp | 13 ++------- 3 files changed, 19 insertions(+), 35 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp index df17becd10360..f4152d5da67b7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -49,15 +49,13 @@ class PathDecisionStateController * @brief update current state and save old current state to prev state */ void transit_state( - const bool found_pull_over_path, const rclcpp::Time & now, + const std::optional & pull_over_path_opt, const rclcpp::Time & now, const autoware_perception_msgs::msg::PredictedObjects & static_target_objects, const autoware_perception_msgs::msg::PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const GoalSearcher & goal_searcher, const bool is_activated, - const std::optional & pull_over_path, + const GoalSearcher & goal_searcher, std::vector & ego_polygons_expanded); PathDecisionState get_current_state() const { return current_state_; } @@ -71,14 +69,12 @@ class PathDecisionStateController * @brief update current state and save old current state to prev state */ PathDecisionState get_next_state( - const bool found_pull_over_path, const rclcpp::Time & now, + const std::optional & pull_over_path_opt, const rclcpp::Time & now, const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const GoalSearcher & goal_searcher, const bool is_activated, - const std::optional & pull_over_path_opt, + const GoalSearcher & goal_searcher, std::vector & ego_polygons_expanded) const; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp index cdd2a474afda6..fbaeb1ca52f59 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp @@ -26,32 +26,27 @@ namespace autoware::behavior_path_planner using autoware::motion_utils::calcSignedArcLength; void PathDecisionStateController::transit_state( - const bool found_pull_over_path, const rclcpp::Time & now, + const std::optional & pull_over_path_opt, const rclcpp::Time & now, const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const GoalSearcher & goal_searcher, const bool is_activated, - const std::optional & pull_over_path, + const GoalSearcher & goal_searcher, std::vector & ego_polygons_expanded) { const auto next_state = get_next_state( - found_pull_over_path, now, static_target_objects, dynamic_target_objects, modified_goal_opt, - planner_data, occupancy_grid_map, is_current_safe, parameters, goal_searcher, is_activated, - pull_over_path, ego_polygons_expanded); + pull_over_path_opt, now, static_target_objects, dynamic_target_objects, planner_data, + occupancy_grid_map, is_current_safe, parameters, goal_searcher, ego_polygons_expanded); current_state_ = next_state; } PathDecisionState PathDecisionStateController::get_next_state( - const bool found_pull_over_path, const rclcpp::Time & now, + const std::optional & pull_over_path_opt, const rclcpp::Time & now, const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const GoalSearcher & goal_searcher, const bool is_activated, - const std::optional & pull_over_path_opt, + const GoalSearcher & goal_searcher, std::vector & ego_polygons_expanded) const { auto next_state = current_state_; @@ -77,7 +72,7 @@ PathDecisionState PathDecisionStateController::get_next_state( } // if path is not safe, not decided - if (!found_pull_over_path || !pull_over_path_opt) { + if (!pull_over_path_opt) { next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; return next_state; } @@ -85,7 +80,7 @@ PathDecisionState PathDecisionStateController::get_next_state( const auto & pull_over_path = pull_over_path_opt.value(); // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved - if (!next_state.is_stable_safe && !is_activated) { + if (!next_state.is_stable_safe) { RCLCPP_DEBUG( logger_, "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " @@ -98,11 +93,11 @@ PathDecisionState PathDecisionStateController::get_next_state( if (current_state_.state == PathDecisionState::DecisionKind::DECIDING) { const double hysteresis_factor = 0.9; + const auto & modified_goal = pull_over_path.modified_goal(); // check goal pose collision - if ( - modified_goal_opt && !goal_searcher.isSafeGoalWithMarginScaleFactor( - modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, - planner_data, static_target_objects)) { + if (!goal_searcher.isSafeGoalWithMarginScaleFactor( + modified_goal, hysteresis_factor, occupancy_grid_map, planner_data, + static_target_objects)) { RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; next_state.deciding_start_time = std::nullopt; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index ef22194e90a74..43656a43888ae 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -761,13 +761,6 @@ void GoalPlannerModule::updateData() ? std::make_optional(context_data_.value().pull_over_path_opt.value()) : std::nullopt; - const auto modified_goal_pose = [&]() -> std::optional { - if (!pull_over_path_recv) { - return std::nullopt; - } - return pull_over_path_recv.value().modified_goal(); - }(); - // save "old" state const auto prev_decision_state = path_decision_controller_.get_current_state(); const auto [is_current_safe, collision_check_map] = @@ -782,9 +775,9 @@ void GoalPlannerModule::updateData() dynamic_target_objects, parameters_.th_moving_object_velocity); path_decision_controller_.transit_state( - found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, - modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, parameters_, - goal_searcher, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); + pull_over_path_recv, clock_->now(), static_target_objects, dynamic_target_objects, + planner_data_, occupancy_grid_map_, is_current_safe, parameters_, goal_searcher, + debug_data_.ego_polygons_expanded); auto [lane_parking_response, freespace_parking_response] = syncWithThreads(); if (context_data_) {