feat(behavior_velocity_planner)!: remove velocity_factor completely (#…

…9943)

* feat(behavior_velocity_planner)!: remove velocity_factor completely

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

* minimize diff

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

---------

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

planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp

@@ -46,7 +46,6 @@ DetectionAreaModule::DetectionAreaModule(
   planner_param_(planner_param),
   debug_data_()
 {
-  velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA);
 }
 
 bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path)
@@ -180,9 +179,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path)
 
   // Create StopReason
   {
-    velocity_factor_.set(
-      path->points, planner_data_->current_odometry->pose, stop_point->second,
-      VelocityFactor::UNKNOWN);
+    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*/, "");
   }
 
   return true;

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_no_stopping_area_module/src/scene_no_stopping_area.cpp

@@ -50,7 +50,6 @@ NoStoppingAreaModule::NoStoppingAreaModule(
   planner_param_(planner_param),
   debug_data_()
 {
-  velocity_factor_.init(PlanningBehavior::NO_STOPPING_AREA);
   state_machine_.setState(StateMachine::State::GO);
   state_machine_.setMarginTime(planner_param_.state_clear_time);
 }
@@ -144,9 +143,11 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path)
 
     // Create StopReason
     {
-      velocity_factor_.set(
-        path->points, planner_data_->current_odometry->pose, stop_point->second,
-        VelocityFactor::UNKNOWN);
+      planning_factor_interface_->add(
+        path->points, planner_data_->current_odometry->pose, stop_point->second, stop_point->second,
+        tier4_planning_msgs::msg::PlanningFactor::STOP,
+        tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0,
+        0.0 /*shift distance*/, "");
     }
 
   } else if (state_machine_.getState() == StateMachine::State::GO) {

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,21 +170,12 @@ 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);
@@ -203,7 +184,6 @@ class SceneModuleManagerInterface
       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_) {
@@ -274,8 +254,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_stop_line_module/src/scene.cpp

@@ -38,7 +38,6 @@ StopLineModule::StopLineModule(
   state_(State::APPROACH),
   debug_data_()
 {
-  velocity_factor_.init(PlanningBehavior::STOP_SIGN);
 }
 
 bool StopLineModule::modifyPathVelocity(PathWithLaneId * path)
@@ -62,7 +61,12 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path)
 
   path->points = trajectory->restore();
 
-  updateVelocityFactor(&velocity_factor_, state_, *stop_point - ego_s);
+  // TODO(soblin): PlanningFactorInterface use trajectory class
+  planning_factor_interface_->add(
+    path->points, trajectory->compute(*stop_point).point.pose,
+    planner_data_->current_odometry->pose, planner_data_->current_odometry->pose,
+    tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
+    true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "stopline");
 
   updateStateAndStoppedTime(
     &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped());
@@ -152,24 +156,6 @@ void StopLineModule::updateStateAndStoppedTime(
   }
 }
 
-void StopLineModule::updateVelocityFactor(
-  autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state,
-  const double & distance_to_stop_point)
-{
-  switch (state) {
-    case State::APPROACH: {
-      velocity_factor->set(distance_to_stop_point, VelocityFactor::APPROACHING);
-      break;
-    }
-    case State::STOPPED: {
-      velocity_factor->set(distance_to_stop_point, VelocityFactor::STOPPED);
-      break;
-    }
-    case State::START:
-      break;
-  }
-}
-
 void StopLineModule::updateDebugData(
   DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const
 {

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp

@@ -18,7 +18,6 @@
 
 #include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp"
 #include "autoware/behavior_velocity_planner_common/utilization/util.hpp"
-#include "autoware/motion_utils/factor/velocity_factor_interface.hpp"
 #include "autoware/trajectory/path_point_with_lane_id.hpp"
 
 #include <Eigen/Core>
@@ -98,10 +97,6 @@ class StopLineModule : public SceneModuleInterface
     State * state, std::optional<rclcpp::Time> * stopped_time, const rclcpp::Time & now,
     const double & distance_to_stop_point, const bool & is_vehicle_stopped) const;
 
-  static void updateVelocityFactor(
-    autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state,
-    const double & distance_to_stop_point);
-
   void updateDebugData(
     DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const;
 

planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp

@@ -53,24 +53,13 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat
 
   autoware_perception_msgs::msg::TrafficLightGroup tl_state;
 
-  autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array;
-  velocity_factor_array.header.frame_id = "map";
-  velocity_factor_array.header.stamp = clock_->now();
-
   nearest_ref_stop_path_point_index_ = static_cast<int>(path->points.size() - 1);
   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);
 
-    // The velocity factor must be called after modifyPathVelocity.
-    const auto velocity_factor = traffic_light_scene_module->getVelocityFactor();
-    if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) {
-      velocity_factor_array.factors.emplace_back(velocity_factor);
-    }
-
     if (
       traffic_light_scene_module->getFirstRefStopPathPointIndex() <
       nearest_ref_stop_path_point_index_) {
@@ -88,7 +77,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat
     virtual_wall_marker_creator_.add_virtual_walls(
       traffic_light_scene_module->createVirtualWalls());
   }
-  pub_velocity_factor_->publish(velocity_factor_array);
   pub_debug_->publish(debug_marker_array);
   pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now()));
   pub_tl_state_->publish(tl_state);

planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp

@@ -54,7 +54,6 @@ TrafficLightModule::TrafficLightModule(
   debug_data_(),
   is_prev_state_stop_(false)
 {
-  velocity_factor_.init(PlanningBehavior::TRAFFIC_SIGNAL);
   planner_param_ = planner_param;
 }
 
@@ -293,9 +292,11 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose(
   size_t insert_index = insert_target_point_idx;
   planning_utils::insertVelocity(modified_path, target_point_with_lane_id, 0.0, insert_index);
 
-  velocity_factor_.set(
+  planning_factor_interface_->add(
     modified_path.points, planner_data_->current_odometry->pose,
-    target_point_with_lane_id.point.pose, VelocityFactor::UNKNOWN);
+    target_point_with_lane_id.point.pose, target_point_with_lane_id.point.pose,
+    tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
+    true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "");
 
   return modified_path;
 }

planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp

@@ -48,8 +48,6 @@ VirtualTrafficLightModule::VirtualTrafficLightModule(
   lane_(lane),
   planner_param_(planner_param)
 {
-  velocity_factor_.init(PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT);
-
   // Get map data
   const auto instrument = reg_elem_.getVirtualTrafficLight();
   const auto instrument_bottom_line = toAutowarePoints(instrument);
@@ -420,9 +418,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine(
   }
 
   // Set StopReason
-  velocity_factor_.set(
-    path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN,
-    command_.type);
+  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*/, "");
 
   // Set data for visualization
   module_data_.stop_head_pose_at_stop_line =
@@ -453,8 +452,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine(
   }
 
   // Set StopReason
-  velocity_factor_.set(
-    path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
+  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*/, "");
 
   // Set data for visualization
   module_data_.stop_head_pose_at_end_line =

planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp

@@ -113,10 +113,6 @@ class VirtualTrafficLightModule : public SceneModuleInterface
 
   void updateInfrastructureCommand();
 
-  void setVelocityFactor(
-    const geometry_msgs::msg::Pose & stop_pose,
-    autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor);
-
   std::optional<size_t> getPathIndexOfFirstEndLine();
 
   bool isBeforeStartLine(const size_t end_line_idx);