fix(run_out): output velocity factor (autowarefoundation#9319)

* fix(run_out): output velocity factor Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(adapi): subscribe run out velocity factor Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1222-takeshi · Nov 14, 2024 · 7eec0bb · 7eec0bb
1 parent 01e78a1
commit 7eec0bb
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 1 deletion.
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
@@ -50,7 +50,7 @@ RunOutModule::RunOutModule(
   debug_ptr_(debug_ptr),
   state_machine_(std::make_unique<run_out_utils::StateMachine>(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
@@ -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",