feat(behavior_path_planner): integrate avoidance by lc

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

planning/behavior_path_planner/CMakeLists.txt

@@ -16,6 +16,7 @@ set(common_src
   src/behavior_path_planner_node.cpp
   src/scene_module/scene_module_visitor.cpp
   src/scene_module/avoidance/avoidance_module.cpp
+  src/scene_module/avoidance_by_lc/module.cpp
   src/scene_module/pull_out/pull_out_module.cpp
   src/scene_module/pull_over/pull_over_module.cpp
   src/scene_module/side_shift/side_shift_module.cpp
@@ -62,6 +63,7 @@ else()
   ament_auto_add_library(behavior_path_planner_node SHARED
     src/planner_manager.cpp
     src/scene_module/avoidance/manager.cpp
+    src/scene_module/avoidance_by_lc/manager.cpp
     src/scene_module/pull_out/manager.cpp
     src/scene_module/pull_over/manager.cpp
     src/scene_module/side_shift/manager.cpp

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -30,6 +30,7 @@
 #else
 #include "behavior_path_planner/planner_manager.hpp"
 #include "behavior_path_planner/scene_module/avoidance/manager.hpp"
+#include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp"
 #include "behavior_path_planner/scene_module/lane_change/manager.hpp"
 #include "behavior_path_planner/scene_module/pull_out/manager.hpp"
 #include "behavior_path_planner/scene_module/pull_over/manager.hpp"
@@ -39,6 +40,7 @@
 #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 "behavior_path_planner/util/avoidance_by_lc/module_data.hpp"
 #include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp"
 #include "behavior_path_planner/util/lane_following/module_data.hpp"
 #include "behavior_path_planner/util/pull_out/pull_out_parameters.hpp"
@@ -160,6 +162,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 
   // parameters
   std::shared_ptr<AvoidanceParameters> avoidance_param_ptr_;
+  std::shared_ptr<AvoidanceByLCParameters> avoidance_by_lc_param_ptr_;
   std::shared_ptr<SideShiftParameters> side_shift_param_ptr_;
   std::shared_ptr<LaneChangeParameters> lane_change_param_ptr_;
   std::shared_ptr<LaneFollowingParameters> lane_following_param_ptr_;
@@ -178,6 +181,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   SideShiftParameters getSideShiftParam();
   PullOverParameters getPullOverParam();
   PullOutParameters getPullOutParam();
+  AvoidanceByLCParameters getAvoidanceByLCParam(
+    const std::shared_ptr<AvoidanceParameters> & avoidance_param,
+    const std::shared_ptr<LaneChangeParameters> & lane_change_param);
 
   // callback
   void onOdometry(const Odometry::ConstSharedPtr msg);

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -30,6 +30,7 @@ struct BehaviorPathPlannerParameters
   bool verbose;
 
   ModuleConfigParameters config_avoidance;
+  ModuleConfigParameters config_avoidance_by_lc;
   ModuleConfigParameters config_pull_out;
   ModuleConfigParameters config_pull_over;
   ModuleConfigParameters config_side_shift;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp

@@ -27,6 +27,7 @@ namespace behavior_path_planner
 {
 // Forward Declaration
 class AvoidanceModule;
+class AvoidanceByLCModule;
 class LaneChangeModule;
 class ExternalRequestLaneChangeModule;
 class LaneFollowingModule;
@@ -45,6 +46,7 @@ class SceneModuleVisitor
   void visitLaneChangeModule(const LaneChangeModule * module) const;
   void visitExternalRequestLaneChangeModule(const ExternalRequestLaneChangeModule * module) const;
   void visitAvoidanceModule(const AvoidanceModule * module) const;
+  void visitAvoidanceByLCModule(const AvoidanceByLCModule * module) const;
 
   std::shared_ptr<AvoidanceDebugMsgArray> getAvoidanceModuleDebugMsg() const;
   std::shared_ptr<LaneChangeDebugMsgArray> getLaneChangeModuleDebugMsg() const;

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -125,6 +125,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     pull_out_param_ptr_ = std::make_shared<PullOutParameters>(getPullOutParam());
     pull_over_param_ptr_ = std::make_shared<PullOverParameters>(getPullOverParam());
     side_shift_param_ptr_ = std::make_shared<SideShiftParameters>(getSideShiftParam());
+    avoidance_by_lc_param_ptr_ = std::make_shared<AvoidanceByLCParameters>(
+      getAvoidanceByLCParam(avoidance_param_ptr_, lane_change_param_ptr_));
   }
 
   m_set_param_res = this->add_on_set_parameters_callback(
@@ -268,6 +270,18 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
         "avoidance", create_publisher<Path>(path_reference_name_space + "avoidance", 1));
     }
 
+    if (p.config_avoidance_by_lc.enable_module) {
+      auto manager = std::make_shared<AvoidanceByLCModuleManager>(
+        this, "avoidance_by_lane_change", p.config_avoidance_by_lc, avoidance_by_lc_param_ptr_);
+      planner_manager_->registerSceneModuleManager(manager);
+      path_candidate_publishers_.emplace(
+        "avoidance_by_lane_change",
+        create_publisher<Path>(path_candidate_name_space + "avoidance_by_lane_change", 1));
+      path_reference_publishers_.emplace(
+        "avoidance_by_lane_change",
+        create_publisher<Path>(path_reference_name_space + "avoidance_by_lane_change", 1));
+    }
+
     mutex_bt_.unlock();
   }
 #endif
@@ -355,6 +369,15 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
     p.config_avoidance.max_module_size = declare_parameter<int>(ns + "max_module_size");
   }
 
+  {
+    const std::string ns = "avoidance_by_lc.";
+    p.config_avoidance_by_lc.enable_module = declare_parameter<bool>(ns + "enable_module");
+    p.config_avoidance_by_lc.enable_simultaneous_execution =
+      declare_parameter<bool>(ns + "enable_simultaneous_execution");
+    p.config_avoidance_by_lc.priority = declare_parameter<int>(ns + "priority");
+    p.config_avoidance_by_lc.max_module_size = declare_parameter<int>(ns + "max_module_size");
+  }
+
   // vehicle info
   const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo();
   p.vehicle_info = vehicle_info;
@@ -456,6 +479,26 @@ SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam()
   return p;
 }
 
+AvoidanceByLCParameters BehaviorPathPlannerNode::getAvoidanceByLCParam(
+  const std::shared_ptr<AvoidanceParameters> & avoidance_param,
+  const std::shared_ptr<LaneChangeParameters> & lane_change_param)
+{
+  AvoidanceByLCParameters p{};
+  p.avoidance = avoidance_param;
+  p.lane_change = lane_change_param;
+
+  {
+    std::string ns = "avoidance_by_lane_change.";
+    p.execute_object_num = declare_parameter<int>(ns + "execute_object_num");
+    p.execute_object_longitudinal_margin =
+      declare_parameter<double>(ns + "execute_object_longitudinal_margin");
+    p.execute_only_when_lane_change_finish_before_object =
+      declare_parameter<bool>(ns + "execute_only_when_lane_change_finish_before_object");
+  }
+
+  return p;
+}
+
 AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
 {
   AvoidanceParameters p{};