add planning_factor_interface to behavior_velocity_planner stopline, …

…traffic_light (#1766)

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

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,10 +53,6 @@ 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(
@@ -65,12 +61,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat
     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 +78,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*/, "traffic light");
 
   return modified_path;
 }