Exemplo n.º 1
0
    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
Exemplo n.º 2
0
    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
Exemplo n.º 3
0
    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()
Exemplo n.º 4
0
 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
Exemplo n.º 5
0
 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
Exemplo n.º 6
0
    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.)
Exemplo n.º 7
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.)
Exemplo n.º 8
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