feat(out_of_lane): use planning factor (#1733)

Signed-off-by: satoshi-ota <[email protected]>
tier4 · Dec 23, 2024 · e6cf25f · e6cf25f
1 parent c10a410
commit e6cf25f
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 0 deletions.
diff --git a/...n_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/...n_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/...n_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/...n_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,