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