feat(bpp): use planning factor (#1719)

feat(bpp): output planning factor

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

Author: satoshi-ota

planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp

@@ -184,6 +184,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
     m->publishRTCStatus();
     m->publishSteeringFactor();
     m->publishVelocityFactor();
+    m->publish_planning_factors();
   });
 
   generateCombinedDrivableArea(result_output.valid_output, data);

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp

@@ -21,6 +21,7 @@
 #include "autoware/behavior_path_planner_common/utils/utils.hpp"
 
 #include <autoware/behavior_path_planner_common/turn_signal_decider.hpp>
+#include <autoware/motion_utils/factor/planning_factor_interface.hpp>
 #include <autoware/motion_utils/factor/steering_factor_interface.hpp>
 #include <autoware/motion_utils/factor/velocity_factor_interface.hpp>
 #include <autoware/motion_utils/marker/marker_helper.hpp>
@@ -54,6 +55,7 @@
 
 namespace autoware::behavior_path_planner
 {
+using autoware::motion_utils::PlanningFactorInterface;
 using autoware::motion_utils::SteeringFactorInterface;
 using autoware::motion_utils::VelocityFactorInterface;
 using autoware::objects_of_interest_marker_interface::ColorName;
@@ -66,6 +68,8 @@ using autoware_adapi_v1_msgs::msg::SteeringFactor;
 using autoware_adapi_v1_msgs::msg::VelocityFactor;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using tier4_planning_msgs::msg::PathWithLaneId;
+using tier4_planning_msgs::msg::PlanningFactor;
+using tier4_planning_msgs::msg::SafetyFactorArray;
 using tier4_rtc_msgs::msg::State;
 using unique_identifier_msgs::msg::UUID;
 using visualization_msgs::msg::MarkerArray;
@@ -82,6 +86,27 @@ enum class ModuleStatus {
 class SceneModuleInterface
 {
 public:
+  SceneModuleInterface(
+    const std::string & name, rclcpp::Node & node,
+    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,
+    const std::shared_ptr<PlanningFactorInterface> planning_factor_interface)
+  : name_{name},
+    logger_{node.get_logger().get_child(name)},
+    clock_{node.get_clock()},
+    rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)),
+    objects_of_interest_marker_interface_ptr_map_(
+      std::move(objects_of_interest_marker_interface_ptr_map)),
+    planning_factor_interface_{planning_factor_interface},
+    time_keeper_(std::make_shared<universe_utils::TimeKeeper>())
+  {
+    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
+      uuid_map_.emplace(module_name, generateUUID());
+    }
+  }
+
+  // TODO(satoshi-ota): remove this constructor after all planning factors have been migrated.
   SceneModuleInterface(
     const std::string & name, rclcpp::Node & node,
     std::unordered_map<std::string, std::shared_ptr<RTCInterface>> rtc_interface_ptr_map,
@@ -93,6 +118,7 @@ class SceneModuleInterface
     rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)),
     objects_of_interest_marker_interface_ptr_map_(
       std::move(objects_of_interest_marker_interface_ptr_map)),
+    planning_factor_interface_{std::make_shared<PlanningFactorInterface>(&node, "tmp")},
     time_keeper_(std::make_shared<universe_utils::TimeKeeper>())
   {
     for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
@@ -574,6 +600,24 @@ class SceneModuleInterface
       path.points, getEgoPose(), slow_pose_.value().pose, VelocityFactor::APPROACHING, "slow down");
   }
 
+  void set_longitudinal_planning_factor(const PathWithLaneId & path)
+  {
+    if (stop_pose_.has_value()) {
+      planning_factor_interface_->add(
+        path.points, getEgoPose(), stop_pose_.value().pose, PlanningFactor::STOP,
+        SafetyFactorArray{});
+      return;
+    }
+
+    if (!slow_pose_.has_value()) {
+      return;
+    }
+
+    planning_factor_interface_->add(
+      path.points, getEgoPose(), slow_pose_.value().pose, PlanningFactor::SLOW_DOWN,
+      SafetyFactorArray{});
+  }
+
   void setDrivableLanes(const std::vector<DrivableLanes> & drivable_lanes);
 
   BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; }
@@ -627,6 +671,8 @@ class SceneModuleInterface
   std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>>
     objects_of_interest_marker_interface_ptr_map_;
 
+  mutable std::shared_ptr<PlanningFactorInterface> planning_factor_interface_;
+
   mutable SteeringFactorInterface steering_factor_interface_;
 
   mutable VelocityFactorInterface velocity_factor_interface_;

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp

@@ -146,6 +146,8 @@ class SceneModuleManagerInterface
     pub_velocity_factors_->publish(velocity_factor_array);
   }
 
+  void publish_planning_factors() { planning_factor_interface_->publish(); }
+
   void publishVirtualWall() const
   {
     using autoware::universe_utils::appendMarkerArray;
@@ -318,6 +320,8 @@ class SceneModuleManagerInterface
 
   std::unique_ptr<SceneModuleInterface> idle_module_ptr_;
 
+  std::shared_ptr<PlanningFactorInterface> planning_factor_interface_;
+
   std::unordered_map<std::string, std::shared_ptr<RTCInterface>> rtc_interface_ptr_map_;
 
   std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>>

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp

@@ -67,6 +67,11 @@ void SceneModuleManagerInterface::initInterface(
       node->create_publisher<VelocityFactorArray>("/planning/velocity_factors/" + name_, 1);
   }
 
+  // planning factor
+  {
+    planning_factor_interface_ = std::make_shared<PlanningFactorInterface>(node, name_);
+  }
+
   // misc
   {
     node_ = node;