feat(behavior path): select excutable behavior

planning/behavior_path_planner/CMakeLists.txt

@@ -34,6 +34,7 @@ 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,3 +9,8 @@
     drivable_area_height: 50.0
     refine_goal_search_radius_range: 7.5
     intersection_search_distance: 30.0
+    enable_lane_change: true
+    enable_side_sift: true
+    enable_avoidance: true
+    enable_pull_over: true
+    enable_pull_out: true

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -18,6 +18,7 @@
 #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"
@@ -119,6 +120,10 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   void onMap(const HADMapBin::ConstSharedPtr map_msg);
   void onRoute(const HADMapRoute::ConstSharedPtr route_msg);
 
+  //! register new module
+  template <typename T, typename P>
+  void registerModule(const bool enable, 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/scene_module/empty/empty_module.hpp

@@ -0,0 +1,52 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__EMPTY_MODULE__EMPTY_MODULE_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__EMPTY_MODULE__EMPTY_MODULE_HPP_
+
+#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <route_handler/route_handler.hpp>
+
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+
+#include <memory>
+#include <string>
+
+namespace behavior_path_planner
+{
+struct EmptyParameters
+{
+};
+class EmptyModule : public SceneModuleInterface
+{
+public:
+  EmptyModule(const std::string & name, rclcpp::Node & node, const EmptyParameters & parameters);
+  bool isExecutionRequested() const override;
+  bool isExecutionReady() const override;
+  BT::NodeStatus updateState() override;
+  BehaviorModuleOutput plan() override;
+  BehaviorModuleOutput planWaitingApproval() override;
+  PathWithLaneId planCandidate() const override;
+  void onEntry() override;
+  void onExit() override;
+
+private:
+  EmptyParameters parameters_;
+};
+
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__EMPTY_MODULE__EMPTY_MODULE_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -16,6 +16,7 @@
 
 #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"
@@ -84,38 +85,24 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
 
   // behavior tree manager
   {
-    bt_manager_ = std::make_shared<BehaviorTreeManager>(*this, getBehaviorTreeManagerParam());
-
-    auto side_shift_module =
-      std::make_shared<SideShiftModule>("SideShift", *this, getSideShiftParam());
-    bt_manager_->registerSceneModule(side_shift_module);
-
-    auto avoidance_module =
-      std::make_shared<AvoidanceModule>("Avoidance", *this, getAvoidanceParam());
-    bt_manager_->registerSceneModule(avoidance_module);
-
-    auto lane_following_module =
-      std::make_shared<LaneFollowingModule>("LaneFollowing", *this, getLaneFollowingParam());
-    bt_manager_->registerSceneModule(lane_following_module);
+    // 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_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());
+    // lane following should always be enabled
+    registerModule<LaneFollowingModule>(true, "LaneFollowing", getLaneFollowingParam());
     const auto lane_change_param = getLaneChangeParam();
-
-    auto lane_change_module =
-      std::make_shared<LaneChangeModule>("LaneChange", *this, lane_change_param);
-    bt_manager_->registerSceneModule(lane_change_module);
-
-    auto force_lane_change_module =
-      std::make_shared<LaneChangeModule>("ForceLaneChange", *this, lane_change_param);
-    bt_manager_->registerSceneModule(force_lane_change_module);
-
+    registerModule<LaneChangeModule>(enable_lane_change, "LaneChange", lane_change_param);
+    registerModule<LaneChangeModule>(enable_lane_change, "ForceLaneChange", lane_change_param);
     bt_manager_->registerForceApproval("ForceLaneChange");
-
-    auto pull_over_module = std::make_shared<PullOverModule>("PullOver", *this, getPullOverParam());
-    bt_manager_->registerSceneModule(pull_over_module);
-
-    auto pull_out_module = std::make_shared<PullOutModule>("PullOut", *this, getPullOutParam());
-    bt_manager_->registerSceneModule(pull_out_module);
-
+    registerModule<PullOverModule>(enable_pull_over, "PullOver", getPullOverParam());
+    registerModule<PullOutModule>(enable_pull_out, "PullOut", getPullOutParam());
     bt_manager_->createBehaviorTree();
   }
 
@@ -140,6 +127,20 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   }
 }
 
+template <typename T, typename P>
+void BehaviorPathPlannerNode::registerModule(
+  const bool enable, 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);
+  }
+}
+
 BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
 {
   // ROS parameters

planning/behavior_path_planner/src/scene_module/empty/empty_module.cpp

@@ -0,0 +1,44 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "behavior_path_planner/scene_module/empty/empty_module.hpp"
+
+#include "behavior_path_planner/utilities.hpp"
+
+#include <memory>
+#include <string>
+
+namespace behavior_path_planner
+{
+EmptyModule::EmptyModule(
+  const std::string & name, rclcpp::Node & node, const EmptyParameters & parameters)
+: SceneModuleInterface{name, node}, parameters_{parameters}
+{
+  approval_handler_.clearWaitApproval();
+  current_state_ = BT::NodeStatus::IDLE;
+}
+bool EmptyModule::isExecutionRequested() const { return false; }
+bool EmptyModule::isExecutionReady() const { return false; }
+BT::NodeStatus EmptyModule::updateState() { return BT::NodeStatus::SUCCESS; }
+BehaviorModuleOutput EmptyModule::plan() { return BehaviorModuleOutput(); }
+PathWithLaneId EmptyModule::planCandidate() const
+{
+  PathWithLaneId path;
+  return path;
+}
+BehaviorModuleOutput EmptyModule::planWaitingApproval() { return BehaviorModuleOutput(); }
+void EmptyModule::onEntry() { RCLCPP_ERROR(getLogger(), "Dont Entry This"); }
+void EmptyModule::onExit() {}
+
+}  // namespace behavior_path_planner