add planning_factor_interface to behavior_velocity_planner

Signed-off-by: Mamoru Sobue <[email protected]>

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<const PlannerData> 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<universe_utils::TimeKeeper> time_keeper,
+    const std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface);
 
   /**
    * @brief plan go-stop velocity at traffic crossing with collision check between reference path

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<BlindSpotModule>(
       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<double>::lowest(),

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<const PlannerData> 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<universe_utils::TimeKeeper> time_keeper,
+  const std::shared_ptr<motion_utils::PlanningFactorInterface> 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),

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<CrosswalkModule>(
-      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,

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -34,12 +34,12 @@
 #include <cmath>
 #include <functional>
 #include <limits>
+#include <memory>
 #include <set>
 #include <string>
 #include <tuple>
 #include <utility>
 #include <vector>
-
 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<int64_t> & 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<universe_utils::TimeKeeper> time_keeper,
+  const std::shared_ptr<motion_utils::PlanningFactorInterface> 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)

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<int64_t> & 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<universe_utils::TimeKeeper> time_keeper,
+    const std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface);
 
   bool modifyPathVelocity(PathWithLaneId * path) override;
 

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<DetectionAreaModule>(
         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_));
     }
   }
 }

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<universe_utils::TimeKeeper> time_keeper,
+  const std::shared_ptr<motion_utils::PlanningFactorInterface> 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),

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<universe_utils::TimeKeeper> time_keeper,
+    const std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface);
 
   bool modifyPathVelocity(PathWithLaneId * path) override;
 

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<IntersectionModule>(
       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<MergeFromPrivateRoadModule>(
           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<MergeFromPrivateRoadModule>(
             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;
         }
       }

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -52,8 +52,10 @@ IntersectionModule::IntersectionModule(
   [[maybe_unused]] std::shared_ptr<const PlannerData> planner_data,
   const PlannerParam & planner_param, const std::set<lanelet::Id> & 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<universe_utils::TimeKeeper> time_keeper,
+  const std::shared_ptr<motion_utils::PlanningFactorInterface> 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),

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<const PlannerData> planner_data,
     const PlannerParam & planner_param, const std::set<lanelet::Id> & 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<universe_utils::TimeKeeper> time_keeper,
+    const std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface);
 
   /**
    ***********************************************************

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<const PlannerData> planner_data,
   const PlannerParam & planner_param, const std::set<lanelet::Id> & 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<universe_utils::TimeKeeper> time_keeper,
+  const std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface)
+: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface),
   lane_id_(lane_id),
   associative_ids_(associative_ids)
 {

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<const PlannerData> planner_data,
     const PlannerParam & planner_param, const std::set<lanelet::Id> & associative_ids,
-    const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock);
+    const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock,
+    const std::shared_ptr<universe_utils::TimeKeeper> time_keeper,
+    const std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface);
 
   /**
    * @brief plan go-stop velocity at traffic crossing with collision check between reference path

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<NoDrivableLaneModule>(
-      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_));
   }
 }
 

planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp

@@ -21,14 +21,18 @@
 #include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <memory>
+
 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<universe_utils::TimeKeeper> time_keeper,
+  const std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface)
+: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface),
   lane_id_(lane_id),
   planner_param_(planner_param),
   debug_data_(),

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<universe_utils::TimeKeeper> time_keeper,
+    const std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface);
 
   bool modifyPathVelocity(PathWithLaneId * path) override;
 

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<NoStoppingAreaModule>(
         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,

planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp

@@ -30,6 +30,7 @@
 #include <lanelet2_core/utility/Optional.h>
 
 #include <cmath>
+#include <memory>
 #include <vector>
 
 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<universe_utils::TimeKeeper> time_keeper,
+  const std::shared_ptr<motion_utils::PlanningFactorInterface> 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),

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<universe_utils::TimeKeeper> time_keeper,
+    const std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface);
 
   bool modifyPathVelocity(PathWithLaneId * path) override;
 

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<OcclusionSpotModule>(
-      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_));
   }
 }
 

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<const PlannerData> & 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<universe_utils::TimeKeeper> time_keeper,
+  const std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface)
+: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface),
+  param_(planner_param)
 {
   velocity_factor_.init(PlanningBehavior::UNKNOWN);
 

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<const PlannerData> & planner_data,
     const PlannerParam & planner_param, const rclcpp::Logger & logger,
-    const rclcpp::Clock::SharedPtr clock);
+    const rclcpp::Clock::SharedPtr clock,
+    const std::shared_ptr<universe_utils::TimeKeeper> time_keeper,
+    const std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface);
 
   /**
    * @brief plan occlusion spot velocity at unknown area in occupancy grid