fix(obstacle_cruise_planner): use planning factor interface (#1718)

Signed-off-by: satoshi-ota <[email protected]>
tier4 · Jan 15, 2025 · bd7378e · bd7378e
1 parent 9b8ee37
commit bd7378e
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 1 deletion.
diff --git a/...re_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/...re_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,7 +48,9 @@ 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),
@@ -101,6 +104,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 +132,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};

diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp
@@ -726,6 +726,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
   // 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();

diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp
@@ -335,6 +335,10 @@ std::vector<TrajectoryPoint> PIDBasedPlanner::planCruise(
       debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle);
       debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data);
 
+      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{});
       velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
         planner_data.current_time, PlanningBehavior::ADAPTIVE_CRUISE,
         stop_traj_points.at(wall_idx).pose));

diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp
@@ -338,6 +338,10 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     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);
@@ -594,6 +598,13 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
         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