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(&node, "out_of_lane"); + debug_publisher_ = node.create_publisher("~/" + 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 & parameters) override; VelocityPlanningResult plan( const std::vector & ego_trajectory_points,