feat(behavior_path_planner): run fixed_goal_planner with other modules simutaneously

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -180,7 +180,7 @@ class GoalPlannerModule : public SceneModuleInterface
   bool isStopped(
     std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer, const double time);
   bool hasFinishedCurrentPath();
-  bool hasFinishedPullOver();
+  bool hasFinishedGoalPlanner();
   void updateOccupancyGrid();
   void resetStatus();
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -369,6 +369,26 @@ class SceneModuleInterface
 
   rclcpp::Logger getLogger() const { return logger_; }
 
+  void setIsSimultaneousExecutableAsApprovedModule(const bool enable)
+  {
+    is_simultaneously_executable_as_approved_module_ = enable;
+  }
+
+  bool isSimultaneousExecutableAsApprovedModule() const
+  {
+    return is_simultaneously_executable_as_approved_module_;
+  }
+
+  void setIsSimultaneousExecutableAsCandidateModule(const bool enable)
+  {
+    is_simultaneously_executable_as_candidate_module_ = enable;
+  }
+
+  bool isSimultaneousExecutableAsCandidateModule() const
+  {
+    return is_simultaneously_executable_as_candidate_module_;
+  }
+
 private:
   std::string name_;
 
@@ -453,6 +473,9 @@ class SceneModuleInterface
     return expanded_lanes;
   }
 
+  bool is_simultaneously_executable_as_approved_module_{false};
+  bool is_simultaneously_executable_as_candidate_module_{false};
+
   rclcpp::Clock::SharedPtr clock_;
 
   std::shared_ptr<const PlannerData> planner_data_;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp

@@ -93,6 +93,10 @@ class SceneModuleManagerInterface
   void registerNewModule(
     const SceneModulePtr & module_ptr, const BehaviorModuleOutput & previous_module_output)
   {
+    module_ptr->setIsSimultaneousExecutableAsApprovedModule(
+      enable_simultaneous_execution_as_approved_module_);
+    module_ptr->setIsSimultaneousExecutableAsCandidateModule(
+      enable_simultaneous_execution_as_candidate_module_);
     module_ptr->setData(planner_data_);
     module_ptr->setPreviousModuleOutput(previous_module_output);
     module_ptr->onEntry();
@@ -187,12 +191,18 @@ class SceneModuleManagerInterface
 
   bool isSimultaneousExecutableAsApprovedModule() const
   {
-    return enable_simultaneous_execution_as_approved_module_;
+    return std::all_of(
+      registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) {
+        return module->isSimultaneousExecutableAsApprovedModule();
+      });
   }
 
   bool isSimultaneousExecutableAsCandidateModule() const
   {
-    return enable_simultaneous_execution_as_candidate_module_;
+    return std::all_of(
+      registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) {
+        return module->isSimultaneousExecutableAsCandidateModule();
+      });
   }
 
   void setData(const std::shared_ptr<PlannerData> & planner_data) { planner_data_ = planner_data; }

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp

@@ -32,8 +32,10 @@ class DefaultFixedGoalPlanner : public FixedGoalPlannerBase
   BehaviorModuleOutput plan(const std::shared_ptr<const PlannerData> & planner_data) const override;
 
 protected:
+#ifdef USE_OLD_ARCHITECTURE
   boost::optional<BehaviorModuleOutput> getReferencePath(
     const std::shared_ptr<const PlannerData> & planner_data) const;
+#endif
   PathWithLaneId modifyPathForSmoothGoalConnection(
     const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data) const;
 };

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp

@@ -39,6 +39,16 @@ class FixedGoalPlannerBase
 
   virtual BehaviorModuleOutput plan(
     const std::shared_ptr<const PlannerData> & planner_data) const = 0;
+
+  void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output)
+  {
+    previous_module_output_ = previous_module_output;
+  }
+
+  BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; }
+
+protected:
+  BehaviorModuleOutput previous_module_output_;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -442,6 +442,11 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const
 
 ModuleStatus GoalPlannerModule::updateState()
 {
+  // finish module only when the goal is fixed
+  if (!allow_goal_modification_ && hasFinishedGoalPlanner()) {
+    current_state_ = ModuleStatus::SUCCESS;
+  }
+
   // pull_out 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_;
@@ -510,6 +515,11 @@ BehaviorModuleOutput GoalPlannerModule::plan()
   if (allow_goal_modification_) {
     return planWithGoalModification();
   } else {
+    // for fixed goals, only minor path refinements are made,
+    // so other modules are always allowed to run.
+    setIsSimultaneousExecutableAsApprovedModule(true);
+    setIsSimultaneousExecutableAsCandidateModule(true);
+    fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput());
     return fixed_goal_planner_->plan(planner_data_);
   }
 }
@@ -777,6 +787,11 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval()
   if (allow_goal_modification_) {
     return planWaitingApprovalWithGoalModification();
   } else {
+    // for fixed goals, only minor path refinements are made,
+    // so other modules are always allowed to run.
+    setIsSimultaneousExecutableAsApprovedModule(true);
+    setIsSimultaneousExecutableAsCandidateModule(true);
+    fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput());
     return fixed_goal_planner_->plan(planner_data_);
   }
 }
@@ -1022,12 +1037,13 @@ bool GoalPlannerModule::hasFinishedCurrentPath()
 
 bool GoalPlannerModule::isOnGoal() const
 {
-  const auto current_pose = planner_data_->self_odometry->pose.pose;
-  return calcDistance2d(current_pose, modified_goal_pose_->goal_pose) <
-         parameters_->th_arrived_distance;
+  const Pose current_pose = planner_data_->self_odometry->pose.pose;
+  const Pose goal_pose = modified_goal_pose_ ? modified_goal_pose_->goal_pose
+                                             : planner_data_->route_handler->getGoalPose();
+  return calcDistance2d(current_pose, goal_pose) < parameters_->th_arrived_distance;
 }
 
-bool GoalPlannerModule::hasFinishedPullOver()
+bool GoalPlannerModule::hasFinishedGoalPlanner()
 {
   return isOnGoal() && isStopped();
 }

planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp

@@ -29,18 +29,28 @@ namespace behavior_path_planner
 BehaviorModuleOutput DefaultFixedGoalPlanner::plan(
   const std::shared_ptr<const PlannerData> & planner_data) const
 {
-  auto output = getReferencePath(planner_data);
-  if (!output) {
-    return {};
-  }
+  BehaviorModuleOutput output =
+#ifdef USE_OLD_ARCHITECTURE
+    // generate reference path in this planner
+    std::invoke([this, &planner_data]() {
+      auto path = getReferencePath(planner_data);
+      if (!path) {
+        return BehaviorModuleOutput{};
+      }
+      return *path;
+    });
+#else
+    // use planner previous module reference path
+    getPreviousModuleOutput();
+#endif
   const PathWithLaneId smoothed_path =
-    modifyPathForSmoothGoalConnection(*(output->path), planner_data);
-
-  output->path = std::make_shared<PathWithLaneId>(smoothed_path);
-  output->reference_path = std::make_shared<PathWithLaneId>(smoothed_path);
-  return *output;
+    modifyPathForSmoothGoalConnection(*(output.path), planner_data);
+  output.path = std::make_shared<PathWithLaneId>(smoothed_path);
+  output.reference_path = std::make_shared<PathWithLaneId>(smoothed_path);
+  return output;
 }
 
+#ifdef USE_OLD_ARCHITECTURE
 boost::optional<BehaviorModuleOutput> DefaultFixedGoalPlanner::getReferencePath(
   const std::shared_ptr<const PlannerData> & planner_data) const
 {
@@ -61,6 +71,7 @@ boost::optional<BehaviorModuleOutput> DefaultFixedGoalPlanner::getReferencePath(
 
   return {};  // something wrong
 }
+#endif
 
 PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data) const