def _prepare_scene_same_direction(self, object_id_ego, object_id_other, ego_car_width, other_car_width, ego_car_length, other_car_length, road_width, road_length, ego_rear_end_lon, other_rear_end_lon, ego_car_right_lat, other_car_right_lat, ego_car_speed, other_car_speed): occupied_region = self._prepare_occupied_region(0) ego_vehicle = self._prepare_vehicle(rss.ObjectType.EgoVehicle, object_id_ego, ego_car_width, ego_car_length, road_width, road_length, ego_rear_end_lon, ego_car_right_lat, ego_car_speed, occupied_region) other_vehicle = self._prepare_vehicle_negative_direction(rss.ObjectType.OtherVehicle, object_id_other, other_car_width, other_car_length, road_width, road_length, other_rear_end_lon, other_car_right_lat, other_car_speed, occupied_region) rss_scene = rss.Scene() rss_scene.egoVehicle = ego_vehicle rss_scene.object = other_vehicle rss_scene.situationType = rss.SituationType.OppositeDirection road_segment = rss.RoadSegment() lane_segment = rss.LaneSegment() lane_segment.id = 0 lane_segment.length.minimum = physics.Distance(road_length) lane_segment.length.maximum = physics.Distance(road_length) lane_segment.width.minimum = physics.Distance(road_width) lane_segment.width.maximum = physics.Distance(road_width) lane_segment.type = rss.LaneSegmentType.Normal lane_segment.drivingDirection = rss.LaneDrivingDirection.Bidirectional 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.) return rss_scene
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 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 _prepare_world_model2(self, rss_scene, time_index): self.world_model = rss.WorldModel() self.world_model.timeIndex = time_index 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.)
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.)
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