public MapData LocateWaypointWithLineOfSight(WaypointDetectionMethod method, List <GPSLocation> history, int waypointIndex = 0, int viewArc = 0, double distance = 1.0)
        {
            MapData waypoint = null;

            var location = history.Last();

            var sortedMAPPoints = GLOSAHelper.SortMAPDataByDistanceFromCurrentLocation(_mapDataCache, location.Latitude, location.Longitude);

            if (sortedMAPPoints != null && sortedMAPPoints.Count > 0)
            {
                switch (method)
                {
                case WaypointDetectionMethod.ShortestDistance:
                    waypoint = sortedMAPPoints.First();
                    break;

                case WaypointDetectionMethod.GPSHistoryDirection:
                    GPSLocation to             = history[history.Count - 1];
                    double?     vehicleHeading = LocationHelper.HeadingFromGPSTrail(history);

                    List <IntersectionNode> listOfMAPDataWaypoints = IntersectionNode.IntersectionNodesFromMAPData(_mapDataCache).ToList();
                    var filtered = IntersectionNode.Filter(listOfMAPDataWaypoints, vehicleHeading, history);
                    var sortedByBearingAndDistance = IntersectionNode.SortIntersectionNodes(filtered, to, vehicleHeading, viewArc, distance).ToList();

                    if (sortedByBearingAndDistance.Count() > waypointIndex)
                    {
                        var tn = sortedByBearingAndDistance.ToList()[waypointIndex];
                        waypoint = tn.MapData;
                    }

                    break;

                default:
                    throw new Exception("Waypoint Detection Method not implemented.");
                    break;
                }
            }
            return(waypoint);
        }
Esempio n. 2
0
        public void IsVehicleHeadingInSameDirectionAsLaneDirection_WithLaneInSameDirection_ShouldReturnTrue()
        {
            // arrange
            //SETTINGS_ROUTE_DIRECTION_NS = 1: - Not Supported Yet
            //SETTINGS_ROUTE_DIRECTION_SN = 2: - Not Supported Yet
            //SETTINGS_ROUTE_DIRECTION_EW = 3;
            //SETTINGS_ROUTE_DIRECTION_WE = 4;
            var intersectionId = "2111";
            var gpsHistory     = KMLHelper.GLOSATestRouteIntersectionHistory(intersectionId, 2);

            // Load Map from file
            string      file   = $"MAP-{Settings.IntersectionId}.xml";
            MapData     map    = XMLHelper.LoadMAPDataFromFile(file);
            GLOSAResult result = GLOSAHelper.ProjectedLaneForManeuver(map, gpsHistory, 0, Constants.MANEUVER_DIRECTION_AHEAD);
            var         lane   = (MapDataIntersectionsIntersectionGeometryGenericLane)result.Object;
            var         nodes  = GLOSAHelper.ExtractTrafficNodesFromLane(lane);

            // Let's sort all lane nodes from MAP Ref Point
            var refPoint    = map.intersections.IntersectionGeometry.refPoint;
            var mapLocation = new GPSLocation()
            {
                Latitude  = refPoint.lat / Constants.MAPCoordinateIntConverterUnit,
                Longitude = refPoint.@long / Constants.MAPCoordinateIntConverterUnit,
            };

            // Sort the nodes by distance ascending
            var sortedNodes = nodes.OrderBy(node => Distance.CalculateDistanceBetween2PointsKMs(node.GPSLocation.Latitude, node.GPSLocation.Longitude, mapLocation.Latitude, mapLocation.Longitude)).ToList();

            bool expectedResult = true;

            //act
            result = GLOSAHelper.IsDirectionOfVehicleInSameDirectionAsLane(map.intersections.IntersectionGeometry.id.id, sortedNodes, 0, gpsHistory, 50, Constants.MANEUVER_DIRECTION_AHEAD);
            bool actualResult = result.Errors == GLOSAErrors.NoErrors;

            //assert
            Assert.Equal(expectedResult, actualResult);
        }
Esempio n. 3
0
 public static IList <IntersectionNode> Filter(IList <IntersectionNode> intersectionNodes, double?heading, List <GPSLocation> gpsTrail)
 {
     return(intersectionNodes.Where(n => GLOSAHelper.ProjectedLaneForManeuver(n.MapData, gpsTrail, heading, Constants.MANEUVER_DIRECTION_AHEAD).Errors == GLOSAErrors.NoErrors).ToList());
 }
Esempio n. 4
0
        private bool TimerServiceCallback()
        {
            bool @continue = true;

            if (TimerHasFinished())
            {
                @continue = false;
            }
            else
            {
                var before = DateTime.Now;

                if (Settings.EnableIntersectionMode == false && CheckLocationServicesPermission() == false)
                {
                    PostVehicleMessage(VehicleServiceStatus.GPSPermissionError);
                    Debug.WriteLine($"Vehicle Service Timer {DateTime.Now} : GPS not enabled");
                    LogDataEvent("GPS not enabled (Permissions)");
                    return(@continue);
                }

                if (Settings.EnableIntersectionMode == false && CheckLocationServices() == false)
                {
                    PostVehicleMessage(VehicleServiceStatus.GPSNotAvailable);
                    Debug.WriteLine($"Vehicle Service Timer {DateTime.Now} : Waiting for GPS");
                    LogDataEvent("Waiting for GPS");
                    return(@continue);
                }

                if (Settings.EnableIntersectionMode == false && string.IsNullOrEmpty(Settings.UniqueVehicleDeviceAppId.Trim()) == true)
                {
                    PostVehicleMessage(VehicleServiceStatus.VehicleIdNotSet);
                    Debug.WriteLine($"Vehicle Service Timer {DateTime.Now} : Vehicle Id missing");
                    LogDataEvent("Vehicle Id missing");
                    return(@continue);
                }

                if (CheckNetworkStatus() == false)
                {
                    PostVehicleMessage(VehicleServiceStatus.NetworkConnectionError);
                    Debug.WriteLine($"Vehicle Service Timer {DateTime.Now} : Waiting for connection (WiFi Mode: {Settings.EnableWiFiMode})");
                    LogDataEvent("No Network Connection");
                    return(@continue);
                }

                Task.Run(() => SyncTime());

                if (_navigationService.IsNavigating == false || _navigationService.IsNavigatingToWaypoint == false || _navigationService.Waypoint == null)
                {
                    PostVehicleMessage(null, null);
                    Debug.WriteLine($"Vehicle Service Timer {DateTime.Now} : Locating intersection");
                    LogDataEvent("Locating intersection");
                    return(@continue);
                }

                // GET MAP SPAT data of intersection
                var nextWaypoint   = _navigationService.LocateWaypointWithLineOfSight(WaypointDetectionMethod.GPSHistoryDirection, 1, 50);
                var nexyWaypointId = nextWaypoint != null?nextWaypoint.intersections.IntersectionGeometry.id.id.ToString() : null;

                Task.Run(async() => await _GLOSAWebService.SyncMAPSPATAsync(_navigationService.WayPointId, nexyWaypointId));

                if (HasMapSPATDataFromCellular() == false && HasMapSPATDataFromWiFi() == false)
                {
                    PostVehicleMessage(null, null);
                    LogDataEvent($"Waiting for data");
                    Debug.WriteLine($"Vehicle Service Timer {DateTime.Now} : Waiting for data {_navigationService.Waypoint.name}");
                    return(@continue);
                }

                MapData map            = null;
                SPAT    spat           = null;
                var     dataConnection = DataConnection.Cellular;

                if (Settings.EnableWiFiMode == true && HasMapSPATDataFromWiFi() == true)
                {
                    _isUsingWiFiSPATData = true;
                    map            = GetWiFIMAP();
                    spat           = GetWiFISPAT();
                    dataConnection = DataConnection.WiFi_Beacon;
                }

                // revert to celluar data
                if (spat == null && HasMapSPATDataFromCellular() == true)
                {
                    _isUsingWiFiSPATData = false;
                    spat           = _GLOSAWebService.SPATData(_navigationService.WayPointId);
                    dataConnection = DataConnection.Cellular;
                }

                // revert to celluar data
                if (map == null && HasMapSPATDataFromCellular() == true)
                {
                    map = _GLOSAWebService.MAPData(_navigationService.WayPointId);
                }

                var history = _navigationService.GPSHistory;

                DateTime date             = CurrentTime();
                int      currentTimeCROCS = GLOSAHelper.ConvertTimeToCROCSTime(date);

                GLOSAResult glosaResult = GLOSAHelper.TimeToTraficLight(map, spat, history, _navigationService.DeviceHeading, _allowedVehicleManeuvers, currentTimeCROCS);

                if (glosaResult.Errors == GLOSAErrors.NoErrors)
                {
                    CalculationResult calculation = AdvisorySpeedCalculationResult.CalculateAdvisorySpeed(_navigationService.DistanceToWaypoint, glosaResult.TimeToTrafficLight, _navigationService.CurrentSpeed, _advisoryCalculatorMode);
                    PostVehicleMessage(calculation, glosaResult);

                    var    after   = DateTime.Now;
                    double latency = (after - before).TotalMilliseconds;
                    LogDataEvent(calculation, glosaResult, latency, currentTimeCROCS, dataConnection);
                }
                else
                {
                    PostVehicleMessage(null, glosaResult);
                    LogDataEvent("GLOSA Result", null, null, $"{map.intersections.IntersectionGeometry.id.id}", null, 0, Convert.ToInt16(_navigationService.DeviceHeading), glosaResult.Description);
                    Debug.WriteLine($"Vehicle Service Timer {DateTime.Now} : GLOSA Error - {glosaResult.Errors}");
                }
            }

            return(@continue);
        }
        public kmlDocumentPlacemark LocateWaypointOnRoute(WaypointDetectionMethod method, List <kmlDocumentPlacemark> route, List <GPSLocation> history, double heading, int waypointIndex = 0)
        {
            kmlDocumentPlacemark nearestMAPPlacemark = null;

            var location = history.Last();

            var sortedMAPPoints = GLOSAHelper.SortMAPKMLDataByDistanceFromCurrentLocation(route, location.Latitude, location.Longitude).ToList();

            if (sortedMAPPoints != null && sortedMAPPoints.Count > 0)
            {
                if (method == WaypointDetectionMethod.ShortestDistance)
                {
                    nearestMAPPlacemark = sortedMAPPoints.First();
                }
                else if (method == WaypointDetectionMethod.DeviceHeading)
                {
                    // No valid method chosen
                    throw new Exception("Waypoint Detection Method not implemented.");
                }
                else if (method == WaypointDetectionMethod.GPSHistoryDirection)
                {
                    List <TrafficNode> listOfWaypoints = new List <TrafficNode>();

                    GPSLocation from = history[history.Count - 2];
                    GPSLocation to   = history[history.Count - 1];

                    double?vehicleBearing = LocationHelper.HeadingBetweenTwoGPSLocations(from, to);

                    foreach (var mapPoint in sortedMAPPoints)
                    {
                        listOfWaypoints.Add(new TrafficNode()
                        {
                            GPSLocation = KMLHelper.XMLCoordinateStringToGPSLocation(mapPoint.Point.coordinates),
                            ID          = mapPoint.name,
                            angleDiff   = 0,
                        });
                    }

                    foreach (var waypoint in listOfWaypoints)
                    {
                        double?bearingLocationToWaypoint = LocationHelper.HeadingBetweenTwoGPSLocations(to, waypoint.GPSLocation);
                        double delta = LocationHelper.DeltaOfVehicleToLaneDirection(vehicleBearing, bearingLocationToWaypoint);
                        waypoint.angleDiff = Math.Abs(delta);//Make positive
                        waypoint.distance  = Distance.CalculateDistanceBetween2PointsKMs(to.Latitude, to.Longitude, waypoint.GPSLocation.Latitude, waypoint.GPSLocation.Longitude);
                    }

                    var sortedByBearingAndDistance = listOfWaypoints.OrderBy(wp => wp.distance)
                                                     .ThenBy(wp => wp.angleDiff)
                                                     .Where(wp => wp.angleDiff >= -50 && wp.angleDiff <= 50 && wp.distance <= 1.0);

                    if (sortedByBearingAndDistance != null && sortedByBearingAndDistance.Count() > waypointIndex)
                    {
                        var tn = sortedByBearingAndDistance.ToList()[waypointIndex];
                        var pm = sortedMAPPoints.Find(mp => mp.name == tn.ID);
                        nearestMAPPlacemark = pm;
                    }
                }
                else
                {
                    // No valid method chosen
                    throw new Exception("Waypoint Detection Method not properly set.");
                }
            }
            return(nearestMAPPlacemark);
        }