remove velocity_factor

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

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -44,8 +44,6 @@ namespace autoware::behavior_velocity_planner
 {
 namespace bg = boost::geometry;
 
-using autoware::motion_utils::VelocityFactorInterface;
-
 IntersectionModule::IntersectionModule(
   const int64_t module_id, const int64_t lane_id,
   [[maybe_unused]] std::shared_ptr<const PlannerData> planner_data,

planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp

@@ -38,7 +38,6 @@ NoDrivableLaneModule::NoDrivableLaneModule(
   debug_data_(),
   state_(State::INIT)
 {
-  velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE);
 }
 
 bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path)
@@ -166,8 +165,10 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path)
   // Get stop point and stop factor
   {
     const auto & stop_pose = op_stop_pose.value();
-    velocity_factor_.set(
-      path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING);
+    planning_factor_interface_->add(
+      path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose,
+      tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
+      true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "");
 
     const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
       path->points, stop_pose.position, debug_data_.base_link2front);
@@ -214,8 +215,10 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId *
   // Get stop point and stop factor
   {
     const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0));
-    velocity_factor_.set(
-      path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING);
+    planning_factor_interface_->add(
+      path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose,
+      tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
+      true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "");
 
     const auto & virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
       path->points, stop_pose.position, debug_data_.base_link2front);
@@ -252,8 +255,10 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path)
   // Get stop point and stop factor
   {
     const auto & stop_pose = ego_pos_on_path.pose;
-    velocity_factor_.set(
-      path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED);
+    planning_factor_interface_->add(
+      path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose,
+      tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
+      true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "");
 
     const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
       path->points, stop_pose.position, debug_data_.base_link2front);

planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp

@@ -70,8 +70,6 @@ OcclusionSpotModule::OcclusionSpotModule(
 : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface),
   param_(planner_param)
 {
-  velocity_factor_.init(PlanningBehavior::UNKNOWN);
-
   if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) {
     debug_data_.detection_type = "occupancy";
     //! occupancy grid limitation( 100 * 100 )

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp

@@ -18,7 +18,6 @@
 #include <autoware/behavior_velocity_planner_common/planner_data.hpp>
 #include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
 #include <autoware/motion_utils/factor/planning_factor_interface.hpp>
-#include <autoware/motion_utils/factor/velocity_factor_interface.hpp>
 #include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp>
@@ -28,8 +27,6 @@
 #include <autoware/universe_utils/system/time_keeper.hpp>
 #include <builtin_interfaces/msg/time.hpp>
 
-#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
-#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
 #include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
 #include <autoware_planning_msgs/msg/path.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
@@ -51,8 +48,6 @@
 namespace autoware::behavior_velocity_planner
 {
 
-using autoware::motion_utils::PlanningBehavior;
-using autoware::motion_utils::VelocityFactor;
 using autoware::objects_of_interest_marker_interface::ColorName;
 using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface;
 using autoware::universe_utils::DebugPublisher;
@@ -97,8 +92,6 @@ class SceneModuleInterface
     planner_data_ = planner_data;
   }
 
-  void resetVelocityFactor() { velocity_factor_.reset(); }
-  VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); }
   std::vector<ObjectOfInterest> getObjectsOfInterestData() const { return objects_of_interest_; }
   void clearObjectsOfInterestData() { objects_of_interest_.clear(); }
 
@@ -107,7 +100,6 @@ class SceneModuleInterface
   rclcpp::Logger logger_;
   rclcpp::Clock::SharedPtr clock_;
   std::shared_ptr<const PlannerData> planner_data_;
-  autoware::motion_utils::VelocityFactorInterface velocity_factor_;  // TODO(soblin): remove this
   std::vector<ObjectOfInterest> objects_of_interest_;
   mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
   std::shared_ptr<motion_utils::PlanningFactorInterface> planning_factor_interface_;
@@ -143,8 +135,6 @@ class SceneModuleManagerInterface
     }
     pub_virtual_wall_ = node.create_publisher<visualization_msgs::msg::MarkerArray>(
       std::string("~/virtual_wall/") + module_name, 5);
-    pub_velocity_factor_ = node.create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
-      std::string("/planning/velocity_factors/") + module_name, 1);
     planning_factor_interface_ =
       std::make_shared<motion_utils::PlanningFactorInterface>(&node, module_name);
 
@@ -180,30 +170,18 @@ class SceneModuleManagerInterface
     StopWatch<std::chrono::milliseconds> stop_watch;
     stop_watch.tic("Total");
     visualization_msgs::msg::MarkerArray debug_marker_array;
-    autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array;
-    velocity_factor_array.header.frame_id = "map";
-    velocity_factor_array.header.stamp = clock_->now();
 
     for (const auto & scene_module : scene_modules_) {
-      scene_module->resetVelocityFactor();
       scene_module->setPlannerData(planner_data_);
       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);
-      }
-
       for (const auto & marker : scene_module->createDebugMarkerArray().markers) {
         debug_marker_array.markers.push_back(marker);
       }
 
       virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls());
     }
 
-    pub_velocity_factor_->publish(velocity_factor_array);
     planning_factor_interface_->publish();
     pub_debug_->publish(debug_marker_array);
     if (is_publish_debug_path_) {
@@ -273,8 +251,6 @@ class SceneModuleManagerInterface
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_virtual_wall_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_debug_;
   rclcpp::Publisher<tier4_planning_msgs::msg::PathWithLaneId>::SharedPtr pub_debug_path_;
-  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
-    pub_velocity_factor_;
 
   std::shared_ptr<DebugPublisher> processing_time_publisher_;
 

planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp

@@ -61,7 +61,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat
   for (const auto & scene_module : scene_modules_) {
     std::shared_ptr<TrafficLightModule> traffic_light_scene_module(
       std::dynamic_pointer_cast<TrafficLightModule>(scene_module));
-    traffic_light_scene_module->resetVelocityFactor();
     traffic_light_scene_module->setPlannerData(planner_data_);
     traffic_light_scene_module->modifyPathVelocity(path);