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);
}
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);
}
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());
}
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);
}
