public static GLOSAResult ProjectedSignalTimingsForLane(MapDataIntersectionsIntersectionGeometryGenericLane lane, SPAT spat, ulong maneuver, int crocsTime)
{
GLOSAResult crocsResult = new GLOSAResult();
SPATIntersectionsIntersectionStateMovementState state = LocateSignalMovementStateForLane(spat, lane, maneuver);
if (state != null)
{
List <StateTimeMovementEvent> stateTimeMovementEvents = ProjectedMovementEventStates(state, crocsTime);
crocsResult.StateTimeMovementEvents = stateTimeMovementEvents;
if (IsTimeWithinCurrentMovementSequence(crocsTime, stateTimeMovementEvents) == true)
{
crocsResult.CurrentStateTimeMovement = FindNextMovementEvent(crocsTime, stateTimeMovementEvents);
}
else
{
crocsResult.Errors = GLOSAErrors.UnableToProjectMovementStates;
}
}
else
{
crocsResult.Errors = GLOSAErrors.UnableToFindProjectedStateForLane;
}
return(crocsResult);
}
public static GLOSAResult ProjectedLaneForManeuver(MapData map, List <GPSLocation> gpsHistory, double?deviceHeading, ulong maneuver)
{
List <TrafficNode> trafficNodePositions = ExtractTrafficNodesFromMAP(map, maneuver);
List <GPSLocation> gpsLocations = gpsHistory;
GPSLocation location = gpsLocations.First();
var sortedList = trafficNodePositions.
OrderBy(m => Distance.CalculateDistanceBetween2PointsKMs(
m.GPSLocation.Latitude,
m.GPSLocation.Longitude,
location.Latitude, location.Longitude)).ToList();
TrafficNode nearestNode = sortedList[0];
MapDataIntersectionsIntersectionGeometryGenericLane lane = null;
MapDataIntersectionsIntersectionGeometryGenericLane[] lanes = map.intersections.IntersectionGeometry.laneSet;
var possibleLanes = lanes.Where(genricLane => genricLane.laneID.ToString() == nearestNode.ID);
if (possibleLanes.Count() > 0)
{
lane = possibleLanes.First();
}
// Verify the lane is in the same direction as the vehicle
var nodes = 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();
GLOSAResult result = IsDirectionOfVehicleInSameDirectionAsLane(map.intersections.IntersectionGeometry.id.id, sortedNodes, deviceHeading, gpsHistory, 50, maneuver);
result.Description = $"LaneId: {lane.laneID}, {result.Description}";
result.Object = lane;
return(result);
}
public static GLOSAResult TimeToTraficLight(MapData map, SPAT spat, List <GPSLocation> gpsHistory, double?deviceHeading, ulong maneuver, int crocsTime)
{
GLOSAResult result = new GLOSAResult()
{
Errors = GLOSAErrors.NoErrors,
};
result = ProjectedLaneForManeuver(map, gpsHistory, deviceHeading, maneuver);
if (result.Errors == GLOSAErrors.NoErrors)
{
var lane = (MapDataIntersectionsIntersectionGeometryGenericLane)result.Object;
var description = result.Description;
result = ProjectedSignalTimingsForLane(lane, spat, maneuver, crocsTime);
result.Object = lane;
result.Description = description;
if (result.Errors == GLOSAErrors.NoErrors)
{
var timeToTrafficNode = -1.0;
if (result.CurrentStateTimeMovement.MovementEvent == MovementEvent.Green)
{
timeToTrafficNode = result.CurrentStateTimeMovement.MovementTimespan.TotalSeconds;
}
else if (result.CurrentStateTimeMovement.MovementEvent == MovementEvent.Red)
{
//WAIT!
}
else if (result.CurrentStateTimeMovement.MovementEvent == MovementEvent.Amber)
{
//TODO
}
result.TimeToTrafficLight = timeToTrafficNode;
}
}
else
{
result.Errors = GLOSAErrors.UnableToFindProjectedLaneForMovement;
}
return(result);
}
public static GLOSAResult IsDirectionOfVehicleInSameDirectionAsLane(object mapId, List <TrafficNode> laneNodes, double?deviceHeading, List <GPSLocation> vehicleGPSHistory, double tolerance, ulong maneuver)
{
// Take the nearest node as this should be closest to the target lane's signal
var firstNode = laneNodes.First().GPSLocation;
var secondNode = laneNodes[1].GPSLocation;
// check disatnce between nodes as some nodes are not within 1m. Need to have distance to calculate direction (10m)?
var distance = Distance.CalculateDistanceBetween2PointsKMs(firstNode.Latitude, firstNode.Longitude, secondNode.Latitude, secondNode.Longitude);
if (distance <= 0.010)
{
var thirdNode = laneNodes[2].GPSLocation;
secondNode = thirdNode;
}
double?laneHeadingDegrees = LocationHelper.DegreeBearing(secondNode.Latitude, secondNode.Longitude, firstNode.Latitude, firstNode.Longitude);
int laneDegreesInt = Convert.ToInt32(laneHeadingDegrees);
GPSLocation previousVehicleLocation = vehicleGPSHistory[vehicleGPSHistory.Count - 2];
GPSLocation currentVehicleLocation = vehicleGPSHistory[vehicleGPSHistory.Count - 1];
double? vehicleHeadingDegrees = LocationHelper.DegreeBearing(previousVehicleLocation.Latitude, previousVehicleLocation.Longitude, currentVehicleLocation.Latitude, currentVehicleLocation.Longitude);
int vehicleDegreesInt = Convert.ToInt32(vehicleHeadingDegrees);
int deviceHeadingInt = Convert.ToInt32(deviceHeading);
GLOSAResult result = new GLOSAResult();
var delta = LocationHelper.DeltaOfVehicleToLaneDirection(vehicleHeadingDegrees, laneHeadingDegrees);
if (delta <= tolerance && delta >= -tolerance)
{
result.Errors = GLOSAErrors.NoErrors;
}
else
{
result.Errors = GLOSAErrors.ProjectedLaneNotInSameDirection;
}
result.Description = $"LaneHeading: { laneDegreesInt}, VehicleHeading: { vehicleDegreesInt}, Delta: { delta}";
return(result);
}
