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