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 e310d2e70b7de..21372ed6e2c2f 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 @@ -145,12 +145,6 @@ class PullOverStatus DEFINE_SETTER_GETTER(std::vector, pull_over_path_candidates) DEFINE_SETTER_GETTER(std::optional, closest_start_pose) - void push_goal_candidate(const GoalCandidate & goal_candidate) - { - std::lock_guard lock(mutex_); - goal_candidates_.push_back(goal_candidate); - } - private: std::shared_ptr pull_over_path_{nullptr}; std::shared_ptr lane_parking_pull_over_path_{nullptr}; @@ -223,31 +217,28 @@ class GoalPlannerModule : public SceneModuleInterface } } - // TODO(someone): remove this, and use base class function - [[deprecated]] BehaviorModuleOutput run() override; - bool isExecutionRequested() const override; - bool isExecutionReady() const override; - // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; + CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; - void processOnEntry() override; + bool isExecutionRequested() const override; + bool isExecutionReady() const override; void processOnExit() override; + void updateData() override; void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override { } - // not used, but need to override - CandidateOutput planCandidate() const override { return CandidateOutput{}; }; - private: + // The start_planner activates when it receives a new route, + // so there is no need to terminate the goal planner. + // If terminating it, it may switch to lane following and could generate an inappropriate path. bool canTransitSuccessState() override { return false; } bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return false; } + bool canTransitIdleToRunningState() override { return true; } mutable StartGoalPlannerData goal_planner_data_; 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 179adc8ccebe8..95ac47494d016 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 @@ -113,6 +113,13 @@ GoalPlannerModule::GoalPlannerModule( freespace_parking_timer_cb_group_); } + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); + } + status_.reset(); } @@ -219,16 +226,18 @@ void GoalPlannerModule::onFreespaceParkingTimer() } } -BehaviorModuleOutput GoalPlannerModule::run() +void GoalPlannerModule::updateData() { - current_state_ = ModuleStatus::RUNNING; - updateOccupancyGrid(); - - if (!isActivated()) { - return planWaitingApproval(); + // Initialize Occupancy Grid Map + // This operation requires waiting for `planner_data_`, hence it is executed here instead of in + // the constructor. Ideally, this operation should only need to be performed once. + if ( + parameters_->use_occupancy_grid_for_goal_search || + parameters_->use_occupancy_grid_for_path_collision_check) { + initializeOccupancyGridMap(); } - return plan(); + updateOccupancyGrid(); } void GoalPlannerModule::initializeOccupancyGridMap() @@ -255,22 +264,6 @@ void GoalPlannerModule::initializeSafetyCheckParameters() objects_filtering_params_, parameters_); } -void GoalPlannerModule::processOnEntry() -{ - // Initialize occupancy grid map - if ( - parameters_->use_occupancy_grid_for_goal_search || - parameters_->use_occupancy_grid_for_path_collision_check) { - 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() { resetPathCandidate(); @@ -437,13 +430,6 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const return refined_goal_pose; } -ModuleStatus GoalPlannerModule::updateState() -{ - // start_planner module will be run when setting new goal, so not need finishing pull_over module. - // Finishing it causes wrong lane_following path generation. - return current_state_; -} - bool GoalPlannerModule::planFreespacePath() { goal_searcher_->setPlannerData(planner_data_); @@ -550,7 +536,9 @@ void GoalPlannerModule::generateGoalCandidates() GoalCandidate goal_candidate{}; goal_candidate.goal_pose = goal_pose; goal_candidate.distance_from_original_goal = 0.0; - status_.push_goal_candidate(goal_candidate); + GoalCandidates goal_candidates{}; + goal_candidates.push_back(goal_candidate); + status_.set_goal_candidates(goal_candidates); } } @@ -895,6 +883,12 @@ void GoalPlannerModule::decideVelocity() status_.set_has_decided_velocity(true); } +CandidateOutput GoalPlannerModule::planCandidate() const +{ + return CandidateOutput( + status_.get_pull_over_path() ? status_.get_pull_over_path()->getFullPath() : PathWithLaneId()); +} + BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() { constexpr double path_update_duration = 1.0; @@ -933,7 +927,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() // set output and status BehaviorModuleOutput output{}; setOutput(output); - path_candidate_ = std::make_shared(status_.get_pull_over_path()->getFullPath()); + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible @@ -986,10 +980,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = - status_.get_is_safe_static_objects() - ? std::make_shared(status_.get_pull_over_path()->getFullPath()) - : out.path; + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; const auto distance_to_path_change = calcDistanceToPathChange(); diff --git a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml index 5b8c019de5a20..54b4f691b6eb1 100644 --- a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml +++ b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml @@ -1,3 +1,4 @@ /**: ros__parameters: update_rate: 10.0 + add_duplicated_node_names_to_msg: false # if true, duplicated node names are added to msg diff --git a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp index ec086058e9041..9d806754f3c20 100644 --- a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp +++ b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp @@ -48,6 +48,7 @@ class DuplicatedNodeChecker : public rclcpp::Node diagnostic_updater::Updater updater_{this}; rclcpp::TimerBase::SharedPtr timer_; + bool add_duplicated_node_names_to_msg_; }; } // namespace duplicated_node_checker diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp index c9aa483724532..46c02d9d6e133 100644 --- a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -28,6 +28,7 @@ DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_op : Node("duplicated_node_checker", node_options) { double update_rate = declare_parameter("update_rate"); + add_duplicated_node_names_to_msg_ = declare_parameter("add_duplicated_node_names_to_msg"); updater_.setHardwareID("duplicated_node_checker"); updater_.add("duplicated_node_checker", this, &DuplicatedNodeChecker::produceDiagnostics); @@ -63,8 +64,14 @@ void DuplicatedNodeChecker::produceDiagnostics(diagnostic_updater::DiagnosticSta std::string msg; int level; if (identical_names.size() > 0) { - msg = "Error"; level = DiagnosticStatus::ERROR; + msg = "Error: Duplicated nodes detected"; + if (add_duplicated_node_names_to_msg_) { + std::set unique_identical_names(identical_names.begin(), identical_names.end()); + for (const auto & name : unique_identical_names) { + msg += " " + name; + } + } for (auto name : identical_names) { stat.add("Duplicated Node Name", name); } diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index c396e2e9f5ed8..9784259490ec2 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -40,6 +40,7 @@ /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/system/duplicated_node_checker: default # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } + # /autoware/system/duplicated_node_checker: default /autoware/vehicle/node_alive_monitoring: default