diff --git a/docs/developer_guide/.pages b/docs/developer_guide/.pages index 8885c0b362d..450f1059bcc 100644 --- a/docs/developer_guide/.pages +++ b/docs/developer_guide/.pages @@ -18,3 +18,4 @@ nav: - CONTRIBUTING.md - Communication.md - ConfiguringPerceptionTopics.md + - ManualOverrideWithFollowTrajectoryAction.md diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index c5b5ef0115e..17c3880e8b9 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -48,14 +48,19 @@ ### Service Clients -| service name | type | note | -|------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------|------| -| `/api/autoware/set/velocity_limit` | [`tier4_external_api_msgs/srv/SetVelocityLimit`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/srv/SetVelocityLimit.srv) | | -| `/api/external/set/engage` | [`tier4_external_api_msgs/srv/Engage`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/srv/Engage.srv) | | -| `/api/external/set/rtc_commands` | [`tier4_rtc_msgs/srv/CooperateCommands`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/srv/CooperateCommands.srv) | | -| `/api/external/set/rtc_auto_mode` | [`tier4_rtc_msgs/srv/AutoModeWithModule`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/srv/AutoModeWithModule.srv) | | - - +| service name | type | note | +|-----------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|------| +| `/api/autoware/set/velocity_limit` | [`tier4_external_api_msgs/srv/SetVelocityLimit`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/srv/SetVelocityLimit.srv) | | +| `/api/external/set/engage` | [`tier4_external_api_msgs/srv/Engage`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/srv/Engage.srv) | | +| `/api/external/set/rtc_commands` | [`tier4_rtc_msgs/srv/CooperateCommands`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/srv/CooperateCommands.srv) | | +| `/api/external/set/rtc_auto_mode` | [`tier4_rtc_msgs/srv/AutoModeWithModule`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/srv/AutoModeWithModule.srv) | | +| `/api/operation_mode/enable_autoware_control` | [`autoware_adapi_v1_msgs/srv/ChangeOperationMode`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/operation_mode/srv/ChangeOperationMode.srv) | | + +### Service Servers + +| service name | type | note | +|---------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------------------------------------------| +| `/control/control_mode_request` | [`autoware_auto_vehicle_msgs/srv/ControlModeCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/srv/ControlModeCommand.srv) | Simulated by `simple_sensor_simulator` for a manual override | [//]: # (/simulation/openscenario_visualizer) diff --git a/docs/developer_guide/ManualOverrideWithFollowTrajectoryAction.md b/docs/developer_guide/ManualOverrideWithFollowTrajectoryAction.md new file mode 100644 index 00000000000..e0e4443fb1a --- /dev/null +++ b/docs/developer_guide/ManualOverrideWithFollowTrajectoryAction.md @@ -0,0 +1,40 @@ +# Manual Override Simulation with FollowTrajectoryAction + +`scenario_simulator_v2` simulates the manual override of Autoware, with `FollowTrajectoryAction`. +During the executing `FollowTrajectoryAction`, the control of the ego entity is taken over from Autoware to the `FollowTrajectoryAction`. + +## 3 types of override for Autoware + +There are 3 types of override for Autoware. + +- Local: Manually control the vehicle from nearby with some device such as a joystick. + - This is one of operation modes. +- Remote: Manually control the vehicle from a web application on the cloud. + - This is one of operation modes. +- Direct: Manually control the vehicle from handle, brake and/or accel directly. + - Please note that this is not a operation mode but a control mode of vehicle interface. + +## override simulation in scenario_simulator_v2 + +vehicle interface simulation is a part of the ego vehicle simulation feature in `scenario_simulator_v2`. +`scenario_simulator_v2` simulates a `Direct` override triggered by safety operators when a scenario commands overriding the ego vehicle by `FollowTrajectoryAction`. + +## 3 steps scenario_simulator_v2 takes to simulate the overrides + +### 1. triggering the override + +In real vehicle, the override detected in vehicle internally and communicated to vehicle interface node such as `pacmod_interface` node. + +In `scenario_simulator_v2`, `openscenario_interpreter` send an override flag via zmq interface between `traffic_simulator` and `simple_sensor_simulator` when `FollowTrajectoryAction` is started. + +`simple_sensor_simulator` receives it and set the control mode to MANUAL like vehicle interface do when hardware override triggers detected. + +### 2. during the override + +`traffic_simulator` send ego status calculated to follow described in the scenario and `simple_sensor_simulator` overrides Autoware control with overwriting ego status by the received ego status. + +### 3. finishing the override + +When `FollowTrajectoryAction` is finished, `traffic_simulator` call service to enable autoware control and stop sending the override flag to `simple_sensor_simulator` via zmq communication. + +This mimics the steps safety operators do in real vehicle via some human interfaces, in API level. diff --git a/external/concealer/include/concealer/autoware.hpp b/external/concealer/include/concealer/autoware.hpp index ff27ba70d0c..12a50710a92 100644 --- a/external/concealer/include/concealer/autoware.hpp +++ b/external/concealer/include/concealer/autoware.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -66,12 +67,17 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster std::vector = 0; + virtual auto getControlModeReport() const + -> autoware_auto_vehicle_msgs::msg::ControlModeReport = 0; + auto set(const geometry_msgs::msg::Accel &) -> void; auto set(const geometry_msgs::msg::Twist &) -> void; auto set(const geometry_msgs::msg::Pose &) -> void; + virtual auto setManualMode() -> void = 0; + virtual auto rethrow() -> void; }; } // namespace concealer diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 9f4ef56c312..8e96c0f4abf 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -37,20 +38,34 @@ namespace concealer class AutowareUniverse : public Autoware { // clang-format off - SubscriberWrapper getAckermannControlCommand; - SubscriberWrapper getGearCommandImpl; - SubscriberWrapper getTurnIndicatorsCommand; - SubscriberWrapper getPathWithLaneId; - - PublisherWrapper setAcceleration; - PublisherWrapper setOdometry; - PublisherWrapper setSteeringReport; - PublisherWrapper setGearReport; - PublisherWrapper setControlModeReport; - PublisherWrapper setVelocityReport; - PublisherWrapper setTurnIndicatorsReport; + using ControlModeCommand = autoware_auto_vehicle_msgs::srv::ControlModeCommand; + using ControlModeReport = autoware_auto_vehicle_msgs::msg::ControlModeReport; + using GearCommand = autoware_auto_vehicle_msgs::msg::GearCommand; + using GearReport = autoware_auto_vehicle_msgs::msg::GearReport; + using TurnIndicatorsCommand = autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; + using PathWithLaneId = autoware_auto_planning_msgs::msg::PathWithLaneId; + using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport; + using VelocityReport = autoware_auto_vehicle_msgs::msg::VelocityReport; + using TurnIndicatorsReport = autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; + using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; + using AccelWithCovarianceStamped = geometry_msgs::msg::AccelWithCovarianceStamped; + + SubscriberWrapper getAckermannControlCommand; + SubscriberWrapper getGearCommandImpl; + SubscriberWrapper getTurnIndicatorsCommand; + SubscriberWrapper getPathWithLaneId; + + PublisherWrapper setAcceleration; + PublisherWrapper setOdometry; + PublisherWrapper setSteeringReport; + PublisherWrapper setGearReport; + PublisherWrapper setControlModeReport; + PublisherWrapper setVelocityReport; + PublisherWrapper setTurnIndicatorsReport; // clang-format on + rclcpp::Service::SharedPtr control_mode_request_server; + const rclcpp::TimerBase::SharedPtr localization_update_timer; const rclcpp::TimerBase::SharedPtr vehicle_state_update_timer; @@ -59,6 +74,8 @@ class AutowareUniverse : public Autoware std::atomic is_stop_requested = false; + std::atomic current_control_mode = ControlModeReport::AUTONOMOUS; + std::atomic is_thrown = false; std::exception_ptr thrown; @@ -82,15 +99,17 @@ class AutowareUniverse : public Autoware auto updateVehicleState() -> void; - auto getGearCommand() const -> autoware_auto_vehicle_msgs::msg::GearCommand override; + auto getGearCommand() const -> GearCommand override; auto getGearSign() const -> double override; - auto getVehicleCommand() const -> std::tuple< - autoware_auto_control_msgs::msg::AckermannControlCommand, - autoware_auto_vehicle_msgs::msg::GearCommand> override; + auto getVehicleCommand() const -> std::tuple override; auto getRouteLanelets() const -> std::vector override; + + auto getControlModeReport() const -> ControlModeReport override; + + auto setManualMode() -> void override; }; } // namespace concealer diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 75b1cd0a62b..57b7e0b4776 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -154,6 +154,8 @@ class FieldOperatorApplication : public rclcpp::Node virtual auto sendCooperateCommand(const std::string &, const std::string &) -> void = 0; virtual auto setVelocityLimit(double) -> void = 0; + + virtual auto enableAutowareControl() -> void = 0; }; } // namespace concealer diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 564f0443a54..481a33b87c0 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -28,6 +28,7 @@ #endif #include +#include #include #include #include @@ -84,6 +85,7 @@ class FieldOperatorApplicationFor ServiceWithValidation requestSetRoutePoints; ServiceWithValidation requestSetRtcAutoMode; ServiceWithValidation requestSetVelocityLimit; + ServiceWithValidation requestEnableAutowareControl; // clang-format on tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array; @@ -188,6 +190,7 @@ class FieldOperatorApplicationFor requestSetRoutePoints("/api/routing/set_route_points", *this, std::chrono::seconds(10)), requestSetRtcAutoMode("/api/external/set/rtc_auto_mode", *this), requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this), + requestEnableAutowareControl("/api/operation_mode/enable_autoware_control", *this), getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this) // clang-format on { @@ -227,6 +230,8 @@ class FieldOperatorApplicationFor auto sendCooperateCommand(const std::string &, const std::string &) -> void override; auto setVelocityLimit(double) -> void override; + + auto enableAutowareControl() -> void override; }; } // namespace concealer diff --git a/external/concealer/include/concealer/service_with_validation.hpp b/external/concealer/include/concealer/service_with_validation.hpp index f083724d722..62421299c85 100644 --- a/external/concealer/include/concealer/service_with_validation.hpp +++ b/external/concealer/include/concealer/service_with_validation.hpp @@ -116,11 +116,12 @@ class ServiceWithValidation return; } else { RCLCPP_ERROR_STREAM( - logger, service_name - << " service request was accepted, but ResponseStatus::success is false " - << (service_call_status.message.empty() - ? "" - : " (" + service_call_status.message + ")")); + logger, service_name << " service request was accepted, but " + "ResponseStatus::success is false with error code: " + << service_call_status.code << ", and message: " + << (service_call_status.message.empty() + ? "" + : " (" + service_call_status.message + ")")); } } else { RCLCPP_INFO_STREAM(logger, service_name << " service request has been accepted."); diff --git a/external/concealer/include/concealer/task_queue.hpp b/external/concealer/include/concealer/task_queue.hpp index 9227f05902c..8f1b84f0f0f 100644 --- a/external/concealer/include/concealer/task_queue.hpp +++ b/external/concealer/include/concealer/task_queue.hpp @@ -40,6 +40,8 @@ class TaskQueue std::exception_ptr thrown; + std::atomic is_exhausted = true; + public: explicit TaskQueue(); diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 7c9b3b14146..78395952151 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -30,6 +30,25 @@ AutowareUniverse::AutowareUniverse() setControlModeReport("/vehicle/status/control_mode", *this), setVelocityReport("/vehicle/status/velocity_status", *this), setTurnIndicatorsReport("/vehicle/status/turn_indicators_status", *this), + control_mode_request_server(create_service( + "/control/control_mode_request", + [this]( + const ControlModeCommand::Request::SharedPtr request, + ControlModeCommand::Response::SharedPtr response) { + if (request->mode == ControlModeCommand::Request::AUTONOMOUS) { + current_control_mode.store(ControlModeReport::AUTONOMOUS); + response->success = true; + } else if (request->mode == ControlModeCommand::Request::MANUAL) { + /* + NOTE: + MANUAL request will come when a remote override is triggered. + But scenario_simulator_v2 don't support a remote override for now. + */ + response->success = false; + } else { + response->success = false; + } + })), // Autoware.Universe requires localization topics to send data at 50Hz localization_update_timer(rclcpp::create_timer( this, get_clock(), std::chrono::milliseconds(20), [this]() { updateLocalization(); })), @@ -82,7 +101,7 @@ auto AutowareUniverse::getSteeringAngle() const -> double auto AutowareUniverse::updateLocalization() -> void { setAcceleration([this]() { - geometry_msgs::msg::AccelWithCovarianceStamped message; + AccelWithCovarianceStamped message; message.header.stamp = get_clock()->now(); message.header.frame_id = "/base_link"; message.accel.accel = current_acceleration.load(); @@ -110,21 +129,17 @@ auto AutowareUniverse::updateLocalization() -> void auto AutowareUniverse::updateVehicleState() -> void { - setControlModeReport([]() { - autoware_auto_vehicle_msgs::msg::ControlModeReport message; - message.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; - return message; - }()); + setControlModeReport(getControlModeReport()); setGearReport([this]() { - autoware_auto_vehicle_msgs::msg::GearReport message; + GearReport message; message.stamp = get_clock()->now(); message.report = getGearCommand().command; return message; }()); setSteeringReport([this]() { - autoware_auto_vehicle_msgs::msg::SteeringReport message; + SteeringReport message; message.stamp = get_clock()->now(); message.steering_tire_angle = getSteeringAngle(); return message; @@ -132,7 +147,7 @@ auto AutowareUniverse::updateVehicleState() -> void setVelocityReport([this]() { const auto twist = current_twist.load(); - autoware_auto_vehicle_msgs::msg::VelocityReport message; + VelocityReport message; message.header.stamp = get_clock()->now(); message.header.frame_id = "base_link"; message.longitudinal_velocity = twist.linear.x; @@ -142,21 +157,17 @@ auto AutowareUniverse::updateVehicleState() -> void }()); setTurnIndicatorsReport([this]() { - autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport message; + TurnIndicatorsReport message; message.stamp = get_clock()->now(); message.report = getTurnIndicatorsCommand().command; return message; }()); } -auto AutowareUniverse::getGearCommand() const -> autoware_auto_vehicle_msgs::msg::GearCommand -{ - return getGearCommandImpl(); -} +auto AutowareUniverse::getGearCommand() const -> GearCommand { return getGearCommandImpl(); } auto AutowareUniverse::getGearSign() const -> double { - using autoware_auto_vehicle_msgs::msg::GearCommand; /// @todo Add support for GearCommand::NONE to return 0.0 /// @sa https://github.com/autowarefoundation/autoware.universe/blob/main/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L475 return getGearCommand().command == GearCommand::REVERSE or @@ -165,9 +176,7 @@ auto AutowareUniverse::getGearSign() const -> double : 1.0; } -auto AutowareUniverse::getVehicleCommand() const -> std::tuple< - autoware_auto_control_msgs::msg::AckermannControlCommand, - autoware_auto_vehicle_msgs::msg::GearCommand> +auto AutowareUniverse::getVehicleCommand() const -> std::tuple { return std::make_tuple(getAckermannControlCommand(), getGearCommand()); } @@ -180,4 +189,16 @@ auto AutowareUniverse::getRouteLanelets() const -> std::vector } return ids; } + +auto AutowareUniverse::getControlModeReport() const -> ControlModeReport +{ + ControlModeReport message; + message.mode = current_control_mode.load(); + return message; +} + +auto AutowareUniverse::setManualMode() -> void +{ + current_control_mode.store(ControlModeReport::MANUAL); +} } // namespace concealer diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index 74cc60580e1..072a42b628f 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -471,6 +471,14 @@ auto FieldOperatorApplicationFor::requestAutoModeForCooperatio } } +auto FieldOperatorApplicationFor::enableAutowareControl() -> void +{ + task_queue.delay([this]() { + auto request = std::make_shared(); + requestEnableAutowareControl(request); + }); +} + auto FieldOperatorApplicationFor::receiveEmergencyState( const tier4_external_api_msgs::msg::Emergency & message) -> void { diff --git a/external/concealer/src/task_queue.cpp b/external/concealer/src/task_queue.cpp index 481ecdcca1e..32c7b003cc8 100644 --- a/external/concealer/src/task_queue.cpp +++ b/external/concealer/src/task_queue.cpp @@ -23,9 +23,8 @@ TaskQueue::TaskQueue() : dispatcher([this] { try { while (rclcpp::ok() and not is_stop_requested.load(std::memory_order_acquire)) { - if (auto lock = std::unique_lock(thunks_mutex); not thunks.empty()) { - // NOTE: To ensure that the task to be queued is completed as expected is the - // responsibility of the side to create a task. + is_exhausted.store(thunks.empty()); + if (auto lock = std::unique_lock(thunks_mutex); not is_exhausted.load()) { auto thunk = std::move(thunks.front()); thunks.pop(); lock.unlock(); @@ -53,7 +52,7 @@ void TaskQueue::stopAndJoin() TaskQueue::~TaskQueue() { stopAndJoin(); } -bool TaskQueue::exhausted() const noexcept { return thunks.empty(); } +bool TaskQueue::exhausted() const noexcept { return is_exhausted.load(); } void TaskQueue::rethrow() const { diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index 6ea8b184437..a4b64bfd044 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -163,7 +163,11 @@ auto ScenarioSimulator::updateEntityStatus( try { if (isEgo(status.name())) { assert(ego_entity_simulation_ && "Ego is spawned but ego_entity_simulation_ is nullptr!"); - if (req.overwrite_ego_status()) { + if ( + req.overwrite_ego_status() or + ego_entity_simulation_->autoware->getControlModeReport().mode == + autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL) { + ego_entity_simulation_->autoware->setManualMode(); traffic_simulator_msgs::msg::EntityStatus ego_status_msg; simulation_interface::toMsg(status, ego_status_msg); ego_entity_simulation_->overwrite( diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index fcd36c957f3..215ea627064 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -158,6 +158,7 @@ void EgoEntity::onUpdate(double current_time, double step_time) // prefer current lanelet on ss2 side setStatus(non_canonicalized_updated_status.value(), status_->getLaneletIds()); } else { + field_operator_application->enableAutowareControl(); is_controlled_by_simulator_ = false; } } diff --git a/test_runner/scenario_test_runner/scenario/RoutingAction.FollowTrajectoryAction-override.yaml b/test_runner/scenario_test_runner/scenario/RoutingAction.FollowTrajectoryAction-override.yaml new file mode 100644 index 00000000000..15420dc70cf --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/RoutingAction.FollowTrajectoryAction-override.yaml @@ -0,0 +1,235 @@ +OpenSCENARIO: + FileHeader: + revMajor: 0 + revMinor: 0 + date: '1970-01-01T09:00:00+09:00' + description: '' + author: 'Kotaro Yoshimoto' + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle + RoadNetwork: + LogicFile: + filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map + Entities: + ScenarioObject: + - name: ego + CatalogReference: &SAMPLE_VEHICLE + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: '' + Properties: + Property: + - name: 'isEgo' + value: 'true' + - name: car_1 + CatalogReference: *SAMPLE_VEHICLE + - name: car_2 + CatalogReference: *SAMPLE_VEHICLE + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 1 + offset: 0 + Orientation: &ORIENTATION_ZERO + type: relative + h: 0 + p: 0 + r: 0 + - RoutingAction: + AcquirePositionAction: + Position: &EGO_DESTINATION + LanePosition: + roadId: '' + laneId: '34507' + s: 50 + offset: 0 + Orientation: *ORIENTATION_ZERO + - entityRef: car_1 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 40 + offset: -1.0 + Orientation: + type: relative + h: -1.57 + p: 0 + r: 0 + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + Story: + - name: '' + Act: + - name: '' + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: 'check stopped' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + SpeedCondition: + rule: lessThan + value: 3.0 + Action: + - name: 'follow_trajectory_override' + PrivateAction: + - RoutingAction: + FollowTrajectoryAction: + initialDistanceOffset: 1 + TimeReference: + Timing: + domainAbsoluteRelative: relative + offset: 0 + scale: 1 + TrajectoryFollowingMode: + followingMode: position + TrajectoryRef: + Trajectory: + closed: false + name: straight + Shape: + Polyline: + Vertex: + - Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 40 + offset: 2 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34507' + s: 0 + offset: 0 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34507' + s: 5 + offset: 0 + Orientation: *ORIENTATION_ZERO + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 10 + rule: greaterThan + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + storyboardElementRef: follow_trajectory_override + storyboardElementType: action + state: completeState + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: *EGO_DESTINATION + tolerance: 1 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 60 + rule: greaterThan + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: []