Commit 92db1774 authored by Naida Goro's avatar Naida Goro Committed by Reinhard Biegel
Browse files

mod(Core): Consider dimension and zOffset of objects in collisions

So far, agents could collide with objects which had no height
(e.g. road marks) or which where above them (e.g. traffic lights).

The collision detection now only considers objects when:
- All dimensions (length, width, and height) are larger than 0
- The objects zOffset + height does not overlap with the agents height

Note: This is not a real 3D implementation, as no 3D bounding box model
      exists yet, and agents are grounded to z = 0.

Signed-off-by: Naida Goro<naida.goro@in-tech.com>
parent d857dc4b
<Profiles SchemaVersion="0.4.8">
<AgentProfiles>
<AgentProfile Name="TFAgent" Type="Dynamic">
<DriverProfiles>
<DriverProfile Name="Regular" Probability="1.0"/>
</DriverProfiles>
<VehicleProfiles>
<VehicleProfile Name="TFVehicle" Probability="1.0"/>
</VehicleProfiles>
</AgentProfile>
</AgentProfiles>
<VehicleProfiles>
<VehicleProfile Name="TFVehicle">
<Model Name="car_bmw_7_1"/>
<Components>
<Component Type="Dynamics_TrajectoryFollower">
<Profiles>
<Profile Name="BasicTrajectoryFollower" Probability="1.0"/>
</Profiles>
</Component>
</Components>
<Sensors/>
</VehicleProfile>
</VehicleProfiles>
<ProfileGroup Type="Driver">
<Profile Name="Regular">
<String Key="Type" Value="AlgorithmAgentFollowingDriverModel"/>
<String Key="AlgorithmLateralModule" Value="Algorithm_LateralAfdm"/>
<String Key="AlgorithmLongitudinalModule" Value="Algorithm_LongitudinalAfdm"/>
<Double Key="VelocityWish" Value="35.0"/>
<Double Key="Delta" Value="4.0"/>
<Double Key="TGapWish" Value="1.5"/>
<Double Key="MinDistance" Value="2.0"/>
<Double Key="MaxAcceleration" Value="1.4"/>
<Double Key="MaxDeceleration" Value="2.0"/>
</Profile>
</ProfileGroup>
<ProfileGroup Type="Dynamics_TrajectoryFollower">
<Profile Name="BasicTrajectoryFollower">
<Bool Key="AutomaticDeactivation" Value="true"/>
<Bool Key="EnforceTrajectory" Value="true"/>
</Profile>
</ProfileGroup>
<ProfileGroup Type="TrafficRules">
<Profile Name="Germany">
<Double Key="OpenSpeedLimit" Value="INF"/>
<Double Key="OpenSpeedLimitTrucks" Value="22.2222222"/>
<Double Key="OpenSpeedLimitBuses" Value="27.7777777"/>
<Bool Key="KeepToOuterLanes" Value="true"/>
<Bool Key="DontOvertakeOnOuterLanes" Value="true"/>
<Bool Key="FormRescueLane" Value="true"/>
<Bool Key="ZipperMerge" Value="true"/>
</Profile>
<Profile Name="USA">
<Double Key="OpenSpeedLimit" Value="31.2928"/>
<Double Key="OpenSpeedLimitTrucks" Value="26.8224"/>
<Double Key="OpenSpeedLimitBuses" Value="26.8224"/>
<Bool Key="KeepToOuterLanes" Value="false"/>
<Bool Key="DontOvertakeOnOuterLanes" Value="false"/>
<Bool Key="FormRescueLane" Value="true"/>
<Bool Key="ZipperMerge" Value="false"/>
</Profile>
<Profile Name="China">
<Double Key="OpenSpeedLimit" Value="33.3333"/>
<Double Key="OpenSpeedLimitTrucks" Value="25.0"/>
<Double Key="OpenSpeedLimitBuses" Value="27.7777777"/>
<Bool Key="KeepToOuterLanes" Value="false"/>
<Bool Key="DontOvertakeOnOuterLanes" Value="true"/>
<Bool Key="FormRescueLane" Value="false"/>
<Bool Key="ZipperMerge" Value="true"/>
</Profile>
</ProfileGroup>
</Profiles>
<?xml version="1.0"?>
<OpenSCENARIO>
<FileHeader revMajor="1" revMinor="0" date="2020-06-26T00:17:00" description="ObjectAboveRoad" author="BMW AG"/>
<ParameterDeclarations>
<ParameterDeclaration name="OP_OSC_SchemaVersion" parameterType="string" value="0.4.0"/>
</ParameterDeclarations>
<CatalogLocations>
<VehicleCatalog>
<Directory path="VehicleModelsCatalog.xosc"/>
</VehicleCatalog>
<PedestrianCatalog>
<Directory path="PedestrianModelsCatalog.xosc"/>
</PedestrianCatalog>
<ControllerCatalog>
<Directory path=""/>
</ControllerCatalog>
<ManeuverCatalog>
<Directory path=""/>
</ManeuverCatalog>
<MiscObjectCatalog>
<Directory path=""/>
</MiscObjectCatalog>
<EnvironmentCatalog>
<Directory path=""/>
</EnvironmentCatalog>
<TrajectoryCatalog>
<Directory path=""/>
</TrajectoryCatalog>
<RouteCatalog>
<Directory path=""/>
</RouteCatalog>
</CatalogLocations>
<RoadNetwork>
<LogicFile filepath="SceneryConfiguration.xodr"/>
<SceneGraphFile filepath=""/>
</RoadNetwork>
<Entities>
<ScenarioObject name="Ego">
<CatalogReference catalogName="ProfilesCatalog.xml" entryName="TFAgent"/>
</ScenarioObject>
<EntitySelection name="ScenarioAgents">
<Members/>
</EntitySelection>
</Entities>
<Storyboard>
<Init>
<Actions>
<Private entityRef="Ego">
<PrivateAction>
<TeleportAction>
<Position>
<LanePosition roadId="1" laneId="-2" offset="0.0" s="0.0"/>
</Position>
</TeleportAction>
</PrivateAction>
<PrivateAction>
<LongitudinalAction>
<SpeedAction>
<SpeedActionDynamics dynamicsShape="linear" value="0.0" dynamicsDimension="rate"/>
<SpeedActionTarget>
<AbsoluteTargetSpeed value="35.0"/>
</SpeedActionTarget>
</SpeedAction>
</LongitudinalAction>
</PrivateAction>
</Private>
</Actions>
</Init>
<Story name="Trajectory">
<Act name="Act1">
<ManeuverGroup maximumExecutionCount="1" name="StateChangeSequenceA">
<Actors selectTriggeringEntities="false">
<EntityRef entityRef="Ego"/>
</Actors>
<Maneuver name="StateChangeManeuverA">
<Event name="StateChangeEventA" priority="overwrite">
<Action name="Trajectory">
<PrivateAction>
<RoutingAction>
<FollowTrajectoryAction>
<Trajectory name="" closed="false">
<Shape>
<Polyline>
<Vertex time="0.0">
<Position>
<WorldPosition x="0.0" y="1.875" h="0.0"/>
</Position>
</Vertex>
<Vertex time="5.0">
<Position>
<WorldPosition x="40.0" y="1.875" h="0.0"/>
</Position>
</Vertex>
</Polyline>
</Shape>
</Trajectory>
</FollowTrajectoryAction>
</RoutingAction>
</PrivateAction>
</Action>
<StartTrigger>
<ConditionGroup>
<Condition name="Conditional">
<ByValueCondition>
<SimulationTimeCondition value="-1.0" rule="greaterThan"/>
</ByValueCondition>
</Condition>
</ConditionGroup>
</StartTrigger>
</Event>
</Maneuver>
</ManeuverGroup>
</Act>
</Story>
<StopTrigger>
<ConditionGroup>
<Condition name="EndTime" delay="0" conditionEdge="rising">
<ByValueCondition>
<SimulationTimeCondition value="5.0" rule="greaterThan"/>
</ByValueCondition>
</Condition>
</ConditionGroup>
</StopTrigger>
</Storyboard>
</OpenSCENARIO>
<OpenDRIVE>
<header revMajor="1" revMinor="1" name="Testfile" version="1" date="Thu Feb 8 14:24:06 2007" north="2.0000000000000000e+003" south="-2.0000000000000000e+003" east="2.0000000000000000e+003" west="-2.0000000000000000e+003"/>
<road name="" length="5139.7188686309819" id="1" junction="-1">
<link/>
<planView>
<geometry s="0.0000000000000000e+000" x="0" y="7.5" hdg="0" length="4000">
<line/>
</geometry>
<geometry s="4000" x="4000" y="7.5" hdg="0" length="285">
<spiral curvStart="0.0000000000000000e+000" curvEnd="-0.0028500000000000066"/>
</geometry>
<geometry s="4285" x="4280.3350391612494" y="-30.629710830536482" hdg="-0.40612500000000096" length="183.7188686309816">
<arc curvature="-0.0028500000000000066"/>
</geometry>
<geometry s="4468.7188686309819" x="4422.9323279193268" y="-143.12204699500415" hdg="-0.92972377559829977" length="571">
<spiral curvStart="-0.0028500000000000066" curvEnd="0.0000000000000000e+000"/>
</geometry>
<geometry s="5039.7188686309819" x="4476.6651848689908" y="-694.88624436808436" hdg="-1.7453292519943295" length="100">
<line/>
</geometry>
</planView>
<elevationProfile/>
<lateralProfile/>
<lanes>
<laneSection s="0.0000000000000000e+000">
<left/>
<center>
<lane id="0" type="border" level="true">
<link>
<successor id="0"/>
</link>
<roadMark weight="standard" sOffset="0" type="solid" color="standard" width="0.12" laneChange="both"/>
</lane>
</center>
<right>
<lane id="-1" type="driving" level="true">
<link>
<successor id="-1"/>
</link>
<width sOffset="0" a="3.75" b="0" c="0" d="0"/>
<roadMark weight="standard" sOffset="0" type="broken" color="standard" width="0.12" laneChange="both"/>
</lane>
<lane id="-2" type="driving" level="true">
<link>
<successor id="-2"/>
</link>
<width sOffset="0" a="3.75" b="0" c="0" d="0"/>
<roadMark weight="standard" sOffset="0" type="solid" color="standard" width="0.12" laneChange="both"/>
</lane>
</right>
</laneSection>
</lanes>
<objects>
<object type="roadMark" name="RoadSideMarkerPost" id="" s="0" t="1" zOffset="0" validLength="0" orientation="+" length="0.12" width="0.12" height="1.2" hdg="0" pitch="0" roll="0">
<repeat s="0" distance="50" length="5139.7188686309819"/>
</object>
<object type="roadMark" name="RoadSideMarkerPost" id="" s="0" t="-8.5" zOffset="0" validLength="0" orientation="-" length="0.12" width="0.12" height="1.2" hdg="0" pitch="0" roll="0">
<repeat s="0" distance="50" length="5139.7188686309819"/>
</object>
<object type="railing" name="GuardRail" id="" s="0" t="0.5" zOffset="0" validLength="0" orientation="+" length="3.0" width="0.25" height="0.9" hdg="0" pitch="0" roll="0">
<repeat s="0" distance="0" length="5139.7188686309819"/>
</object>
<object type="railing" name="GuardRail" id="" s="0" t="-8" zOffset="0" validLength="0" orientation="-" length="3.0" width="0.25" height="0.9" hdg="0" pitch="0" roll="0">
<repeat s="0" distance="0" length="5139.7188686309819"/>
</object>
<!-- Object for test above street to test if vehicle can pass without collision -->
<object type="gantry" name="Obstacle" id="" s="20" t="-3.75" zOffset="4" validLength="0" orientation="-" length="0.1" width="7.5" height="0.9" hdg="0" pitch="0" roll="0" />
</objects>
<signals/>
</road>
</OpenDRIVE>
<simulationConfig SchemaVersion="0.8.2">
<ProfilesCatalog>ProfilesCatalog.xml</ProfilesCatalog>
<Experiment>
<ExperimentID>123</ExperimentID>
<NumberOfInvocations>1</NumberOfInvocations>
<RandomSeed>532725206</RandomSeed>
<Libraries>
<WorldLibrary>World_OSI</WorldLibrary>
</Libraries>
</Experiment>
<Scenario>
<OpenScenarioFile>Scenario.xosc</OpenScenarioFile>
</Scenario>
<Environment>
<TimeOfDays>
<TimeOfDay Probability="0.4" Value="15"/>
<TimeOfDay Probability="0.6" Value="18"/>
</TimeOfDays>
<VisibilityDistances>
<VisibilityDistance Probability="0.7" Value="300"/>
<VisibilityDistance Probability="0.3" Value="400"/>
</VisibilityDistances>
<Frictions>
<Friction Probability="1.0" Value="1.0"/>
</Frictions>
<Weathers>
<Weather Probability="0.5" Value="Rainy"/>
<Weather Probability="0.5" Value="Snowy"/>
</Weathers>
<TrafficRules>Germany</TrafficRules>
</Environment>
<Observations>
<Observation>
<Library>Observation_Log</Library>
<Parameters>
<String Key="OutputFilename" Value="simulationOutput.xml"/>
<Bool Key="LoggingCyclicsToCsv" Value="false"/>
<StringVector Key="LoggingGroup_Trace" Value="XPosition,YPosition,YawAngle"/>
<StringVector Key="LoggingGroup_RoadPosition" Value="AgentInFront,Lane,PositionRoute,Road,TCoordinate"/>
<StringVector Key="LoggingGroup_RoadPositionExtended" Value="SecondaryLanes"/>
<StringVector Key="LoggingGroup_Sensor" Value="Sensor*_DetectedAgents,Sensor*_VisibleAgents"/>
<StringVector Key="LoggingGroup_Vehicle" Value="AccelerationPedalPosition,BrakePedalPosition,EngineMoment,Gear,SteeringAngle,TotalDistanceTraveled,YawRate"/>
<StringVector Key="LoggingGroup_Visualization" Value="AccelerationEgo,BrakeLight,IndicatorState,LightStatus,VelocityEgo"/>
<StringVector Key="LoggingGroups" Value="Trace,Visualization,RoadPosition,Sensor"/>
</Parameters>
</Observation>
<Observation>
<Library>Observation_EntityRepository</Library>
<Parameters>
<!-- If "FilenamePrefix" is skipped, defaults to Value="Repository" -->
<String Key="FilenamePrefix" Value="Repository"/>
<!-- If "WritePersistentEntities" is skipped, defaults to Value="Consolidated"
Options:
- Consolidated: Concatenate with {FilenamePrefix}_Run_###.csv for each run
- Separate: Write to {FilenamePrefix}_Persistent.csv
- Skip: No output for persistent entities
-->
<String Key="WritePersistentEntities" Value="Consolidated"/>
</Parameters>
</Observation>
</Observations>
<Spawners>
<Spawner>
<Library>SpawnerScenario</Library>
<Type>PreRun</Type>
<Priority>1</Priority>
</Spawner>
</Spawners>
</simulationConfig>
<Profiles SchemaVersion="0.4.8">
<AgentProfiles>
<AgentProfile Name="TFAgent" Type="Dynamic">
<DriverProfiles>
<DriverProfile Name="Regular" Probability="1.0"/>
</DriverProfiles>
<VehicleProfiles>
<VehicleProfile Name="TFVehicle" Probability="1.0"/>
</VehicleProfiles>
</AgentProfile>
</AgentProfiles>
<VehicleProfiles>
<VehicleProfile Name="TFVehicle">
<Model Name="car_bmw_7_1"/>
<Components>
<Component Type="Dynamics_TrajectoryFollower">
<Profiles>
<Profile Name="BasicTrajectoryFollower" Probability="1.0"/>
</Profiles>
</Component>
</Components>
<Sensors/>
</VehicleProfile>
</VehicleProfiles>
<ProfileGroup Type="Driver">
<Profile Name="Regular">
<String Key="Type" Value="AlgorithmAgentFollowingDriverModel"/>
<String Key="AlgorithmLateralModule" Value="Algorithm_LateralAfdm"/>
<String Key="AlgorithmLongitudinalModule" Value="Algorithm_LongitudinalAfdm"/>
<Double Key="VelocityWish" Value="35.0"/>
<Double Key="Delta" Value="4.0"/>
<Double Key="TGapWish" Value="1.5"/>
<Double Key="MinDistance" Value="2.0"/>
<Double Key="MaxAcceleration" Value="1.4"/>
<Double Key="MaxDeceleration" Value="2.0"/>
</Profile>
</ProfileGroup>
<ProfileGroup Type="Dynamics_TrajectoryFollower">
<Profile Name="BasicTrajectoryFollower">
<Bool Key="AutomaticDeactivation" Value="true"/>
<Bool Key="EnforceTrajectory" Value="true"/>
</Profile>
</ProfileGroup>
<ProfileGroup Type="TrafficRules">
<Profile Name="Germany">
<Double Key="OpenSpeedLimit" Value="INF"/>
<Double Key="OpenSpeedLimitTrucks" Value="22.2222222"/>
<Double Key="OpenSpeedLimitBuses" Value="27.7777777"/>
<Bool Key="KeepToOuterLanes" Value="true"/>
<Bool Key="DontOvertakeOnOuterLanes" Value="true"/>
<Bool Key="FormRescueLane" Value="true"/>
<Bool Key="ZipperMerge" Value="true"/>
</Profile>
<Profile Name="USA">
<Double Key="OpenSpeedLimit" Value="31.2928"/>
<Double Key="OpenSpeedLimitTrucks" Value="26.8224"/>
<Double Key="OpenSpeedLimitBuses" Value="26.8224"/>
<Bool Key="KeepToOuterLanes" Value="false"/>
<Bool Key="DontOvertakeOnOuterLanes" Value="false"/>
<Bool Key="FormRescueLane" Value="true"/>
<Bool Key="ZipperMerge" Value="false"/>
</Profile>
<Profile Name="China">
<Double Key="OpenSpeedLimit" Value="33.3333"/>
<Double Key="OpenSpeedLimitTrucks" Value="25.0"/>
<Double Key="OpenSpeedLimitBuses" Value="27.7777777"/>
<Bool Key="KeepToOuterLanes" Value="false"/>
<Bool Key="DontOvertakeOnOuterLanes" Value="true"/>
<Bool Key="FormRescueLane" Value="false"/>
<Bool Key="ZipperMerge" Value="true"/>
</Profile>
</ProfileGroup>
</Profiles>
<?xml version="1.0"?>
<OpenSCENARIO>
<FileHeader revMajor="1" revMinor="0" date="2020-06-26T00:17:00" description="ObjectOfTypeRoadMark" author="BMW AG"/>
<ParameterDeclarations>
<ParameterDeclaration name="OP_OSC_SchemaVersion" parameterType="string" value="0.4.0"/>
</ParameterDeclarations>
<CatalogLocations>
<VehicleCatalog>
<Directory path="VehicleModelsCatalog.xosc"/>
</VehicleCatalog>
<PedestrianCatalog>
<Directory path="PedestrianModelsCatalog.xosc"/>
</PedestrianCatalog>
<ControllerCatalog>
<Directory path=""/>
</ControllerCatalog>
<ManeuverCatalog>
<Directory path=""/>
</ManeuverCatalog>
<MiscObjectCatalog>
<Directory path=""/>
</MiscObjectCatalog>
<EnvironmentCatalog>
<Directory path=""/>
</EnvironmentCatalog>
<TrajectoryCatalog>
<Directory path=""/>
</TrajectoryCatalog>
<RouteCatalog>
<Directory path=""/>
</RouteCatalog>
</CatalogLocations>
<RoadNetwork>
<LogicFile filepath="SceneryConfiguration.xodr"/>
<SceneGraphFile filepath=""/>
</RoadNetwork>
<Entities>
<ScenarioObject name="Ego">
<CatalogReference catalogName="ProfilesCatalog.xml" entryName="TFAgent"/>
</ScenarioObject>
<EntitySelection name="ScenarioAgents">
<Members/>
</EntitySelection>
</Entities>
<Storyboard>
<Init>
<Actions>
<Private entityRef="Ego">
<PrivateAction>
<TeleportAction>
<Position>
<LanePosition roadId="1" laneId="-2" offset="0.0" s="0.0"/>
</Position>
</TeleportAction>
</PrivateAction>
<PrivateAction>
<LongitudinalAction>
<SpeedAction>
<SpeedActionDynamics dynamicsShape="linear" value="0.0" dynamicsDimension="rate"/>
<SpeedActionTarget>
<AbsoluteTargetSpeed value="35.0"/>
</SpeedActionTarget>
</SpeedAction>
</LongitudinalAction>
</PrivateAction>
</Private>
</Actions>
</Init>
<Story name="Trajectory">
<Act name="Act1">
<ManeuverGroup maximumExecutionCount="1" name="StateChangeSequenceA">
<Actors selectTriggeringEntities="false">
<EntityRef entityRef="Ego"/>
</Actors>
<Maneuver name="StateChangeManeuverA">
<Event name="StateChangeEventA" priority="overwrite">
<Action name="Trajectory">
<PrivateAction>
<RoutingAction>
<FollowTrajectoryAction>
<Trajectory name="" closed="false">
<Shape>
<Polyline>
<Vertex time="0.0">
<Position>
<WorldPosition x="0.0" y="1.875" h="0.0"/>
</Position>
</Vertex>
<Vertex time="5.0">
<Position>
<WorldPosition x="40.0" y="1.875" h="0.0"/>
</Position>
</Vertex>
</Polyline>
</Shape>
</Trajectory>
</FollowTrajectoryAction>
</RoutingAction>
</PrivateAction>
</Action>
<StartTrigger>
<ConditionGroup>
<Condition name="Conditional">
<ByValueCondition>
<SimulationTimeCondition value="-1.0" rule="greaterThan"/>
</ByValueCondition>
</Condition>
</ConditionGroup>
</StartTrigger>
</Event>
</Maneuver>
</ManeuverGroup>
</Act>
</Story>
<StopTrigger>
<ConditionGroup>
<Condition name="EndTime" delay="0" conditionEdge="rising">
<ByValueCondition>
<SimulationTimeCondition value="5.0" rule="greaterThan"/>
</ByValueCondition>
</Condition>
</ConditionGroup>
</StopTrigger>
</Storyboard>
</OpenSCENARIO>
<OpenDRIVE>
<header revMajor="1" revMinor="1" name="Testfile" version="1" date="Thu Feb 8 14:24:06 2007" north="2.0000000000000000e+003" south="-2.0000000000000000e+003" east="2.0000000000000000e+003" west="-2.0000000000000000e+003"/>
<road name="" length="5139.7188686309819" id="1" junction="-1">
<link/>
<planView>
<geometry s="0.0000000000000000e+000" x="0" y="7.5" hdg="0" length="4000">
<line/>
</geometry>
<geometry s="4000" x="4000" y="7.5" hdg="0" length="285">
<spiral curvStart="0.0000000000000000e+000" curvEnd="-0.0028500000000000066"/>
</geometry>
<geometry s="4285" x="4280.3350391612494" y="-30.629710830536482" hdg="-0.40612500000000096" length="183.7188686309816">
<arc curvature="-0.0028500000000000066"/>
</geometry>
<geometry s="4468.7188686309819" x="4422.9323279193268" y="-143.12204699500415" hdg="-0.92972377559829977" length="571">
<spiral curvStart="-0.0028500000000000066" curvEnd="0.0000000000000000e+000"/>
</geometry>
<geometry s="5039.7188686309819" x="4476.6651848689908" y="-694.88624436808436" hdg="-1.7453292519943295" length="100">
<line/>
</geometry>
</planView>
<elevationProfile/>
<lateralProfile/>
<lanes>
<laneSection s="0.0000000000000000e+000">
<left/>
<center>
<lane id="0" type="border" level="true">
<link>
<successor id="0"/>
</link>
<roadMark weight="standard" sOffset="0" type="solid" color="standard" width="0.12" laneChange="both"/>
</lane>
</center>
<right>
<lane id="-1" type="driving" level="true">
<link>
<successor id="-1"/>
</link>
<width sOffset="0" a="3.75" b="0" c="0" d="0"/>
<roadMark weight="standard" sOffset="0" type="broken" color="standard" width="0.12" laneChange="both"/>
</lane>
<lane id="-2" type="driving" level="true">
<link>
<successor id="-2"/>
</link>
<width sOffset="0" a="3.75" b="0" c="0" d="0"/>
<roadMark weight="standard" sOffset="0" type="solid" color="standard" width="0.12" laneChange="both"/>
</lane>