/// <summary> /// Constructor /// </summary> /// <param name="aw"></param> public IgnorableWaypoint(ArbiterWaypoint aw) { this.Waypoint = aw; this.numberOfCyclesIgnored = 0; }
/// <summary> /// Constructor /// </summary> /// <param name="aw"></param> /// <param name="cycles"></param> public IgnorableWaypoint(ArbiterWaypoint aw, int cycles) { this.Waypoint = aw; this.numberOfCyclesIgnored = cycles; }
/// <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> /// Figure out standard control to follow /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public ForwardVehicleTrackingControl GetControl(IFQMPlanable lane, VehicleState state, List <ArbiterWaypoint> ignorable) { // check exists a vehicle to track if (this.CurrentVehicle != null) { // get the maximum velocity of the segment we're closest to double segV = lane.CurrentMaximumSpeed(state.Front); double segMaxV = segV; // flag if the forward vehicles is in a stop waypoint type safety zone bool safetyZone = false; ArbiterWaypoint nextStop = lane.GetNext(state.Front, WaypointType.Stop, ignorable); double distance = nextStop != null?lane.DistanceBetween(state.Front, nextStop.Position) : Double.MaxValue; // check if no stop exists or distance > 30 if (nextStop == null || distance > 30 || distance < 0) { // there is no next stop or the forward vehcile is not in a safety zone safetyZone = false; } // otherwise the tracked vehicle is in a safety zone else { // set the forward vehicle as being in a safety zone safetyZone = true; } // minimum distance double xAbsMin; // if we're in a safety zone if (safetyZone) { // set minimum to 1 times the tahoe's vehicle length xAbsMin = TahoeParams.VL * 1.5; } // otherwise we're not in a safety zone else { // set minimum to 2 times the tahoe's vehicle length xAbsMin = TahoeParams.VL * 2.0; } // retrieve the tracked vehicle's scalar absolute speed double vTarget = CurrentVehicle.StateMonitor.Observed.isStopped ? 0.0 : this.CurrentVehicle.Speed; // check if vehicle is moving away from us VehicleDirectionAlongPath vdap = CurrentVehicle.VehicleDirectionAlong(lane); // if moving perpendic, set speed to 0.0 if (vdap == VehicleDirectionAlongPath.Perpendicular) { vTarget = 0.0; } #region Forward // can use normal tracker if vehicle not oncoming if (true) //vdap != VehicleDirectionAlongPath.Reverse) { // get the good distance behind the target, is xAbsMin if vehicle velocity is small enough double xGood = xAbsMin + (1.5 * (TahoeParams.VL / 4.4704) * vTarget); // get our current separation double xSeparation = this.currentDistance; // determine the envelope to reason about the vehicle, that is, to slow from vMax to vehicle speed double xEnvelope = (Math.Pow(vTarget, 2.0) - Math.Pow(segMaxV, 2.0)) / (2.0 * -0.5); // the distance to the good double xDistanceToGood; // determine the velocity to follow the vehicle ahead double vFollowing; // check if we are basically stopped in the right place behind another vehicle if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // check if we are within the minimum distance else if (xSeparation <= xAbsMin) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // determine if our separation is less than the good distance but not inside minimum else if (xAbsMin < xSeparation && xSeparation < xGood) { // get the distance we are from xMin double xAlong = xSeparation - xAbsMin; // get the total distance from xGood to xMin double xGoodToMin = xGood - xAbsMin; // slow to 0 by min vFollowing = (((xGoodToMin - xAlong) / xGoodToMin) * (-vTarget)) + vTarget; // good distance xDistanceToGood = 0; } // our separation is greater than the good distance else { // good distance xDistanceToGood = xSeparation - xGood; // get differences in max and target velocities vFollowing = SpeedTools.GenerateSpeed(xDistanceToGood, vTarget, segMaxV); } // return return(new ForwardVehicleTrackingControl(true, safetyZone, vFollowing, xSeparation, xDistanceToGood, vTarget, xAbsMin, xGood, false)); } #endregion #region Oncoming /* * else * { * // determine the distance for the other vehicle to stop * double xTargetEnvelope = -Math.Pow(vTarget, 2.0) / (2.0 * -0.5); * * // determine the distance for our vehicle to stop * double xOurEnvelope = -Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2.0) / (2.0 * -0.5); * * // get the good distance behind the target, is xAbsMin if vehicle velocity is small enough * double xGood = xAbsMin + (1.5 * (TahoeParams.VL / 4.4704) * vTarget) + xOurEnvelope + xTargetEnvelope; * * // get our current separation * double xSeparation = this.currentDistance; * * // determine the envelope for us to slow to 0 by the good distance * double xEnvelope = -Math.Pow(segMaxV, 2.0) / (2.0 * -0.5); * * // the distance to the good * double xDistanceToGood; * * // determine the velocity to follow the vehicle ahead * double vFollowing; * * // check if we are basically stopped in the right place behind another vehicle * if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1) * { * // stop * vFollowing = 0; * * // good distance * xDistanceToGood = 0; * } * // check if we are within the minimum distance * else if (xSeparation <= xGood) * { * // stop * vFollowing = 0; * * // good distance * xDistanceToGood = 0; * } * // our separation is greater than the good distance but within the envelope * else if (xGood <= xSeparation && xSeparation <= xEnvelope + xGood) * { * // get differences in max and target velocities * double vDifference = segMaxV; * * // get the distance we are along the speed envolope from xGood * double xAlong = xEnvelope - (xSeparation - xGood); * * // slow to vTarget by good * vFollowing = (((xEnvelope - xAlong) / xEnvelope) * (vDifference)); * * // good distance * xDistanceToGood = xSeparation - xGood; * } * // otherwise xSeparation > xEnvelope * else * { * // can go max speed * vFollowing = segMaxV; * * // good distance * xDistanceToGood = xSeparation - xGood; * } * * // return * return new ForwardVehicleTrackingControl(true, safetyZone, vFollowing, xSeparation, * xDistanceToGood, vTarget, xAbsMin, xGood, true); * } * */ #endregion } else { // return return(new ForwardVehicleTrackingControl(false, false, Double.MaxValue, Double.MaxValue, Double.MaxValue, Double.MaxValue, 0.0, Double.MaxValue, false)); } }
/// <summary> /// Gets primary maneuver given our position and the turn we are traveling upon /// </summary> /// <param name="vehicleState"></param> /// <returns></returns> public Maneuver PrimaryManeuver(VehicleState vehicleState, List <ITacticalBlockage> blockages, TurnState turnState) { #region Check are planning over the correct turn if (CoreCommon.CorePlanningState is TurnState) { TurnState ts = (TurnState)CoreCommon.CorePlanningState; if (this.turn == null || !this.turn.Equals(ts.Interconnect)) { this.turn = ts.Interconnect; this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null); } else if (this.forwardMonitor.turn == null || !this.forwardMonitor.turn.Equals(ts.Interconnect)) { this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null); } } #endregion #region Blockages // check blockages if (blockages != null && blockages.Count > 0 && blockages[0] is TurnBlockage) { // create the blockage state EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState); // check not at highest level already if (turnState.Saudi != SAUDILevel.L1 || turnState.UseTurnBounds) { // check not from a dynamicly moving vehicle if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic || (TacticalDirector.ValidVehicles.ContainsKey(blockages[0].BlockageReport.TrackID) && TacticalDirector.ValidVehicles[blockages[0].BlockageReport.TrackID].IsStopped)) { // go to a blockage handling tactical return(new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } else { ArbiterOutput.Output("Turn blockage reported for moving vehicle, ignoring"); } } else { ArbiterOutput.Output("Turn blockage, but recovery escalation already at highest state, ignoring report"); } } #endregion #region Intersection Check if (!this.CanGo(vehicleState)) { if (turn.FinalGeneric is ArbiterWaypoint) { TravelingParameters tp = this.GetParameters(0.0, 0.0, (ArbiterWaypoint)turn.FinalGeneric, vehicleState, false); return(new Maneuver(tp.Behavior, CoreCommon.CorePlanningState, tp.NextState.DefaultStateDecorators, vehicleState.Timestamp)); } else { // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL); // hold brake IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0)); TurnBehavior b = new TurnBehavior(null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0), this.turn.InterconnectId); return(new Maneuver(b, CoreCommon.CorePlanningState, nextState.DefaultStateDecorators, vehicleState.Timestamp)); } } #endregion #region Final is Lane Waypoint if (turn.FinalGeneric is ArbiterWaypoint) { // final point ArbiterWaypoint final = (ArbiterWaypoint)turn.FinalGeneric; // plan down entry lane RoadPlan rp = navigation.PlanNavigableArea(final.Lane, final.Position, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], new List <ArbiterWaypoint>()); // point of interest downstream DownstreamPointOfInterest dpoi = rp.BestPlan.laneWaypointOfInterest; // get path this represents List <Coordinates> pathCoordinates = new List <Coordinates>(); pathCoordinates.Add(vehicleState.Position); foreach (ArbiterWaypoint aw in final.Lane.WaypointsInclusive(final, final.Lane.WaypointList[final.Lane.WaypointList.Count - 1])) { pathCoordinates.Add(aw.Position); } LinePath lp = new LinePath(pathCoordinates); // list of all parameterizations List <TravelingParameters> parameterizations = new List <TravelingParameters>(); // get lane navigation parameterization TravelingParameters navigationParameters = this.NavigationParameterization(vehicleState, dpoi, final, lp); parameterizations.Add(navigationParameters); // update forward tracker and get vehicle parameterizations if forward vehicle exists this.forwardMonitor.Update(vehicleState, final, lp); if (this.forwardMonitor.ShouldUseForwardTracker()) { // get vehicle parameterization TravelingParameters vehicleParameters = this.VehicleParameterization(vehicleState, lp, final); parameterizations.Add(vehicleParameters); } // sort and return funal parameterizations.Sort(); // get the final behavior TurnBehavior tb = (TurnBehavior)parameterizations[0].Behavior; // get vehicles to ignore tb.VehiclesToIgnore = this.forwardMonitor.VehiclesToIgnore; // add persistent information about saudi level if (turnState.Saudi == SAUDILevel.L1) { tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray()); tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1)); } // add persistent information about turn bounds if (!turnState.UseTurnBounds) { tb.LeftBound = null; tb.RightBound = null; } // return the behavior return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp)); } #endregion #region Final is Zone Waypoint else if (turn.FinalGeneric is ArbiterPerimeterWaypoint) { // get inteconnect path Coordinates entryVec = ((ArbiterPerimeterWaypoint)turn.FinalGeneric).Perimeter.PerimeterPolygon.BoundingCircle.center - turn.FinalGeneric.Position; entryVec = entryVec.Normalize(TahoeParams.VL / 2.0); LinePath ip = new LinePath(new Coordinates[] { turn.InitialGeneric.Position, turn.FinalGeneric.Position, entryVec + this.turn.FinalGeneric.Position }); // get distance from end double d = ip.DistanceBetween( ip.GetClosestPoint(vehicleState.Front), ip.EndPoint); // get speed command SpeedCommand sc = null; if (d < TahoeParams.VL) { sc = new StopAtDistSpeedCommand(d); } else { sc = new ScalarSpeedCommand(SpeedTools.GenerateSpeed(d - TahoeParams.VL, 1.7, turn.MaximumDefaultSpeed)); } // final perimeter waypoint ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)this.turn.FinalGeneric; // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL); // hold brake IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, sc); TurnBehavior tb = new TurnBehavior(null, finalPath, leftLL, rightLL, sc, null, new List <int>(), this.turn.InterconnectId); // add persistent information about saudi level if (turnState.Saudi == SAUDILevel.L1) { tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray()); tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1)); } // add persistent information about turn bounds if (!turnState.UseTurnBounds) { tb.LeftBound = null; tb.RightBound = null; } // return maneuver return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp)); } #endregion #region Unknown else { throw new Exception("unrecognized type: " + turn.FinalGeneric.ToString()); } #endregion }
/// <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> /// Gets the next stop /// </summary> /// <param name="al"></param> /// <param name="alp"></param> /// <param name="c"></param> /// <returns></returns> public static void NextStop(ArbiterLane al, ArbiterLanePartition alp, Coordinates c, out ArbiterWaypoint waypoint, out double distance) { ArbiterWaypoint current = alp.Final; while (current != null) { if (current.IsStop) { distance = al.PartitionPath.DistanceBetween( al.PartitionPath.GetClosest(c), al.PartitionPath.GetClosest(current.Position)); waypoint = current; return; } else { current = current.NextPartition != null ? current.NextPartition.Final : null; } } waypoint = null; distance = Double.MaxValue; }
/// <summary> /// Behavior given we stay in the current lane /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <param name="downstreamPoint"></param> /// <returns></returns> public TravelingParameters Primary(IFQMPlanable lane, VehicleState state, RoadPlan roadPlan, List <ITacticalBlockage> blockages, List <ArbiterWaypoint> ignorable, bool log) { // possible parameterizations List <TravelingParameters> tps = new List <TravelingParameters>(); #region Lane Major Parameterizations with Current Lane Goal Params, If Best Goal Exists in Current Lane // check if the best goal is in the current lane ArbiterWaypoint lanePoint = null; if (lane.AreaComponents.Contains(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Lane)) { lanePoint = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest; } // get the next thing we need to stop at no matter what and parameters for stopping at it ArbiterWaypoint laneNavStop; double laneNavStopSpeed; double laneNavStopDistance; StopType laneNavStopType; this.NextNavigationalStop(lane, lanePoint, state.Front, state.ENCovariance, ignorable, out laneNavStopSpeed, out laneNavStopDistance, out laneNavStopType, out laneNavStop); // create parameterization of the stop TravelingParameters laneNavParams = this.NavStopParameterization(lane, roadPlan, laneNavStopSpeed, laneNavStopDistance, laneNavStop, laneNavStopType, state); this.navigationParameters = laneNavParams; this.laneParameters = laneNavParams; tps.Add(laneNavParams); #region Log if (log) { // add to current parames to arbiter information CoreCommon.CurrentInformation.FQMBehavior = laneNavParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FQMBehaviorInfo = laneNavParams.Behavior.ShortBehaviorInformation(); CoreCommon.CurrentInformation.FQMSpeedCommand = laneNavParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FQMDistance = laneNavParams.DistanceToGo.ToString("F6"); CoreCommon.CurrentInformation.FQMSpeed = laneNavParams.RecommendedSpeed.ToString("F6"); CoreCommon.CurrentInformation.FQMState = laneNavParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FQMStateInfo = laneNavParams.NextState.StateInformation(); CoreCommon.CurrentInformation.FQMStopType = laneNavStopType.ToString(); CoreCommon.CurrentInformation.FQMWaypoint = laneNavStop.ToString(); CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = lane.CurrentMaximumSpeed(state.Position).ToString("F1"); } #endregion #endregion #region Forward Vehicle Parameterization // forward vehicle update this.ForwardVehicle.Update(lane, state); // clear current params this.followingParameters = null; // check not in a sparse area bool sparseArea = lane is ArbiterLane ? ((ArbiterLane)lane).GetClosestPartition(state.Front).Type == PartitionType.Sparse : ((SupraLane)lane).ClosestComponent(state.Front) == SLComponentType.Initial && ((SupraLane)lane).Initial.GetClosestPartition(state.Front).Type == PartitionType.Sparse; // exists forward vehicle if (!sparseArea && this.ForwardVehicle.ShouldUseForwardTracker) { // get forward vehicle params, set lane decorators TravelingParameters vehicleParams = this.ForwardVehicle.Follow(lane, state, ignorable); vehicleParams.Behavior.Decorators.AddRange(this.laneParameters.Decorators); this.FollowingParameters = vehicleParams; #region Log if (log) { // add to current parames to arbiter information CoreCommon.CurrentInformation.FVTBehavior = vehicleParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FVTSpeed = this.ForwardVehicle.FollowingParameters.RecommendedSpeed.ToString("F3"); CoreCommon.CurrentInformation.FVTSpeedCommand = vehicleParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FVTDistance = vehicleParams.DistanceToGo.ToString("F2"); CoreCommon.CurrentInformation.FVTState = vehicleParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FVTStateInfo = vehicleParams.NextState.StateInformation(); // set xSeparation CoreCommon.CurrentInformation.FVTXSeparation = this.ForwardVehicle.ForwardControl.xSeparation.ToString("F0"); } #endregion // check if we're stopped behind fv, allow wait timer if true, stop wait timer if not behind fv bool forwardVehicleStopped = this.ForwardVehicle.CurrentVehicle.IsStopped; bool forwardSeparationGood = this.ForwardVehicle.ForwardControl.xSeparation < TahoeParams.VL * 2.5; bool wereStopped = CoreCommon.Communications.GetVehicleSpeed().Value < 0.1; bool forwardDistanceToGo = vehicleParams.DistanceToGo < 3.5; if (forwardVehicleStopped && forwardSeparationGood && wereStopped && forwardDistanceToGo) { this.ForwardVehicle.StoppedBehindForwardVehicle = true; } else { this.ForwardVehicle.StoppedBehindForwardVehicle = false; this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop(); this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset(); } // add vehicle param tps.Add(vehicleParams); } else { // no forward vehicle this.followingParameters = null; this.ForwardVehicle.StoppedBehindForwardVehicle = false; } #endregion #region Sparse Waypoint Parameterization // check for sparse waypoints downstream bool sparseDownstream; bool sparseNow; double sparseDistance; lane.SparseDetermination(state.Front, out sparseDownstream, out sparseNow, out sparseDistance); // check if sparse areas downstream if (sparseDownstream) { // set the distance to the sparse area if (sparseNow) { sparseDistance = 0.0; } // get speed double speed = SpeedTools.GenerateSpeed(sparseDistance, 2.24, lane.CurrentMaximumSpeed(state.Front)); // maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; SpeedCommand sc = new ScalarSpeedCommand(speed); #region Parameterize Given Speed Command // 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)); b.Decorators = this.laneParameters.Decorators; // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), this.laneParameters.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>()); b.Decorators = this.laneParameters.Decorators; // standard behavior is fine for maneuver m = new Maneuver(b, sisls, this.laneParameters.Decorators, state.Timestamp); } #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = this.laneParameters.Decorators; tp.DistanceToGo = Double.MaxValue; 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 tps.Add(tp); #endregion } #endregion // sort params by most urgent tps.Sort(); // set current params this.currentParameters = tps[0]; // get behavior to check add vehicles to ignore if (this.currentParameters.Behavior is StayInLaneBehavior) { ((StayInLaneBehavior)this.currentParameters.Behavior).IgnorableObstacles = this.ForwardVehicle.VehiclesToIgnore; } // out of navigation, blockages, and vehicle following determine the actual primary parameters for this lane return(tps[0]); }
/// <summary> /// Adds waypoint to ignore list /// </summary> /// <param name="toIgnore"></param> public void IgnoreWaypoint(ArbiterWaypoint toIgnore) { this.IgnorableWaypoints.Add(new IgnorableWaypoint(toIgnore)); }
/// <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> /// Gets parameterization /// </summary> /// <param name="lane"></param> /// <param name="speed"></param> /// <param name="distance"></param> /// <param name="state"></param> /// <returns></returns> public TravelingParameters GetParameters(double speed, double distance, ArbiterWaypoint final, VehicleState state, bool canUseDistance) { double distanceCutOff = CoreCommon.OperationalStopDistance; SpeedCommand sc; bool usingSpeed = true; Maneuver m; #region Generate Next State // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.TurnInfo(final, out finalPath, out leftLL, out rightLL); TurnState ts = new TurnState(this.turn, this.turn.TurnDirection, final.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(speed)); #endregion #region Distance Cutoff // check if distance is less than cutoff if (distance < distanceCutOff && canUseDistance) { // default behavior sc = new StopAtDistSpeedCommand(distance); // stopping so not using speed param usingSpeed = false; } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // default behavior sc = new ScalarSpeedCommand(speed); } #endregion #region Generate Maneuver if (ts.Interconnect.InitialGeneric is ArbiterWaypoint && CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(((ArbiterWaypoint)ts.Interconnect.InitialGeneric).AreaSubtypeWaypointId)) { Polygon p = CoreCommon.RoadNetwork.IntersectionLookup[((ArbiterWaypoint)ts.Interconnect.InitialGeneric).AreaSubtypeWaypointId].IntersectionPolygon; // behavior Behavior b = new TurnBehavior(ts.TargetLane.LaneId, ts.EndingPath, ts.LeftBound, ts.RightBound, sc, p, this.forwardMonitor.VehiclesToIgnore, ts.Interconnect.InterconnectId); m = new Maneuver(b, ts, ts.DefaultStateDecorators, state.Timestamp); } else { // behavior Behavior b = new TurnBehavior(ts.TargetLane.LaneId, ts.EndingPath, ts.LeftBound, ts.RightBound, sc, null, this.forwardMonitor.VehiclesToIgnore, ts.Interconnect.InterconnectId); m = new Maneuver(b, ts, ts.DefaultStateDecorators, state.Timestamp); } #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 = this.forwardMonitor.VehiclesToIgnore; #endregion // return navigation params return(tp); }
/// <summary> /// Navigation parameterization given point of intereset and our vehicle state /// </summary> /// <param name="vehicleState"></param> /// <param name="dpoi"></param> /// <returns></returns> private TravelingParameters NavigationParameterization(VehicleState vehicleState, DownstreamPointOfInterest dpoi, ArbiterWaypoint final, LinePath fullPath) { // stop waypoint and distance ArbiterWaypoint stopWaypoint = dpoi.PointOfInterest; double distanceToStop = final.Lane.DistanceBetween(final.Position, stopWaypoint.Position) + vehicleState.Front.DistanceTo(this.turn.FinalGeneric.Position); // checks if we need to stop at this point bool isStop = dpoi.IsExit || (dpoi.IsGoal && dpoi.PointOfInterest.IsCheckpoint && dpoi.PointOfInterest.CheckpointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber) && CoreCommon.Mission.MissionCheckpoints.Count == 1); // get next lane type stop List <WaypointType> stopTypes = new List <WaypointType>(); stopTypes.Add(WaypointType.End); stopTypes.Add(WaypointType.Stop); ArbiterWaypoint nextStop = final.Lane.GetNext(final, stopTypes, new List <ArbiterWaypoint>()); // get total distance to lane stop double distanceToLaneStop = final.Lane.DistanceBetween(final.Position, nextStop.Position) + vehicleState.Front.DistanceTo(this.turn.FinalGeneric.Position); // check that we have nearest and correct stop if (!isStop || (isStop && distanceToLaneStop <= distanceToStop)) { stopWaypoint = nextStop; distanceToStop = distanceToLaneStop; } // get speed to stop double stopSpeed = this.StoppingParameters(distanceToStop, final.Lane.Way.Segment.SpeedLimits.MaximumSpeed); // get params TravelingParameters navParams = this.GetParameters(stopSpeed, distanceToStop, final, vehicleState, true); // add to current parames to arbiter information CoreCommon.CurrentInformation.FQMBehavior = navParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FQMBehaviorInfo = navParams.Behavior.ShortBehaviorInformation(); CoreCommon.CurrentInformation.FQMSpeedCommand = navParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FQMDistance = navParams.DistanceToGo.ToString("F6"); CoreCommon.CurrentInformation.FQMSpeed = navParams.RecommendedSpeed.ToString("F6"); CoreCommon.CurrentInformation.FQMState = navParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FQMStateInfo = navParams.NextState.StateInformation(); CoreCommon.CurrentInformation.FQMStopType = "Dpoi: " + dpoi.PointOfInterest.Equals(stopWaypoint) + ", Stop: " + isStop.ToString(); CoreCommon.CurrentInformation.FQMWaypoint = stopWaypoint.ToString(); CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = Math.Min(final.Lane.CurrentMaximumSpeed(vehicleState.Front), this.turn.MaximumDefaultSpeed).ToString("F2"); // return return(navParams); }
/// <summary> /// Gets parameters to follow vehicle /// </summary> /// <param name="vehicleState"></param> /// <param name="final"></param> /// <returns></returns> private TravelingParameters VehicleParameterization(VehicleState vehicleState, LinePath fullPath, ArbiterWaypoint final) { // get simple following Speed and distance for following double followingSpeed; double distanceToGood; double xSeparation; this.forwardMonitor.Follow(vehicleState, fullPath, final.Lane, this.turn, out followingSpeed, out distanceToGood, out xSeparation); // get parameterization TravelingParameters vehicleParams = this.GetParameters(followingSpeed, distanceToGood, final, vehicleState, false); // add to current parames to arbiter information CoreCommon.CurrentInformation.FVTBehavior = vehicleParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FVTSpeed = vehicleParams.RecommendedSpeed.ToString("F3"); CoreCommon.CurrentInformation.FVTSpeedCommand = vehicleParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FVTDistance = vehicleParams.DistanceToGo.ToString("F2"); CoreCommon.CurrentInformation.FVTState = vehicleParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FVTStateInfo = vehicleParams.NextState.StateInformation(); CoreCommon.CurrentInformation.FVTXSeparation = xSeparation.ToString("F3"); CoreCommon.CurrentInformation.FVTIgnorable = this.forwardMonitor.VehiclesToIgnore.ToArray(); // return the parameterization return(vehicleParams); }
/// <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> /// Constructor /// </summary> /// <param name="turnFinal"></param> public LaneEntryAreaMonitor(ArbiterWaypoint turnFinal) { this.turnFinalWaypoint = turnFinal; this.timer = new Stopwatch(); }
/// <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; } } } }
private static void GenerateInterconnectPolygon(ArbiterInterconnect ai) { List <Coordinates> polyPoints = new List <Coordinates>(); // 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 * 1.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; } } }
/// <summary> /// Writes waypoint informaton /// </summary> /// <param name="sw"></param> private void WriteWaypointInformation(StreamWriter sw) { // list of all waypoints List <IArbiterWaypoint> waypoints = new List <IArbiterWaypoint>(); // add all foreach (IArbiterWaypoint iaw in roadNetwork.ArbiterWaypoints.Values) { waypoints.Add(iaw); } // notify sw.WriteLine("NumberOfWaypoints" + "\t" + waypoints.Count.ToString()); sw.WriteLine("Waypoints"); // loop for (int i = 0; i < waypoints.Count; i++) { // stop string isStop = "IsNotStop"; if (waypoints[i] is ArbiterWaypoint && ((ArbiterWaypoint)waypoints[i]).IsStop) { isStop = "IsStop"; } // info sw.WriteLine( waypoints[i].ToString() + "\t" + isStop + "\t" + waypoints[i].Position.X.ToString("F6") + "\t" + waypoints[i].Position.Y.ToString("F6")); List <string> memberPartitions = new List <string>(); if (waypoints[i] is ArbiterWaypoint) { ArbiterWaypoint aw = (ArbiterWaypoint)waypoints[i]; if (aw.NextPartition != null) { memberPartitions.Add(aw.NextPartition.ToString()); } if (aw.PreviousPartition != null) { memberPartitions.Add(aw.PreviousPartition.ToString()); } if (aw.IsExit) { foreach (ArbiterInterconnect ai in aw.Exits) { memberPartitions.Add(ai.ToString()); } } if (aw.IsEntry) { foreach (ArbiterInterconnect ai in aw.Entries) { memberPartitions.Add(ai.ToString()); } } } else if (waypoints[i] is ArbiterPerimeterWaypoint) { ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)waypoints[i]; memberPartitions.Add((new SceneZonePartition(apw.Perimeter.Zone)).ToString()); if (apw.IsExit) { foreach (ArbiterInterconnect ai in apw.Exits) { memberPartitions.Add(ai.ToString()); } } if (apw.IsEntry) { foreach (ArbiterInterconnect ai in apw.Entries) { memberPartitions.Add(ai.ToString()); } } } else if (waypoints[i] is ArbiterParkingSpotWaypoint) { ArbiterParkingSpotWaypoint apsw = (ArbiterParkingSpotWaypoint)waypoints[i]; memberPartitions.Add((new SceneZonePartition(apsw.ParkingSpot.Zone)).ToString()); } sw.WriteLine("NumberOfMemberPartitions" + "\t" + memberPartitions.Count.ToString()); if (memberPartitions.Count > 0) { sw.WriteLine("MemberPartitions"); foreach (string s in memberPartitions) { sw.WriteLine(s); } sw.WriteLine("EndMemberPartitions"); } } // notify sw.WriteLine("End_Waypoints"); }
/// <summary> /// Plans what maneuer we should take next /// </summary> /// <param name="planningState"></param> /// <param name="navigationalPlan"></param> /// <param name="vehicleState"></param> /// <param name="vehicles"></param> /// <param name="obstacles"></param> /// <param name="blockage"></param> /// <returns></returns> public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, List <ITacticalBlockage> blockages) { #region Waiting At Intersection Exit if (planningState is WaitingAtIntersectionExitState) { // state WaitingAtIntersectionExitState waies = (WaitingAtIntersectionExitState)planningState; // get intersection plan IntersectionPlan ip = (IntersectionPlan)navigationalPlan; // nullify turn reasoning this.TurnReasoning = null; #region Intersection Monitor Updates // check correct intersection monitor if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(waies.exitWaypoint.AreaSubtypeWaypointId) && (IntersectionTactical.IntersectionMonitor == null || !IntersectionTactical.IntersectionMonitor.OurMonitor.Waypoint.Equals(waies.exitWaypoint))) { // create new intersection monitor IntersectionTactical.IntersectionMonitor = new IntersectionMonitor( waies.exitWaypoint, CoreCommon.RoadNetwork.IntersectionLookup[waies.exitWaypoint.AreaSubtypeWaypointId], vehicleState, ip.BestOption); } // update if exists if (IntersectionTactical.IntersectionMonitor != null) { // update monitor IntersectionTactical.IntersectionMonitor.Update(vehicleState); // print current ArbiterOutput.Output(IntersectionTactical.IntersectionMonitor.IntersectionStateString()); } #endregion #region Desired Behavior // get best option from previously saved IConnectAreaWaypoints icaw = null; if (waies.desired != null) { ArbiterInterconnect tmpInterconnect = waies.desired; if (waies.desired.InitialGeneric is ArbiterWaypoint) { ArbiterWaypoint init = (ArbiterWaypoint)waies.desired.InitialGeneric; if (init.NextPartition != null && init.NextPartition.Final.Equals(tmpInterconnect.FinalGeneric)) { icaw = init.NextPartition; } else { icaw = waies.desired; } } else { icaw = waies.desired; } } else { icaw = ip.BestOption; waies.desired = icaw.ToInterconnect; } #endregion #region Turn Feasibility Reasoning // check uturn if (waies.desired.TurnDirection == ArbiterTurnDirection.UTurn) { waies.turnTestState = TurnTestState.Completed; } // check already determined feasible if (waies.turnTestState == TurnTestState.Unknown || waies.turnTestState == TurnTestState.Failed) { #region Determine Behavior to Accomplish Turn // get default turn behavior TurnBehavior testTurnBehavior = this.DefaultTurnBehavior(icaw); // set saudi decorator if (waies.saudi != SAUDILevel.None) { testTurnBehavior.Decorators.Add(new ShutUpAndDoItDecorator(waies.saudi)); } // set to ignore all vehicles testTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 }); #endregion #region Check Turn Feasible // check if we have completed CompletionReport turnCompletionReport; bool completedTest = CoreCommon.Communications.TestExecute(testTurnBehavior, out turnCompletionReport); //CoreCommon.Communications.AsynchronousTestHasCompleted(testTurnBehavior, out turnCompletionReport, true); // if we have completed the test if (completedTest || ((TrajectoryBlockedReport)turnCompletionReport).BlockageType != BlockageType.Dynamic) { #region Can Complete // check success if (turnCompletionReport.Result == CompletionResult.Success) { // set completion state of the turn waies.turnTestState = TurnTestState.Completed; } #endregion #region No Saudi Level, Found Initial Blockage // otherwise we cannot do the turn, check if saudi is still none else if (waies.saudi == SAUDILevel.None) { // notify ArbiterOutput.Output("Increased Saudi Level of Turn to L1"); // up the saudi level, set as turn failed and no other option waies.saudi = SAUDILevel.L1; waies.turnTestState = TurnTestState.Failed; } #endregion #region Already at L1 Saudi else if (waies.saudi == SAUDILevel.L1) { // notify ArbiterOutput.Output("Turn with Saudi L1 Level failed"); // get an intersection plan without this interconnect IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect( waies.exitWaypoint, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], waies.desired); // check that the plan exists if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) && testPlan.BestRouteTime < double.MaxValue - 1.0) { // get the desired interconnect ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect; #region Check that the reset interconnect is feasible // test the reset interconnect TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset); // set to ignore all vehicles testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 }); // check if we have completed CompletionReport turnResetCompletionReport; bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport); // check to see if this is feasible if (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95) { // notify ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to current interconnect: " + waies.desired.ToString()); // set the interconnect as being blocked CoreCommon.Navigation.AddInterconnectBlockage(waies.desired); // reset all waies.desired = reset; waies.turnTestState = TurnTestState.Completed; waies.saudi = SAUDILevel.None; waies.useTurnBounds = true; IntersectionMonitor.ResetDesired(reset); } #endregion #region No Lane Bounds // otherwise try without lane bounds else { // notify ArbiterOutput.Output("Had to fallout to using no turn bounds"); // up the saudi level, set as turn failed and no other option waies.saudi = SAUDILevel.L1; waies.turnTestState = TurnTestState.Completed; waies.useTurnBounds = false; } #endregion } #region No Lane Bounds // otherwise try without lane bounds else { // up the saudi level, set as turn failed and no other option waies.saudi = SAUDILevel.L1; waies.turnTestState = TurnTestState.Unknown; waies.useTurnBounds = false; } #endregion } #endregion // want to reset ourselves return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } #endregion } #endregion #region Entry Monitor Blocked // checks the entry monitor vehicle for failure if (IntersectionMonitor != null && IntersectionMonitor.EntryAreaMonitor != null && IntersectionMonitor.EntryAreaMonitor.Vehicle != null && IntersectionMonitor.EntryAreaMonitor.Failed) { ArbiterOutput.Output("Entry area blocked"); // get an intersection plan without this interconnect IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect( waies.exitWaypoint, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], waies.desired, true); // check that the plan exists if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) && testPlan.BestRouteTime < double.MaxValue - 1.0) { // get the desired interconnect ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect; #region Check that the reset interconnect is feasible // test the reset interconnect TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset); // set to ignore all vehicles testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 }); // check if we have completed CompletionReport turnResetCompletionReport; bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport); // check to see if this is feasible if (reset.TurnDirection == ArbiterTurnDirection.UTurn || (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95)) { // notify ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to all possible turns into final"); // set all the interconnects to the final as being blocked if (((ITraversableWaypoint)waies.desired.FinalGeneric).IsEntry) { foreach (ArbiterInterconnect toBlock in ((ITraversableWaypoint)waies.desired.FinalGeneric).Entries) { CoreCommon.Navigation.AddInterconnectBlockage(toBlock); } } // check if exists previous partition to block if (waies.desired.FinalGeneric is ArbiterWaypoint) { ArbiterWaypoint finWaypoint = (ArbiterWaypoint)waies.desired.FinalGeneric; if (finWaypoint.PreviousPartition != null) { CoreCommon.Navigation.AddBlockage(finWaypoint.PreviousPartition, finWaypoint.Position, false); } } // reset all waies.desired = reset; waies.turnTestState = TurnTestState.Completed; waies.saudi = SAUDILevel.None; waies.useTurnBounds = true; IntersectionMonitor.ResetDesired(reset); // want to reset ourselves return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } #endregion } else { ArbiterOutput.Output("Entry area blocked, but no otehr valid route found"); } } #endregion // check if can traverse if (IntersectionTactical.IntersectionMonitor == null || IntersectionTactical.IntersectionMonitor.CanTraverse(icaw, vehicleState)) { #region If can traverse the intersection // quick check not interconnect if (!(icaw is ArbiterInterconnect)) { icaw = icaw.ToInterconnect; } // get interconnect ArbiterInterconnect ai = (ArbiterInterconnect)icaw; // clear all old completion reports CoreCommon.Communications.ClearCompletionReports(); // check if uturn if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint && ai.TurnDirection == ArbiterTurnDirection.UTurn) { // go into turn List <ArbiterLane> involvedLanes = new List <ArbiterLane>(); involvedLanes.Add(((ArbiterWaypoint)ai.InitialGeneric).Lane); involvedLanes.Add(((ArbiterWaypoint)ai.FinalGeneric).Lane); uTurnState nextState = new uTurnState(((ArbiterWaypoint)ai.FinalGeneric).Lane, IntersectionToolkit.uTurnBounds(vehicleState, involvedLanes)); nextState.Interconnect = ai; // hold brake Behavior b = new HoldBrakeBehavior(); // return maneuver return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp)); } else { if (ai.FinalGeneric is ArbiterWaypoint) { ArbiterWaypoint finalWaypoint = (ArbiterWaypoint)ai.FinalGeneric; // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.TurnInfo(finalWaypoint, out finalPath, out leftLL, out rightLL); // go into turn IState nextState = new TurnState(ai, ai.TurnDirection, finalWaypoint.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds); // hold brake Behavior b = new HoldBrakeBehavior(); // return maneuver return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp)); } else { // final perimeter waypoint ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric; // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.ZoneTurnInfo(ai, apw, out finalPath, out leftLL, out rightLL); // go into turn IState nextState = new TurnState(ai, ai.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds); // hold brake Behavior b = new HoldBrakeBehavior(); // return maneuver return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp)); } } #endregion } // otherwise need to wait else { IState next = waies; return(new Maneuver(new HoldBrakeBehavior(), next, next.DefaultStateDecorators, vehicleState.Timestamp)); } } #endregion #region Stopping At Exit else if (planningState is StoppingAtExitState) { // cast to exit stopping StoppingAtExitState saes = (StoppingAtExitState)planningState; saes.currentPosition = vehicleState.Front; // get intersection plan IntersectionPlan ip = (IntersectionPlan)navigationalPlan; // if has an intersection if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(saes.waypoint.AreaSubtypeWaypointId)) { // create new intersection monitor IntersectionTactical.IntersectionMonitor = new IntersectionMonitor( saes.waypoint, CoreCommon.RoadNetwork.IntersectionLookup[saes.waypoint.AreaSubtypeWaypointId], vehicleState, ip.BestOption); // update it IntersectionTactical.IntersectionMonitor.Update(vehicleState); } else { IntersectionTactical.IntersectionMonitor = null; } // otherwise update the stop parameters saes.currentPosition = vehicleState.Front; Behavior b = saes.Resume(vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); return(new Maneuver(b, saes, saes.DefaultStateDecorators, vehicleState.Timestamp)); } #endregion #region In uTurn else if (planningState is uTurnState) { // get state uTurnState uts = (uTurnState)planningState; // check if in other lane if (CoreCommon.Communications.HasCompleted((new UTurnBehavior(null, null, null, null)).GetType())) { // quick check if (uts.Interconnect != null && uts.Interconnect.FinalGeneric is ArbiterWaypoint) { // get the closest partition to the new lane ArbiterLanePartition alpClose = uts.TargetLane.GetClosestPartition(vehicleState.Front); // waypoints ArbiterWaypoint partitionInitial = alpClose.Initial; ArbiterWaypoint uturnEnd = (ArbiterWaypoint)uts.Interconnect.FinalGeneric; // check initial past end if (partitionInitial.WaypointId.Number > uturnEnd.WaypointId.Number) { // get waypoints inclusive List <ArbiterWaypoint> inclusive = uts.TargetLane.WaypointsInclusive(uturnEnd, partitionInitial); bool found = false; // loop through foreach (ArbiterWaypoint aw in inclusive) { if (!found && aw.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId)) { // notiofy ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as passed over in uturn"); // remove CoreCommon.Mission.MissionCheckpoints.Dequeue(); // set found found = true; } } } // default check else if (uts.Interconnect.FinalGeneric.Equals(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId])) { // notiofy ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as end of uturn"); // remove CoreCommon.Mission.MissionCheckpoints.Dequeue(); } } // check if the uturn is for a blockage else if (uts.Interconnect == null) { // get final lane ArbiterLane targetLane = uts.TargetLane; // check has opposing if (targetLane.Way.Segment.Lanes.Count > 1) { // check the final checkpoint is in our lane if (CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.AreaSubtypeId.Equals(targetLane.LaneId)) { // check that the final checkpoint is within the uturn polygon if (uts.Polygon != null && uts.Polygon.IsInside(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId].Position)) { // remove the checkpoint ArbiterOutput.Output("Found checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + " inside blockage uturn area, dequeuing"); CoreCommon.Mission.MissionCheckpoints.Dequeue(); } } } } // stay in target lane IState nextState = new StayInLaneState(uts.TargetLane, new Probability(0.8, 0.2), true, CoreCommon.CorePlanningState); Behavior b = new StayInLaneBehavior(uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0), new List <int>(), uts.TargetLane.LanePath(), uts.TargetLane.Width, uts.TargetLane.NumberOfLanesLeft(vehicleState.Front, true), uts.TargetLane.NumberOfLanesRight(vehicleState.Front, true)); return(new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } // otherwise continue uturn else { // get polygon Polygon p = uts.Polygon; // add polygon to observable CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(p, ArbiterInformationDisplayObjectType.uTurnPolygon)); // check the type of uturn if (!uts.singleLaneUturn) { // get ending path LinePath endingPath = uts.TargetLane.LanePath(); // next state is current IState nextState = uts; // behavior Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0)); // maneuver return(new Maneuver(b, nextState, null, vehicleState.Timestamp)); } else { // get ending path LinePath endingPath = uts.TargetLane.LanePath().Clone(); endingPath = endingPath.ShiftLateral(-2.0); //uts.TargetLane.Width); // add polygon to observable CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(endingPath, ArbiterInformationDisplayObjectType.leftBound)); // next state is current IState nextState = uts; // behavior Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0)); // maneuver return(new Maneuver(b, nextState, null, vehicleState.Timestamp)); } } } #endregion #region In Turn else if (planningState is TurnState) { // get state TurnState ts = (TurnState)planningState; // add bounds to observable if (ts.LeftBound != null && ts.RightBound != null) { CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.LeftBound, ArbiterInformationDisplayObjectType.leftBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.RightBound, ArbiterInformationDisplayObjectType.rightBound)); } // create current turn reasoning if (this.TurnReasoning == null) { this.TurnReasoning = new TurnReasoning(ts.Interconnect, IntersectionTactical.IntersectionMonitor != null ? IntersectionTactical.IntersectionMonitor.EntryAreaMonitor : null); } // get primary maneuver Maneuver primary = this.TurnReasoning.PrimaryManeuver(vehicleState, blockages, ts); // get secondary maneuver Maneuver?secondary = this.TurnReasoning.SecondaryManeuver(vehicleState, (IntersectionPlan)navigationalPlan); // return the manevuer return(secondary.HasValue ? secondary.Value : primary); } #endregion #region Itnersection Startup else if (planningState is IntersectionStartupState) { // state and plan IntersectionStartupState iss = (IntersectionStartupState)planningState; IntersectionStartupPlan isp = (IntersectionStartupPlan)navigationalPlan; // initial path LinePath vehiclePath = new LinePath(new Coordinates[] { vehicleState.Rear, vehicleState.Front }); List <ITraversableWaypoint> feasibleEntries = new List <ITraversableWaypoint>(); // vehicle polygon forward of us Polygon vehicleForward = vehicleState.ForwardPolygon; // best waypoint ITraversableWaypoint best = null; double bestCost = Double.MaxValue; // given feasible choose best, no feasible choose random if (feasibleEntries.Count == 0) { foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values) { if (vehicleForward.IsInside(itw.Position)) { feasibleEntries.Add(itw); } } if (feasibleEntries.Count == 0) { foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values) { feasibleEntries.Add(itw); } } } // get best foreach (ITraversableWaypoint itw in feasibleEntries) { if (isp.NodeTimeCosts.ContainsKey(itw)) { KeyValuePair <ITraversableWaypoint, double> lookup = new KeyValuePair <ITraversableWaypoint, double>(itw, isp.NodeTimeCosts[itw]); if (best == null || lookup.Value < bestCost) { best = lookup.Key; bestCost = lookup.Value; } } } // get something going to this waypoint ArbiterInterconnect interconnect = null; if (best.IsEntry) { ArbiterInterconnect closest = null; double closestDistance = double.MaxValue; foreach (ArbiterInterconnect ai in best.Entries) { double dist = ai.InterconnectPath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front); if (closest == null || dist < closestDistance) { closest = ai; closestDistance = dist; } } interconnect = closest; } else if (best is ArbiterWaypoint && ((ArbiterWaypoint)best).PreviousPartition != null) { interconnect = ((ArbiterWaypoint)best).PreviousPartition.ToInterconnect; } // get state if (best is ArbiterWaypoint) { // go to this turn state LinePath finalPath; LineList leftBound; LineList rightBound; IntersectionToolkit.TurnInfo((ArbiterWaypoint)best, out finalPath, out leftBound, out rightBound); return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, ((ArbiterWaypoint)best).Lane, finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp)); } else { // go to this turn state LinePath finalPath; LineList leftBound; LineList rightBound; IntersectionToolkit.ZoneTurnInfo(interconnect, (ArbiterPerimeterWaypoint)best, out finalPath, out leftBound, out rightBound); return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, null, finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp)); } } #endregion #region Unknown else { throw new Exception("Unknown planning state in intersection tactical plan: " + planningState.ToString()); } #endregion }