feat(dynamic_obstacle_stop): set velocity factor to ROUTE_OBSTACLE (#6669)

maxime-clem · web-flow · commit af0456668b6b · 2024-03-22T17:20:59.000+09:00
Signed-off-by: Maxime CLEMENT &lt;maxime.clement@tier4.jp&gt;
diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp
@@ -116,7 +116,11 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
     }
   }
 
-  if (current_stop_pose_) motion_utils::insertStopPoint(*current_stop_pose_, 0.0, path->points);
+  if (current_stop_pose_) {
+    motion_utils::insertStopPoint(*current_stop_pose_, 0.0, path->points);
+    velocity_factor_.set(
+      path->points, ego_data.pose, *current_stop_pose_, VelocityFactor::ROUTE_OBSTACLE);
+  }
 
   const auto total_time_us = stopwatch.toc();
   RCLCPP_DEBUG(

Original file line number	Diff line number	Diff line change
`@@ -116,7 +116,11 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe`
`116`	`116`	`}`
`117`	`117`	`}`
`118`	`118`
`119`		`- if (current_stop_pose_) motion_utils::insertStopPoint(*current_stop_pose_, 0.0, path->points);`
	`119`	`+ if (current_stop_pose_) {`
	`120`	`+ motion_utils::insertStopPoint(*current_stop_pose_, 0.0, path->points);`
	`121`	`+ velocity_factor_.set(`
	`122`	`+ path->points, ego_data.pose, *current_stop_pose_, VelocityFactor::ROUTE_OBSTACLE);`
	`123`	`+ }`
`120`	`124`
`121`	`125`	`const auto total_time_us = stopwatch.toc();`
`122`	`126`	`RCLCPP_DEBUG(`