diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 4c9f139365a06..fffb86767b0a8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -31,13 +31,15 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) : LaneChangeInterface{ name, node, parameters, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, + steering_factor_interface_ptr, std::make_unique(parameters, avoidance_by_lane_change_parameters)} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp index c7fbba34b1adb..4d200411904b0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp @@ -40,7 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); bool isExecutionRequested() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 8096d2944ee2b..ddcfda50d18c4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -192,7 +192,7 @@ AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp index 9c85a463067de..a7f470002b01b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp @@ -42,7 +42,7 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 8f19613b50e6d..48c6084a4f424 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -343,7 +343,8 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); void updateModuleParams(const std::any & parameters) override { diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index f195b91a54f21..debd13acd31dd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -331,8 +331,9 @@ DynamicObstacleAvoidanceModule::DynamicObstacleAvoidanceModule( std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT parameters_{std::move(parameters)}, target_objects_manager_{TargetObjectsManager( parameters_->successive_num_to_entry_dynamic_avoidance_condition, diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index c04914ed1c72b..3c730e6d36376 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -26,7 +26,7 @@ ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_, std::make_unique(parameters_, direction_)); } @@ -35,7 +35,7 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_, std::make_unique(parameters_, direction_)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 749ed57f95cc9..4c23292f63e61 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -307,7 +307,8 @@ class GoalPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); ~GoalPlannerModule() { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp index 32d816d9a37e8..1ab7b6cb265a8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp @@ -38,7 +38,7 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 7792b427c5fa1..54a1c0830fb06 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -57,8 +57,9 @@ GoalPlannerModule::GoalPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, thread_safe_data_{mutex_, clock_}, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index bd309dd35a260..fd9375c7e9f75 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -52,6 +52,7 @@ class LaneChangeInterface : public SceneModuleInterface const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr, std::unique_ptr && module_type); LaneChangeInterface(const LaneChangeInterface &) = delete; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 9354b117cb160..1bbc2d25fc0bc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -38,14 +38,14 @@ LaneChangeInterface::LaneChangeInterface( const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr, std::unique_ptr && module_type) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT parameters_{std::move(parameters)}, module_type_{std::move(module_type)}, prev_approved_path_{std::make_unique()} { module_type_->setTimeKeeper(getTimeKeeper()); - steering_factor_interface_ptr_ = std::make_unique(&node, name); logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 42166c4dff0e0..c197d0d63aa37 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -299,7 +299,7 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_, std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index c4a05d171654d..d9ad89283d304 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -90,15 +90,15 @@ class SceneModuleInterface const std::string & name, rclcpp::Node & node, std::unordered_map> rtc_interface_ptr_map, std::unordered_map> - objects_of_interest_marker_interface_ptr_map) + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)), objects_of_interest_marker_interface_ptr_map_( std::move(objects_of_interest_marker_interface_ptr_map)), - steering_factor_interface_ptr_( - std::make_unique(&node, utils::convertToSnakeCase(name))), + steering_factor_interface_ptr_{steering_factor_interface_ptr}, time_keeper_(std::make_shared()) { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { @@ -640,7 +640,7 @@ class SceneModuleInterface std::unordered_map> objects_of_interest_marker_interface_ptr_map_; - std::unique_ptr steering_factor_interface_ptr_; + std::shared_ptr steering_factor_interface_ptr_; mutable std::optional stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 82a364a9c37c7..ceecb7b02408a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -315,6 +315,12 @@ class SceneModuleManagerInterface "~/processing_time/" + name_, 20); } + // init steering factor + { + steering_factor_interface_ptr_ = + std::make_shared(node, utils::convertToSnakeCase(name_)); + } + // misc { node_ = node; @@ -339,6 +345,8 @@ class SceneModuleManagerInterface std::shared_ptr planner_data_; + std::shared_ptr steering_factor_interface_ptr_; + std::vector observers_; std::unique_ptr idle_module_ptr_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp index 28c310ae20ec2..778afd1698ff2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp @@ -38,7 +38,7 @@ class SamplingPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } void updateModuleParams( diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp index 2b6f52998cb23..8dc04947923e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -86,7 +86,8 @@ class SamplingPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 526a761e6957a..b820e8d03322f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -33,8 +33,9 @@ SamplingPlannerModule::SamplingPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()} { internal_params_ = std::make_shared(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp index b495e6619a1c7..d1c9c8e2535ec 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp @@ -39,7 +39,7 @@ class SideShiftModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp index 543b17aca9352..74953b7927b5d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp @@ -45,7 +45,8 @@ class SideShiftModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index bcb972bf5dc8b..cd4298c56929a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -40,8 +40,9 @@ SideShiftModule::SideShiftModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT parameters_{parameters} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp index 5d3d224673124..a26c48ad065c9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp @@ -38,7 +38,7 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index bb70ae2638056..e0bb5d95f565a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -88,7 +88,8 @@ class StartPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); ~StartPlannerModule() override { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index e318fbedf3200..dd9129216fbba 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -59,8 +59,9 @@ StartPlannerModule::StartPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp index 895390f91cc16..fa54ec52203f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp @@ -41,7 +41,7 @@ class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 34d06a46d9ac8..dc6e41fbc5f7e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -46,7 +46,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr); CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 39a8d1b853e47..3c76e288fdd55 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -76,8 +76,9 @@ StaticObstacleAvoidanceModule::StaticObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + std::shared_ptr & steering_factor_interface_ptr) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr}, // NOLINT helper_{std::make_shared(parameters)}, parameters_{parameters}, generator_{parameters}