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