public void Start(List <kmlDocumentPlacemark> route, string routeId, ulong allowedVehicleManeuvers, WaypointDetectionMethod junctionDetectionMethod)
{
_testRoute = route;
_routeId = routeId;
_waypointDetectionMethod = junctionDetectionMethod;
_routeDirection = Constants.SETTINGS_ROUTE_DIRECTION_ANY;
_routeSession = Guid.NewGuid().ToString();
_routeMode = true;
_useSimulatedGPS = false;
}
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 Start(List <kmlDocumentPlacemark> route, string routeId, ulong allowedVehicleManeuvers, WaypointDetectionMethod junctionDetectionMethod, AdvisoryCalculatorMode advisoryCalculatorMode)
{
if (route == null || route.Count == 0)
{
throw new Exception("Route not specified");
}
Debug.WriteLine("Starting Vehicle Service");
_advisoryCalculatorMode = advisoryCalculatorMode;
_loggingEnabled = true;
_allowedVehicleManeuvers = allowedVehicleManeuvers;
_navigationService.Start(route, routeId, allowedVehicleManeuvers, junctionDetectionMethod);
_timerFinished = false;
if (_timerRunning == false)
{
_timerRunning = true;
_timerService.StartTimer(TimeSpan.FromMilliseconds(Constants.AdvisorySpeedCalculatorProcessingIntervalMilliseconds), (() => TimerServiceCallback()));
}
}
public MapData LocateWaypointWithLineOfSight(WaypointDetectionMethod method, int waypointIndex = 0, int viewArc = 0, double distance = 1.0)
{
return(LocateWaypointWithLineOfSight(method, GetGPSHistory(), waypointIndex, viewArc, distance));
}
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);
}
public kmlDocumentPlacemark LocateWaypointOnRoute(WaypointDetectionMethod method, int waypointIndex = 0)
{
return(LocateWaypointOnRoute(method, _testRoute, GetGPSHistory(), 0, waypointIndex));
}
