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