internal void OnSkillCasted(TurnBehavior caster) { if (NotifySkillCasted != null) { NotifySkillCasted(caster); } }
public override void Initialize(Behavior b) { base.Initialize(b); Services.ObstaclePipeline.ExtraSpacing = 0; Services.ObstaclePipeline.UseOccupancyGrid = true; // extract the relevant information TurnBehavior cb = (TurnBehavior)b; targetLaneID = cb.TargetLane; // create a fake start lane so we can do the intersection pull stuff pseudoStartLane = new LinePath(); pseudoStartLane.Add(new Coordinates(-1, 0)); pseudoStartLane.Add(Coordinates.Zero); AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp); pseudoStartLane = pseudoStartLane.Transform(absTransform.Invert()); HandleTurnBehavior(cb); curTimestamp = cb.TimeStamp; // do an initial plan without obstacle avoidance DoInitialPlan(); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "turn behavior - initialized"); }
private void AddCharacter(GameObject character) { TurnBehavior behavior = character.GetComponent <TurnBehavior>(); if (behavior == null) { Debug.LogError("All characters you register must have a subclass of TurnBehavior on it!"); } string team = behavior.GetTeam(); bool newTeam = true; foreach (var teamList in teams) { if (teamList[0].GetComponent <TurnBehavior>().team == team) { teamList.Add(character); newTeam = false; } } if (newTeam) { var newList = new List <GameObject>(); newList.Add(character); if (teams.Count == 0) { CurrentCharacter = character; } teams.Add(newList); } }
private TurnBehavior DefaultTurnBehavior(IConnectAreaWaypoints icaw) { #region Determine Behavior to Accomplish Turn // get interconnect ArbiterInterconnect ai = icaw.ToInterconnect; // behavior we wish to accomplish TurnBehavior testTurnBehavior = null; TurnState testTurnState = null; #region Turn to Lane if (ai.FinalGeneric is ArbiterWaypoint) { // get final wp 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 testTurnState = new TurnState(ai, ai.TurnDirection, finalWaypoint.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5)); } #endregion #region Turn to Zone 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 testTurnState = new TurnState(ai, ai.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5)); } #endregion // get behavior testTurnBehavior = (TurnBehavior)testTurnState.Resume(CoreCommon.Communications.GetVehicleState(), CoreCommon.Communications.GetVehicleSpeed().Value); testTurnBehavior.TimeStamp = CoreCommon.Communications.GetVehicleState().Timestamp; // return the behavior return(testTurnBehavior); #endregion }
/// <summary> /// Resume from pause /// </summary> /// <returns></returns> public Behavior Resume(VehicleState currentState, double speed) { // turn behavior into chute TurnBehavior b = new TurnBehavior(this.Final.Lane.LaneId, this.Final.PartitionPath, this.Final.PartitionPath.ShiftLateral(this.Final.Lane.Width / 2.0), this.Final.PartitionPath.ShiftLateral(-this.Final.Lane.Width / 2.0), new ScalarSpeedCommand(1.4), null); // return behavior return(b); }
private void HandleTurnBehavior(TurnBehavior cb) { // get the transformer to take us from absolute to relative coordinates AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp); targetPath = cb.TargetLanePath.Transform(absTransform).RemoveZeroLengthSegments(); if (cb.LeftBound != null) { leftBound = new LinePath(cb.LeftBound).Transform(absTransform); } else { leftBound = null; } if (cb.RightBound != null) { rightBound = new LinePath(cb.RightBound).Transform(absTransform); } else { rightBound = null; } if (targetPath.PathLength < 12) { double pathLength = targetPath.PathLength; double ext = 12 - pathLength; targetPath.Add(targetPath.EndPoint.Location + targetPath.EndSegment.Vector.Normalize(ext)); } behaviorTimestamp = absTransform.Timestamp; if (cb.IntersectionPolygon != null) { intersectionPolygon = cb.IntersectionPolygon.Transform(absTransform); // set the polygon to the ui Services.UIService.PushPolygon(cb.IntersectionPolygon, cb.TimeStamp, "intersection polygon", false); } else { intersectionPolygon = null; } this.ignorableObstacles = cb.IgnorableObstacles; Services.ObstaclePipeline.LastIgnoredObstacles = cb.IgnorableObstacles; HandleSpeedCommand(cb.SpeedCommand); }
public UrbanChallenge.Behaviors.Behavior Resume(VehicleState currentState, double speed) { TurnBehavior turnBehavior = null; if (TargetLane != null) { turnBehavior = new TurnBehavior(TargetLane.LaneId, EndingPath, LeftBound, RightBound, SpeedCommand, this.Interconnect.InterconnectId); } else { turnBehavior = new TurnBehavior(null, EndingPath, LeftBound, RightBound, SpeedCommand, this.Interconnect.InterconnectId); } turnBehavior.TimeStamp = currentState.Timestamp; return(turnBehavior); }
public override void OnBehaviorReceived(Behavior b) { if (b is TurnBehavior) { TurnBehavior cb = (TurnBehavior)b; if (object.Equals(cb.TargetLane, targetLaneID)) { Services.BehaviorManager.QueueParam(cb); } else { Services.BehaviorManager.Execute(b, null, false); } } else { Services.BehaviorManager.Execute(b, null, false); } }
public override void ExecuteBehavior(Behavior behavior, Common.Coordinates location, RndfWaypointID lowerBound, RndfWaypointID upperBound) { if (behavior is UTurnBehavior) { // } // check to see if we are at the upper bound RndfWayPoint upperWaypoint = channelListener.RndfNetwork.Waypoints[upperBound]; if (!(behavior is TurnBehavior) && upperWaypoint.NextLanePartition != null && location.DistanceTo(upperWaypoint.Position) < 3) { lowerBound = upperWaypoint.WaypointID; upperBound = upperWaypoint.NextLanePartition.FinalWaypoint.WaypointID; } Console.WriteLine(" > Received Instruction to Execute Behavior: " + behavior.ToString()); if (behavior is StayInLaneBehavior) { StayInLaneBehavior stayInLane = (StayInLaneBehavior)behavior; if (stayInLane.SpeedCommand is StopLineLaneSpeedCommand) { StopLineLaneSpeedCommand speedCommand = (StopLineLaneSpeedCommand)stayInLane.SpeedCommand; if (speedCommand.Distance < 2) { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = location; LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1); List <LaneEstimate> laneEstimates = new List <LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 0; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); ///Console.WriteLine(" > Published Position"); } else { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[upperBound].Position; LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1); List <LaneEstimate> laneEstimates = new List <LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 3; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); ///Console.WriteLine(" > Published Position"); } } else if (stayInLane.SpeedCommand is StopLaneSpeedCommand) { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = location; LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1); List <LaneEstimate> laneEstimates = new List <LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = -5; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); ///Console.WriteLine(" > Published Position"); } else if (stayInLane.SpeedCommand is DefaultLaneSpeedCommand) { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[upperBound].Position; LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1); List <LaneEstimate> laneEstimates = new List <LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 3; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); //Console.WriteLine(" > Published Position"); } else { throw new ArgumentException("Unknown Lane Speed Type", "stayInLane.SpeedCommand"); } } // TODO: include midway point else if (behavior is TurnBehavior) { TurnBehavior currentBehavior = (TurnBehavior)behavior; RndfWayPoint exitWaypoint = channelListener.RndfNetwork.Waypoints[currentBehavior.ExitPoint]; if (location.DistanceTo(exitWaypoint.Position) < 0.1) { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); RndfWayPoint entryWaypoint = channelListener.RndfNetwork.Waypoints[currentBehavior.EntryPoint]; Common.Coordinates change = entryWaypoint.Position - exitWaypoint.Position; Common.Coordinates midpoint = exitWaypoint.Position + change / 2; LaneEstimate laneEstimate = new LaneEstimate(currentBehavior.ExitPoint.LaneID, currentBehavior.EntryPoint.LaneID, 1); vehicleState.xyPosition = midpoint; List <LaneEstimate> laneEstimates = new List <LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 3; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); } else { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[currentBehavior.EntryPoint].Position; LaneEstimate laneEstimate = new LaneEstimate(currentBehavior.EntryPoint.LaneID, currentBehavior.EntryPoint.LaneID, 1); List <LaneEstimate> laneEstimates = new List <LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 3; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); //Console.WriteLine(" > Published Position"); } } else { throw new ArgumentException("Unknown Behavior Type", "behavior"); } //Console.WriteLine("Sent Back Position \n"); }
protected virtual void Initialize() { behavior = GetComponent <TurnBehavior>(); mainCamera = Camera.main; useVisuals = behavior.visualIndicationOfSkills; }
public override void Process(object param) { if (!base.BeginProcess()) { return; } // check if we're given a param if (param != null && param is TurnBehavior) { TurnBehavior turnParam = (TurnBehavior)param; HandleTurnBehavior(turnParam); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "turn behavior - got new param, speed command {0}", turnParam.SpeedCommand); } LinePath curTargetPath = targetPath.Clone(); LinePath curLeftBound = null; if (leftBound != null) { curLeftBound = leftBound.Clone(); } LinePath curRightBound = null; if (rightBound != null) { curRightBound = rightBound.Clone(); } // transform the path into the current timestamp if (behaviorTimestamp != curTimestamp) { // get the relative transform RelativeTransform relTransform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in turn behavior, transforming from {0}->{1}, wanted {2}->{3}", relTransform.OriginTimestamp, relTransform.EndTimestamp, behaviorTimestamp, curTimestamp); curTargetPath.TransformInPlace(relTransform); if (curLeftBound != null) { curLeftBound.TransformInPlace(relTransform); } if (curRightBound != null) { curRightBound.TransformInPlace(relTransform); } } // get the distance between our current location and the start point double distToStart = Coordinates.Zero.DistanceTo(curTargetPath[0]); double startDist = Math.Max(0, TahoeParams.FL - distToStart); // extract off the first 5 m of the target path LinePath.PointOnPath endTarget = curTargetPath.AdvancePoint(curTargetPath.StartPoint, TahoeParams.VL); curTargetPath = curTargetPath.SubPath(curTargetPath.StartPoint, endTarget); curTargetPath = curTargetPath.RemoveZeroLengthSegments(); // adjust the left bound and right bounds starting distance if (curLeftBound != null) { LinePath.PointOnPath leftBoundStart = curLeftBound.AdvancePoint(curLeftBound.StartPoint, 2); curLeftBound = curLeftBound.SubPath(leftBoundStart, curLeftBound.EndPoint); AddLeftBound(curLeftBound, false); Services.UIService.PushLineList(curLeftBound, curTimestamp, "left bound", true); } if (curRightBound != null) { LinePath.PointOnPath rightBoundStart = curRightBound.AdvancePoint(curRightBound.StartPoint, 2); curRightBound = curRightBound.SubPath(rightBoundStart, curRightBound.EndPoint); AddRightBound(curRightBound, false); Services.UIService.PushLineList(curRightBound, curTimestamp, "right bound", true); } if (cancelled) { return; } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in turn behavior, dist to start: {0}", distToStart); if (distToStart < TahoeParams.FL * 0.75) { BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in turn behavior, past start point of target path"); // return a completion report ForwardCompletionReport(new SuccessCompletionReport(typeof(TurnBehavior))); } AddTargetPath(curTargetPath.Clone(), 0.005); // transform the pseudo start lane to the current timestamp AbsoluteTransformer trans = Services.StateProvider.GetAbsoluteTransformer(curTimestamp); LinePath curStartPath = pseudoStartLane.Transform(trans); // add the intersection pull path LinePath intersectionPullPath = new LinePath(); double pullWeight = 0; GetIntersectionPullPath(curStartPath, curTargetPath, intersectionPolygon, true, true, intersectionPullPath, ref pullWeight); if (intersectionPullPath.Count > 0) { AddTargetPath(intersectionPullPath, pullWeight); } // set up planning details // add our position to the current target path LinePath origTargetPath = curTargetPath.Clone(); curTargetPath.Insert(0, new Coordinates(0, 0)); //curTargetPath.Insert(1, new Coordinates(1, 0)); smootherBasePath = curTargetPath; // add the bounds // calculate max speed settings.maxSpeed = GetMaxSpeed(null, LinePath.PointOnPath.Invalid); settings.Options.w_diff = 4; BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed); if (cancelled) { return; } Services.UIService.PushLineList(curTargetPath, curTimestamp, "subpath", true); // set the avoidance path to the previously planned path // transform to the current timestamp disablePathAngleCheck = true; if (turnBasePath != null) { avoidanceBasePath = turnBasePath.Transform(trans); avoidanceBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.GetClosestPoint(Coordinates.Zero), avoidanceBasePath.EndPoint); maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.GetPointOnPath(1); if (avoidanceBasePath.PathLength > 7.5) { smootherBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.StartPoint, avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -7.5)); } else { smootherBasePath = avoidanceBasePath.Clone(); } disablePathAngleCheck = false; } // fill in auxiliary settings if (curLeftBound != null || curRightBound != null) { settings.endingHeading = curTargetPath.EndSegment.UnitVector.ArcTan; } settings.endingPositionFixed = false; settings.endingPositionMax = 16; settings.endingPositionMin = -16; if (curLeftBound != null && curLeftBound.Count > 0 && curTargetPath.Count > 0) { double leftWidth = curLeftBound[0].DistanceTo(origTargetPath[0]) - TahoeParams.T / 2; settings.endingPositionMax = leftWidth; settings.endingPositionFixed = true; } if (curRightBound != null && curRightBound.Count > 0 && curTargetPath.Count > 0) { double rightWidth = curRightBound[0].DistanceTo(origTargetPath[0]) - TahoeParams.T / 2; settings.endingPositionFixed = true; settings.endingPositionMin = -rightWidth; } //disablePathAngleCheck = true; //useAvoidancePath = true; // do the planning SmoothAndTrack(commandLabel, true); }
/// <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> /// 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> /// 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 }