diff --git a/docs/ReleaseNotes.md b/docs/ReleaseNotes.md index 2a17ea89857..e7c2cd31362 100644 --- 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/simulation/traffic_simulator/src/behavior/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/simulation/traffic_simulator/src/behavior/vehicle/follow_lane_sequence/follow_front_entity_action.cpp index 1673cba3dff..4e4a13f9e9e 100644 --- a/simulation/traffic_simulator/src/behavior/vehicle/follow_lane_sequence/follow_front_entity_action.cpp +++ b/simulation/traffic_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 =