diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 50ce8634ec600..1baa72f5c6ced 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -87,7 +87,9 @@ class BlindSpotModule : public SceneModuleInterfaceWithRTC BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index a1410d2fecfd5..f6f16e5cd413a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -75,7 +75,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa registerModule(std::make_shared( module_id, lane_id, turn_direction, planner_data_, planner_param_, - logger_.get_child("blind_spot_module"), clock_)); + logger_.get_child("blind_spot_module"), clock_, time_keeper_, planning_factor_interface_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 7d1a68c5006a0..d5d47b11f160a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -40,8 +40,10 @@ namespace bg = boost::geometry; BlindSpotModule::BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), planner_param_{planner_param}, turn_direction_(turn_direction), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index cdaff7225a7d2..5bfd31b75976a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -192,8 +192,8 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) // NOTE: module_id is always a lane id so that isModuleRegistered works correctly in the case // where both regulatory element and non-regulatory element crosswalks exist. registerModule(std::make_shared( - node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, - clock_)); + node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_, + time_keeper_, planning_factor_interface_)); generateUUID(crosswalk_lanelet_id); updateRTCStatus( getUUID(crosswalk_lanelet_id), true, State::WAITING_FOR_EXECUTION, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 64f7e9e14dcd1..7c9035d135065 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -34,12 +34,12 @@ #include #include #include +#include #include #include #include #include #include - namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -175,8 +175,10 @@ CrosswalkModule::CrosswalkModule( rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), planner_param_(planner_param), use_regulatory_element_(reg_elem_id) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index d5a9e463c730b..f32ee96623822 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -334,7 +334,9 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index fb2c17d9e3745..f231d7adc3326 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -64,7 +64,8 @@ void DetectionAreaModuleManager::launchNewModules( if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, lane_id, *detection_area_with_lane_id.first, planner_param_, - logger_.get_child("detection_area_module"), clock_)); + logger_.get_child("detection_area_module"), clock_, time_keeper_, + planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 031c45753022e..59f6f20937f41 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -36,8 +36,10 @@ DetectionAreaModule::DetectionAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 9224cf4624687..1d3e5f3540ff3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -68,7 +68,9 @@ class DetectionAreaModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index 41bdbe3e43c8b..d6f7dfbac92d4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -338,7 +338,8 @@ void IntersectionModuleManager::launchNewModules( } const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, - has_traffic_light, node_, logger_.get_child("intersection_module"), clock_); + has_traffic_light, node_, logger_.get_child("intersection_module"), clock_, time_keeper_, + planning_factor_interface_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); @@ -526,7 +527,8 @@ void MergeFromPrivateModuleManager::launchNewModules( planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); registerModule(std::make_shared( module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, - logger_.get_child("merge_from_private_road_module"), clock_)); + logger_.get_child("merge_from_private_road_module"), clock_, time_keeper_, + planning_factor_interface_)); continue; } } else { @@ -540,7 +542,8 @@ void MergeFromPrivateModuleManager::launchNewModules( planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); registerModule(std::make_shared( module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, - logger_.get_child("merge_from_private_road_module"), clock_)); + logger_.get_child("merge_from_private_road_module"), clock_, time_keeper_, + planning_factor_interface_)); continue; } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 93a13a4d62737..09fe8db600229 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -52,8 +52,10 @@ IntersectionModule::IntersectionModule( [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), planner_param_(planner_param), lane_id_(lane_id), associative_ids_(associative_ids), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index 6c31be2ce83b9..db0cfbe98d53c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -304,7 +304,9 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** *********************************************************** diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 3dfdcc36c0cff..2392817aebaf2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -41,8 +41,10 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), associative_ids_(associative_ids) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index dc8bf1a96a7a2..fe446d304d9e0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -61,7 +61,9 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp index 9cb1153a8edd2..517568c93bd35 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -51,7 +51,8 @@ void NoDrivableLaneModuleManager::launchNewModules( } registerModule(std::make_shared( - module_id, lane_id, planner_param_, logger_.get_child("no_drivable_lane_module"), clock_)); + module_id, lane_id, planner_param_, logger_.get_child("no_drivable_lane_module"), clock_, + time_keeper_, planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index 8365251904b18..f32210bc502bf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -21,14 +21,18 @@ #include #include +#include + namespace autoware::behavior_velocity_planner { using autoware::universe_utils::createPoint; NoDrivableLaneModule::NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), planner_param_(planner_param), debug_data_(), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index d8c5e3e0425d1..835e946443e90 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -55,7 +55,9 @@ class NoDrivableLaneModule : public SceneModuleInterface NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index dca2dde33b693..55b6c0dffd1a4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -60,7 +60,7 @@ void NoStoppingAreaModuleManager::launchNewModules( // assign 1 no stopping area for each module registerModule(std::make_shared( module_id, lane_id, *m.first, planner_param_, logger_.get_child("no_stopping_area_module"), - clock_)); + clock_, time_keeper_, planning_factor_interface_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 5c79ec69a9d98..bcb8b365f6205 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -30,6 +30,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -40,8 +41,10 @@ NoStoppingAreaModule::NoStoppingAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), no_stopping_area_reg_elem_(no_stopping_area_reg_elem), planner_param_(planner_param), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 1eafcf157623d..0d53441924805 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -57,7 +57,9 @@ class NoStoppingAreaModule : public SceneModuleInterfaceWithRTC const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index c4dace9d49244..d28cff55a7a8d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -123,8 +123,8 @@ void OcclusionSpotModuleManager::launchNewModules( // general if (!isModuleRegistered(module_id_)) { registerModule(std::make_shared( - module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), - clock_)); + module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), clock_, + time_keeper_, planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index c5d1cf61614b2..1163b00e08ccb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -64,8 +64,11 @@ namespace utils = occlusion_spot_utils; OcclusionSpotModule::OcclusionSpotModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), param_(planner_param) + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), + param_(planner_param) { velocity_factor_.init(PlanningBehavior::UNKNOWN); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 35c409a9c459b..22b2a91be66b0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -48,7 +48,9 @@ class OcclusionSpotModule : public SceneModuleInterface OcclusionSpotModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief plan occlusion spot velocity at unknown area in occupancy grid diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index d8d640a078267..cb57974ed6700 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -79,7 +80,9 @@ class SceneModuleInterface { public: explicit SceneModuleInterface( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); virtual ~SceneModuleInterface() = default; virtual bool modifyPathVelocity(PathWithLaneId * path) = 0; @@ -94,13 +97,6 @@ class SceneModuleInterface planner_data_ = planner_data; } - void setTimeKeeper(const std::shared_ptr & time_keeper) - { - time_keeper_ = time_keeper; - } - - std::shared_ptr getTimeKeeper() { return time_keeper_; } - void resetVelocityFactor() { velocity_factor_.reset(); } VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } std::vector getObjectsOfInterestData() const { return objects_of_interest_; } @@ -111,9 +107,10 @@ class SceneModuleInterface rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - autoware::motion_utils::VelocityFactorInterface velocity_factor_; + autoware::motion_utils::VelocityFactorInterface velocity_factor_; // TODO(soblin): remove this std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; + std::shared_ptr planning_factor_interface_; void setObjectsOfInterestData( const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, @@ -148,6 +145,8 @@ class SceneModuleManagerInterface std::string("~/virtual_wall/") + module_name, 5); pub_velocity_factor_ = node.create_publisher( std::string("/planning/velocity_factors/") + module_name, 1); + planning_factor_interface_ = + std::make_shared(&node, module_name); processing_time_publisher_ = std::make_shared(&node, "~/debug"); @@ -191,6 +190,7 @@ class SceneModuleManagerInterface scene_module->modifyPathVelocity(path); // The velocity factor must be called after modifyPathVelocity. + // TODO(soblin): remove this const auto velocity_factor = scene_module->getVelocityFactor(); if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { velocity_factor_array.factors.emplace_back(velocity_factor); @@ -204,6 +204,7 @@ class SceneModuleManagerInterface } pub_velocity_factor_->publish(velocity_factor_array); + planning_factor_interface_->publish(); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { tier4_planning_msgs::msg::PathWithLaneId debug_path; @@ -246,7 +247,6 @@ class SceneModuleManagerInterface logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); registered_module_id_set_.emplace(scene_module->getModuleId()); - scene_module->setTimeKeeper(time_keeper_); scene_modules_.insert(scene_module); } @@ -281,6 +281,8 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_processing_time_detail_; std::shared_ptr time_keeper_; + + std::shared_ptr planning_factor_interface_; }; extern template SceneModuleManagerInterface::SceneModuleManagerInterface( rclcpp::Node & node, [[maybe_unused]] const char * module_name); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index ffd454012d13e..db2a274272f9f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -25,11 +25,14 @@ namespace autoware::behavior_velocity_planner { SceneModuleInterface::SceneModuleInterface( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) : module_id_(module_id), logger_(logger), clock_(clock), - time_keeper_(std::shared_ptr()) + time_keeper_(time_keeper), + planning_factor_interface_(planning_factor_interface) { } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp index 4e30ab019aa4e..a955538f4f9fe 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp @@ -52,7 +52,9 @@ class SceneModuleInterfaceWithRTC : public SceneModuleInterface { public: explicit SceneModuleInterfaceWithRTC( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); virtual ~SceneModuleInterfaceWithRTC() = default; void setActivation(const bool activated) { activated_ = activated; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp index abac509fd2b2b..2e73b35e20ed9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp @@ -27,8 +27,10 @@ namespace autoware::behavior_velocity_planner { SceneModuleInterfaceWithRTC::SceneModuleInterfaceWithRTC( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), activated_(false), safe_(false), distance_(std::numeric_limits::lowest()) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp index 4a16f263d0a54..4af7882461643 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -156,7 +156,8 @@ void RunOutModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathW if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, planner_data_, planner_param_, logger_.get_child("run_out_module"), - std::move(dynamic_obstacle_creator_), debug_ptr_, clock_)); + std::move(dynamic_obstacle_creator_), debug_ptr_, clock_, time_keeper_, + planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index efc2d458ae2a7..639794bbd6036 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -45,8 +45,10 @@ RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, std::unique_ptr dynamic_obstacle_creator, - const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), planner_param_(planner_param), dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)), debug_ptr_(debug_ptr), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 83123c71f461e..49de16753a869 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -47,7 +47,9 @@ class RunOutModule : public SceneModuleInterface const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, std::unique_ptr dynamic_obstacle_creator, - const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock); + const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp index 4677d49b78f4b..a69e33e9e0391 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp @@ -63,7 +63,7 @@ void SpeedBumpModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, lane_id, *speed_bump_with_lane_id.first, planner_param_, - logger_.get_child("speed_bump_module"), clock_)); + logger_.get_child("speed_bump_module"), clock_, time_keeper_, planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp index 54ea807f3268b..4af7801e50ba3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -33,8 +34,10 @@ using geometry_msgs::msg::Point32; SpeedBumpModule::SpeedBumpModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), lane_id_(lane_id), speed_bump_reg_elem_(std::move(speed_bump_reg_elem)), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp index 176a01d5112c5..e3ee2c0566381 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -54,7 +55,9 @@ class SpeedBumpModule : public SceneModuleInterface SpeedBumpModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp index b305a1d2aa404..e289779df34b6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -81,7 +81,7 @@ void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pat if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, stop_line_with_lane_id.first, planner_param_, - logger_.get_child("stop_line_module"), clock_)); + logger_.get_child("stop_line_module"), clock_, time_keeper_, planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 2a9c5dab1ebd9..71a5ea3b9e807 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -20,6 +20,7 @@ #include +#include #include #include @@ -28,8 +29,10 @@ namespace autoware::behavior_velocity_planner StopLineModule::StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), stop_line_(std::move(stop_line)), planner_param_(planner_param), state_(State::APPROACH), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index 9ac1463da8618..aac830b0e9f24 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -30,6 +30,7 @@ #include +#include #include #include @@ -68,7 +69,9 @@ class StopLineModule : public SceneModuleInterface StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp index 1eafb71eb403c..e304b479a005d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -74,7 +74,10 @@ class StopLineModuleTest : public ::testing::Test clock_ = std::make_shared(); module_ = std::make_shared( - 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_); + 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_, + std::make_shared(), + std::make_shared( + node_.get(), "test_stopline")); module_->setPlannerData(planner_data_); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp index 4325cc1d5aaf9..a004f5b823138 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp @@ -42,8 +42,9 @@ void TemplateModuleManager::launchNewModules( { int64_t module_id = 0; if (!isModuleRegistered(module_id)) { - registerModule( - std::make_shared(module_id, logger_.get_child(getModuleName()), clock_)); + registerModule(std::make_shared( + module_id, logger_.get_child(getModuleName()), clock_, time_keeper_, + planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp index 3ce8502baaf63..d8eea337e7186 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp @@ -19,14 +19,17 @@ #include +#include #include namespace autoware::behavior_velocity_planner { TemplateModule::TemplateModule( - const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock) + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface) { } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp index 70cd5cb1235e7..ba078f3fd6421 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp @@ -18,9 +18,9 @@ #include #include +#include #include #include - namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -29,7 +29,9 @@ class TemplateModule : public SceneModuleInterface { public: TemplateModule( - const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief Modify the velocity of path points. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 1b66824d04623..9a402f31af0e4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -114,7 +114,8 @@ void TrafficLightModuleManager::launchNewModules( if (!isModuleRegisteredFromExistingAssociatedModule(lane_id)) { registerModule(std::make_shared( lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, - logger_.get_child("traffic_light_module"), clock_)); + logger_.get_child("traffic_light_module"), clock_, time_keeper_, + planning_factor_interface_)); generateUUID(lane_id); updateRTCStatus( getUUID(lane_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 458d8e1588a5b..b051d5ff7eb76 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -43,8 +43,10 @@ namespace autoware::behavior_velocity_planner TrafficLightModule::TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(lane_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(lane_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index 3af13bc1927ce..c30cbbdfc1477 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -69,7 +69,9 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 3815c83d3b6ab..dade98dfc1133 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -94,7 +94,8 @@ void VirtualTrafficLightModuleManager::launchNewModules( ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) { registerModule(std::make_shared( module_id, lane_id, *m.first, m.second, planner_param_, - logger_.get_child("virtual_traffic_light_module"), clock_)); + logger_.get_child("virtual_traffic_light_module"), clock_, time_keeper_, + planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 86239816ed6f2..9ba7b0bccf1b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -38,8 +39,10 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), reg_elem_(reg_elem), lane_(lane), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 7f37e7078a431..9e7a1192d9869 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -31,6 +31,7 @@ #include #include +#include #include #include @@ -81,7 +82,9 @@ class VirtualTrafficLightModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp index 0f1a7509043b5..bfa8a96531090 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp @@ -61,7 +61,8 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); registerModule(std::make_shared( - lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); + lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_, time_keeper_, + planning_factor_interface_)); }; const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 6a7505930a334..c60c3def50270 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -18,6 +18,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -32,8 +33,10 @@ using autoware::universe_utils::getPose; WalkwayModule::WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const bool use_regulatory_element, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), state_(State::APPROACH), planner_param_(planner_param), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index df54eafd11cc2..1bd5003498d34 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -44,7 +44,9 @@ class WalkwayModule : public SceneModuleInterface WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const bool use_regulatory_element, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override;