feat(behavior path): selectable via request

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

planning/behavior_path_planner/CMakeLists.txt

@@ -34,7 +34,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/scene_module/pull_over/util.cpp
   src/scene_module/pull_out/pull_out_module.cpp
   src/scene_module/pull_out/util.cpp
-  src/scene_module/empty/empty_module.cpp
 )
 
 target_link_libraries(behavior_path_planner_node

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -9,6 +9,7 @@
     drivable_area_height: 50.0
     refine_goal_search_radius_range: 7.5
     intersection_search_distance: 30.0
+    # select necessary modules from below
     enable_lane_following: true
     enable_lane_change: true
     enable_side_sift: true

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -18,7 +18,6 @@
 #include "behavior_path_planner/behavior_tree_manager.hpp"
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
-#include "behavior_path_planner/scene_module/empty/empty_module.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp"
 #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp"
 #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"
@@ -122,7 +121,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 
   //! register new module
   template <typename T, typename P>
-  void registerModule(const bool enable, const std::string & name, const P & param);
+  void registerModule(const std::string & name, const P & param);
 
   /**
    * @brief Modify the path points near the goal to smoothly connect the lanelet and the goal point.

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -44,6 +44,14 @@ struct BehaviorPathPlannerParameters
   double right_over_hang;
   double base_link2front;
   double base_link2rear;
+
+  // is module ready
+  bool enable_lane_following{true};
+  bool enable_side_shift{true};
+  bool enable_avoidance{true};
+  bool enable_lane_change{true};
+  bool enable_pull_over{true};
+  bool enable_pull_out{true};
 };
 
 #endif  // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -16,7 +16,6 @@
 
 #include "behavior_path_planner/path_utilities.hpp"
 #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
-#include "behavior_path_planner/scene_module/empty/empty_module.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp"
 #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"
 #include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp"
@@ -85,25 +84,16 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
 
   // behavior tree manager
   {
-    // get enable parameter of behavior
-    const bool enable_side_sift = declare_parameter("enable_side_sift", true);
-    const bool enable_avoidance = declare_parameter("enable_avoidance", true);
-    const bool enable_lane_following = declare_parameter("enable_lane_following", true);
-    const bool enable_lane_change = declare_parameter("enable_lane_change", true);
-    const bool enable_pull_over = declare_parameter("enable_pull_over", true);
-    const bool enable_pull_out = declare_parameter("enable_pull_out", true);
-
     bt_manager_ = std::make_shared<BehaviorTreeManager>(*this, getBehaviorTreeManagerParam());
-    registerModule<SideShiftModule>(enable_side_sift, "SideShift", getSideShiftParam());
-    registerModule<AvoidanceModule>(enable_avoidance, "Avoidance", getAvoidanceParam());
-    registerModule<LaneFollowingModule>(
-      enable_lane_following, "LaneFollowing", getLaneFollowingParam());
-    const auto lane_change_param = getLaneChangeParam();
-    registerModule<LaneChangeModule>(enable_lane_change, "LaneChange", lane_change_param);
-    registerModule<LaneChangeModule>(enable_lane_change, "ForceLaneChange", lane_change_param);
+    registerModule<SideShiftModule>("SideShift", getSideShiftParam());
+    registerModule<AvoidanceModule>("Avoidance", getAvoidanceParam());
+    registerModule<LaneFollowingModule>("LaneFollowing", getLaneFollowingParam());
+    auto lane_change_param = getLaneChangeParam();
+    registerModule<LaneChangeModule>("LaneChange", lane_change_param);
+    registerModule<LaneChangeModule>("ForceLaneChange", lane_change_param);
     bt_manager_->registerForceApproval("ForceLaneChange");
-    registerModule<PullOverModule>(enable_pull_over, "PullOver", getPullOverParam());
-    registerModule<PullOutModule>(enable_pull_out, "PullOut", getPullOutParam());
+    registerModule<PullOverModule>("PullOver", getPullOverParam());
+    registerModule<PullOutModule>("PullOut", getPullOutParam());
     bt_manager_->createBehaviorTree();
   }
 
@@ -129,17 +119,10 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
 }
 
 template <typename T, typename P>
-void BehaviorPathPlannerNode::registerModule(
-  const bool enable, const std::string & name, const P & p)
+void BehaviorPathPlannerNode::registerModule(const std::string & name, const P & p)
 {
-  if (enable) {
-    const auto m = std::make_shared<T>(name, *this, p);
-    bt_manager_->registerSceneModule(m);
-  } else {
-    EmptyParameters ep;
-    auto e = std::make_shared<EmptyModule>(name, *this, ep);
-    bt_manager_->registerSceneModule(e);
-  }
+  const auto m = std::make_shared<T>(name, *this, p);
+  bt_manager_->registerSceneModule(m);
 }
 
 BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
@@ -177,6 +160,13 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.base_link2front = vehicle_info.max_longitudinal_offset_m;
   p.base_link2rear = p.rear_overhang;
 
+  // get enable parameter of behavior
+  p.enable_lane_change = declare_parameter("enable_lane_change", true);
+  p.enable_side_shift = declare_parameter("enable_side_shift", true);
+  p.enable_avoidance = declare_parameter("enable_avoidance", true);
+  p.enable_lane_following = declare_parameter("enable_lane_following", true);
+  p.enable_pull_over = declare_parameter("enable_pull_over", true);
+  p.enable_pull_out = declare_parameter("enable_pull_out", true);
   return p;
 }
 

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -57,6 +57,7 @@ AvoidanceModule::AvoidanceModule(
 
 bool AvoidanceModule::isExecutionRequested() const
 {
+  if (!planner_data_->parameters.enable_avoidance) return false;
   DEBUG_PRINT("AVOIDANCE isExecutionRequested");
 
   if (current_state_ == BT::NodeStatus::RUNNING) {

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -72,6 +72,7 @@ void LaneChangeModule::onExit()
 
 bool LaneChangeModule::isExecutionRequested() const
 {
+  if (!planner_data_->parameters.enable_lane_change) return false;
   if (current_state_ == BT::NodeStatus::RUNNING) {
     return true;
   }

planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp

@@ -33,7 +33,11 @@ void LaneFollowingModule::initParam()
   approval_handler_.clearWaitApproval();  // no need approval
 }
 
-bool LaneFollowingModule::isExecutionRequested() const { return true; }
+bool LaneFollowingModule::isExecutionRequested() const
+{
+  if (!planner_data_->parameters.enable_lane_following) return false;
+  return true;
+}
 
 bool LaneFollowingModule::isExecutionReady() const { return true; }
 

planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp

@@ -74,6 +74,7 @@ void PullOutModule::onExit()
 
 bool PullOutModule::isExecutionRequested() const
 {
+  if (!planner_data_->parameters.enable_pull_out) return false;
   if (current_state_ == BT::NodeStatus::RUNNING) {
     return true;
   }

planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp

@@ -71,6 +71,7 @@ void PullOverModule::onExit()
 
 bool PullOverModule::isExecutionRequested() const
 {
+  if (!planner_data_->parameters.enable_pull_over) return false;
   if (current_state_ == BT::NodeStatus::RUNNING) {
     return true;
   }

planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp

@@ -97,6 +97,7 @@ void SideShiftModule::setParameters(const SideShiftParameters & parameters)
 
 bool SideShiftModule::isExecutionRequested() const
 {
+  if (!planner_data_->parameters.enable_side_shift) return false;
   if (current_state_ == BT::NodeStatus::RUNNING) {
     return true;
   }