def _getUnstructuedSettings(self):
unstructuredSettings = rss.UnstructuredSettings()
unstructuredSettings.pedestrianTurningRadius = physics.Distance(2.0)
unstructuredSettings.driveAwayMaxAngle = physics.Angle(2.4)
unstructuredSettings.vehicleYawRateChange = physics.AngularAcceleration(
0.3)
unstructuredSettings.vehicleMinRadius = physics.Distance(3.5)
unstructuredSettings.vehicleTrajectoryCalculationStep = physics.Duration(
0.2)
return unstructuredSettings
def test_interface(self):
"""
Main test part
"""
self.assertTrue(admap.init("test_files/TPK.adm.txt"))
# map loaded
lanes = admap.getLanes()
self.assertEqual(len(lanes), 141)
# map matching
mapMatching = admap.AdMapMatching()
geoPoint = admap.GeoPoint()
geoPoint.longitude = admap.Longitude(8.4401803)
geoPoint.latitude = admap.Latitude(49.0191987)
geoPoint.altitude = admap.Altitude(0.)
mapMatchingResults = mapMatching.getMapMatchedPositions(
geoPoint, physics.Distance(0.01), physics.Probability(0.05))
self.assertEqual(len(mapMatchingResults), 1)
# route planning
routingStart = mapMatchingResults[0].lanePoint.paraPoint
routingEnd = admap.ParaPoint()
routingEnd.laneId = routingStart.laneId
routingEnd.parametricOffset = physics.ParametricValue(0.0)
routeResult = admap.planRoute(admap.createRoutingPoint(routingStart),
admap.createRoutingPoint(routingEnd))
routeLength = admap.calcLength(routeResult.roadSegments[0])
self.assertEqual(int(float(routeLength)), 4)
admap.cleanup()
def _get_ego_vehicle_dynamics(self):
egoRssDynamics = rss.RssDynamics()
egoRssDynamics.responseTime = physics.Duration(0.2)
egoRssDynamics.alphaLat.accelMax = physics.Acceleration(0.1)
egoRssDynamics.alphaLat.brakeMin = physics.Acceleration(-0.1)
egoRssDynamics.alphaLon.accelMax = physics.Acceleration(0.1)
egoRssDynamics.alphaLon.brakeMax = physics.Acceleration(-8.)
egoRssDynamics.alphaLon.brakeMinCorrect = physics.Acceleration(-4.)
egoRssDynamics.alphaLon.brakeMin = physics.Acceleration(-4.)
egoRssDynamics.lateralFluctuationMargin = physics.Distance(0.)
return egoRssDynamics
def _get_object_vehicle_dynamics(self):
objectRssDynamics = rss.RssDynamics()
objectRssDynamics.responseTime = physics.Duration(0.5)
objectRssDynamics.alphaLat.accelMax = physics.Acceleration(1.)
objectRssDynamics.alphaLat.brakeMin = physics.Acceleration(-1.)
objectRssDynamics.alphaLon.accelMax = physics.Acceleration(4.)
objectRssDynamics.alphaLon.brakeMax = physics.Acceleration(-8.)
objectRssDynamics.alphaLon.brakeMinCorrect = physics.Acceleration(-4.)
objectRssDynamics.alphaLon.brakeMin = physics.Acceleration(-4.)
objectRssDynamics.lateralFluctuationMargin = physics.Distance(0.)
return objectRssDynamics
def _initialize_object(self, lon, lat, yawAngle, resultObject):
positionGeo = admap.createGeoPoint(lon, lat, admap.Altitude(0.))
# fill the result position
resultObject.enuPosition.centerPoint = admap.toENU(positionGeo)
resultObject.enuPosition.heading = admap.createENUHeading(yawAngle)
resultObject.enuPosition.dimension.length = physics.Distance(4.5)
resultObject.enuPosition.dimension.width = physics.Distance(2.)
resultObject.enuPosition.dimension.height = physics.Distance(1.5)
resultObject.enuPosition.enuReferencePoint = admap.getENUReferencePoint(
)
mapMatching = admap.AdMapMatching()
resultObject.mapMatchedBoundingBox = mapMatching.getMapMatchedBoundingBox(
resultObject.enuPosition, physics.Distance(0.1))
self.assertGreaterEqual(
len(resultObject.mapMatchedBoundingBox.referencePointPositions),
admap.ObjectReferencePoints.Center)
self.assertGreaterEqual(
len(resultObject.mapMatchedBoundingBox.referencePointPositions[
admap.ObjectReferencePoints.Center]), 0)
def _get_ego_vehicle_dynamics(self):
egoRssDynamics = rss.RssDynamics()
egoRssDynamics.responseTime = physics.Duration(0.2)
egoRssDynamics.maxSpeedOnAcceleration = physics.Speed(0.)
egoRssDynamics.alphaLat.accelMax = physics.Acceleration(0.1)
egoRssDynamics.alphaLat.brakeMin = physics.Acceleration(-0.1)
egoRssDynamics.alphaLon.accelMax = physics.Acceleration(0.1)
egoRssDynamics.alphaLon.brakeMax = physics.Acceleration(-8.)
egoRssDynamics.alphaLon.brakeMinCorrect = physics.Acceleration(-4.)
egoRssDynamics.alphaLon.brakeMin = physics.Acceleration(-4.)
egoRssDynamics.lateralFluctuationMargin = physics.Distance(0.)
egoRssDynamics.unstructuredSettings = self._getUnstructuedSettings()
return egoRssDynamics
def _createObject(self, objectId, objectType, lonVelocity, latVelocity, occupiedRegion):
"""
Fill basic object data
"""
rss_vehicle = rss.Object()
rss_vehicle.objectId = objectId
rss_vehicle.objectType = objectType
rss_vehicle.velocity.speedLonMin = lonVelocity
rss_vehicle.velocity.speedLonMax = lonVelocity
rss_vehicle.velocity.speedLatMin = latVelocity
rss_vehicle.velocity.speedLatMax = latVelocity
rss_vehicle.state.yaw = physics.Angle(0.0)
rss_vehicle.state.dimension.length = physics.Distance(4.0)
rss_vehicle.state.dimension.width = physics.Distance(2.0)
rss_vehicle.state.yawRate = physics.AngularVelocity(0.0)
rss_vehicle.state.centerPoint.x = physics.Distance(0.0)
rss_vehicle.state.centerPoint.y = physics.Distance(0.0)
rss_vehicle.state.speed = math.sqrt(lonVelocity * lonVelocity + latVelocity * latVelocity)
rss_vehicle.state.steeringAngle = physics.Angle(0.0)
rss_vehicle.occupiedRegions.append(occupiedRegion)
return rss_vehicle
def _prepare_world_model(self):
"""
Setup RSS world model
"""
occupied_region = rss.OccupiedRegion()
occupied_region.segmentId = 0
occupied_region.latRange.minimum = physics.ParametricValue(0)
occupied_region.latRange.maximum = physics.ParametricValue(2.0 / self.road_width)
occupied_region.lonRange.minimum = physics.ParametricValue(0.)
occupied_region.lonRange.maximum = physics.ParametricValue(5.0 / self.road_length)
rss_vehicle_a = self._createObject(0, rss.ObjectType.EgoVehicle, 20., 0., occupied_region)
occupied_region.lonRange.minimum = physics.ParametricValue(100.0 / self.road_length)
occupied_region.lonRange.maximum = physics.ParametricValue((5.0 + 100) / self.road_length)
rss_vehicle_b = self._createObject(1, rss.ObjectType.OtherVehicle, 10., 0., occupied_region)
rss_scene = rss.Scene()
rss_scene.egoVehicle = rss_vehicle_a
rss_scene.object = rss_vehicle_b
rss_scene.situationType = rss.SituationType.SameDirection
road_segment = rss.RoadSegment()
lane_segment = rss.LaneSegment()
lane_segment.id = 0
lane_segment.length.minimum = physics.Distance(self.road_length)
lane_segment.length.maximum = physics.Distance(self.road_length)
lane_segment.width.minimum = physics.Distance(self.road_width)
lane_segment.width.maximum = physics.Distance(self.road_width)
lane_segment.type = rss.LaneSegmentType.Normal
lane_segment.drivingDirection = rss.LaneDrivingDirection.Positive
road_segment.append(lane_segment)
rss_scene.egoVehicleRoad.append(road_segment)
rss_scene.objectRssDynamics.responseTime = physics.Duration(0.5)
rss_scene.objectRssDynamics.maxSpeedOnAcceleration = physics.Speed(0.)
rss_scene.objectRssDynamics.alphaLat.accelMax = physics.Acceleration(1.)
rss_scene.objectRssDynamics.alphaLat.brakeMin = physics.Acceleration(-1.)
rss_scene.objectRssDynamics.alphaLon.accelMax = physics.Acceleration(4.)
rss_scene.objectRssDynamics.alphaLon.brakeMax = physics.Acceleration(-8.)
rss_scene.objectRssDynamics.alphaLon.brakeMinCorrect = physics.Acceleration(-4.)
rss_scene.objectRssDynamics.alphaLon.brakeMin = physics.Acceleration(-4.)
rss_scene.objectRssDynamics.lateralFluctuationMargin = physics.Distance(0.)
rss_scene.objectRssDynamics.unstructuredSettings = self._getUnstructuedSettings()
rss_scene.egoVehicleRssDynamics.responseTime = physics.Duration(0.2)
rss_scene.egoVehicleRssDynamics.maxSpeedOnAcceleration = physics.Speed(0.)
rss_scene.egoVehicleRssDynamics.alphaLat.accelMax = physics.Acceleration(0.1)
rss_scene.egoVehicleRssDynamics.alphaLat.brakeMin = physics.Acceleration(-0.1)
rss_scene.egoVehicleRssDynamics.alphaLon.accelMax = physics.Acceleration(0.1)
rss_scene.egoVehicleRssDynamics.alphaLon.brakeMax = physics.Acceleration(-8.)
rss_scene.egoVehicleRssDynamics.alphaLon.brakeMin = physics.Acceleration(-4.)
rss_scene.egoVehicleRssDynamics.alphaLon.brakeMinCorrect = physics.Acceleration(-4.)
rss_scene.egoVehicleRssDynamics.lateralFluctuationMargin = physics.Distance(0.)
rss_scene.egoVehicleRssDynamics.unstructuredSettings = self._getUnstructuedSettings()
self.world_model = rss.WorldModel()
self.world_model.timeIndex = 1
self.world_model.scenes.append(rss_scene)
self.world_model.defaultEgoVehicleRssDynamics.responseTime = physics.Duration(0.2)
self.world_model.defaultEgoVehicleRssDynamics.maxSpeedOnAcceleration = physics.Speed(0.)
self.world_model.defaultEgoVehicleRssDynamics.alphaLat.accelMax = physics.Acceleration(0.1)
self.world_model.defaultEgoVehicleRssDynamics.alphaLat.brakeMin = physics.Acceleration(-0.1)
self.world_model.defaultEgoVehicleRssDynamics.alphaLon.accelMax = physics.Acceleration(0.1)
self.world_model.defaultEgoVehicleRssDynamics.alphaLon.brakeMax = physics.Acceleration(-8.)
self.world_model.defaultEgoVehicleRssDynamics.alphaLon.brakeMin = physics.Acceleration(-4.)
self.world_model.defaultEgoVehicleRssDynamics.alphaLon.brakeMinCorrect = physics.Acceleration(-4.)
self.world_model.defaultEgoVehicleRssDynamics.lateralFluctuationMargin = physics.Distance(0.)
self.world_model.defaultEgoVehicleRssDynamics.unstructuredSettings = self._getUnstructuedSettings()
def test_interface(self):
"""
Main test part
"""
self.assertTrue(admap.init("resources/Town01.txt"))
print("== Map Loaded ==")
self._initialize_ego_vehicle()
scene_creation = rssmap.RssSceneCreation(
1, self._get_ego_vehicle_dynamics())
otherVehicleId = 20
otherVehicleSpeed = physics.Speed(10.)
otherVehicleObject = admap.Object()
# laneId: offset 120149:0.16 around the intersection in front same direction on ego route
self._initialize_object(admap.Longitude(8.00188527300496979),
admap.Latitude(48.99821051747871792), 0,
otherVehicleObject)
self.assertTrue(
scene_creation.appendScenes(
self.egoVehicleId, self.egoObject, self.egoSpeed,
self._get_ego_vehicle_dynamics(), self.egoRoute,
otherVehicleId, rss.ObjectType.OtherVehicle,
otherVehicleObject, otherVehicleSpeed,
self._get_object_vehicle_dynamics(), rssmap.RssSceneCreation.
RestrictSpeedLimitMode.IncreasedSpeedLimit10,
admap.LandmarkIdSet()))
self.world_model = scene_creation.getWorldModel()
print("== Final world model ==")
print(self.world_model)
rss_response_resolving = rss.RssResponseResolving()
rss_situation_checking = rss.RssSituationChecking()
rss_sitation_extraction = rss.RssSituationExtraction()
rss_situation_snapshot = rss.SituationSnapshot()
self.assertTrue(
rss_sitation_extraction.extractSituations(self.world_model,
rss_situation_snapshot))
print("== Situation Snaphost ==")
print(rss_situation_snapshot)
rss_state_snapshot = rss.RssStateSnapshot()
self.assertTrue(
rss_situation_checking.checkSituations(rss_situation_snapshot,
rss_state_snapshot))
rss_proper_response = rss.ProperResponse()
acceleration_restriction = rss.AccelerationRestriction()
self.assertTrue(
rss_response_resolving.provideProperResponse(
rss_state_snapshot, rss_proper_response,
acceleration_restriction))
print("== Acceleration Restrictions ==")
print(acceleration_restriction)
longitudinal_distance = rss_situation_snapshot.situations[
0].relativePosition.longitudinalDistance
self.assertTrue(rss_proper_response.isSafe)
self.assertEqual(longitudinal_distance, physics.Distance(104.413))
self.world_model.timeIndex += 1
