Skip to content

Commit

Permalink
Fix parametric input xodr specification (#24)
Browse files Browse the repository at this point in the history 
Signed-off-by: romanodanilo <[email protected]>

Signed-off-by: romanodanilo <[email protected]>
  • Loading branch information
@romanodanilo
romanodanilo authored Jul 10, 2024
1 parent ec9dd42 commit 5999161
Show file tree
Hide file tree
Showing 4 changed files with 1,175 additions and 0 deletions.
38 changes: 38 additions & 0 deletions qc_openscenario/checks/utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,35 @@ def compare_versions(version1: str, version2: str) -> int:
return 0


def get_parameter_value(
root: etree._ElementTree, parameter_name: str
) -> Union[None, str, int, float]:
"""Read all ParameterDeclaration nodes from root and get the value of parameter_name if present
Args:
root (etree._ElementTree): root node of the xml document
parameter_name (str): the parameter name to search
Returns:
Union[None, str, int, float]: the parameter value is present, with its type. None if the parameter_name is not found
"""
param_declarations = root.findall(".//ParameterDeclaration")
if param_declarations is None:
return None

for param_declaration in param_declarations:
current_name = param_declaration.get("name")
current_value = param_declaration.get("value")
if (
current_name is not None
and current_value is not None
and current_name == parameter_name
):
return current_value

return None


def get_xodr_road_network(root: etree._ElementTree) -> Union[etree._ElementTree, None]:
"""Get parsed xodr tree indicated in the RoadNetwork/LogicFile node of the input root
Expand All @@ -62,4 +91,13 @@ def get_xodr_road_network(root: etree._ElementTree) -> Union[etree._ElementTree,
filepath = logic_file.get("filepath")
if filepath is None:
return None

# If filepath is specified using param, get all param declaration and update the filepath
if filepath.startswith("$"):

filepath_param = filepath[1:]
filepath = get_parameter_value(root, filepath_param)
if filepath is None:
return None

return etree.parse(filepath)
207 changes: 207 additions & 0 deletions tests/data/parametric_input_xodr/CloseVehicleCrossing.xosc
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,207 @@
<?xml version="1.0" encoding="utf-8"?>
<OpenSCENARIO xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../Schema/OpenSCENARIO.xsd">
<FileHeader revMajor="1" revMinor="3" date="2021-01-13T10:00:00" description="An example where the ego arrives at a 4 way junction as 2 closely timed non-egos cross." author="ASAM e.V." />
<ParameterDeclarations>
<ParameterDeclaration name="Road_Logics" parameterType="string" value="Databases/fabriksgatan.xodr" />
<ParameterDeclaration name="Road_SceneGraph" parameterType="string" value="Databases/fabriksgatan.osgb" />
<ParameterDeclaration name="Ego_Speed" parameterType="double" value="6.4" />
<ParameterDeclaration name="A1_InitialTrajectoryDistance" parameterType="double" value="10.0" />
<ParameterDeclaration name="A2_InitialTrajectoryDistance" parameterType="double" value="0.0" />
</ParameterDeclarations>
<CatalogLocations>
<TrajectoryCatalog>
<Directory path="Catalogs/Trajectories" />
</TrajectoryCatalog>
</CatalogLocations>
<RoadNetwork>
<LogicFile filepath="$Road_Logics" />
<SceneGraphFile filepath="$Road_SceneGraph" />
</RoadNetwork>
<Entities>
<ScenarioObject name="Ego">
<Vehicle name="HAF" vehicleCategory="car">
<ParameterDeclarations />
<Performance maxSpeed="69.444" maxAcceleration="200" maxDeceleration="10.0" />
<BoundingBox>
<Center x="1.5" y="0.0" z="0.9" />
<Dimensions width="2.1" length="4.5" height="1.8" />
</BoundingBox>
<Axles>
<FrontAxle maxSteering="0.5" wheelDiameter="0.6" trackWidth="1.8" positionX="3.1" positionZ="0.3" />
<RearAxle maxSteering="0.0" wheelDiameter="0.6" trackWidth="1.8" positionX="0.0" positionZ="0.3" />
</Axles>
</Vehicle>
</ScenarioObject>
<ScenarioObject name="A1">
<Vehicle name="Default_Car" vehicleCategory="car">
<ParameterDeclarations />
<Performance maxSpeed="69.444" maxAcceleration="200" maxDeceleration="10.0" />
<BoundingBox>
<Center x="1.4" y="0.0" z="0.8" />
<Dimensions width="2.0" length="4.2" height="1.6" />
</BoundingBox>
<Axles>
<FrontAxle maxSteering="0.5" wheelDiameter="0.5" trackWidth="1.75" positionX="2.8" positionZ="0.25" />
<RearAxle maxSteering="0.0" wheelDiameter="0.5" trackWidth="1.75" positionX="0.0" positionZ="0.25" />
</Axles>
</Vehicle>
</ScenarioObject>
<ScenarioObject name="A2">
<Vehicle name="Default_Car" vehicleCategory="car">
<ParameterDeclarations />
<Performance maxSpeed="69.444" maxAcceleration="200" maxDeceleration="10.0" />
<BoundingBox>
<Center x="1.4" y="0.0" z="0.8" />
<Dimensions width="2.0" length="4.2" height="1.6" />
</BoundingBox>
<Axles>
<FrontAxle maxSteering="0.5" wheelDiameter="0.5" trackWidth="1.75" positionX="2.8" positionZ="0.25" />
<RearAxle maxSteering="0.0" wheelDiameter="0.5" trackWidth="1.75" positionX="0.0" positionZ="0.25" />
</Axles>
</Vehicle>
</ScenarioObject>
</Entities>
<Storyboard>
<Init>
<Actions>
<GlobalAction>
<EnvironmentAction>
<Environment name="Environment1">
<TimeOfDay animation="false" dateTime="2020-02-21T12:00:00" />
<Weather fractionalCloudCover="zeroOktas">
<Sun illuminance="100000.0" azimuth="0.0" elevation="1.571" />
<Fog visualRange="100000.0" />
<Precipitation precipitationType="dry" precipitationIntensity="0.0" />
</Weather>
<RoadCondition frictionScaleFactor="1.0" />
</Environment>
</EnvironmentAction>
</GlobalAction>
<Private entityRef="Ego">
<PrivateAction>
<LongitudinalAction>
<SpeedAction>
<SpeedActionDynamics dynamicsShape="step" value="0" dynamicsDimension="time" />
<SpeedActionTarget>
<AbsoluteTargetSpeed value="$Ego_Speed" />
</SpeedActionTarget>
</SpeedAction>
</LongitudinalAction>
</PrivateAction>
<PrivateAction>
<TeleportAction>
<Position>
<LanePosition roadId="3" laneId="-1" s="8.0" />
</Position>
</TeleportAction>
</PrivateAction>
</Private>
<Private entityRef="A1">
<PrivateAction>
<TeleportAction>
<Position>
<TrajectoryPosition s="$A1_InitialTrajectoryDistance">
<TrajectoryRef>
<CatalogReference catalogName="TrajectoryCatalog" entryName="FabriksgatanNorthToSouth" />
</TrajectoryRef>
</TrajectoryPosition>
</Position>
</TeleportAction>
</PrivateAction>
</Private>
<Private entityRef="A2">
<PrivateAction>
<TeleportAction>
<Position>
<TrajectoryPosition s="$A2_InitialTrajectoryDistance">
<TrajectoryRef>
<CatalogReference catalogName="TrajectoryCatalog" entryName="FabriksgatanNorthToSouth" />
</TrajectoryRef>
</TrajectoryPosition>
</Position>
</TeleportAction>
</PrivateAction>
</Private>
</Actions>
</Init>
<Story name="A1_Story">
<Act name="A1_Act1">
<ManeuverGroup maximumExecutionCount="1" name="A1_Sequence1">
<Actors selectTriggeringEntities="false">
<EntityRef entityRef="A1" />
</Actors>
<Maneuver name="A1_CrossingManeuver">
<Event name="A1_CrossingEvent" priority="override">
<Action name="A1_CrossingAction">
<PrivateAction>
<RoutingAction>
<FollowTrajectoryAction initialDistanceOffset="$A1_InitialTrajectoryDistance">
<TrajectoryRef>
<CatalogReference catalogName="TrajectoryCatalog" entryName="FabriksgatanNorthToSouth" />
</TrajectoryRef>
<TimeReference>
<Timing domainAbsoluteRelative="relative" scale="1" offset="0" />
</TimeReference>
<TrajectoryFollowingMode followingMode="position" />
</FollowTrajectoryAction>
</RoutingAction>
</PrivateAction>
</Action>
</Event>
</Maneuver>
</ManeuverGroup>
</Act>
</Story>
<Story name="A2_Story">
<Act name="A2_Act1">
<ManeuverGroup maximumExecutionCount="1" name="A2_Sequence1">
<Actors selectTriggeringEntities="false">
<EntityRef entityRef="A2" />
</Actors>
<Maneuver name="A2_CrossingManeuver">
<Event name="A2_CrossingEvent" priority="override">
<Action name="A2_CrossingAction">
<PrivateAction>
<RoutingAction>
<FollowTrajectoryAction initialDistanceOffset="$A2_InitialTrajectoryDistance">
<TrajectoryRef>
<CatalogReference catalogName="TrajectoryCatalog" entryName="FabriksgatanNorthToSouth" />
</TrajectoryRef>
<TimeReference>
<Timing domainAbsoluteRelative="relative" scale="1" offset="0" />
</TimeReference>
<TrajectoryFollowingMode followingMode="position" />
</FollowTrajectoryAction>
</RoutingAction>
</PrivateAction>
</Action>
</Event>
</Maneuver>
</ManeuverGroup>
<StartTrigger>
<ConditionGroup>
<Condition name="Start_of_A2" delay="0" conditionEdge="rising">
<ByValueCondition>
<SimulationTimeCondition value="0.3" rule="greaterThan" />
</ByValueCondition>
</Condition>
</ConditionGroup>
</StartTrigger>
</Act>
</Story>
<StopTrigger>
<ConditionGroup>
<Condition name="End_of_simulation" delay="0" conditionEdge="none">
<ByEntityCondition>
<TriggeringEntities triggeringEntitiesRule="all">
<EntityRef entityRef="A2" />
</TriggeringEntities>
<EntityCondition>
<TraveledDistanceCondition value="320"></TraveledDistanceCondition>
</EntityCondition>
</ByEntityCondition>
</Condition>
</ConditionGroup>
</StopTrigger>
</Storyboard>
</OpenSCENARIO>
Loading

0 comments on commit 5999161

Please sign in to comment.