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
"""
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 _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.)