feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp

@@ -15,6 +15,7 @@
 #ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_
 #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_
 
+#include "autoware/motion_utils/factor/planning_factor_interface.hpp"
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/obstacle_cruise_planner/common_structs.hpp"
 #include "autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp"
@@ -47,15 +48,15 @@ class PlannerInterface
     rclcpp::Node & node, const LongitudinalInfo & longitudinal_info,
     const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
     const EgoNearestParam & ego_nearest_param, const std::shared_ptr<DebugData> debug_data_ptr)
-  : longitudinal_info_(longitudinal_info),
+  : planning_factor_interface_{std::make_unique<autoware::motion_utils::PlanningFactorInterface>(
+      &node, "obstacle_cruise_planner")},
+    longitudinal_info_(longitudinal_info),
     vehicle_info_(vehicle_info),
     ego_nearest_param_(ego_nearest_param),
     debug_data_ptr_(debug_data_ptr),
     slow_down_param_(SlowDownParam(node)),
     stop_param_(StopParam(node, longitudinal_info))
   {
-    velocity_factors_pub_ =
-      node.create_publisher<VelocityFactorArray>("/planning/velocity_factors/obstacle_cruise", 1);
     stop_speed_exceeded_pub_ =
       node.create_publisher<StopSpeedExceeded>("~/output/stop_speed_exceeded", 1);
     metrics_pub_ = node.create_publisher<MetricArray>("~/metrics", 10);
@@ -101,6 +102,7 @@ class PlannerInterface
     const std::optional<geometry_msgs::msg::Pose> & stop_pose = std::nullopt,
     const std::optional<StopObstacle> & stop_obstacle = std::nullopt);
   void publishMetrics(const rclcpp::Time & current_time);
+  void publishPlanningFactors() { planning_factor_interface_->publish(); }
   void clearMetrics();
 
   void onParam(const std::vector<rclcpp::Parameter> & parameters)
@@ -128,6 +130,8 @@ class PlannerInterface
   double getSafeDistanceMargin() const { return longitudinal_info_.safe_distance_margin; }
 
 protected:
+  std::unique_ptr<autoware::motion_utils::PlanningFactorInterface> planning_factor_interface_;
+
   // Parameters
   bool enable_debug_info_{false};
   bool enable_calculation_time_info_{false};
@@ -145,7 +149,6 @@ class PlannerInterface
     stop_watch_;
 
   // Publishers
-  rclcpp::Publisher<VelocityFactorArray>::SharedPtr velocity_factors_pub_;
   rclcpp::Publisher<StopSpeedExceeded>::SharedPtr stop_speed_exceeded_pub_;
   rclcpp::Publisher<MetricArray>::SharedPtr metrics_pub_;
 

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp

@@ -17,8 +17,6 @@
 
 #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
 
-#include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp"
-#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp"
 #include "autoware_perception_msgs/msg/predicted_object.hpp"
 #include "autoware_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
@@ -39,9 +37,6 @@
 #include <pcl/point_types.h>
 
 using autoware::vehicle_info_utils::VehicleInfo;
-using autoware_adapi_v1_msgs::msg::PlanningBehavior;
-using autoware_adapi_v1_msgs::msg::VelocityFactor;
-using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
 using autoware_perception_msgs::msg::ObjectClassification;
 using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedObjects;

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp

@@ -94,9 +94,6 @@ size_t getIndexWithLongitudinalOffset(
   return 0;
 }
 
-VelocityFactorArray makeVelocityFactorArray(
-  const rclcpp::Time & time, const std::string & behavior = PlanningBehavior::ROUTE_OBSTACLE,
-  const std::optional<geometry_msgs::msg::Pose> pose = std::nullopt);
 }  // namespace obstacle_cruise_utils
 
 #endif  // AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_

planning/autoware_obstacle_cruise_planner/package.xml

@@ -18,7 +18,6 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>

planning/autoware_obstacle_cruise_planner/src/node.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -726,6 +726,7 @@
   // 8. Publish debug data
   published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp);
   planner_ptr_->publishMetrics(now());
+  planner_ptr_->publishPlanningFactors();
   publishDebugMarker();
   publishDebugInfo();
   objects_of_interest_marker_interface_.publishMarkerArray();

planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -335,9 +335,10 @@
       debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle);
       debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data);
 
-      velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
-        planner_data.current_time, PlanningBehavior::ADAPTIVE_CRUISE,
-        stop_traj_points.at(wall_idx).pose));
+      planning_factor_interface_->add(
+        stop_traj_points, planner_data.ego_pose, stop_traj_points.at(wall_idx).pose,
+        tier4_planning_msgs::msg::PlanningFactor::NONE,
+        tier4_planning_msgs::msg::SafetyFactorArray{});
     }
 
     // do cruise planning

planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp

@@ -182,8 +182,6 @@
                                   : std::abs(vehicle_info_.min_longitudinal_offset_m);
 
   if (stop_obstacles.empty()) {
-    velocity_factors_pub_->publish(
-      obstacle_cruise_utils::makeVelocityFactorArray(planner_data.current_time));
     // delete marker
     const auto markers =
       autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
@@ -336,8 +334,10 @@
 
     // Publish Stop Reason
     const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose;
-    velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
-      planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, stop_pose));
+    planning_factor_interface_->add(
+      output_traj_points, planner_data.ego_pose, stop_pose,
+      tier4_planning_msgs::msg::PlanningFactor::STOP,
+      tier4_planning_msgs::msg::SafetyFactorArray{});
     // Store stop reason debug data
     debug_data_ptr_->stop_metrics =
       makeMetrics("PlannerInterface", "stop", planner_data, stop_pose, *determined_stop_obstacle);
@@ -589,11 +589,15 @@
 
       const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker(
         slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down",
         planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward);
-      velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
-        planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE,
-        slow_down_traj_points.at(slow_down_wall_idx).pose));
       autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker);
+      planning_factor_interface_->add(
+        slow_down_traj_points, planner_data.ego_pose,
+        slow_down_traj_points.at(*slow_down_start_idx).pose,
+        slow_down_traj_points.at(*slow_down_end_idx).pose,
+        tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN,
+        tier4_planning_msgs::msg::SafetyFactorArray{}, planner_data.is_driving_forward,
+        stable_slow_down_vel);
     }
 
     // add debug virtual wall

planning/autoware_obstacle_cruise_planner/src/utils.cpp

@@ -118,25 +118,4 @@ std::vector<StopObstacle> getClosestStopObstacles(const std::vector<StopObstacle
   return candidates;
 }
 
-VelocityFactorArray makeVelocityFactorArray(
-  const rclcpp::Time & time, const std::string & behavior,
-  const std::optional<geometry_msgs::msg::Pose> pose)
-{
-  VelocityFactorArray velocity_factor_array;
-  velocity_factor_array.header.frame_id = "map";
-  velocity_factor_array.header.stamp = time;
-
-  if (pose) {
-    using distance_type = VelocityFactor::_distance_type;
-    VelocityFactor velocity_factor;
-    velocity_factor.behavior = behavior;
-    velocity_factor.pose = pose.value();
-    velocity_factor.distance = std::numeric_limits<distance_type>::quiet_NaN();
-    velocity_factor.status = VelocityFactor::UNKNOWN;
-    velocity_factor.detail = std::string();
-    velocity_factor_array.factors.push_back(velocity_factor);
-  }
-  return velocity_factor_array;
-}
-
 }  // namespace obstacle_cruise_utils

planning/autoware_surround_obstacle_checker/package.xml

@@ -14,7 +14,6 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
-  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>

planning/autoware_surround_obstacle_checker/src/debug_marker.cpp

@@ -67,7 +67,9 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode(
   const double & surround_check_back_distance, const double & surround_check_hysteresis_distance,
   const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock,
   rclcpp::Node & node)
-: vehicle_info_(vehicle_info),
+: planning_factor_interface_{std::make_unique<autoware::motion_utils::PlanningFactorInterface>(
+    &node, "surround_obstacle_checker")},
+  vehicle_info_(vehicle_info),
   object_label_(object_label),
   surround_check_front_distance_(surround_check_front_distance),
   surround_check_side_distance_(surround_check_side_distance),
@@ -77,8 +79,6 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode(
   clock_(clock)
 {
   debug_viz_pub_ = node.create_publisher<visualization_msgs::msg::MarkerArray>("~/debug/marker", 1);
-  velocity_factor_pub_ =
-    node.create_publisher<VelocityFactorArray>("/planning/velocity_factors/surround_obstacle", 1);
   vehicle_footprint_pub_ = node.create_publisher<PolygonStamped>("~/debug/footprint", 1);
   vehicle_footprint_offset_pub_ =
     node.create_publisher<PolygonStamped>("~/debug/footprint_offset", 1);
@@ -143,8 +143,12 @@ void SurroundObstacleCheckerDebugNode::publish()
   debug_viz_pub_->publish(visualization_msg);
 
   /* publish stop reason for autoware api */
-  const auto velocity_factor_msg = makeVelocityFactorArray();
-  velocity_factor_pub_->publish(velocity_factor_msg);
+  if (stop_pose_ptr_ != nullptr) {
+    planning_factor_interface_->add(
+      0.0, *stop_pose_ptr_, tier4_planning_msgs::msg::PlanningFactor::STOP,
+      tier4_planning_msgs::msg::SafetyFactorArray{});
+  }
+  planning_factor_interface_->publish();
 
   /* reset variables */
   stop_pose_ptr_ = nullptr;
@@ -170,25 +174,6 @@ MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker()
   return msg;
 }
 
-VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray()
-{
-  VelocityFactorArray velocity_factor_array;
-  velocity_factor_array.header.frame_id = "map";
-  velocity_factor_array.header.stamp = clock_->now();
-
-  if (stop_pose_ptr_) {
-    using distance_type = VelocityFactor::_distance_type;
-    VelocityFactor velocity_factor;
-    velocity_factor.behavior = PlanningBehavior::SURROUNDING_OBSTACLE;
-    velocity_factor.pose = *stop_pose_ptr_;
-    velocity_factor.distance = std::numeric_limits<distance_type>::quiet_NaN();
-    velocity_factor.status = VelocityFactor::UNKNOWN;
-    velocity_factor.detail = std::string();
-    velocity_factor_array.factors.push_back(velocity_factor);
-  }
-  return velocity_factor_array;
-}
-
 PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped(
   const Polygon2d & boost_polygon, const double & z)
 {

planning/autoware_surround_obstacle_checker/src/debug_marker.hpp

@@ -15,13 +15,13 @@
 #ifndef DEBUG_MARKER_HPP_
 #define DEBUG_MARKER_HPP_
 
+#include <autoware/motion_utils/factor/planning_factor_interface.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
-#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
 #include <geometry_msgs/msg/polygon_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
+#include <tier4_planning_msgs/msg/planning_factor_array.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -34,10 +34,10 @@ namespace autoware::surround_obstacle_checker
 {
 
 using autoware::vehicle_info_utils::VehicleInfo;
-using autoware_adapi_v1_msgs::msg::PlanningBehavior;
-using autoware_adapi_v1_msgs::msg::VelocityFactor;
-using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
 using geometry_msgs::msg::PolygonStamped;
+using tier4_planning_msgs::msg::ControlPoint;
+using tier4_planning_msgs::msg::PlanningFactor;
+using tier4_planning_msgs::msg::PlanningFactorArray;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
@@ -65,12 +65,13 @@ class SurroundObstacleCheckerDebugNode
 
 private:
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_viz_pub_;
-  rclcpp::Publisher<VelocityFactorArray>::SharedPtr velocity_factor_pub_;
 
   rclcpp::Publisher<PolygonStamped>::SharedPtr vehicle_footprint_pub_;
   rclcpp::Publisher<PolygonStamped>::SharedPtr vehicle_footprint_offset_pub_;
   rclcpp::Publisher<PolygonStamped>::SharedPtr vehicle_footprint_recover_offset_pub_;
 
+  std::unique_ptr<autoware::motion_utils::PlanningFactorInterface> planning_factor_interface_;
+
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
   std::string object_label_;
   double surround_check_front_distance_;
@@ -80,7 +81,6 @@ class SurroundObstacleCheckerDebugNode
   geometry_msgs::msg::Pose self_pose_;
 
   MarkerArray makeVisualizationMarker();
-  VelocityFactorArray makeVelocityFactorArray();
 
   PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z);
 

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp

@@ -32,13 +32,15 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(
   const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
   const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
   std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-    objects_of_interest_marker_interface_ptr_map)
+    objects_of_interest_marker_interface_ptr_map,
+  const std::shared_ptr<PlanningFactorInterface> & planning_factor_interface)
 : LaneChangeInterface{
     name,
     node,
     parameters,
     rtc_interface_ptr_map,
     objects_of_interest_marker_interface_ptr_map,
+    planning_factor_interface,
     std::make_unique<AvoidanceByLaneChange>(parameters, avoidance_by_lane_change_parameters)}
 {
 }

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp

@@ -40,7 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface
     const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-      objects_of_interest_marker_interface_ptr_map);
+      objects_of_interest_marker_interface_ptr_map,
+    const std::shared_ptr<PlanningFactorInterface> & planning_factor_interface);
 
   bool isExecutionRequested() const override;
 

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -190,7 +190,7 @@ SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance()
 {
   return std::make_unique<AvoidanceByLaneChangeInterface>(
     name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_,
-    objects_of_interest_marker_interface_ptr_map_);
+    objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_);
 }
 
 }  // namespace autoware::behavior_path_planner

planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp

@@ -42,7 +42,7 @@
   {
     return std::make_unique<DynamicObstacleAvoidanceModule>(
       name_, *node_, parameters_, rtc_interface_ptr_map_,
-      objects_of_interest_marker_interface_ptr_map_);
+      objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp

@@ -351,7 +351,8 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
     std::shared_ptr<DynamicAvoidanceParameters> parameters,
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-      objects_of_interest_marker_interface_ptr_map);
+      objects_of_interest_marker_interface_ptr_map,
+    const std::shared_ptr<PlanningFactorInterface> planning_factor_interface);
 
   void updateModuleParams(const std::any & parameters) override
   {

planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp

@@ -331,8 +331,9 @@
   std::shared_ptr<DynamicAvoidanceParameters> parameters,
   const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
   std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-    objects_of_interest_marker_interface_ptr_map)
-: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
+    objects_of_interest_marker_interface_ptr_map,
+  const std::shared_ptr<PlanningFactorInterface> planning_factor_interface)
+: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface},  // NOLINT
   parameters_{std::move(parameters)},
   target_objects_manager_{TargetObjectsManager(
     parameters_->successive_num_to_entry_dynamic_avoidance_condition,

planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp

@@ -28,7 +28,7 @@ ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance()
 {
   return std::make_unique<LaneChangeInterface>(
     name_, *node_, parameters_, rtc_interface_ptr_map_,
-    objects_of_interest_marker_interface_ptr_map_,
+    objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_,
     std::make_unique<ExternalRequestLaneChange>(parameters_, direction_));
 }
 
@@ -37,7 +37,7 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance()
 {
   return std::make_unique<LaneChangeInterface>(
     name_, *node_, parameters_, rtc_interface_ptr_map_,
-    objects_of_interest_marker_interface_ptr_map_,
+    objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_,
     std::make_unique<ExternalRequestLaneChange>(parameters_, direction_));
 }