diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index 5a219f654561a..2296655d700d3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -50,7 +50,7 @@ RunOutModule::RunOutModule( debug_ptr_(debug_ptr), state_machine_(std::make_unique(planner_param.approaching.state)) { - velocity_factor_.init(PlanningBehavior::UNKNOWN); + velocity_factor_.init(PlanningBehavior::ROUTE_OBSTACLE); if (planner_param.run_out.use_partition_lanelet) { const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); @@ -768,6 +768,11 @@ bool RunOutModule::insertStopPoint( stop_point_with_lane_id = path.points.at(nearest_seg_idx); stop_point_with_lane_id.point.pose = *stop_point; planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); + + velocity_factor_.set( + path.points, planner_data_->current_odometry->pose, stop_point.value(), VelocityFactor::UNKNOWN, + "run_out"); + return true; } @@ -870,6 +875,9 @@ void RunOutModule::insertApproachingVelocity( return; } + velocity_factor_.set( + output_path.points, current_pose, stop_point.value(), VelocityFactor::UNKNOWN, "run_out"); + // debug debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( *stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); diff --git a/system/autoware_default_adapi/src/planning.cpp b/system/autoware_default_adapi/src/planning.cpp index 49c4e8f432de6..4ff0edfd2b7d5 100644 --- a/system/autoware_default_adapi/src/planning.cpp +++ b/system/autoware_default_adapi/src/planning.cpp @@ -81,6 +81,7 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning "/planning/velocity_factors/obstacle_stop", "/planning/velocity_factors/obstacle_cruise", "/planning/velocity_factors/occlusion_spot", + "/planning/velocity_factors/run_out", "/planning/velocity_factors/stop_line", "/planning/velocity_factors/surround_obstacle", "/planning/velocity_factors/traffic_light",