/// <summary> /// Constructor /// </summary> /// <param name="lane"></param> /// <param name="waypoint"></param> /// <param name="turnDirection"></param> /// <param name="isNavigationExit"></param> /// <param name="desiredExit"></param> public StoppingAtStopState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection, bool isNavigationExit, ArbiterInterconnect desiredExit) { this.lane = lane; this.waypoint = waypoint; this.turnDirection = turnDirection; this.isNavigationExit = isNavigationExit; this.desiredExit = desiredExit; this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId); }
/// <summary> /// Constructor /// </summary> /// <param name="lane"></param> /// <param name="waypoint"></param> /// <param name="turnDirection"></param> /// <param name="isNavigationExit"></param> /// <param name="desiredExit"></param> public StoppingAtExitState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection, bool isNavigationExit, ArbiterInterconnect desiredExit, double timeStamp, Coordinates currentPosition) { this.lane = lane; this.waypoint = waypoint; this.turnDirection = turnDirection; this.isNavigationExit = isNavigationExit; this.desiredExit = desiredExit; this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId); this.timeStamp = timeStamp; this.currentPosition = currentPosition; this.internalLaneState = new InternalState(this.lane.LaneId, this.lane.LaneId); }
/// <summary> /// Waypoints to ignore /// </summary> /// <param name="way"></param> /// <param name="position"></param> /// <returns></returns> public static List<ArbiterWaypoint> WaypointsClose(ArbiterWay way, Coordinates position, ArbiterWaypoint ignoreSpecifically) { List<ArbiterWaypoint> waypoints = new List<ArbiterWaypoint>(); foreach (ArbiterLane al in way.Lanes.Values) { ArbiterWaypoint aw = al.GetClosestWaypoint(position, TahoeParams.VL * 2.0); if(aw != null) waypoints.Add(aw); } if (ignoreSpecifically != null && !waypoints.Contains(ignoreSpecifically)) waypoints.Add(ignoreSpecifically); return waypoints; }
/// <summary> /// Constructor /// </summary> /// <param name="globalMonitor"></param> /// <param name="involved"></param> public DominantLaneEntryMonitor(IntersectionMonitor globalMonitor, IntersectionInvolved involved) { this.waitingTimer = new Stopwatch(); this.globalMonitor = globalMonitor; this.lane = (ArbiterLane)involved.Area; this.involved = involved; if (involved.Exit != null) this.exit = (ArbiterWaypoint)involved.Exit; else this.exit = null; if (this.exit != null) { // create the polygon this.exitPolygon = this.ExitPolygon(); } }
/// <summary> /// Constructor /// </summary> /// <param name="globalMonitor"></param> /// <param name="stop"></param> public SubmissiveEntryMonitor(IntersectionMonitor globalMonitor, ArbiterWaypoint stop, bool isOurs) { this.globalMonitor = globalMonitor; this.timer = new Stopwatch(); this.isOurs = isOurs; foreach (ArbiterStoppedExit ase in this.globalMonitor.Intersection.StoppedExits) { if (ase.Waypoint.Equals(stop)) { this.stoppedExit = ase; } } if (this.stoppedExit == null) throw new Exception("needs stopped exit"); }
/// <summary> /// Turn information /// </summary> /// <param name="entry"></param> /// <param name="finalPath"></param> /// <param name="leftBound"></param> /// <param name="rightBound"></param> public static void TurnInfo(ArbiterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound) { if (entry.NextPartition != null) { double distance = entry.NextPartition.Length; // get left bound rightBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position, entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).RotateM90()); // get right bound leftBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position, entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).Rotate90()); ArbiterWaypoint current = entry.NextPartition.Final; while (current.NextPartition != null && distance < 50) { distance += current.NextPartition.Length; LineList rtTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position, current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).RotateM90()); rightBound.Add(rtTemp[rtTemp.Count - 1]); LineList ltTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position, current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).Rotate90()); leftBound.Add(ltTemp[ltTemp.Count - 1]); current = current.NextPartition.Final; } finalPath = entry.Lane.LanePath(entry, 50.0); } else { Coordinates final = entry.Position + entry.PreviousPartition.Vector().Normalize(TahoeParams.VL); finalPath = new LinePath(new Coordinates[] { entry.Position, final }); LinePath lB = finalPath.ShiftLateral(entry.Lane.Width / 2.0); LinePath rB = finalPath.ShiftLateral(-entry.Lane.Width / 2.0); leftBound = new LineList(lB); rightBound = new LineList(rB); } }
/// <summary> /// Distance between two waypoints /// </summary> /// <param name="w1"></param> /// <param name="w2"></param> /// <returns></returns> public double DistanceBetween(ArbiterWaypoint w1, ArbiterWaypoint w2) { return(this.LanePath(w1, w2).PathLength); }
/// <summary> /// Constructor /// </summary> /// <param name="aw"></param> /// <param name="ep"></param> public ArbiterStoppedExit(ArbiterWaypoint aw, Polygon ep) { this.Waypoint = aw; this.ExitPolygon = ep; }
/// <summary> /// Get the next waypoint of a certain type /// </summary> /// <param name="w1">Starting waypoint of search</param> /// <param name="wt">Waypoint type to look for</param> /// <returns></returns> public ArbiterWaypoint GetNext(ArbiterWaypoint w1, WaypointType wt) { return(this.GetNext(w1, wt, new List <ArbiterWaypoint>())); }
public void SetTurnDirection(ArbiterInterconnect ai) { #region Turn Direction if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint) { ArbiterWaypoint initWp = (ArbiterWaypoint)ai.InitialGeneric; ArbiterWaypoint finWp = (ArbiterWaypoint)ai.FinalGeneric; // check not uturn if (!initWp.Lane.Way.Segment.Equals(finWp.Lane.Way.Segment) || initWp.Lane.Way.Equals(finWp.Lane.Way)) { Coordinates iVec = initWp.PreviousPartition != null?initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0); double iRot = -iVec.ArcTan; Coordinates fVec = finWp.NextPartition != null?finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0); fVec = fVec.Rotate(iRot); double fDeg = fVec.ToDegrees(); double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI; if (arcTan > 45.0) { ai.TurnDirection = ArbiterTurnDirection.Left; } else if (arcTan < -45.0) { ai.TurnDirection = ArbiterTurnDirection.Right; } else { ai.TurnDirection = ArbiterTurnDirection.Straight; } } else { Coordinates iVec = initWp.PreviousPartition != null?initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0); double iRot = -iVec.ArcTan; Coordinates fVec = finWp.NextPartition != null?finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0); fVec = fVec.Rotate(iRot); double fDeg = fVec.ToDegrees(); double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI; if (arcTan > 45.0 && arcTan < 135.0) { ai.TurnDirection = ArbiterTurnDirection.Left; } else if (arcTan < -45.0 && arcTan > -135.0) { ai.TurnDirection = ArbiterTurnDirection.Right; } else if (Math.Abs(arcTan) < 45.0) { ai.TurnDirection = ArbiterTurnDirection.Straight; } else { ai.TurnDirection = ArbiterTurnDirection.UTurn; } } } else { Coordinates iVec = new Coordinates(); double iRot = 0.0; Coordinates fVec = new Coordinates(); double fDeg = 0.0; if (ai.InitialGeneric is ArbiterWaypoint) { ArbiterWaypoint initWp = (ArbiterWaypoint)ai.InitialGeneric; iVec = initWp.PreviousPartition != null?initWp.PreviousPartition.Vector().Normalize(1.0) : initWp.NextPartition.Vector().Normalize(1.0); iRot = -iVec.ArcTan; } else if (ai.InitialGeneric is ArbiterPerimeterWaypoint) { ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.InitialGeneric; Coordinates centerPoly = apw.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center; iVec = apw.Position - centerPoly; iVec = iVec.Normalize(1.0); iRot = -iVec.ArcTan; } if (ai.FinalGeneric is ArbiterWaypoint) { ArbiterWaypoint finWp = (ArbiterWaypoint)ai.FinalGeneric; fVec = finWp.NextPartition != null?finWp.NextPartition.Vector().Normalize(1.0) : finWp.PreviousPartition.Vector().Normalize(1.0); fVec = fVec.Rotate(iRot); fDeg = fVec.ToDegrees(); } else if (ai.FinalGeneric is ArbiterPerimeterWaypoint) { ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric; Coordinates centerPoly = apw.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center; fVec = centerPoly - apw.Position; fVec = fVec.Normalize(1.0); fVec = fVec.Rotate(iRot); fDeg = fVec.ToDegrees(); } double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI; if (arcTan > 45.0) { ai.TurnDirection = ArbiterTurnDirection.Left; } else if (arcTan < -45.0) { ai.TurnDirection = ArbiterTurnDirection.Right; } else { ai.TurnDirection = ArbiterTurnDirection.Straight; } } #endregion }
/// <summary> /// Constructor /// </summary> /// <param name="aw"></param> public IgnorableWaypoint(ArbiterWaypoint aw) { this.Waypoint = aw; this.numberOfCyclesIgnored = 0; }
/// <summary> /// Gets next waypoint of a certain type ignoring certain waypoints /// </summary> /// <param name="w1"></param> /// <param name="wt"></param> /// <param name="ignorable"></param> /// <returns></returns> public ArbiterWaypoint GetNext(ArbiterWaypoint w1, WaypointType wt, List<ArbiterWaypoint> ignorable) { ArbiterWaypoint tmp = w1; while (tmp != null) { if (tmp.WaypointTypeEquals(wt) && !ignorable.Contains(tmp)) return tmp; if (tmp.NextPartition != null) tmp = tmp.NextPartition.Final; else tmp = null; } return null; }
/// <summary> /// Path of lane from a waypoint for a certain distance /// </summary> /// <param name="w1">Initial waypoint</param> /// <param name="distance">Distance to get path</param> /// <returns></returns> public LinePath LanePath(ArbiterWaypoint w1, double distance) { return this.LanePath().SubPath(this.LanePath().GetClosestPoint(w1.Position), distance); }
/// <summary> /// Adds waypoint to ignore list /// </summary> /// <param name="toIgnore"></param> public void IgnoreWaypoint(ArbiterWaypoint toIgnore) { this.IgnorableWaypoints.Add(new IgnorableWaypoint(toIgnore)); }
/// <summary> /// Constructor /// </summary> /// <param name="turnFinal"></param> public LaneEntryAreaMonitor(ArbiterWaypoint turnFinal) { this.turnFinalWaypoint = turnFinal; this.timer = new Stopwatch(); }
/// <summary> /// Gets time cost between two waypoints in the same lane /// </summary> /// <param name="initial"></param> /// <param name="final"></param> /// <returns></returns> private double TimeCostInLane(ArbiterWaypoint initial, ArbiterWaypoint final) { ArbiterWaypoint current = initial; double cost = 0.0; while (current != null) { if (current.Equals(final)) { return cost; } else { if (current.NextPartition == null) return cost; cost += current.NextPartition.TimeCost(); if (current.NextPartition != null) current = current.NextPartition.Final; else current = null; } } return Double.MaxValue; }
/// <summary> /// Path of lane from a waypoint for a certain distance /// </summary> /// <param name="w1">Initial waypoint</param> /// <param name="distance">Distance to get path</param> /// <returns></returns> public LinePath LanePath(ArbiterWaypoint w1, double distance) { return(this.LanePath().SubPath(this.LanePath().GetClosestPoint(w1.Position), distance)); }
/// <summary> /// Gets next entry downstream from this waypoint /// </summary> /// <param name="waypoint"></param> /// <returns></returns> private ArbiterWaypoint NextEntry(ArbiterWaypoint waypoint) { if (waypoint.NextPartition != null) { List<DownstreamPointOfInterest> entriesDownstream = this.Downstream(waypoint.Position, waypoint.Lane.Way, false, new List<ArbiterWaypoint>()); return entriesDownstream.Count > 0 ? entriesDownstream[0].PointOfInterest : null; } else { return null; } }
/// <summary> /// Generates the xySegments into segments and inputs them into the input road network /// </summary> /// <param name="arn"></param> /// <returns></returns> public ArbiterRoadNetwork GenerateSegments(ArbiterRoadNetwork arn) { foreach (SimpleSegment ss in segments) { // seg ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(ss.Id)); ArbiterSegment asg = new ArbiterSegment(asi); arn.ArbiterSegments.Add(asi, asg); asg.RoadNetwork = arn; asg.SpeedLimits = new ArbiterSpeedLimit(); asg.SpeedLimits.MaximumSpeed = 13.4112; // 30mph max speed // way1 ArbiterWayId awi1 = new ArbiterWayId(1, asi); ArbiterWay aw1 = new ArbiterWay(awi1); aw1.Segment = asg; asg.Ways.Add(awi1, aw1); asg.Way1 = aw1; // way2 ArbiterWayId awi2 = new ArbiterWayId(2, asi); ArbiterWay aw2 = new ArbiterWay(awi2); aw2.Segment = asg; asg.Ways.Add(awi2, aw2); asg.Way2 = aw2; // make lanes foreach (SimpleLane sl in ss.Lanes) { // lane ArbiterLaneId ali; ArbiterLane al; // get way of lane id if (ss.Way1Lanes.Contains(sl)) { ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi1); al = new ArbiterLane(ali); aw1.Lanes.Add(ali, al); al.Way = aw1; } else { ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi2); al = new ArbiterLane(ali); aw2.Lanes.Add(ali, al); al.Way = aw2; } // add to display arn.DisplayObjects.Add(al); // width al.Width = sl.LaneWidth == 0 ? TahoeParams.T * 2.0 : sl.LaneWidth * 0.3048; if (sl.LaneWidth == 0) { Console.WriteLine("lane: " + ali.ToString() + " contains no lane width, setting to 4m"); } // lane boundaries al.BoundaryLeft = this.GenerateLaneBoundary(sl.LeftBound); al.BoundaryRight = this.GenerateLaneBoundary(sl.RightBound); // add lane to seg asg.Lanes.Add(ali, al); // waypoints List <ArbiterWaypoint> waypointList = new List <ArbiterWaypoint>(); // generate waypoints foreach (SimpleWaypoint sw in sl.Waypoints) { // waypoint ArbiterWaypointId awi = new ArbiterWaypointId(GenerationTools.GetId(sw.ID)[2], ali); ArbiterWaypoint aw = new ArbiterWaypoint(sw.Position, awi); aw.Lane = al; // stop if (sl.Stops.Contains(sw.ID)) { aw.IsStop = true; } // checkpoint foreach (SimpleCheckpoint sc in sl.Checkpoints) { if (sw.ID == sc.WaypointId) { aw.IsCheckpoint = true; aw.CheckpointId = int.Parse(sc.CheckpointId); arn.Checkpoints.Add(aw.CheckpointId, aw); } } // add asg.Waypoints.Add(awi, aw); arn.ArbiterWaypoints.Add(awi, aw); al.Waypoints.Add(awi, aw); waypointList.Add(aw); arn.DisplayObjects.Add(aw); arn.LegacyWaypointLookup.Add(sw.ID, aw); } al.WaypointList = waypointList; // lane partitions List <ArbiterLanePartition> alps = new List <ArbiterLanePartition>(); al.Partitions = alps; // generate lane partitions for (int i = 0; i < waypointList.Count - 1; i++) { // create lane partition ArbiterLanePartitionId alpi = new ArbiterLanePartitionId(waypointList[i].WaypointId, waypointList[i + 1].WaypointId, ali); ArbiterLanePartition alp = new ArbiterLanePartition(alpi, waypointList[i], waypointList[i + 1], asg); alp.Lane = al; waypointList[i].NextPartition = alp; waypointList[i + 1].PreviousPartition = alp; alps.Add(alp); arn.DisplayObjects.Add(alp); // crete initial user partition ArbiterUserPartitionId aupi = new ArbiterUserPartitionId(alp.PartitionId, waypointList[i].WaypointId, waypointList[i + 1].WaypointId); ArbiterUserPartition aup = new ArbiterUserPartition(aupi, alp, waypointList[i], waypointList[i + 1]); List <ArbiterUserPartition> aups = new List <ArbiterUserPartition>(); aups.Add(aup); alp.UserPartitions = aups; alp.SetDefaultSparsePolygon(); arn.DisplayObjects.Add(aup); } // path segments of lane List <IPathSegment> ips = new List <IPathSegment>(); List <Coordinates> pathSegments = new List <Coordinates>(); pathSegments.Add(alps[0].Initial.Position); // loop foreach (ArbiterLanePartition alPar in alps) { ips.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position)); // make new segment pathSegments.Add(alPar.Final.Position); } // generate lane partition path LinePath partitionPath = new LinePath(pathSegments); al.SetLanePath(partitionPath); al.PartitionPath = new Path(ips, CoordinateMode.AbsoluteProjected); // safeto zones foreach (ArbiterWaypoint aw in al.Waypoints.Values) { if (aw.IsStop) { LinePath.PointOnPath end = al.GetClosestPoint(aw.Position); double dist = -30; LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist); if (dist != 0) { begin = al.LanePath().StartPoint; } ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin); asz.isExit = true; asz.Exit = aw; al.SafetyZones.Add(asz); arn.DisplayObjects.Add(asz); arn.ArbiterSafetyZones.Add(asz); } } } } return(arn); }
/// <summary> /// Updates the montitor with the closest forward vehicle along the lane /// </summary> /// <param name="vehicleState"></param> /// <param name="final"></param> public void Update(VehicleState vehicleState, ArbiterWaypoint final, LinePath fullPath) { this.VehiclesToIgnore = new List<int>(); if (TacticalDirector.VehicleAreas.ContainsKey(final.Lane)) { List<VehicleAgent> laneVehicles = TacticalDirector.VehicleAreas[final.Lane]; this.CurrentVehicle = null; double currentDistance = Double.MaxValue; foreach (VehicleAgent va in laneVehicles) { double endDistance = final.Lane.DistanceBetween(final.Position, va.ClosestPosition); if (endDistance > 0) this.VehiclesToIgnore.Add(va.VehicleId); double tmpDist = endDistance; if (tmpDist > 0 && (this.CurrentVehicle == null || tmpDist < currentDistance)) { this.CurrentVehicle = va; currentDistance = tmpDist; } } } else this.CurrentVehicle = null; }
/// <summary> /// Distance between two waypoints /// </summary> /// <param name="w1"></param> /// <param name="w2"></param> /// <returns></returns> public double DistanceBetween(ArbiterWaypoint w1, ArbiterWaypoint w2) { return this.LanePath(w1, w2).PathLength; }
/// <summary> /// Checks if intersection contains entry ownstream in lane from waypoint /// </summary> /// <param name="aw"></param> /// <returns></returns> public ArbiterWaypoint EntryDownstream(ArbiterWaypoint aw) { foreach (ITraversableWaypoint itw in this.Intersection.AllEntries.Values) { if (itw is ArbiterWaypoint) { ArbiterWaypoint testWaypoint = (ArbiterWaypoint)itw; if (aw.Lane.Equals(testWaypoint.Lane) && aw.WaypointId.Number < testWaypoint.WaypointId.Number) { return testWaypoint; } } } return null; }
/// <summary> /// Get the next waypoint of a certain type /// </summary> /// <param name="w1">Starting waypoint of search</param> /// <param name="wt">Waypoint type to look for</param> /// <returns></returns> public ArbiterWaypoint GetNext(ArbiterWaypoint w1, WaypointType wt) { ArbiterWaypoint tmp = w1; while (tmp != null) { if (tmp.WaypointTypeEquals(wt)) return tmp; if (tmp.NextPartition != null) tmp = tmp.NextPartition.Final; else tmp = null; } return null; }
/// <summary> /// Determines proper speed commands given we want to stop at a certain waypoint /// </summary> /// <param name="waypoint"></param> /// <param name="lane"></param> /// <param name="position"></param> /// <param name="enCovariance"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public void StoppingParams(ArbiterWaypoint waypoint, ArbiterLane lane, Coordinates position, double extraDistance, out double stopSpeed, out double stopDistance) { // get dist to waypoint stopDistance = lane.DistanceBetween(position, waypoint.Position) - extraDistance; // speed tools stopSpeed = SpeedTools.GenerateSpeed(stopDistance, CoreCommon.OperationalStopSpeed, 2.24); }
/// <summary> /// Gets next waypoint of a certain type ignoring certain waypoints /// </summary> /// <param name="w1"></param> /// <param name="wt"></param> /// <param name="ignorable"></param> /// <returns></returns> public ArbiterWaypoint GetNext(ArbiterWaypoint w1, List<WaypointType> wts, List<ArbiterWaypoint> ignorable) { ArbiterWaypoint tmp = w1; while (tmp != null) { bool back = false; foreach(WaypointType wt in wts) { if(tmp.WaypointTypeEquals(wt)) back = true; } if (back && !ignorable.Contains(tmp)) return tmp; if (tmp.NextPartition != null) tmp = tmp.NextPartition.Final; else tmp = null; } return null; }
/// <summary> /// Generate the traveling parameterization for the desired behaivor /// </summary> /// <param name="lane"></param> /// <param name="navStopSpeed"></param> /// <param name="navStopDistance"></param> /// <param name="navStop"></param> /// <param name="navStopType"></param> /// <param name="state"></param> /// <returns></returns> private TravelingParameters NavStopParameterization(ArbiterLane lane, double navStopSpeed, double navStopDistance, ArbiterWaypoint navStop, StopType navStopType, VehicleState state) { // get min dist double distanceCutOff = CoreCommon.OperationalStopDistance; // turn direction default List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators; // create new params TravelingParameters tp = new TravelingParameters(); #region Get Maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; // get lane path LinePath lp = lane.LanePath().Clone(); lp.Reverse(); #region Distance Cutoff // check if distance is less than cutoff if (navStopDistance < distanceCutOff) { // default behavior tp.SpeedCommand = new StopAtDistSpeedCommand(navStopDistance); Behavior b = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(navStopDistance), new List<int>(), lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); // stopping so not using speed param usingSpeed = false; IState nextState = CoreCommon.CorePlanningState; m = new Maneuver(b, nextState, decorators, state.Timestamp); } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // get lane ArbiterLane al = lane; // default behavior tp.SpeedCommand = new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)); Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)), new List<int>(), lp, al.Width, al.NumberOfLanesRight(state.Front, false), al.NumberOfLanesLeft(state.Front, false)); // standard behavior is fine for maneuver m = new Maneuver(b, CoreCommon.CorePlanningState, decorators, state.Timestamp); } #endregion #endregion #region Parameterize tp.Behavior = m.PrimaryBehavior; tp.Decorators = m.PrimaryBehavior.Decorators; tp.DistanceToGo = navStopDistance; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = navStopSpeed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.VehiclesToIgnore = new List<int>(); // return navigation params return tp; #endregion }
/// <summary> /// Path of lane between two waypoints /// </summary> /// <param name="w1">Initial waypoint</param> /// <param name="w2">Final waypoint</param> /// <returns></returns> public LinePath LanePath(ArbiterWaypoint w1, ArbiterWaypoint w2) { return this.LanePath().SubPath( this.LanePath().GetClosestPoint(w1.Position), this.LanePath().GetClosestPoint(w2.Position)); }
/// <summary> /// Generates entry adjacency /// </summary> /// <param name="arn"></param> /// <remarks>Determines for every entry in a segment, the closest, reachable /// waypoints on lanes in the same way if it exists</remarks> private void GenerateNavigationalAdjacency(ArbiterRoadNetwork arn) { // loop over segments foreach (ArbiterSegment asg in arn.ArbiterSegments.Values) { // loop over segment waypoints foreach (ArbiterWaypoint aw in asg.Waypoints.Values) { #region Next Waypoint // check if has next if (aw.NextPartition != null) { // add next waypoint aw.OutgoingConnections.Add(aw.NextPartition); } #endregion #region Exits // check if exit if (aw.IsExit) { // loop over interconnect foreach (ArbiterInterconnect ai in aw.Exits) { // add entries aw.OutgoingConnections.Add(ai); } } #endregion #region Adjacent Lanes // check if entry if (aw.IsEntry) { // foreach lane test in same way as aw foreach (ArbiterLane al in aw.Lane.Way.Lanes.Values) { // check if not same lane if (!aw.Lane.Equals(al) && al.RelativelyInside(aw.Position)) { // check ok if ((aw.Lane.LaneOnLeft != null && aw.Lane.LaneOnLeft.Equals(al) && aw.Lane.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) || (aw.Lane.LaneOnRight != null && aw.Lane.LaneOnRight.Equals(al) && aw.Lane.BoundaryRight != ArbiterLaneBoundary.SolidWhite)) { // get closest partition to aw's point ArbiterLanePartition alp = al.GetClosestPartition(aw.Position); // check downstream from this lane? if (aw.Lane.DistanceBetween(aw.Position, alp.Final.Position) >= -0.01 || (alp.Final.NextPartition != null && aw.Lane.DistanceBetween(aw.Position, alp.Final.NextPartition.Final.Position) >= -0.01)) { // new list of contained partitions List <IConnectAreaWaypoints> containedPartitions = new List <IConnectAreaWaypoints>(); containedPartitions.Add(alp); containedPartitions.Add(aw.NextPartition); // set aw as linking to that partition's final waypoint aw.OutgoingConnections.Add(new NavigableEdge(false, null, true, al.Way.Segment, containedPartitions, aw, alp.Final)); } } } } } #endregion } #region Adjacent starting in middle // loop over segment lanes foreach (ArbiterLane al in asg.Lanes.Values) { // get init wp ArbiterWaypoint initial = al.WaypointList[0]; // get adjacent lanes foreach (ArbiterLane adj in al.Way.Lanes.Values) { if (!adj.Equals(al)) { // check if initial is inside other if (adj.RelativelyInside(initial.Position)) { if ((adj.LaneOnLeft != null && adj.LaneOnLeft.Equals(al) && adj.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) || (adj.LaneOnRight != null && adj.LaneOnRight.Equals(al) && adj.BoundaryRight != ArbiterLaneBoundary.SolidWhite)) { // closest partition ArbiterLanePartition alp = adj.GetClosestPartition(initial.Position); // new list of contained partitions List <IConnectAreaWaypoints> containedPartitions = new List <IConnectAreaWaypoints>(); containedPartitions.Add(alp); containedPartitions.Add(initial.NextPartition); // set aw as linking to that partition's final waypoint alp.Initial.OutgoingConnections.Add(new NavigableEdge(false, null, true, al.Way.Segment, containedPartitions, alp.Initial, initial)); } } } } } #endregion } // loop over zones foreach (ArbiterZone az in arn.ArbiterZones.Values) { #region Perimeter Point adjacency // loop over zone perimeter points foreach (ArbiterPerimeterWaypoint apw in az.Perimeter.PerimeterPoints.Values) { #region Old Connectivity // check if this is an entry /*if (apw.IsEntry) * { #region Link Perimeter Points * * // loop over perimeter points * foreach(ArbiterPerimeterWaypoint apwTest in az.Perimeter.PerimeterPoints.Values) * { * // check not this and is exit * if (!apw.Equals(apwTest) && apwTest.IsExit) * { * // add to connections * apw.OutgoingConnections.Add(new NavigableEdge(true, apw.Perimeter.Zone, false, null, new List<IConnectAreaWaypoints>(), apw, apwTest)); * } * } * #endregion * #region Link Checkpoints * * // loop over parking spot waypoints * foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values) * { * // check if checkpoint * if (apsw.IsCheckpoint) * { * // add to connections * apw.OutgoingConnections.Add(new NavigableEdge(true, apw.Perimeter.Zone, false, null, new List<IConnectAreaWaypoints>(), apw, apsw)); * } * } * #endregion * }*/ #endregion // check if the point is an exit if (apw.IsExit) { foreach (ArbiterInterconnect ai in apw.Exits) { // add to connections apw.OutgoingConnections.Add(ai); } } } #endregion #region Checkpoint adjacency // loop over parking spot waypoints foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values) { if (apsw.ParkingSpot.NormalWaypoint.Equals(apsw)) { apsw.OutgoingConnections.Add( new NavigableEdge(true, az, false, null, new List <IConnectAreaWaypoints>(), apsw, apsw.ParkingSpot.Checkpoint)); } else { apsw.OutgoingConnections.Add( new NavigableEdge(true, az, false, null, new List <IConnectAreaWaypoints>(), apsw, apsw.ParkingSpot.NormalWaypoint)); } } #region Old /* * // loop over parking spot waypoints * foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values) * { * // check if checkpoint * if (apsw.IsCheckpoint) * { #region Link Perimeter Points * * // loop over perimeter points * foreach (ArbiterPerimeterWaypoint apwTest in az.Perimeter.PerimeterPoints.Values) * { * // check not this and is exit * if (apwTest.IsExit) * { * // add to connections * apsw.OutgoingConnections.Add(new NavigableEdge(true, apsw.ParkingSpot.Zone, false, null, new List<IConnectAreaWaypoints>(), apsw, apwTest)); * } * } * #endregion * #region Link Checkpoints * * // loop over parking spot waypoints * foreach (ArbiterParkingSpotWaypoint apswTest in az.ParkingSpotWaypoints.Values) * { * // check if checkpoint * if (!apsw.Equals(apswTest) && apswTest.IsCheckpoint) * { * // add to connections * apsw.OutgoingConnections.Add(new NavigableEdge(true, apsw.ParkingSpot.Zone, false, null, new List<IConnectAreaWaypoints>(), apsw, apswTest)); * } * } * #endregion * } * } */ #endregion #endregion } }
public void GenerateInterconnectPolygon(ArbiterInterconnect ai) { List <Coordinates> polyPoints = new List <Coordinates>(); try { // width double width = 3.0; if (ai.InitialGeneric is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric; width = width < aw.Lane.Width ? aw.Lane.Width : width; } if (ai.FinalGeneric is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric; width = width < aw.Lane.Width ? aw.Lane.Width : width; } if (ai.TurnDirection == ArbiterTurnDirection.UTurn || ai.TurnDirection == ArbiterTurnDirection.Straight || !(ai.InitialGeneric is ArbiterWaypoint) || !(ai.FinalGeneric is ArbiterWaypoint)) { LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0); LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0); polyPoints.AddRange(lp); polyPoints.AddRange(rp); ai.TurnPolygon = Polygon.GrahamScan(polyPoints); if (ai.TurnDirection == ArbiterTurnDirection.UTurn) { List <Coordinates> updatedPts = new List <Coordinates>(); LinePath interTmp = ai.InterconnectPath.Clone(); Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position; interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0); interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0); lp = interTmp.ShiftLateral(TahoeParams.VL); rp = interTmp.ShiftLateral(-TahoeParams.VL); updatedPts.AddRange(lp); updatedPts.AddRange(rp); ai.TurnPolygon = Polygon.GrahamScan(updatedPts); } } else { // polygon points List <Coordinates> interPoints = new List <Coordinates>(); // waypoint ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric; ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric; // left and right path LinePath leftPath = new LinePath(); LinePath rightPath = new LinePath(); // some initial points LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position }); LinePath il = initialPath.ShiftLateral(width / 2.0); LinePath ir = initialPath.ShiftLateral(-width / 2.0); leftPath.Add(il[1]); rightPath.Add(ir[1]); // some final points LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position }); LinePath fl = finalPath.ShiftLateral(width / 2.0); LinePath fr = finalPath.ShiftLateral(-width / 2.0); leftPath.Add(fl[0]); rightPath.Add(fr[0]); // initial and final paths Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position); Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position); // get where the paths intersect and vector to normal path Coordinates c; iPath.Intersect(fPath, out c); Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c; Coordinates center = c + vector.Normalize((vector.Length / 2.0)); // get width expansion Coordinates iVec = awI.PreviousPartition != null?awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0); double iRot = -iVec.ArcTan; Coordinates fVec = awF.NextPartition != null?awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0); fVec = fVec.Rotate(iRot); double fDeg = fVec.ToDegrees(); double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI; double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0; // get inner point (small scale) Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0); // get outer Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0); if (ai.TurnDirection == ArbiterTurnDirection.Right) { rightPath.Insert(1, innerPoint); ai.InnerCoordinates = rightPath; leftPath.Reverse(); leftPath.Insert(1, outerPoint); Polygon p = new Polygon(leftPath.ToArray()); p.AddRange(rightPath.ToArray()); ai.TurnPolygon = p; } else { leftPath.Insert(1, innerPoint); ai.InnerCoordinates = leftPath; rightPath.Reverse(); rightPath.Insert(1, outerPoint); Polygon p = new Polygon(leftPath.ToArray()); p.AddRange(rightPath.ToArray()); ai.TurnPolygon = p; } } } catch (Exception e) { Console.WriteLine("error generating turn polygon: " + ai.ToString()); ai.TurnPolygon = ai.DefaultPoly(); } }
/// <summary> /// Next point at which to stop /// </summary> /// <param name="lane"></param> /// <param name="position"></param> /// <param name="stopPoint"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public void NextLaneStop(IFQMPlanable lane, Coordinates position, double[] enCovariance, List<ArbiterWaypoint> ignorable, out ArbiterWaypoint stopPoint, out double stopSpeed, out double stopDistance, out StopType stopType) { // get the stop List<WaypointType> wts = new List<WaypointType>(); wts.Add(WaypointType.Stop); wts.Add(WaypointType.End); stopPoint = lane.GetNext(position, wts, ignorable); // set stop type stopType = stopPoint.IsStop ? StopType.StopLine : StopType.EndOfLane; // parameterize this.StoppingParams(stopPoint, lane, position, enCovariance, out stopSpeed, out stopDistance); }
/// <summary> /// Constructor /// </summary> /// <param name="aw"></param> /// <param name="cycles"></param> public IgnorableWaypoint(ArbiterWaypoint aw, int cycles) { this.Waypoint = aw; this.numberOfCyclesIgnored = cycles; }
/// <summary> /// Determines proper speed commands given we want to stop at a certain waypoint /// </summary> /// <param name="waypoint"></param> /// <param name="lane"></param> /// <param name="position"></param> /// <param name="enCovariance"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public void StoppingParams(ArbiterWaypoint waypoint, IFQMPlanable lane, Coordinates position, double[] enCovariance, out double stopSpeed, out double stopDistance) { // get dist to waypoint stopDistance = lane.DistanceBetween(position, waypoint.Position); // subtract distance based upon type to help calculate speed double stopTypeDistance = waypoint.IsStop ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance; double stopSpeedDistance = stopDistance - stopTypeDistance; // check if we are positive distance away if (stopSpeedDistance >= 0) { // segment max speed double segmentMaxSpeed = lane.CurrentMaximumSpeed(position); // get speed using constant acceleration stopSpeed = SpeedTools.GenerateSpeed(stopSpeedDistance, CoreCommon.OperationalStopSpeed, segmentMaxSpeed); } else { // inside stop dist stopSpeed = (stopDistance / stopTypeDistance) * CoreCommon.OperationalStopSpeed; stopSpeed = stopSpeed < 0 ? 0.0 : stopSpeed; } }
/// <summary> /// Gets the next navigational stop relavant to us (stop or end) in the closest good lane or our current opposing lane /// </summary> /// <param name="closestGood"></param> /// <param name="coordinates"></param> /// <param name="ignorable"></param> /// <param name="navStopSpeed"></param> /// <param name="navStopDistance"></param> /// <param name="navStopType"></param> /// <param name="navStop"></param> private void NextOpposingNavigationalStop(ArbiterLane opposing, ArbiterLane closestGood, Coordinates coordinates, double extraDistance, out double navStopSpeed, out double navStopDistance, out StopType navStopType, out ArbiterWaypoint navStop) { ArbiterWaypoint current = null; double minDist = Double.MaxValue; StopType st = StopType.EndOfLane; #region Closest Good Parameterization foreach (ArbiterWaypoint aw in closestGood.WaypointList) { if (aw.IsStop || aw.NextPartition == null) { double dist = closestGood.DistanceBetween(coordinates, aw.Position); if (dist < minDist && dist >= 0) { current = aw; minDist = dist; st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane; } } } #endregion #region Opposing Parameterization ArbiterWaypoint opStart = opposing.GetClosestPartition(coordinates).Initial; int startIndex = opposing.WaypointList.IndexOf(opStart); for (int i = startIndex; i >= 0; i--) { ArbiterWaypoint aw = opposing.WaypointList[i]; if (aw.IsStop || aw.PreviousPartition == null) { double dist = opposing.DistanceBetween(aw.Position, coordinates); if (dist < minDist && dist >= 0) { current = aw; minDist = dist; st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane; } } } #endregion double tmpDistanceIgnore; this.StoppingParams(current, closestGood, coordinates, extraDistance, out navStopSpeed, out tmpDistanceIgnore); navStop = current; navStopDistance = minDist; navStopType = st; }
/// <summary> /// Path of lane between two waypoints /// </summary> /// <param name="w1">Initial waypoint</param> /// <param name="w2">Final waypoint</param> /// <returns></returns> public LinePath LanePath(ArbiterWaypoint w1, ArbiterWaypoint w2) { return(this.LanePath().SubPath( this.LanePath().GetClosestPoint(w1.Position), this.LanePath().GetClosestPoint(w2.Position))); }
/// <summary> /// Makes new parameterization for nav /// </summary> /// <param name="lane"></param> /// <param name="lanePlan"></param> /// <param name="speed"></param> /// <param name="distance"></param> /// <param name="stopType"></param> /// <returns></returns> public TravelingParameters NavStopParameterization(IFQMPlanable lane, RoadPlan roadPlan, double speed, double distance, ArbiterWaypoint stopWaypoint, StopType stopType, VehicleState state) { // get min dist double distanceCutOff = stopType == StopType.StopLine ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance; #region Get Decorators // turn direction default ArbiterTurnDirection atd = ArbiterTurnDirection.Straight; List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators; // check if need decorators if (lane is ArbiterLane && stopWaypoint.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest) && roadPlan.BestPlan.laneWaypointOfInterest.IsExit && distance < 40.0) { if (roadPlan.BestPlan.laneWaypointOfInterest.BestExit == null) ArbiterOutput.Output("NAV BUG: lanePlan.laneWaypointOfInterest.BestExit: FQM NavStopParameterization"); else { switch (roadPlan.BestPlan.laneWaypointOfInterest.BestExit.TurnDirection) { case ArbiterTurnDirection.Left: decorators = TurnDecorators.LeftTurnDecorator; atd = ArbiterTurnDirection.Left; break; case ArbiterTurnDirection.Right: atd = ArbiterTurnDirection.Right; decorators = TurnDecorators.RightTurnDecorator; break; case ArbiterTurnDirection.Straight: atd = ArbiterTurnDirection.Straight; decorators = TurnDecorators.NoDecorators; break; case ArbiterTurnDirection.UTurn: atd = ArbiterTurnDirection.UTurn; decorators = TurnDecorators.LeftTurnDecorator; break; } } } else if (lane is SupraLane) { SupraLane sl = (SupraLane)lane; double distToInterconnect = sl.DistanceBetween(state.Front, sl.Interconnect.InitialGeneric.Position); if ((distToInterconnect > 0 && distToInterconnect < 40.0) || sl.ClosestComponent(state.Front) == SLComponentType.Interconnect) { switch (sl.Interconnect.TurnDirection) { case ArbiterTurnDirection.Left: decorators = TurnDecorators.LeftTurnDecorator; atd = ArbiterTurnDirection.Left; break; case ArbiterTurnDirection.Right: atd = ArbiterTurnDirection.Right; decorators = TurnDecorators.RightTurnDecorator; break; case ArbiterTurnDirection.Straight: atd = ArbiterTurnDirection.Straight; decorators = TurnDecorators.NoDecorators; break; case ArbiterTurnDirection.UTurn: atd = ArbiterTurnDirection.UTurn; decorators = TurnDecorators.LeftTurnDecorator; break; } } } #endregion #region Get Maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; SpeedCommand sc = new StopAtDistSpeedCommand(distance); #region Distance Cutoff // check if distance is less than cutoff if (distance < distanceCutOff && stopType != StopType.EndOfLane) { // default behavior Behavior b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtDistSpeedCommand(distance), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true)); // stopping so not using speed param usingSpeed = false; // exit is next if (stopType == StopType.Exit) { // exit means stopping at a good exit in our current lane IState nextState = new StoppingAtExitState(stopWaypoint.Lane, stopWaypoint, atd, true, roadPlan.BestPlan.laneWaypointOfInterest.BestExit, state.Timestamp, state.Front); m = new Maneuver(b, nextState, decorators, state.Timestamp); } // stop line is left else if (stopType == StopType.StopLine) { // determine if hte stop line is the best exit bool isNavExit = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Equals(stopWaypoint); // get turn direction atd = isNavExit ? atd : ArbiterTurnDirection.Straight; // predetermine interconnect if best exit ArbiterInterconnect desired = null; if (isNavExit) desired = roadPlan.BestPlan.laneWaypointOfInterest.BestExit; else if (stopWaypoint.NextPartition != null && state.Front.DistanceTo(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > 25) desired = stopWaypoint.NextPartition.ToInterconnect; // set decorators decorators = isNavExit ? decorators : TurnDecorators.NoDecorators; // stop at the stop IState nextState = new StoppingAtStopState(stopWaypoint.Lane, stopWaypoint, atd, isNavExit, desired); b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtLineSpeedCommand(), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true)); m = new Maneuver(b, nextState, decorators, state.Timestamp); sc = new StopAtLineSpeedCommand(); } else if(stopType == StopType.LastGoal) { // stop at the last goal IState nextState = new StayInLaneState(stopWaypoint.Lane, CoreCommon.CorePlanningState); m = new Maneuver(b, nextState, decorators, state.Timestamp); } } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // set speed sc = new ScalarSpeedCommand(speed); // check if lane if (lane is ArbiterLane) { // get lane ArbiterLane al = (ArbiterLane)lane; // default behavior Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), decorators, state.Timestamp); } // check if supra lane else if (lane is SupraLane) { // get lane SupraLane sl = (SupraLane)lane; // get sl state StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; // get default beheavior Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List<int>()); // standard behavior is fine for maneuver m = new Maneuver(b, sisls, decorators, state.Timestamp); } } #endregion #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = m.PrimaryBehavior.Decorators; tp.DistanceToGo = distance; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = speed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.SpeedCommand = sc; tp.VehiclesToIgnore = new List<int>(); // return navigation params return tp; #endregion }
/// <summary> /// Check if found a stop in a lane as part of exits or a test waypoint /// </summary> /// <param name="initialTest"></param> /// <param name="exits"></param> /// <param name="lane"></param> /// <returns></returns> private bool FoundStop(ArbiterWaypoint initialTest, IEnumerable<ITraversableWaypoint> exits, ArbiterLane lane) { if (initialTest != null && initialTest.IsStop) return true; foreach (ITraversableWaypoint exit in exits) { if (exit is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)exit; if (aw.IsStop && aw.Lane.Equals(lane)) { return true; } } } return false; }
/// <summary> /// Determines the point at which we need to stop in the current lane /// </summary> /// <param name="lanePoint"></param> /// <param name="position"></param> /// <param name="stopRequired"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public void NextNavigationalStop(IFQMPlanable lane, ArbiterWaypoint lanePoint, Coordinates position, double[] enCovariance, List<ArbiterWaypoint> ignorable, out double stopSpeed, out double stopDistance, out StopType stopType, out ArbiterWaypoint stopWaypoint) { // variables for default next stop line or end of lane this.NextLaneStop(lane, position, enCovariance, ignorable, out stopWaypoint, out stopSpeed, out stopDistance, out stopType); if (lanePoint != null) { // check if the point downstream is the last checkpoint if (CoreCommon.Mission.MissionCheckpoints.Count == 1 && lanePoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId)) { ArbiterWaypoint cpStop = lanePoint; double cpStopSpeed; double cpStopDistance; StopType cpStopType = StopType.LastGoal; this.StoppingParams(cpStop, lane, position, enCovariance, out cpStopSpeed, out cpStopDistance); if (cpStopDistance <= stopDistance) { stopSpeed = cpStopSpeed; stopDistance = cpStopDistance; stopType = cpStopType; stopWaypoint = cpStop; } } // check if point is not the checkpoint and is an exit else if (lanePoint.IsExit && !lanePoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId)) { ArbiterWaypoint exitStop = lanePoint; double exitStopSpeed; double exitStopDistance; StopType exitStopType = lanePoint.IsStop ? StopType.StopLine : StopType.Exit; this.StoppingParams(exitStop, lane, position, enCovariance, out exitStopSpeed, out exitStopDistance); if (exitStopDistance <= stopDistance) { stopSpeed = exitStopSpeed; stopDistance = exitStopDistance; stopType = exitStopType; stopWaypoint = exitStop; } } } }
/// <summary> /// Gets exits downstream, or the goal we are currently reaching /// </summary> /// <param name="currentPosition"></param> /// <param name="ignorable"></param> /// <param name="goal"></param> /// <returns></returns> public List <DownstreamPointOfInterest> Downstream(Coordinates currentPosition, List <ArbiterWaypoint> ignorable, INavigableNode goal) { // downstream final List <DownstreamPointOfInterest> waypoints = new List <DownstreamPointOfInterest>(); foreach (ArbiterLane al in Way.Lanes.Values) { if (al.Equals(this) || (al.GetClosestPartition(currentPosition).Type != PartitionType.Startup && ((this.LaneOnLeft != null && this.LaneOnLeft.Equals(al) && this.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) || (this.LaneOnRight != null && this.LaneOnRight.Equals(al) && this.BoundaryRight != ArbiterLaneBoundary.SolidWhite)))) { // get starting waypoint ArbiterWaypoint waypoint = null; // get closest partition ArbiterLanePartition alp = al.GetClosestPartition(currentPosition); if (alp.Initial.Position.DistanceTo(currentPosition) < TahoeParams.VL - 2) { waypoint = alp.Initial; } else if (alp.IsInside(currentPosition) || alp.Final.Position.DistanceTo(currentPosition) < TahoeParams.VL) { waypoint = alp.Final; } else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition)) { waypoint = alp.Initial; } else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition) && alp.Final.NextPartition == null) { waypoint = null; } else { waypoint = null; } // check waypoint exists if (waypoint != null) { // save start ArbiterWaypoint initial = waypoint; // initial cost double initialCost = 0.0; if (al.Equals(this)) { initialCost = currentPosition.DistanceTo(waypoint.Position) / waypoint.Lane.Way.Segment.SpeedLimits.MaximumSpeed; } else if (waypoint.WaypointId.Number != 1) { // get closest partition ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue); initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number); initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed; } else { // get closest partition ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue); ArbiterWaypoint tmpF = this.GetClosestWaypoint(waypoint.Position, Double.MaxValue); initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number); initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed; initialCost += this.TimeCostInLane(tmpI, tmpF, new List <ArbiterWaypoint>()); } // loop while waypoint not null while (waypoint != null) { if (waypoint.IsCheckpoint && (goal is ArbiterWaypoint) && ((ArbiterWaypoint)goal).WaypointId.Equals(waypoint.WaypointId)) { double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable); DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position); dpoi.IsExit = false; dpoi.IsGoal = true; dpoi.PointOfInterest = waypoint; dpoi.TimeCostToPoint = timeCost; waypoints.Add(dpoi); } else if (waypoint.IsExit && !ignorable.Contains(waypoint)) { double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable); DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position); dpoi.IsExit = true; dpoi.IsGoal = false; dpoi.PointOfInterest = waypoint; dpoi.TimeCostToPoint = timeCost; waypoints.Add(dpoi); } else if (waypoint.NextPartition == null && !ignorable.Contains(waypoint)) { DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest(); dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position); dpoi.IsExit = false; dpoi.IsGoal = false; dpoi.PointOfInterest = waypoint; dpoi.TimeCostToPoint = Double.MaxValue; waypoints.Add(dpoi); } waypoint = waypoint.NextPartition != null ? waypoint.NextPartition.Final : null; } } } } return(waypoints); }
/// <summary> /// Gets time cost between two waypoints in the same lane /// </summary> /// <param name="initial"></param> /// <param name="final"></param> /// <returns></returns> public double TimeCostInLane(ArbiterWaypoint initial, ArbiterWaypoint final, List<ArbiterWaypoint> ignorable) { ArbiterWaypoint current = initial; double cost = ignorable.Contains(initial) ? 0.0 : initial.ExtraTimeCost; while (current != null) { if (current.Equals(final)) { return cost; } else { if(current.NextPartition != null) cost += current.NextPartition.TimeCost(); if (current.NextPartition != null) current = current.NextPartition.Final; else current = null; } } return Double.MaxValue; }
/// <summary> /// Get waypoints from initial to final /// </summary> /// <param name="initial"></param> /// <param name="final"></param> /// <returns></returns> public List<ArbiterWaypoint> WaypointsInclusive(ArbiterWaypoint initial, ArbiterWaypoint final) { List<ArbiterWaypoint> middle = new List<ArbiterWaypoint>(); int i = waypointList.IndexOf(initial); int j = waypointList.IndexOf(final); for (int k = i; k <= j; k++) { middle.Add(waypointList[k]); } return middle; }