feat(behavior_path_planner): add new manager independent from behavio…

…r tree (autowarefoundation#2919)

* feat(data_manager): add member variable for new framework

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(utilities): add util function

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(behavior_path_planner): add framework that based on new architecture

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(behavior_path_planner): changeable framework by macro

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(behavior_path_planner): change framework in cmake

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(behavior_path_planner): fix cmake lint error

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(planner_manager): rename functions

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(planner_manager): not use ModuleID

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(planner_manager): use lambda

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(planner_manager): reserve vector

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(behavior_path_planner): fix copyright

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

* fix(behavior_path_planner): fix copyright

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

* fix(behavior_path_planner): fix copyright

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

planning/behavior_path_planner/CMakeLists.txt

@@ -7,22 +7,15 @@ autoware_package()
 find_package(OpenCV REQUIRED)
 find_package(magic_enum CONFIG REQUIRED)
 
-ament_auto_add_library(behavior_path_planner_node SHARED
-  src/behavior_path_planner_node.cpp
-  src/behavior_tree_manager.cpp
+# use planner manager that supports behavior tree
+set(USE_BT TRUE)
+message(STATUS "USE_BEHAVIOR_TREE: " ${USE_BT})
+
+set(common_src
   src/utilities.cpp
   src/path_utilities.cpp
   src/steering_factor_interface.cpp
   src/turn_signal_decider.cpp
-  src/scene_module/scene_module_bt_node_interface.cpp
-  src/scene_module/side_shift/side_shift_module.cpp
-  src/scene_module/avoidance/avoidance_module.cpp
-  src/scene_module/lane_following/lane_following_module.cpp
-  src/scene_module/lane_change/external_request_lane_change_module.cpp
-  src/scene_module/lane_change/lane_change_module.cpp
-  src/scene_module/pull_over/pull_over_module.cpp
-  src/scene_module/pull_out/pull_out_module.cpp
-  src/scene_module/scene_module_visitor.cpp
   src/util/avoidance/util.cpp
   src/util/lane_change/util.cpp
   src/util/side_shift/util.cpp
@@ -33,18 +26,52 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/util/pull_out/util.cpp
   src/util/pull_out/shift_pull_out.cpp
   src/util/pull_out/geometric_pull_out.cpp
-  src/util/geometric_parallel_parking/geometric_parallel_parking.cpp
-  src/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
   src/util/path_shifter/path_shifter.cpp
   src/util/drivable_area_expansion/drivable_area_expansion.cpp
   src/util/drivable_area_expansion/map_utils.cpp
   src/util/drivable_area_expansion/footprints.cpp
   src/util/drivable_area_expansion/expansion.cpp
+  src/util/geometric_parallel_parking/geometric_parallel_parking.cpp
+  src/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
   src/marker_util/debug_utilities.cpp
   src/marker_util/avoidance/debug.cpp
   src/marker_util/lane_change/debug.cpp
 )
 
+if(USE_BT)
+  ament_auto_add_library(behavior_path_planner_node SHARED
+    src/behavior_path_planner_node.cpp
+    src/behavior_tree_manager.cpp
+    src/scene_module/scene_module_visitor.cpp
+    src/scene_module/scene_module_bt_node_interface.cpp
+    src/scene_module/side_shift/side_shift_module.cpp
+    src/scene_module/avoidance/avoidance_module.cpp
+    src/scene_module/lane_following/lane_following_module.cpp
+    src/scene_module/lane_change/external_request_lane_change_module.cpp
+    src/scene_module/lane_change/lane_change_module.cpp
+    src/scene_module/pull_over/pull_over_module.cpp
+    src/scene_module/pull_out/pull_out_module.cpp
+    ${common_src}
+  )
+
+  target_compile_definitions(behavior_path_planner_node PRIVATE USE_BEHAVIOR_TREE)
+
+  message(WARNING "Build behavior_path_planner with BT...")
+
+else()
+  ament_auto_add_library(behavior_path_planner_node SHARED
+    src/behavior_path_planner_node.cpp
+    src/planner_manager.cpp
+    src/scene_module/scene_module_visitor.cpp
+    # src/scene_module_v2/avoidance
+    # src/scene_module_v2/lane_change
+    # src/scene_module_v2/...
+    ${common_src}
+  )
+
+  message(WARNING "Build behavior_path_planner without BT...")
+endif()
+
 target_include_directories(behavior_path_planner_node SYSTEM PUBLIC
   ${EIGEN3_INCLUDE_DIR}
 )

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -15,19 +15,28 @@
 #ifndef BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_
 #define BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_
 
-#include "behavior_path_planner/behavior_tree_manager.hpp"
 #include "behavior_path_planner/data_manager.hpp"
+
+#ifdef USE_BEHAVIOR_TREE
+#include "behavior_path_planner/behavior_tree_manager.hpp"
+#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
 #include "behavior_path_planner/scene_module/lane_change/external_request_lane_change_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"
 #include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp"
 #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
+#else
+#include "behavior_path_planner/planner_manager.hpp"
+#include "behavior_path_planner/scene_module_v2/scene_module_interface.hpp"
+#endif
+
 #include "behavior_path_planner/steering_factor_interface.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
 #include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp"
 
 #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp"
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path.hpp>
@@ -65,6 +74,7 @@ inline void update_param(
 
 namespace behavior_path_planner
 {
+using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_auto_mapping_msgs::msg::HADMapBin;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::Path;
@@ -97,6 +107,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Subscription<Scenario>::SharedPtr scenario_subscriber_;
   rclcpp::Subscription<PredictedObjects>::SharedPtr perception_subscriber_;
   rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_subscriber_;
+#ifndef USE_BEHAVIOR_TREE
+  rclcpp::Subscription<OperationModeState>::SharedPtr operation_mode_subscriber_;
+#endif
   rclcpp::Publisher<PathWithLaneId>::SharedPtr path_publisher_;
   rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr turn_signal_publisher_;
   rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_signal_publisher_;
@@ -105,9 +118,18 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr timer_;
 
   std::map<std::string, rclcpp::Publisher<Path>::SharedPtr> path_candidate_publishers_;
+#ifndef USE_BEHAVIOR_TREE
+  std::map<std::string, rclcpp::Publisher<Path>::SharedPtr> path_reference_publishers_;
+#endif
 
   std::shared_ptr<PlannerData> planner_data_;
+
+#ifdef USE_BEHAVIOR_TREE
   std::shared_ptr<BehaviorTreeManager> bt_manager_;
+#else
+  std::shared_ptr<PlannerManager> planner_manager_;
+#endif
+
   std::unique_ptr<SteeringFactorInterface> steering_factor_interface_ptr_;
   Scenario::SharedPtr current_scenario_{nullptr};
 
@@ -127,18 +149,23 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   // update planner data
   std::shared_ptr<PlannerData> createLatestPlannerData();
 
+#ifdef USE_BEHAVIOR_TREE
   // parameters
   std::shared_ptr<AvoidanceParameters> avoidance_param_ptr;
   std::shared_ptr<LaneChangeParameters> lane_change_param_ptr;
+#endif
 
   BehaviorPathPlannerParameters getCommonParam();
+
+#ifdef USE_BEHAVIOR_TREE
   BehaviorTreeManagerParam getBehaviorTreeManagerParam();
   SideShiftParameters getSideShiftParam();
   AvoidanceParameters getAvoidanceParam();
   LaneFollowingParameters getLaneFollowingParam();
   LaneChangeParameters getLaneChangeParam();
   PullOverParameters getPullOverParam();
   PullOutParameters getPullOutParam();
+#endif
 
   // callback
   void onOdometry(const Odometry::ConstSharedPtr msg);
@@ -147,6 +174,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg);
   void onMap(const HADMapBin::ConstSharedPtr map_msg);
   void onRoute(const LaneletRoute::ConstSharedPtr route_msg);
+#ifndef USE_BEHAVIOR_TREE
+  void onOperationMode(const OperationModeState::ConstSharedPtr msg);
+#endif
   SetParametersResult onSetParam(const std::vector<rclcpp::Parameter> & parameters);
 
   /**
@@ -205,8 +235,16 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   /**
    * @brief publish path candidate
    */
+#ifdef USE_BEHAVIOR_TREE
   void publishPathCandidate(
     const std::vector<std::shared_ptr<SceneModuleInterface>> & scene_modules);
+#else
+  void publishPathCandidate(
+    const std::vector<std::shared_ptr<SceneModuleManagerInterface>> & managers);
+
+  void publishPathReference(
+    const std::vector<std::shared_ptr<SceneModuleManagerInterface>> & managers);
+#endif
 
   /**
    * @brief convert path with lane id to path for publish path candidate

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -21,6 +21,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
@@ -40,6 +41,7 @@
 
 namespace behavior_path_planner
 {
+using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
@@ -97,6 +99,9 @@ struct BehaviorModuleOutput
   // path planed by module
   PlanResult path{};
 
+  // reference path planed by module
+  PlanResult reference_path{};
+
   TurnSignalInfo turn_signal_info{};
 
   std::optional<PoseWithUuidStamped> modified_goal{};
@@ -118,6 +123,7 @@ struct PlannerData
   AccelWithCovarianceStamped::ConstSharedPtr self_acceleration{};
   PredictedObjects::ConstSharedPtr dynamic_object{};
   OccupancyGrid::ConstSharedPtr occupancy_grid{};
+  OperationModeState::ConstSharedPtr operation_mode{};
   PathWithLaneId::SharedPtr reference_path{std::make_shared<PathWithLaneId>()};
   PathWithLaneId::SharedPtr prev_output_path{std::make_shared<PathWithLaneId>()};
   lanelet::ConstLanelets current_lanes{};

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -19,6 +19,8 @@
 
 struct BehaviorPathPlannerParameters
 {
+  bool verbose;
+
   double backward_path_length;
   double forward_path_length;
   double backward_length_buffer_for_end_of_lane;