public static void NextStop(RndfNetwork rndf, RndfLocation vehicleLocation,
out RndfWayPoint nextStop, out double distance)
{
// set current as final waypoint on current partition
RndfWayPoint current = vehicleLocation.Partition.FinalWaypoint;
// set initial distance
distance = vehicleLocation.AbsolutePositionOnPartition.DistanceTo(current.Position);
// iterate over waypoints
while (!current.IsStop && current.NextLanePartition != null)
{
// update distance
distance += current.Position.DistanceTo(current.NextLanePartition.FinalWaypoint.Position);
// update current
current = current.NextLanePartition.FinalWaypoint;
}
if (current.IsStop)
{
nextStop = current;
// return current as stop or end of lane
return;
}
else
{
nextStop = null;
return;
}
}
public void MessageArrived(string channelName, object message)
{
if (channelName == "RndfNetworkChannel")
{
this.rndfNetwork = (Common.RndfNetwork.RndfNetwork)message;
Console.WriteLine(" > Rndf Received");
}
else if (channelName == "MdfChannel")
{
this.mdf = (Common.RndfNetwork.Mdf)message;
Console.WriteLine(" > Mdf Received");
}
else if (channelName == "PositionChannel")
{
this.vehicleState = (VehicleState)message;
Console.WriteLine(" > Position Received: " + this.vehicleState.ToString());
}
else if (channelName == "TestStringChannel")
{
Console.WriteLine("Received String On TestStringChannel: " + ((string)message));
}
else
{
throw new ArgumentException("Unknown Channel", channelName);
}
}
/// <summary>
/// Singleton pattern. Static read-only instance property.
/// </summary>
public static TestServerFacade Instance(RndfNetwork rndfNetwork, Mdf mdf, VehicleState vehicleState)
{
if (instance == null)
{
instance = new TestDataServerFacadeImpl(rndfNetwork, mdf, vehicleState);
}
return(instance);
}
/// <summary>
/// Set the display's rndf
/// </summary>
/// <param name="rndf"></param>
public void SetRndf(RndfNetwork rndf)
{
if (rndf != null)
{
this.roadDisplay1.SetRndf(rndf);
}
else
{
RemoraOutput.WriteLine("Received Null Rndf");
}
}
/// <summary>
/// Generates speed limits for the rndf segments
/// </summary>
/// <param name="rndfNetwork"></param>
private static List <SpeedInformation> generateSpeedLimits(RndfNetwork rndfNetwork)
{
List <SpeedInformation> speedLimits = new List <SpeedInformation>();
foreach (Segment segment in rndfNetwork.Segments.Values)
{
SpeedInformation speedLimit = new SpeedInformation(segment.SegmentID, 0, 8.8);
speedLimits.Add(speedLimit);
}
return(speedLimits);
}
/// <summary>
/// Gets the next point at which we must stop
/// </summary>
/// <param name="rndf"></param>
/// <param name="vehicleLocation"></param>
/// <returns></returns>
public static RndfWayPoint NextStopOrEnd(RndfNetwork rndf, RndfLocation vehicleLocation)
{
// set current as final waypoint on current partition
RndfWayPoint current = vehicleLocation.Partition.FinalWaypoint;
// iterate over waypoints
while (!current.IsStop && current.NextLanePartition != null)
{
// update current
current = current.NextLanePartition.FinalWaypoint;
}
// return current as stop or end of lane
return(current);
}
/// <summary>
/// Sets the rndf, removing older ones
/// </summary>
/// <param name="rndf"></param>
public void SetRndf(RndfNetwork rndf)
{
this.rndf = rndf;
for (int i = 0; i < displayObjects.Count; i++)
{
if (displayObjects[i] is RndfDisplay)
{
displayObjects.RemoveAt(i);
}
}
displayObjects.Add(new RndfDisplay(rndf));
this.Invalidate();
}
public void RestartArbiter(RndfNetwork rndfNetwork, Mdf mdf)
{
if (arbiterRemote != null)
{
try
{
this.arbiterRemote.Restart(rndfNetwork, mdf, UrbanChallenge.Arbiter.ArbiterCommon.ArbiterMode.Debug);
}
catch (Exception e)
{
RemoraOutput.WriteLine(e.ToString());
}
}
else
{
RemoraOutput.WriteLine("Arbter Remote does not exist");
}
}
/// <summary>
/// Generates some goals from an Rndf
/// </summary>
/// <param name="rndfNetwork"></param>
/// <returns></returns>
private static Queue <Goal> generateGoals(RndfNetwork rndfNetwork)
{
Queue <Goal> goals = new Queue <Goal>();
try
{
// add goals in order of id
for (int i = 1; i < 5; i++)
{
goals.Enqueue(rndfNetwork.Goals[i]);
}
}
catch (Exception e)
{
Console.WriteLine(e.ToString());
throw e;
}
return(goals);
}
public RouteDisplay(RndfNetwork rndf, FullRoute route)
{
this.rndf = rndf;
this.route = route;
}
public GoalsDisplay(RndfNetwork rndf, RndfWaypointID current, Queue <RndfWaypointID> remaining)
{
this.rndf = rndf;
this.Current = current;
this.GoalsRemaining = remaining;
}
/// <summary>
/// Determines the lane adjacency
/// </summary>
/// <param name="rndf"></param>
/// <returns></returns>
private RndfNetwork DetermineLaneAdjacency(RndfNetwork rndf)
{
// loop over segment
foreach (Segment segment in rndf.Segments.Values)
{
// make sure both ways valid
if (segment.Way1.IsValid && segment.Way2.IsValid)
{
// dictionary of lanes in the segment
Dictionary <LaneID, Lane> segmentLanes = new Dictionary <LaneID, Lane>();
// construct dictionary
foreach (Way way in segment.Ways.Values)
{
foreach (Lane lane in way.Lanes.Values)
{
segmentLanes.Add(lane.LaneID, lane);
}
}
// check sample lane in way1
Lane way1SampleLane = null;
foreach (Lane tmp in segment.Way1.Lanes.Values)
{
way1SampleLane = tmp;
}
// check sample lane in way2
Lane way2SampleLane = null;
foreach (Lane tmp in segment.Way2.Lanes.Values)
{
way2SampleLane = tmp;
}
// modifies to denote increasing or decreasing form way1 to way 2
int modifier = 1;
// check if way2 has lower numbers
if (way1SampleLane.LaneID.LaneNumber > way2SampleLane.LaneID.LaneNumber)
{
modifier = -1;
}
int i = 1;
LaneID currentLaneID = new LaneID(way1SampleLane.LaneID.WayID, i);
// loop over lanes
while (segmentLanes.ContainsKey(currentLaneID))
{
Lane currentLane = segmentLanes[currentLaneID];
// increasing lane
LaneID increasingLaneID1 = new LaneID(segment.Way1.WayID, i + (modifier * 1));
LaneID increasingLaneID2 = new LaneID(segment.Way2.WayID, i + (modifier * 1));
if (segmentLanes.ContainsKey(increasingLaneID1))
{
if (currentLaneID.WayID.WayNumber == 1)
{
currentLane.OnLeft = segmentLanes[increasingLaneID1];
}
else
{
currentLane.OnRight = segmentLanes[increasingLaneID1];
}
}
else if (segmentLanes.ContainsKey(increasingLaneID2))
{
if (currentLaneID.WayID.WayNumber == 1)
{
currentLane.OnLeft = segmentLanes[increasingLaneID2];
}
else
{
currentLane.OnRight = segmentLanes[increasingLaneID2];
}
}
// check for decreasing
// increasing lane
increasingLaneID1 = new LaneID(segment.Way1.WayID, i - (modifier * 1));
increasingLaneID2 = new LaneID(segment.Way2.WayID, i - (modifier * 1));
if (segmentLanes.ContainsKey(increasingLaneID1))
{
if (currentLaneID.WayID.WayNumber == 1)
{
currentLane.OnRight = segmentLanes[increasingLaneID1];
}
else
{
currentLane.OnLeft = segmentLanes[increasingLaneID1];
}
}
else if (segmentLanes.ContainsKey(increasingLaneID2))
{
if (currentLaneID.WayID.WayNumber == 1)
{
currentLane.OnRight = segmentLanes[increasingLaneID2];
}
else
{
currentLane.OnLeft = segmentLanes[increasingLaneID2];
}
}
if (currentLane.OnLeft != null)
{
Console.WriteLine("Lane: " + currentLane.LaneID.ToString() + ". On Left: " + currentLane.OnLeft.LaneID.ToString());
}
i++;
currentLaneID = new LaneID(segment.Way1.WayID, i);
if (segmentLanes.ContainsKey(new LaneID(segment.Way2.WayID, i)))
{
currentLaneID = new LaneID(segment.Way2.WayID, i);
}
}
}
else
{
// HACK
//throw new Exception("single way lane!");
}
}
return(rndf);
}
/// <summary>
/// Modifies the zones into the rndf network format
/// </summary>
/// <param name="rndf"></param>
/// <returns></returns>
private RndfNetwork GenerateRndfNetworkZones(RndfNetwork rndf)
{
return(rndf);
}
/// <summary>
/// Generates the zone graph for travel in the zone
/// </summary>
/// <param name="rndf"></param>
/// <returns></returns>
private RndfNetwork CreateZoneGraph(RndfNetwork rndf)
{
return(rndf);
}
/// <summary>
/// Determines exit adjacency maps, i.e. where on each lane is an entry adjacent to (with some penalty for number of lane switches)
/// </summary>
/// <param name="rndf"></param>
/// <returns></returns>
private RndfNetwork DetermineEntryAdjacency(RndfNetwork rndf)
{
return(rndf);
}
/// <summary>
/// Gets the closest position to the coordinates on a specific lane in the rndf network
/// </summary>
/// <param name="coordinate"></param>
/// <param name="lane"></param>
/// <param name="rndfNetwork"></param>
/// <returns></returns>
public static LocationAnalysis ClosestRndfRelativeParition(Coordinates coordinate, LaneID lane, RndfNetwork rndfNetwork)
{
// use point analysis tool
return(PointAnalysis.ClosestPartitionOnLane(coordinate, lane, rndfNetwork));
}
/// <summary>
/// Private constructor (singleton pattern). (Can add fields as needed to constructor)
/// </summary>
/// <param name="rndfNetwork">the rndf network</param>
private TestDataServerFacadeImpl(RndfNetwork rndfNetwork, Mdf mdf, VehicleState vehicleState)
{
this.rndfNetwork = rndfNetwork;
this.mdf = mdf;
this.vehicleState = vehicleState;
}
public abstract void Restart(RndfNetwork rndf, Mdf mdf, ArbiterMode mode);
/// <summary>
/// Gets the closest relative rndf position to any LANE in the rndf
/// </summary>
/// <param name="coordinate"></param>
/// <param name="rndf"></param>
/// <returns></returns>
public static LocationAnalysis GetClosestLanePartition(Coordinates coordinate, RndfNetwork rndfNetwork)
{
// set the value to return
LocationAnalysis closest = null;
// values for comparing partitions
double offsetMin = Double.MaxValue;
foreach (Segment s in rndfNetwork.Segments.Values)
{
foreach (Way w in s.Ways.Values)
{
foreach (Lane lane in w.Lanes.Values)
{
// iterate over lanePartitions within the lane
foreach (LanePartition lanePartition in lane.LanePartitions)
{
// if there are user partitions, operate over them
if (lanePartition.UserPartitions != null && lanePartition.UserPartitions.Count > 1)
{
foreach (UserPartition userPartiton in lanePartition.UserPartitions)
{
throw new Exception("user partition relation to vehicle absolute coordinates not implemented yet");
}
}
// otherwise look at how close lane partition is
else
{
// analyze the partition
LocationAnalysis partitionAnalysis = AnalyzePartition(coordinate, (IWaypoint)lanePartition.InitialWaypoint, (IWaypoint)lanePartition.FinalWaypoint);
// if this partition has less of an offset from the vehicle than the current best, set as current
if (partitionAnalysis.Offset < offsetMin)
{
offsetMin = partitionAnalysis.Offset;
closest = partitionAnalysis;
// set partition of result
closest.Partition = lanePartition;
}
// otherwise, if the vehicle is relatively close to this partition
// and the closest Coordinates are not those of the initial final waypoitns of the lane
// and this error is less than the current best offset
else if (partitionAnalysis.Offset == 1234567 &&
(partitionAnalysis.RelativeRndfPosition != lane.LanePartitions[0].InitialWaypoint.Position) &&
(partitionAnalysis.RelativeRndfPosition != lane.LanePartitions[lane.LanePartitions.Count - 1].FinalWaypoint.Position) &&
(partitionAnalysis.Error < offsetMin))
{
offsetMin = partitionAnalysis.Error;
closest = partitionAnalysis;
// set partition of result
closest.Partition = lanePartition;
}
}
}
}
}
}
// return closest value found
return(closest);
}
public RndfDisplay(RndfNetwork rndf)
{
this.rndf = rndf;
}
/// <summary>
/// Gets the closest position to the coordinates on any lane in the rndf network
/// </summary>
/// <param name="coordinate"></param>
/// <param name="rndfNetwork"></param>
/// <returns></returns>
public static LocationAnalysis ClosestRndfRelativeLanePartition(Coordinates coordinate, RndfNetwork rndfNetwork)
{
// use the point analysis tool
return(PointAnalysis.GetClosestLanePartition(coordinate, rndfNetwork));
}
