Merge pull request #421 from tier4/fix/npc_target_speed_in_follow_fro…

…nt_entity_action
tier4 · Jul 30, 2021 · 2e2eaa3 · 2e2eaa3
2 parents 479f9ef + cc82e7f
commit 2e2eaa3
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 0 deletions.
diff --git a/docs/ReleaseNotes.md b/docs/ReleaseNotes.md
@@ -1,6 +1,7 @@
 # Release Notes
 
 ## Difference between the latest release and master
+- Fix problem in follow front entity action, velocity planner was ignored requested target speed.
 
 ## Version 0.4.0
 - [Release Page](https://github.com/tier4/scenario_simulator_v2/releases/0.4.0) on Github :fa-github:

diff --git a/...raffic_simulator/src/behavior/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/...raffic_simulator/src/behavior/vehicle/follow_lane_sequence/follow_front_entity_action.cpp
@@ -103,17 +103,34 @@ BT::NodeStatus FollowFrontEntityAction::tick()
   if (!front_entity_status) {
     return BT::NodeStatus::FAILURE;
   }
+  if (!target_speed) {
+    target_speed = hdmap_utils->getSpeedLimit(route_lanelets);
+  }
+  if (target_speed.get() <= front_entity_status.get().action_status.twist.linear.x) {
+    auto entity_status_updated = calculateEntityStatusUpdated(target_speed.get());
+    setOutput("updated_status", entity_status_updated);
+    const auto obstacle = calculateObstacle(waypoints);
+    setOutput("waypoints", waypoints);
+    setOutput("obstacle", obstacle);
+    return BT::NodeStatus::RUNNING;
+  }
   if (
     distance_to_front_entity_.get() >=
     (calculateStopDistance() + vehicle_parameters.bounding_box.dimensions.x + 5)) {
     auto entity_status_updated =
       calculateEntityStatusUpdated(front_entity_status.get().action_status.twist.linear.x + 2);
     setOutput("updated_status", entity_status_updated);
+    const auto obstacle = calculateObstacle(waypoints);
+    setOutput("waypoints", waypoints);
+    setOutput("obstacle", obstacle);
     return BT::NodeStatus::RUNNING;
   } else if (distance_to_front_entity_.get() <= calculateStopDistance()) {
     auto entity_status_updated =
       calculateEntityStatusUpdated(front_entity_status.get().action_status.twist.linear.x - 2);
     setOutput("updated_status", entity_status_updated);
+    const auto obstacle = calculateObstacle(waypoints);
+    setOutput("waypoints", waypoints);
+    setOutput("obstacle", obstacle);
     return BT::NodeStatus::RUNNING;
   } else {
     auto entity_status_updated =