Merge branch 'main' into fix/goal_planner_recursive_mutex

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<PullOverPath>, pull_over_path_candidates)
   DEFINE_SETTER_GETTER(std::optional<Pose>, closest_start_pose)
 
-  void push_goal_candidate(const GoalCandidate & goal_candidate)
-  {
-    std::lock_guard<std::recursive_mutex> lock(mutex_);
-    goal_candidates_.push_back(goal_candidate);
-  }
-
 private:
   std::shared_ptr<PullOverPath> pull_over_path_{nullptr};
   std::shared_ptr<PullOverPath> 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<GoalPlannerParameters> & parameters);
   void acceptVisitor(
     [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & 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_;
 

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<PathWithLaneId>(status_.get_pull_over_path()->getFullPath());
+  path_candidate_ = std::make_shared<PathWithLaneId>(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<PathWithLaneId>(generateStopPath());
   out.reference_path = getPreviousModuleOutput().reference_path;
-  path_candidate_ =
-    status_.get_is_safe_static_objects()
-      ? std::make_shared<PathWithLaneId>(status_.get_pull_over_path()->getFullPath())
-      : out.path;
+  path_candidate_ = std::make_shared<PathWithLaneId>(planCandidate().path_candidate);
   path_reference_ = getPreviousModuleOutput().reference_path;
   const auto distance_to_path_change = calcDistanceToPathChange();
 

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

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
 

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<double>("update_rate");
+  add_duplicated_node_names_to_msg_ = declare_parameter<bool>("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<std::string> 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);
     }

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