From 5d520f9e34eb5e1dac9ae5854e8b5107b2fffaa8 Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Wed, 18 Dec 2024 14:58:58 +0900
Subject: [PATCH] feat(out_of_lane): use planning factor

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../src/out_of_lane_module.cpp                              | 6 ++++++
 .../src/out_of_lane_module.hpp                              | 1 +
 2 files changed, 7 insertions(+)

diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp
index 63ca1b5784fe8..76ade4f81aada 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp
@@ -60,6 +60,9 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name)
   init_parameters(node);
   velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE);
 
+  planning_factor_interface_ =
+    std::make_unique<autoware::motion_utils::PlanningFactorInterface>(&node, "out_of_lane");
+
   debug_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
   virtual_wall_publisher_ =
@@ -315,6 +318,9 @@ VelocityPlanningResult OutOfLaneModule::plan(
       planner_data->current_odometry.twist.twist.linear.x > 0.1;
     const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING
                                        : motion_utils::VelocityFactor::STOPPED;
+    planning_factor_interface_->add(
+      ego_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN,
+      SafetyFactorArray{});
     velocity_factor_interface_.set(
       ego_trajectory_points, ego_data.pose, *slowdown_pose, status, "out_of_lane");
     result.velocity_factor = velocity_factor_interface_.get();
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp
index c39c6e17101c5..2ca6becbfdc28 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp
@@ -40,6 +40,7 @@ class OutOfLaneModule : 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,