feat(dynamic_obstacle_stop): output planning factor interface (#1732)

* feat(motion_velocity_planner): use planning factor interface

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(dynamic_obstacle_stop): output planning factor interface

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp

@@ -45,6 +45,9 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo
   clock_ = node.get_clock();
   velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE);
 
+  planning_factor_interface_ = std::make_unique<autoware::motion_utils::PlanningFactorInterface>(
+    &node, "dynamic_obstacle_stop");
+
   debug_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
   virtual_wall_publisher_ =
@@ -163,6 +166,9 @@ VelocityPlanningResult DynamicObstacleStopModule::plan(
       const auto stop_pose_reached =
         planner_data->current_odometry.twist.twist.linear.x < 1e-3 &&
         autoware::universe_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3;
+      planning_factor_interface_->add(
+        ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP,
+        SafetyFactorArray{});
       velocity_factor_interface_.set(
         ego_trajectory_points, ego_data.pose, *stop_pose,
         stop_pose_reached ? autoware::motion_utils::VelocityFactor::STOPPED

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp

@@ -33,6 +33,7 @@ class DynamicObstacleStopModule : public PluginModuleInterface
 {
 public:
   void init(rclcpp::Node & node, const std::string & module_name) override;
+  void publish_planning_factor() override { planning_factor_interface_->publish(); };
   void update_parameters(const std::vector<rclcpp::Parameter> & parameters) override;
   VelocityPlanningResult plan(
     const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp

@@ -18,6 +18,7 @@
 #include "planner_data.hpp"
 #include "velocity_planning_result.hpp"
 
+#include <autoware/motion_utils/factor/planning_factor_interface.hpp>
 #include <autoware/motion_utils/factor/velocity_factor_interface.hpp>
 #include <autoware/universe_utils/ros/processing_time_publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -32,6 +33,9 @@
 namespace autoware::motion_velocity_planner
 {
 
+using tier4_planning_msgs::msg::PlanningFactor;
+using tier4_planning_msgs::msg::SafetyFactorArray;
+
 class PluginModuleInterface
 {
 public:
@@ -42,6 +46,7 @@ class PluginModuleInterface
     const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
     const std::shared_ptr<const PlannerData> planner_data) = 0;
   virtual std::string get_module_name() const = 0;
+  virtual void publish_planning_factor() {}
   autoware::motion_utils::VelocityFactorInterface velocity_factor_interface_;
   rclcpp::Logger logger_ = rclcpp::get_logger("");
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_publisher_;
@@ -50,6 +55,9 @@ class PluginModuleInterface
   rclcpp::Publisher<autoware_internal_debug_msgs::msg::Float64Stamped>::SharedPtr
     processing_time_publisher_;
   autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{};
+
+protected:
+  std::unique_ptr<autoware::motion_utils::PlanningFactorInterface> planning_factor_interface_;
 };
 
 }  // namespace autoware::motion_velocity_planner

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp

@@ -102,6 +102,8 @@ std::vector<VelocityPlanningResult> MotionVelocityPlannerManager::plan_velocitie
     VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data);
     results.push_back(res);
 
+    plugin->publish_planning_factor();
+
     if (res.stop_points.size() > 0) {
       const auto stop_decision_metric = make_decision_metric(plugin->get_module_name(), "stop");
       metrics_.push_back(stop_decision_metric);