diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 8bd768affd3bc..7afdd5b18785e 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/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 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 9172e666f0d28..70eb4481916d0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/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 avoidance_param_ptr_; + std::shared_ptr avoidance_by_lc_param_ptr_; std::shared_ptr side_shift_param_ptr_; std::shared_ptr lane_change_param_ptr_; std::shared_ptr 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 & avoidance_param, + const std::shared_ptr & lane_change_param); // callback void onOdometry(const Odometry::ConstSharedPtr msg); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 4630f2f324e84..132fbb7df8b13 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/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; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp index a5f74529cc073..7209ddd95eaba 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp +++ b/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; #ifdef USE_OLD_ARCHITECTURE class ExternalRequestLaneChangeModule; @@ -49,6 +50,7 @@ class SceneModuleVisitor void visitExternalRequestLaneChangeModule(const ExternalRequestLaneChangeModule * module) const; #endif void visitAvoidanceModule(const AvoidanceModule * module) const; + void visitAvoidanceByLCModule(const AvoidanceByLCModule * module) const; std::shared_ptr getAvoidanceModuleDebugMsg() const; std::shared_ptr getLaneChangeModuleDebugMsg() const; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 041bf25c86df8..b303fbd707583 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/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(getPullOutParam()); pull_over_param_ptr_ = std::make_shared(getPullOverParam()); side_shift_param_ptr_ = std::make_shared(getSideShiftParam()); + avoidance_by_lc_param_ptr_ = std::make_shared( + getAvoidanceByLCParam(avoidance_param_ptr_, lane_change_param_ptr_)); } m_set_param_res = this->add_on_set_parameters_callback( @@ -285,6 +287,18 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "avoidance", create_publisher(path_reference_name_space + "avoidance", 1)); } + if (p.config_avoidance_by_lc.enable_module) { + auto manager = std::make_shared( + 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_candidate_name_space + "avoidance_by_lane_change", 1)); + path_reference_publishers_.emplace( + "avoidance_by_lane_change", + create_publisher(path_reference_name_space + "avoidance_by_lane_change", 1)); + } + mutex_bt_.unlock(); } #endif @@ -394,6 +408,15 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.config_avoidance.max_module_size = declare_parameter(ns + "max_module_size"); } + { + const std::string ns = "avoidance_by_lc."; + p.config_avoidance_by_lc.enable_module = declare_parameter(ns + "enable_module"); + p.config_avoidance_by_lc.enable_simultaneous_execution = + declare_parameter(ns + "enable_simultaneous_execution"); + p.config_avoidance_by_lc.priority = declare_parameter(ns + "priority"); + p.config_avoidance_by_lc.max_module_size = declare_parameter(ns + "max_module_size"); + } + // vehicle info const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); p.vehicle_info = vehicle_info; @@ -495,6 +518,26 @@ SideShiftParameters BehaviorPathPlannerNode::getSideShiftParam() return p; } +AvoidanceByLCParameters BehaviorPathPlannerNode::getAvoidanceByLCParam( + const std::shared_ptr & avoidance_param, + const std::shared_ptr & 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(ns + "execute_object_num"); + p.execute_object_longitudinal_margin = + declare_parameter(ns + "execute_object_longitudinal_margin"); + p.execute_only_when_lane_change_finish_before_object = + declare_parameter(ns + "execute_only_when_lane_change_finish_before_object"); + } + + return p; +} + AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() { AvoidanceParameters p{};