/// <summary>
/// Generates the bounding waypoints of the acceptable U-Turn area given Rndf Hazards and specified exit and entry waypoints
/// </summary>
/// <param name="exit">The specified exit point of the U-Turn</param>
/// <param name="entry">The specified entry point of the U-Turn</param>
/// <returns></returns>
/// <remarks>See "Technical Evaluation Criteria: A.12. U-turn for specific definition of allowable space</remarks>
/// <remarks>Assumes the exit/entry coordinates are in the center of their respective lanes</remarks>
public static List <Coordinates> GenerateUTurnBounds(RndfWayPoint exit, RndfWayPoint entry)
{
// initialize the bounding box
List <Coordinates> boundingBox = new List <Coordinates>();
// get the length translation vector
Coordinates translation = exit.Position - exit.PreviousLanePartition.InitialWaypoint.Position;
translation = translation.Normalize(15);
// get the width lane shift length approximation
Coordinates approxWidthVector = (entry.Position - exit.Position);
double widthApprox = approxWidthVector.Length;
// get the width lane shift vector
Coordinates laneShift = translation.Rotate90().Normalize(widthApprox);
// get the center coordinate
Coordinates center = exit.Position + laneShift.Normalize(widthApprox / 2);
// Calculate the bounding coordinates
Coordinates uR = center + translation + laneShift;
Coordinates lR = center + translation - laneShift;
Coordinates uL = center - translation + laneShift;
Coordinates lL = center - translation - laneShift;
// add coordinates to the bounding box
boundingBox.Add(uR);
boundingBox.Add(lR);
boundingBox.Add(uL);
boundingBox.Add(lL);
// return the bounding box
return(boundingBox);
}
/// <summary>
/// Checks what type parameters the waypoint holds and sets color accordingly
/// </summary>
/// <param name="wp"></param>
/// <returns></returns>
public static Color GetWaypointColor(IWaypoint wp)
{
// check if its correct type of waypoint
if (wp is RndfWayPoint)
{
// cast
RndfWayPoint waypointData = (RndfWayPoint)wp;
// assign color based upon parameters
if (waypointData.IsStop && waypointData.IsCheckpoint)
{
return(CheckStopColor);
}
else if (waypointData.IsCheckpoint)
{
return(CheckpointColor);
}
else if (waypointData.IsStop)
{
return(StoplineColor);
}
else
{
return(WaypointColor);
}
}
else if (wp is UserWaypoint)
{
return(UserWaypointColor);
}
else
{
throw new ArgumentException("argument not WaypointData type", "wp");
}
}
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;
}
}
/// <summary>
/// Determine if one of the observed vehicles is stopped at a specified stop line
/// </summary>
/// <param name="stop"></param>
/// <param name="observedVehicles"></param>
/// <returns></returns>
/// <remarks>Uses a combination of determining if vehicle is within area of stop line and has a small enough velocity</remarks>
public static ObservedVehicle?FindVehicleStoppedAtStop(RndfWayPoint stop, List <ObservedVehicle> observedVehicles)
{
//foreach (ObservedVehicle ov in observedVehicles)
//{
// if (stop.Position.DistanceTo(ov.AbsolutePosition) < (ov.Length - ov.PositionOffsetFromRear) + 1 && ov.Speed < 1) //ANGLE FROM STOP POINT, NOT PERFECT
// return ov;
//}
//return null;
return(null);
}
/// <summary>
/// Generates a polygon representing the allowable area to perform a U-Turn
/// given Rndf Hazards and Allowed U-Turn Space
/// </summary>
/// <param name="exit"></param>
/// <param name="entry"></param>
/// <returns></returns>
public static List <BoundaryLine> GenerateUTurnPolygon(RndfWayPoint exit, RndfWayPoint entry)
{
// Generate the boundary points of the U-Turn
List <Coordinates> uTurnBounds = GenerateUTurnBounds(exit, entry);
// Turn the boundary points into a polygon using a jarvis march
List <BoundaryLine> uTurnPolygonBoundaries = JarvisMarch(uTurnBounds);
// Return the u turn polygon
return(uTurnPolygonBoundaries);
}
public void Render(System.Drawing.Graphics g, WorldTransform t)
{
if (DrawingUtility.DisplayFullRoute)
{
for (int i = 0; i < route.RouteNodes.Count - 1; i++)
{
RndfWayPoint initial = rndf.Waypoints[route.RouteNodes[i]];
RndfWayPoint final = rndf.Waypoints[route.RouteNodes[i + 1]];
DrawingUtility.DrawControlLine(initial.Position, final.Position, DrawingUtility.RouteColor, g, t);
}
}
}
/// <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>
/// Returns the <see cref="PointOnPath"/> associated with the supplied <see cref="RndfWayPoint"/>.
/// </summary>
/// <param name="waypoint"><see cref="RndfWayPoint"/> to find on the path.</param>
/// <returns><see cref="PointOnPath"/> instance of <paramref name="waypoint"/>.</returns>
/// <remarks>
/// The methods does not do any searching but returns the <see cref="PointOnPath"/> object directly. It will put
/// the point at the end of a <see cref="PartitionPathSegment"/> if possible or at the beginning if the waypoint
/// is the first in the lane.
/// </remarks>
/// <exception cref="ArgumentException">
/// Throw when the waypoint does not belong to the <see cref="Lane"/> associated with the <see cref="LanePath"/>.
/// </exception>
public PointOnPath GetWaypointPoint(RndfWayPoint waypoint)
{
if (!lane.Equals(waypoint.Lane))
{
throw new ArgumentException("Waypoint does not belong to the current lane");
}
// get the user partition for that stuff
if (waypoint.PreviousLanePartition != null)
{
LanePartition laneParition = waypoint.PreviousLanePartition;
PartitionPathSegment pathSeg = GetPathSegment(laneParition.UserPartitions[laneParition.UserPartitions.Count - 1]);
return(pathSeg.EndPoint);
}
else
{
LanePartition laneParition = waypoint.NextLanePartition;
PartitionPathSegment pathSeg = GetPathSegment(laneParition.UserPartitions[0]);
return(pathSeg.StartPoint);
}
}
/// <summary>
/// Gets the distance to a forward waypoint
/// </summary>
/// <param name="location"></param>
/// <param name="waypoint"></param>
/// <returns></returns>
public static double DistanceToWaypoint(RndfLocation location, RndfWaypointID waypoint)
{
double distance = location.AbsolutePositionOnPartition.DistanceTo(location.Partition.FinalWaypoint.Position);
RndfWayPoint current = location.Partition.FinalWaypoint;
while (current != null)
{
if (current.WaypointID.Equals(waypoint))
{
return(distance);
}
else if (current.NextLanePartition == null)
{
throw new Exception("waypoint not ahead of vehicle on current lane");
}
else
{
distance += current.Position.DistanceTo(current.NextLanePartition.FinalWaypoint.Position);
current = current.NextLanePartition.FinalWaypoint;
}
}
throw new Exception("waypoint not ahead of vehicle on current lane");
}
/// <summary>
/// Gets up to a certain distance of the lane's coordinates
/// </summary>
/// <param name="lane"></param>
/// <param name="includeUserWaypoint"></param>
/// <param name="d"></param>
/// <param name="forwards">whether or not to get the front or back of a lane</param>
/// <returns></returns>
public static List <Coordinates> GetLaneWaypointCoordinates(Lane lane, bool includeUserWaypoints, double d, bool forwards)
{
// 1. list of absolute positions of lane coordinates
List <Coordinates> absoluteCoordinates = new List <Coordinates>();
if (forwards)
{
RndfWayPoint current = lane.LanePartitions[0].InitialWaypoint;
double distance = 0;
// loop over waypoints while distance within params
while (current != null && distance <= d)
{
// add rndf waypoint
absoluteCoordinates.Add(current.Position);
// update a coarse distance
if (current.NextLanePartition != null)
{
// update distance
distance += current.NextLanePartition.FinalWaypoint.Position.DistanceTo(current.Position);
// check to see if we're including user waypoints
if (includeUserWaypoints)
{
// add the user waypoints
foreach (UserPartition userPartition in current.NextLanePartition.UserPartitions)
{
// check if not an rndfwaypoint
if (!(userPartition.InitialWaypoint is RndfWayPoint))
{
// add user waypoint's position
absoluteCoordinates.Add(userPartition.InitialWaypoint.Position);
}
}
}
// iterate
current = (current.NextLanePartition.FinalWaypoint);
}
else
{
return(absoluteCoordinates);
}
}
return(absoluteCoordinates);
}
else
{
RndfWayPoint current = lane.LanePartitions[lane.LanePartitions.Count - 1].FinalWaypoint;
double distance = 0;
// loop over waypoints while distance within params
while (current != null && distance <= d)
{
// add rndf waypoint
absoluteCoordinates.Add(current.Position);
// update a coarse distance
if (current.PreviousLanePartition != null)
{
// update distance
distance += current.PreviousLanePartition.InitialWaypoint.Position.DistanceTo(current.Position);
// check to see if we're including user waypoints
if (includeUserWaypoints && current.NextLanePartition != null)
{
// add the user waypoints
foreach (UserPartition userPartition in current.NextLanePartition.UserPartitions)
{
List <Coordinates> forwardUserCoords = new List <Coordinates>();
// check if not an rndfwaypoint
if (!(userPartition.InitialWaypoint is RndfWayPoint))
{
// add user waypoint's position
forwardUserCoords.Add(userPartition.InitialWaypoint.Position);
}
forwardUserCoords.Reverse();
absoluteCoordinates.AddRange(forwardUserCoords);
}
}
// iterate
current = (current.PreviousLanePartition.InitialWaypoint);
}
else
{
absoluteCoordinates.Reverse();
return(absoluteCoordinates);
}
}
absoluteCoordinates.Reverse();
return(absoluteCoordinates);
}
}
public override void ExecuteBehavior(Behavior behavior, Common.Coordinates location, RndfWaypointID lowerBound, RndfWaypointID upperBound)
{
if (behavior is UTurnBehavior)
{
//
}
// check to see if we are at the upper bound
RndfWayPoint upperWaypoint = channelListener.RndfNetwork.Waypoints[upperBound];
if (!(behavior is TurnBehavior) && upperWaypoint.NextLanePartition != null && location.DistanceTo(upperWaypoint.Position) < 3)
{
lowerBound = upperWaypoint.WaypointID;
upperBound = upperWaypoint.NextLanePartition.FinalWaypoint.WaypointID;
}
Console.WriteLine(" > Received Instruction to Execute Behavior: " + behavior.ToString());
if (behavior is StayInLaneBehavior)
{
StayInLaneBehavior stayInLane = (StayInLaneBehavior)behavior;
if (stayInLane.SpeedCommand is StopLineLaneSpeedCommand)
{
StopLineLaneSpeedCommand speedCommand = (StopLineLaneSpeedCommand)stayInLane.SpeedCommand;
if (speedCommand.Distance < 2)
{
// Create a fake vehicle state
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = location;
LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1);
List <LaneEstimate> laneEstimates = new List <LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = 0;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
this.PublishVehicleState(vehicleState);
///Console.WriteLine(" > Published Position");
}
else
{
// Create a fake vehicle state
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[upperBound].Position;
LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1);
List <LaneEstimate> laneEstimates = new List <LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = 3;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
this.PublishVehicleState(vehicleState);
///Console.WriteLine(" > Published Position");
}
}
else if (stayInLane.SpeedCommand is StopLaneSpeedCommand)
{
// Create a fake vehicle state
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = location;
LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1);
List <LaneEstimate> laneEstimates = new List <LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = -5;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
this.PublishVehicleState(vehicleState);
///Console.WriteLine(" > Published Position");
}
else if (stayInLane.SpeedCommand is DefaultLaneSpeedCommand)
{
// Create a fake vehicle state
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[upperBound].Position;
LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1);
List <LaneEstimate> laneEstimates = new List <LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = 3;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
this.PublishVehicleState(vehicleState);
//Console.WriteLine(" > Published Position");
}
else
{
throw new ArgumentException("Unknown Lane Speed Type", "stayInLane.SpeedCommand");
}
}
// TODO: include midway point
else if (behavior is TurnBehavior)
{
TurnBehavior currentBehavior = (TurnBehavior)behavior;
RndfWayPoint exitWaypoint = channelListener.RndfNetwork.Waypoints[currentBehavior.ExitPoint];
if (location.DistanceTo(exitWaypoint.Position) < 0.1)
{
// Create a fake vehicle state
VehicleState vehicleState = new VehicleState();
RndfWayPoint entryWaypoint = channelListener.RndfNetwork.Waypoints[currentBehavior.EntryPoint];
Common.Coordinates change = entryWaypoint.Position - exitWaypoint.Position;
Common.Coordinates midpoint = exitWaypoint.Position + change / 2;
LaneEstimate laneEstimate = new LaneEstimate(currentBehavior.ExitPoint.LaneID, currentBehavior.EntryPoint.LaneID, 1);
vehicleState.xyPosition = midpoint;
List <LaneEstimate> laneEstimates = new List <LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = 3;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
this.PublishVehicleState(vehicleState);
}
else
{
// Create a fake vehicle state
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[currentBehavior.EntryPoint].Position;
LaneEstimate laneEstimate = new LaneEstimate(currentBehavior.EntryPoint.LaneID, currentBehavior.EntryPoint.LaneID, 1);
List <LaneEstimate> laneEstimates = new List <LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = 3;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
this.PublishVehicleState(vehicleState);
//Console.WriteLine(" > Published Position");
}
}
else
{
throw new ArgumentException("Unknown Behavior Type", "behavior");
}
//Console.WriteLine("Sent Back Position \n");
}
static void Main(string[] args)
{
// Initialize Communications
Console.WriteLine("1. Initializing Communications...");
Communicator communicator = new Communicator();
Console.WriteLine(" > Communications Initialized\n");
// Wait for Rndf,Mdf to continue
Console.WriteLine("2. Waiting for Rndf, Mdf to continue...");
communicator.InitializeTestServer();
Console.WriteLine(" > Rndf, Mdf Received\n");
// Wait for User continue
Console.WriteLine("3. Ready To Start Testing, Press ENTER to Continue...");
Console.ReadLine();
// Wait for 1 second
Thread.Sleep(1000);
/*
* // Turn => Stopped at Stop
* // Create a fake vehicle state
* RndfWayPoint position = communicator.RndfNetwork.Goals[1].Waypoint;
* Console.WriteLine(" > Position: " + position.WaypointID.ToString());
* VehicleState vehicleState = new VehicleState();
* vehicleState.xyPosition = position.Position;
* LaneEstimate laneEstimate = new LaneEstimate(position.Lane.LaneID, position.Lane.LaneID, 1);
* List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
* laneEstimates.Add(laneEstimate);
* vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
* communicator.PublishVehicleState(vehicleState);
* Console.WriteLine(" > Published Position");*/
/*
* // Road
* // Create a fake vehicle state
* RndfWaypointID tmpID = new RndfWaypointID(new LaneID(new WayID(new SegmentID(1), 1), 1), 1);
* RndfWayPoint position = communicator.RndfNetwork.Waypoints[tmpID];
* Console.WriteLine(" > Position: " + position.WaypointID.ToString());
* VehicleState vehicleState = new VehicleState();
* vehicleState.xyPosition = position.Position;
* LaneEstimate laneEstimate = new LaneEstimate(position.Lane.LaneID, position.Lane.LaneID, 1);
* List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
* laneEstimates.Add(laneEstimate);
* vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
* vehicleState.speed = 3;
* communicator.PublishVehicleState(vehicleState);
* Console.WriteLine(" > Published Position");*/
/*
* // Intersection
* // Create a fake vehicle state
* RndfWaypointID tmpID = new RndfWaypointID(new LaneID(new WayID(new SegmentID(1), 1), 1), 2);
* RndfWaypointID tmpEndID = new RndfWaypointID(new LaneID(new WayID(new SegmentID(26), 2), 3), 1);
* InterconnectID testID = new InterconnectID(tmpID, tmpEndID);
* Interconnect testInter = communicator.RndfNetwork.Interconnects[testID];
* RndfWayPoint position = communicator.RndfNetwork.Waypoints[tmpID];
* VehicleState vehicleState = new VehicleState();
* vehicleState.xyPosition = position.Position + new UrbanChallenge.Common.Coordinates(4, 4);
* LaneEstimate laneEstimate = new LaneEstimate(tmpID.LaneID, tmpEndID.LaneID, 1);
* List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
* laneEstimates.Add(laneEstimate);
* vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
* vehicleState.speed = 3;
* communicator.PublishVehicleState(vehicleState);
* Console.WriteLine(" > Published Position");
*/
// In Lane
// Create a fake vehicle state
RndfWayPoint position = communicator.RndfNetwork.Goals[2].Waypoint;
Console.WriteLine(" > Position: " + position.WaypointID.ToString());
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = position.Position;
LaneEstimate laneEstimate = new LaneEstimate(position.Lane.LaneID, position.Lane.LaneID, 1);
List <LaneEstimate> laneEstimates = new List <LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = 0;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
communicator.PublishVehicleState(vehicleState);
//Console.WriteLine(" > Published Position");
/*// Read the configuration file.
* RemotingConfiguration.Configure("..\\..\\App.config", false);
* WellKnownServiceTypeEntry[] wkst = RemotingConfiguration.GetRegisteredWellKnownServiceTypes();
*
* // "Activate" the NameService singleton.
* ObjectDirectory objectDirectory = (ObjectDirectory)Activator.GetObject(typeof(ObjectDirectory), wkst[0].ObjectUri);
*
* // Receive the facades of components we want to use
* RndfEditorFacade editorFacade = (RndfEditorFacade)objectDirectory.Resolve("RndfEditor");
*
* // Get the RndfNetwork
* RndfNetwork rndfNetwork = editorFacade.RndfNetwork;
*
* // Create the Goals from the RndfNetwork
* Queue<Goal> goals = generateGoals(rndfNetwork);
* Console.WriteLine("Created Goals");
*
* // Create Speed Limits from the RndfNetwork
* List<SpeedInformation> speedLimits = generateSpeedLimits(rndfNetwork);
* Console.WriteLine("Created speed limits");
*
* // Create Mdf
* Mdf mdf = new Mdf(goals, speedLimits);
* Console.WriteLine("Created Mdf");
*
* // Create a fake vehicle state
* RndfWayPoint position = rndfNetwork.Goals[1].Waypoint;
* Console.WriteLine("Position: " + position.WaypointID.ToString());
* VehicleState vehicleState = new VehicleState();
* vehicleState.xyPosition = position.Position;
* LaneEstimate laneEstimate = new LaneEstimate(position.Lane.LaneID, position.Lane.LaneID, 1);
* List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
* laneEstimates.Add(laneEstimate);
* vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
*
* // Test
* Console.WriteLine("Number of RndfNetwork Segments: " + rndfNetwork.Segments.Values.Count);
*
* // Bind the facades of components we implement.
* objectDirectory.Rebind(TestDataServerFacadeImpl.Instance(rndfNetwork, mdf, vehicleState), "TestServer");*/
// Show that the navigation system is ready and waiting for input
Console.WriteLine("");
Console.WriteLine("4. Test Serve Complete, Waiting. Press ENTER to Shut Down");
Console.ReadLine();
Console.WriteLine("");
communicator.ShutDown();
}
/// <summary>
/// Checks if a specific vehicle is within generous vehicle-like area of an exit
/// </summary>
/// <param name="observedVehicle"></param>
/// <param name="stop"></param>
/// <returns></returns>
public static bool CheckVehicleAtExit(ObservedVehicle observedVehicle, RndfWayPoint exit)
{
throw new Exception("This method is not yet implemented");
}