From c93c12b839227bf3293d08e9f7e7458c2d839d7a Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 15 Nov 2024 13:05:19 +0900 Subject: [PATCH 1/2] Add new test scenario `RoutingAction.AssignRouteAction.yaml` Signed-off-by: yamacir-kit --- .../RoutingAction.AssignRouteAction.yaml | 195 ++++++++++++++++++ 1 file changed, 195 insertions(+) create mode 100644 test_runner/scenario_test_runner/scenario/RoutingAction.AssignRouteAction.yaml diff --git a/test_runner/scenario_test_runner/scenario/RoutingAction.AssignRouteAction.yaml b/test_runner/scenario_test_runner/scenario/RoutingAction.AssignRouteAction.yaml new file mode 100644 index 00000000000..ac9d616dde7 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/RoutingAction.AssignRouteAction.yaml @@ -0,0 +1,195 @@ +ScenarioModifiers: + ScenarioModifier: [] +OpenSCENARIO: + FileHeader: + revMajor: 1 + revMinor: 1 + date: '2024-11-15T02:21:26.404Z' + description: '' + author: Tatsuya Yamasaki + 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: + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: 'Autoware' + Properties: + Property: [] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: &INITIAL_POSITION + LanePosition: + roadId: '' + laneId: '34513' + s: 5 + Orientation: &DEFAULT_ORIENTATION + type: relative + h: 0 + p: 0 + r: 0 + - RoutingAction: + AcquirePositionAction: + Position: &STANDBY_POSITION + LanePosition: + roadId: '' + laneId: '34513' + s: 30 + Orientation: *DEFAULT_ORIENTATION + Story: + - name: '' + Act: + - name: act_reroute + ManeuverGroup: + - maximumExecutionCount: 1 + name: act_reroute + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: reroute + priority: parallel + Action: + - name: '' + PrivateAction: + RoutingAction: + AssignRouteAction: + Route: + name: '' + closed: false + Waypoint: + - Position: &CHECK_POINT_0 + LanePosition: + roadId: '' + laneId: '34606' + s: 15 + Orientation: *DEFAULT_ORIENTATION + routeStrategy: shortest + - Position: &CHECK_POINT_1 + LanePosition: + roadId: '' + laneId: '34600' + s: 30 + Orientation: *DEFAULT_ORIENTATION + routeStrategy: shortest + - Position: &CHECK_POINT_2 + LanePosition: + roadId: '' + laneId: '34630' + s: 13 + Orientation: *DEFAULT_ORIENTATION + routeStrategy: shortest + - Position: &DESTINATION + LanePosition: + roadId: '' + laneId: '34741' + s: 25 + Orientation: *DEFAULT_ORIENTATION + routeStrategy: shortest + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + UserDefinedValueCondition: + value: WAITING_FOR_ROUTE + name: ego.currentState + rule: equalTo + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: *STANDBY_POSITION + tolerance: 1 + - 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 + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: *DESTINATION + tolerance: 1 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 180 + rule: greaterThan + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: [] From fe413ef7d19c64892c21c369a5b70b2491f0083a Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 15 Nov 2024 13:05:49 +0900 Subject: [PATCH 2/2] Fix AssignRouteAction to clear route before requesting a new route Signed-off-by: yamacir-kit --- .../include/openscenario_interpreter/simulator_core.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 628b8b61c12..393b4a11420 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -474,9 +474,10 @@ class SimulatorCore } template - static auto applyAssignRouteAction(Ts &&... xs) + static auto applyAssignRouteAction(const std::string & entity_ref, Ts &&... xs) { - return core->requestAssignRoute(std::forward(xs)...); + core->requestClearRoute(entity_ref); + return core->requestAssignRoute(entity_ref, std::forward(xs)...); } template