def _prepare_vehicle(self, vehicle_type, object_id, car_width, car_length,
road_width, road_length, car_rear_end_lon,
car_right_lat, car_speed, occupied_region):
"""
Setup Ego vehicle
"""
rss_vehicle_a = rss.Object()
rss_vehicle_a.objectType = vehicle_type
rss_vehicle_a.velocity.speedLonMin = physics.Speed(car_speed)
rss_vehicle_a.velocity.speedLonMax = physics.Speed(car_speed)
rss_vehicle_a.velocity.speedLatMin = physics.Speed(0.0)
rss_vehicle_a.velocity.speedLatMax = physics.Speed(0.0)
rss_vehicle_a.objectId = object_id
occupied_region.latRange.minimum = physics.ParametricValue(
car_right_lat)
occupied_region.latRange.maximum = physics.ParametricValue(
(car_right_lat + car_width) / road_width)
occupied_region.lonRange.minimum = physics.ParametricValue(
car_rear_end_lon / road_length)
occupied_region.lonRange.maximum = physics.ParametricValue(
(car_rear_end_lon + car_length) / road_length)
rss_vehicle_a.occupiedRegions.append(occupied_region)
return rss_vehicle_a
def test_interface(self):
"""
Main test part
"""
speed_a = physics.Speed(10.)
speed_b = physics.Speed(20.)
speed_c = speed_a + speed_b
self.assertEqual(speed_c, 30.)
t = physics.Duration(2.)
a_2 = speed_a / t
self.assertEqual(a_2, 5.)
a = physics.Acceleration(10.)
speed_c = t * a
self.assertEqual(speed_c, 20.)
def _initialize_ego_vehicle(self):
# laneId: offset 240151:0.55
self._initialize_object(admap.Longitude(8.00125444865324766),
admap.Latitude(48.99758627528235877),
math.pi / 2, self.egoPosition,
self.egoMapMatchedBoundingBox)
self.egoSpeed = physics.Speed(5.)
# laneId: offset 120149:0.16
positionEndGeo = admap.createGeoPoint(
admap.Longitude(8.00188527300496979),
admap.Latitude(48.99821051747871792), admap.Altitude(0.))
self.egoRoute = admap.planRoute(
self.egoMapMatchedBoundingBox.referencePointPositions[
admap.ObjectReferencePoints.Center][0].lanePoint.paraPoint,
positionEndGeo)
def _prepare_world_model(self):
"""
Setup RSS world model
"""
rss_vehicle_a = rss.Object()
rss_vehicle_b = rss.Object()
rss_vehicle_a.objectType = rss.ObjectType.EgoVehicle
rss_vehicle_a.velocity.speedLonMin = physics.Speed(20.0)
rss_vehicle_a.velocity.speedLonMax = physics.Speed(20.0)
rss_vehicle_a.velocity.speedLatMin = physics.Speed(0.0)
rss_vehicle_a.velocity.speedLatMax = physics.Speed(0.0)
rss_vehicle_a.objectId = 0
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.occupiedRegions.append(occupied_region)
rss_vehicle_b.objectType = rss.ObjectType.OtherVehicle
rss_vehicle_b.velocity.speedLonMin = physics.Speed(10.0)
rss_vehicle_b.velocity.speedLonMax = physics.Speed(10.0)
rss_vehicle_b.velocity.speedLatMin = physics.Speed(0.0)
rss_vehicle_b.velocity.speedLatMax = physics.Speed(0.0)
rss_vehicle_b.objectId = 1
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.occupiedRegions.append(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.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.)
self.world_model = rss.WorldModel()
self.world_model.timeIndex = 1
self.world_model.scenes.append(rss_scene)
self.world_model.egoVehicleRssDynamics.responseTime = physics.Duration(
0.2)
self.world_model.egoVehicleRssDynamics.alphaLat.accelMax = physics.Acceleration(
0.1)
self.world_model.egoVehicleRssDynamics.alphaLat.brakeMin = physics.Acceleration(
0.1)
self.world_model.egoVehicleRssDynamics.alphaLon.accelMax = physics.Acceleration(
0.1)
self.world_model.egoVehicleRssDynamics.alphaLon.brakeMax = physics.Acceleration(
8.)
self.world_model.egoVehicleRssDynamics.alphaLon.brakeMin = physics.Acceleration(
4.)
self.world_model.egoVehicleRssDynamics.alphaLon.brakeMinCorrect = physics.Acceleration(
4.)
self.world_model.egoVehicleRssDynamics.lateralFluctuationMargin = physics.Distance(
0.)
class InterfaceTest(unittest.TestCase):
"""
Test class for Python interface
"""
world_model = None
egoVehicleId = 123
egoPosition = admap.ENUObjectPosition()
egoSpeed = physics.Speed()
egoMapMatchedBoundingBox = admap.MapMatchedObjectBoundingBox()
egoRoute = admap.FullRoute()
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 _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 _initialize_object(self, lon, lat, yawAngle, resultPosition,
resultMapMatchedBoundingBox):
positionGeo = admap.createGeoPoint(lon, lat, admap.Altitude(0.))
# fill the result position
resultPosition.centerPoint = admap.toENU(positionGeo)
resultPosition.heading = admap.createENUHeading(yawAngle)
resultPosition.dimension.length = physics.Distance(4.5)
resultPosition.dimension.width = physics.Distance(2.)
resultPosition.dimension.height = physics.Distance(1.5)
resultPosition.enuReferencePoint = admap.getENUReferencePoint()
mapMatching = admap.AdMapMatching()
mapMatchedBoundingBox = mapMatching.getMapMatchedBoundingBox(
resultPosition, physics.Distance(0.1), physics.Probability(0.5))
self.assertGreaterEqual(
len(mapMatchedBoundingBox.referencePointPositions),
admap.ObjectReferencePoints.Center)
self.assertGreaterEqual(
len(mapMatchedBoundingBox.referencePointPositions[
admap.ObjectReferencePoints.Center]), 0)
# fill the result bounding box
resultMapMatchedBoundingBox.laneOccupiedRegions = mapMatchedBoundingBox.laneOccupiedRegions
resultMapMatchedBoundingBox.referencePointPositions = mapMatchedBoundingBox.referencePointPositions
def _initialize_ego_vehicle(self):
# laneId: offset 240151:0.55
self._initialize_object(admap.Longitude(8.00125444865324766),
admap.Latitude(48.99758627528235877),
math.pi / 2, self.egoPosition,
self.egoMapMatchedBoundingBox)
self.egoSpeed = physics.Speed(5.)
# laneId: offset 120149:0.16
positionEndGeo = admap.createGeoPoint(
admap.Longitude(8.00188527300496979),
admap.Latitude(48.99821051747871792), admap.Altitude(0.))
self.egoRoute = admap.planRoute(
self.egoMapMatchedBoundingBox.referencePointPositions[
admap.ObjectReferencePoints.Center][0].lanePoint.paraPoint,
positionEndGeo)
def test_interface(self):
"""
Main test part
"""
self.assertTrue(admap.init("resources/Town01.txt"))
print("== Map Loaded ==")
self._initialize_ego_vehicle()
self.world_model = rssmap.initializeWorldModel(
1, self._get_ego_vehicle_dynamics())
print("== Initial world model ==")
print(self.world_model)
otherVehicleId = 20
otherVehicleSpeed = physics.Speed(10.)
otherVehiclePosition = admap.ENUObjectPosition()
otherVehicleMapMatchedBoundingBox = admap.MapMatchedObjectBoundingBox()
# 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,
otherVehiclePosition,
otherVehicleMapMatchedBoundingBox)
self.assertTrue(
rssmap.appendScenes(
self.egoVehicleId, self.egoMapMatchedBoundingBox,
self.egoSpeed, self.egoRoute, otherVehicleId,
rss.ObjectType.OtherVehicle, otherVehicleMapMatchedBoundingBox,
otherVehicleSpeed, self._get_object_vehicle_dynamics(),
rssmap.RestrictSpeedLimitMode.IncreasedSpeedLimit10,
admap.LandmarkIdSet(), self.world_model))
self.assertTrue(
rssmap.finalizeWorldModel(self._get_ego_vehicle_dynamics(),
self.world_model))
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()
self.assertTrue(
rss_response_resolving.provideProperResponse(
rss_state_snapshot, rss_proper_response))
acceleration_restriction = rss.AccelerationRestriction()
self.assertTrue(
rss.transformProperResponse(self.world_model, 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
def test_interface(self):
"""
Main test part
"""
self.assertTrue(admap.init("resources/Town01.txt"))
print("== Map Loaded ==")
self._initialize_ego_vehicle()
self.world_model = rssmap.initializeWorldModel(
1, self._get_ego_vehicle_dynamics())
print("== Initial world model ==")
print(self.world_model)
otherVehicleId = 20
otherVehicleSpeed = physics.Speed(10.)
otherVehiclePosition = admap.ENUObjectPosition()
otherVehicleMapMatchedBoundingBox = admap.MapMatchedObjectBoundingBox()
# 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,
otherVehiclePosition,
otherVehicleMapMatchedBoundingBox)
self.assertTrue(
rssmap.appendScenes(
self.egoVehicleId, self.egoMapMatchedBoundingBox,
self.egoSpeed, self.egoRoute, otherVehicleId,
rss.ObjectType.OtherVehicle, otherVehicleMapMatchedBoundingBox,
otherVehicleSpeed, self._get_object_vehicle_dynamics(),
rssmap.RestrictSpeedLimitMode.IncreasedSpeedLimit10,
admap.LandmarkIdSet(), self.world_model))
self.assertTrue(
rssmap.finalizeWorldModel(self._get_ego_vehicle_dynamics(),
self.world_model))
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()
self.assertTrue(
rss_response_resolving.provideProperResponse(
rss_state_snapshot, rss_proper_response))
acceleration_restriction = rss.AccelerationRestriction()
self.assertTrue(
rss.transformProperResponse(self.world_model, 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