feat(bpp-intersection): use planning factor (#1728)

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

Author: satoshi-ota

planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp

@@ -20,6 +20,7 @@
 #include "autoware/universe_utils/ros/logger_level_configure.hpp"
 #include "planner_manager.hpp"
 
+#include <autoware/motion_utils/factor/planning_factor_interface.hpp>
 #include <autoware/motion_utils/factor/steering_factor_interface.hpp>
 #include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
@@ -51,6 +52,7 @@
 
 namespace autoware::behavior_path_planner
 {
+using autoware::motion_utils::PlanningFactorInterface;
 using autoware::motion_utils::SteeringFactorInterface;
 using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_map_msgs::msg::LaneletMapBin;
@@ -137,6 +139,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   std::shared_ptr<PlannerManager> planner_manager_;
 
   SteeringFactorInterface steering_factor_interface_;
+  std::unique_ptr<PlanningFactorInterface> planning_factor_interface_;
 
   std::mutex mutex_pd_;       // mutex for planner_data_
   std::mutex mutex_manager_;  // mutex for bt_manager_ or planner_manager_

planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -32,7 +32,9 @@ using autoware::vehicle_info_utils::VehicleInfoUtils;
 using tier4_planning_msgs::msg::PathChangeModuleId;
 
 BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options)
-: Node("behavior_path_planner", node_options)
+: Node("behavior_path_planner", node_options),
+  planning_factor_interface_{
+    std::make_unique<PlanningFactorInterface>(this, "behavior_path_planner")}
 {
   using std::placeholders::_1;
   using std::chrono_literals::operator""ms;
@@ -476,6 +478,17 @@ void BehaviorPathPlannerNode::publish_steering_factor(
     steering_factor_interface_.set(
       {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance},
       steering_factor_direction, SteeringFactor::TURNING, "");
+
+    const uint16_t planning_factor_direction = std::invoke([&turn_signal]() {
+      if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
+        return PlanningFactor::TURN_LEFT;
+      }
+      return PlanningFactor::TURN_RIGHT;
+    });
+
+    planning_factor_interface_->add(
+      intersection_distance, intersection_distance, intersection_pose, intersection_pose,
+      planning_factor_direction, SafetyFactorArray{});
   } else {
     steering_factor_interface_.reset();
   }
@@ -490,6 +503,8 @@ void BehaviorPathPlannerNode::publish_steering_factor(
   }
 
   pub_steering_factors_->publish(steering_factor_array);
+
+  planning_factor_interface_->publish();
 }
 
 void BehaviorPathPlannerNode::publish_reroute_availability() const