/// <summary> /// Generic plan /// </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, double vehicleSpeed) { // update stuff this.Update(vehicles, vehicleState); #region Plan Roads if (planningState is TravelState) { Maneuver roadFinal = roadTactical.Plan( planningState, (RoadPlan)navigationalPlan, vehicleState, vehicles, obstacles, blockages, vehicleSpeed); // return return(roadFinal); } #endregion #region Plan Intersection else if (planningState is IntersectionState) { Maneuver intersectionFinal = intersectionTactical.Plan( planningState, navigationalPlan, vehicleState, vehicles, obstacles, blockages); // return return(intersectionFinal); } #endregion #region Plan Zone else if (planningState is ZoneState) { Maneuver zoneFinal = zoneTactical.Plan( planningState, navigationalPlan, vehicleState, vehicles, obstacles, blockages); // return return(zoneFinal); } #endregion #region Plan Blockage else if (planningState is BlockageState) { Maneuver blockageFinal = blockageTactical.Plan( planningState, vehicleState, vehicleSpeed, blockages, navigationalPlan); return(blockageFinal); } #endregion #region Unknown else { throw new Exception("Unknown planning state type: " + planningState.GetType().ToString()); } #endregion }
/// <summary> /// Plan the maneuver /// </summary> /// <param name="planningState"></param> /// <param name="vehicleState"></param> /// <param name="vehicleSpeed"></param> /// <returns></returns> public Maneuver Plan(IState planningState, VehicleState vehicleState, double vehicleSpeed, List<ITacticalBlockage> blockages, INavigationalPlan plan) { #region Encountered a Blockage // something is blocked if (CoreCommon.CorePlanningState is EncounteredBlockageState) { // cast blockage state EncounteredBlockageState ebs = (EncounteredBlockageState)CoreCommon.CorePlanningState; // get the saudi level SAUDILevel saudi = ebs.Saudi; // get the defcon level BlockageRecoveryDEFCON defcon = ebs.Defcon; #region Stay In Lane Blockage if (ebs.PlanningState is StayInLaneState) { // get the recovery state BlockageRecoveryState brs = new BlockageRecoveryState(null, ebs.PlanningState, ebs.PlanningState, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED); // set cooldown BlockageHandler.SetDefaultBlockageCooldown(); // return return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp); } #endregion #region Supra Lane Blockage else if (ebs.PlanningState is StayInSupraLaneState) { // get all supra lane info StayInSupraLaneState sisls = (StayInSupraLaneState)ebs.PlanningState; SupraLane sl = sisls.Lane; // the closest component of the supra lane to us is the initial lane if (sl.ClosestComponent(vehicleState.Front) == SLComponentType.Initial) { // get stay in lane state StayInLaneState sils = new StayInLaneState(sl.Initial, CoreCommon.CorePlanningState); ebs.PlanningState = sils; // get the recovery state BlockageRecoveryState brs = new BlockageRecoveryState(null, sils, sils, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED); // set cooldown BlockageHandler.SetDefaultBlockageCooldown(); // return return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // check if closest part of the supra lane is the final lane else if (sl.ClosestComponent(vehicleState.Front) == SLComponentType.Final) { // get stay in lane state StayInLaneState sils = new StayInLaneState(sl.Final, CoreCommon.CorePlanningState); ebs.PlanningState = sils; // get the recovery state BlockageRecoveryState brs = new BlockageRecoveryState(null, sils, sils, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED); // set cooldown BlockageHandler.SetDefaultBlockageCooldown(); // return return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // the closest component of the supra lane to us is the interconnect otherwise else { // get turn info LinePath fP; LineList lB; LineList rB; IntersectionToolkit.TurnInfo(((ArbiterWaypoint)sl.Interconnect.FinalGeneric), out fP, out lB, out rB); // make sure to update the turn reasoning this.tacticalUmbrella.intersectionTactical.TurnReasoning = new TurnReasoning(sl.Interconnect, null); // get turn state TurnState ts = new TurnState(sl.Interconnect, sl.Interconnect.TurnDirection, sl.Final, fP, lB, rB, new ScalarSpeedCommand(sl.Interconnect.MaximumDefaultSpeed)); ebs.PlanningState = ts; // get final lane state StayInLaneState sils = new StayInLaneState(sl.Final, CoreCommon.CorePlanningState); // get the recovery state BlockageRecoveryState brs = new BlockageRecoveryState(null, sils, ts, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED); // set cooldown BlockageHandler.SetDefaultBlockageCooldown(); // return return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } #endregion #region Change Lanes Blockage else if (ebs.PlanningState is ChangeLanesState) { // get lane change state ChangeLanesState cls = (ChangeLanesState)ebs.PlanningState; // figure out which lane we are closest to being inside ArbiterLane goLane = cls.Parameters.Initial.LanePolygon.IsInside(vehicleState.Front) ? cls.Parameters.Initial : cls.Parameters.Target; // check if lane is oncoming bool goLaneOncoming = cls.Parameters.Initial.LanePolygon.IsInside(vehicleState.Front) ? cls.Parameters.InitialOncoming : cls.Parameters.TargetOncoming; // normal if (!goLaneOncoming) { // get lane state StayInLaneState sils = new StayInLaneState(goLane, CoreCommon.CorePlanningState); ebs.PlanningState = sils; // get the recovery state BlockageRecoveryState brs = new BlockageRecoveryState(null, sils, sils, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED); // set cooldown BlockageHandler.SetDefaultBlockageCooldown(); // return return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // opposing else { // get lane state OpposingLanesState ols = new OpposingLanesState(goLane, true, CoreCommon.CorePlanningState, vehicleState); ebs.PlanningState = ols; // get the recovery state BlockageRecoveryState brs = new BlockageRecoveryState(null, ols, ols, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED); // set cooldown BlockageHandler.SetDefaultBlockageCooldown(); // return return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } #endregion #region Turn Blockage else if (ebs.PlanningState is TurnState) { // get turn state TurnState ts = (TurnState)ebs.PlanningState; // notify ArbiterOutput.Output("Encountered turn blockage, entering blockage recovery state"); // get the recovery state BlockageRecoveryState brs = new BlockageRecoveryState(null, ts, ts, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED); // set cooldown BlockageHandler.SetDefaultBlockageCooldown(); // return return new Maneuver(new HoldBrakeBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp); } #endregion #region Opposing Lanes Blockage else if (ebs.PlanningState is OpposingLanesState) { // get the recovery state BlockageRecoveryState brs = new BlockageRecoveryState(null, ebs.PlanningState, ebs.PlanningState, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED); // set cooldown BlockageHandler.SetDefaultBlockageCooldown(); // return return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp); } #endregion #region Fallout // plan through the blockage state Maneuver final = new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); // return the final maneuver return final; #endregion } #endregion #region Blockage Recovery State // recover from blockages else if (CoreCommon.CorePlanningState is BlockageRecoveryState) { // get the blockage recovery state BlockageRecoveryState brs = (BlockageRecoveryState)CoreCommon.CorePlanningState; #region Blockage Encountered if (brs.RecoveryStatus == BlockageRecoverySTATUS.ENCOUNTERED) { // check encoutnered state for a turn if (brs.EncounteredState.PlanningState is TurnState) { // reset timing this.Reset(); // return the turn recovery maneuver TurnState ts = (TurnState)brs.EncounteredState.PlanningState; return this.TurnRecoveryManeuver(ts.Interconnect, vehicleState, vehicleSpeed, brs); } // check encountered state for a lane else if (brs.EncounteredState.PlanningState is StayInLaneState) { // reset execution timer ExecutionTimer.Stop(); ExecutionTimer.Reset(); // lane blockage recovery bool fakeWait; StayInLaneState sils = (StayInLaneState)brs.EncounteredState.PlanningState; return this.LaneRecoveryManeuver(sils.Lane, vehicleState, vehicleSpeed, plan, brs, false, out fakeWait); } // check encountered state for opposing lane else if (brs.EncounteredState.PlanningState is OpposingLanesState) { // reset timing this.Reset(); // reset timing OpposingLanesState ols = (OpposingLanesState)brs.EncounteredState.PlanningState; return this.OpposingLaneRecoveryManeuver(ols.OpposingLane, vehicleState, vehicleSpeed, plan, brs, false); } } #endregion #region Blockage Recovery Executing else if (brs.RecoveryStatus == BlockageRecoverySTATUS.EXECUTING) { // reset uturn timer uTurnTimer.Stop(); uTurnTimer.Reset(); // check if lane change if (brs.RecoveryBehavior is ChangeLaneBehavior) { // reset timing this.Reset(); // bypass ArbiterOutput.Output("Executing behavior of type: " + brs.RecoveryBehavior.GetType().ToString() + ", waiting for completion"); return new Maneuver(brs.RecoveryBehavior, CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } else { // check execution timer if (!ExecutionTimer.IsRunning) { ExecutionTimer.Stop(); ExecutionTimer.Reset(); ExecutionTimer.Start(); } // check too long if (ExecutionTimer.ElapsedMilliseconds / 1000.0 > 25) { // reset execution timer ExecutionTimer.Stop(); ExecutionTimer.Reset(); // bypass // notify ArbiterOutput.Output("Blockage recovery execution timeout, bypassing to send hold brake"); try { CoreCommon.Communications.Execute(new HoldBrakeBehavior()); Thread.Sleep(100); } catch (Exception) { } // notify ArbiterOutput.Output("Blockage recovery execution timeout, resending behavior"); // resend recovery behavior return new Maneuver(brs.RecoveryBehavior, CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // bypass ArbiterOutput.Output("Executing behavior of type: " + brs.RecoveryBehavior.GetType().ToString() + ", Currently Sending Null Behavior, waiting for completion"); return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } #endregion #region Blockage Recovery Behavior Completed else if (brs.RecoveryStatus == BlockageRecoverySTATUS.COMPLETED) { // reset timing this.Reset(); // check encoutnered state for a turn if (brs.EncounteredState.PlanningState is TurnState) { // return the turn recovery maneuver TurnState ts = (TurnState)brs.EncounteredState.PlanningState; return this.TurnRecoveryManeuver(ts.Interconnect, vehicleState, vehicleSpeed, brs); } // check lane state else if (brs.EncounteredState.PlanningState is StayInLaneState) { // return the completion state brs.RecoveryStatus = BlockageRecoverySTATUS.ENCOUNTERED; return new Maneuver(new HoldBrakeBehavior(), brs.CompletionState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // check opposing else if(brs.EncounteredState.PlanningState is OpposingLanesState) { // return the completion state brs.RecoveryStatus = BlockageRecoverySTATUS.ENCOUNTERED; return new Maneuver(new HoldBrakeBehavior(), brs.CompletionState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } #endregion #region Fallout Of Recovery // reset timing this.Reset(); ArbiterOutput.Output("Fell out of blockage recovery state: recovery status: " + brs.RecoveryStatus.ToString()); return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, brs.RecoveryBehavior != null ? brs.RecoveryBehavior.Decorators : TurnDecorators.NoDecorators, vehicleState.Timestamp); #endregion } #endregion #region Unknown else { throw new Exception("Unknown blockage planning state type"); } #endregion }
/// <summary> /// Generic plan /// </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, double vehicleSpeed) { // update stuff this.Update(vehicles, vehicleState); #region Plan Roads if (planningState is TravelState) { Maneuver roadFinal = roadTactical.Plan( planningState, (RoadPlan)navigationalPlan, vehicleState, vehicles, obstacles, blockages, vehicleSpeed); // return return roadFinal; } #endregion #region Plan Intersection else if (planningState is IntersectionState) { Maneuver intersectionFinal = intersectionTactical.Plan( planningState, navigationalPlan, vehicleState, vehicles, obstacles, blockages); // return return intersectionFinal; } #endregion #region Plan Zone else if (planningState is ZoneState) { Maneuver zoneFinal = zoneTactical.Plan( planningState, navigationalPlan, vehicleState, vehicles, obstacles, blockages); // return return zoneFinal; } #endregion #region Plan Blockage else if (planningState is BlockageState) { Maneuver blockageFinal = blockageTactical.Plan( planningState, vehicleState, vehicleSpeed, blockages, navigationalPlan); return blockageFinal; } #endregion #region Unknown else { throw new Exception("Unknown planning state type: " + planningState.GetType().ToString()); } #endregion }
/// <summary> /// Maneuver for recovering from a lane blockage, used for lane changes as well /// </summary> /// <param name="lane"></param> /// <param name="vehicleState"></param> /// <param name="vehicleSpeed"></param> /// <param name="defcon"></param> /// <param name="saudi"></param> /// <returns></returns> public Maneuver LaneRecoveryManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, INavigationalPlan plan, BlockageRecoveryState brs, bool failedVehiclesPersistentlyOnly, out bool waitForUTurn) { // get the blockage ITacticalBlockage laneBlockageReport = brs.EncounteredState.TacticalBlockage; // get the turn state StayInLaneState sils = new StayInLaneState(lane, CoreCommon.CorePlanningState); // set wait false waitForUTurn = false; #region Reverse // check the state of the recovery for being the initial state if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL) { // determine if the reverse behavior is recommended if (laneBlockageReport.BlockageReport.ReverseRecommended) { // notify ArbiterOutput.Output("Initial encounter, reverse recommended, reversing"); // get reverse behavior StayInLaneBehavior reverseRecovery = this.LaneReverseRecovery(lane, vehicleState, vehicleSpeed, brs.EncounteredState.TacticalBlockage.BlockageReport); reverseRecovery.TimeStamp = vehicleState.Timestamp; // get recovery state BlockageRecoveryState brsT = new BlockageRecoveryState( reverseRecovery, null, new StayInLaneState(lane, CoreCommon.CorePlanningState), brs.Defcon = BlockageRecoveryDEFCON.REVERSE, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); brsT.CompletionState = brsT; // reset blockages BlockageHandler.SetDefaultBlockageCooldown(); // maneuver return new Maneuver(reverseRecovery, brsT, TurnDecorators.HazardDecorator, vehicleState.Timestamp); } } #endregion #region uTurn // check if uturn is available ArbiterLane opp = this.tacticalUmbrella.roadTactical.forwardReasoning.GetClosestOpposing(lane, vehicleState); // resoning OpposingLateralReasoning olrTmp = new OpposingLateralReasoning(opp, SideObstacleSide.Driver); if (opp != null && olrTmp.ExistsExactlyHere(vehicleState) && opp.IsInside(vehicleState.Front) && opp.LanePath().GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front) < TahoeParams.VL * 3.0) { // check possible to reach goal given block partition way RoadPlan uTurnNavTest = CoreCommon.Navigation.PlanRoadOppositeWithoutPartition( lane.GetClosestPartition(vehicleState.Front), opp.GetClosestPartition(vehicleState.Front), CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], true, vehicleState.Front, true); // flag to try the uturn bool uturnTry = false; // all polygons to include List<Polygon> allPolys = BlockageTactical.AllForwardPolygons(vehicleState, failedVehiclesPersistentlyOnly);// .AllObstacleAndStoppedVehiclePolygons(vehicleState); // test blockage takes up segment double numLanesLeft = lane.NumberOfLanesLeft(vehicleState.Front, true); double numLanesRight = lane.NumberOfLanesRight(vehicleState.Front, true); LinePath leftBound = lane.LanePath().ShiftLateral((lane.Width * numLanesLeft) + (lane.Width / 2.0)); LinePath rightBound = lane.LanePath().ShiftLateral((lane.Width * numLanesRight) + (lane.Width / 2.0)); bool segmentBlockage = BlockageTester.TestBlockage(allPolys, leftBound, rightBound); uturnTry = segmentBlockage; // check if we should try the uturn if(uturnTry) { // check uturn timer if(uTurnTimer.ElapsedMilliseconds / 1000.0 > 2.0) { #region Determine Checkpoint // check route feasible if (uTurnNavTest.BestPlan.laneWaypointOfInterest.RouteTime < double.MaxValue - 1 && uTurnNavTest.BestPlan.laneWaypointOfInterest.TimeCostToPoint < double.MaxValue - 1) { ArbiterOutput.Output("valid route to checkpoint by rerouting, uturning"); } // otherwise remove the next checkpoint else { // notiify ArbiterOutput.Output("NO valid route to checkpoint by rerouting"); // get the next cp ArbiterCheckpoint cp1 = CoreCommon.Mission.MissionCheckpoints.Peek(); // get distance to blockage double blockageDistance = 15.0; // check cp is in our lane if (cp1.WaypointId.AreaSubtypeId.Equals(lane.LaneId)) { // check distance to cp1 double distanceToCp1 = lane.DistanceBetween(vehicleState.Front, CoreCommon.RoadNetwork.ArbiterWaypoints[cp1.WaypointId].Position); // check that this is an already inserted waypoint if (cp1.Type == CheckpointType.Inserted) { // remove cp ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue(); ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as inserted type of checkpoint"); ArbiterCheckpoint ac2 = CoreCommon.Mission.MissionCheckpoints.Dequeue(); ArbiterOutput.Output("removed checkpoint: " + ac2.WaypointId.ToString() + " as blocked type of checkpoint"); } // closer to us than the blockage else if (distanceToCp1 < blockageDistance) { // remove cp ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue(); ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as between us and the blockage ~ 15m away"); } // very close to blockage on the other side else if (distanceToCp1 < blockageDistance + 5.0) { // remove cp ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue(); ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as on the other side of blockage ~ 15m away"); } // further away from the blockage else { // check over all checkpoints if there exists a checkpoint cp2 in the opposing lane within 5m along our lane ArbiterCheckpoint cp2 = null; foreach (ArbiterWaypoint oppAw in opp.WaypointList) { // check checkpoint if (oppAw.IsCheckpoint) { // distance between us and that waypoint double distanceToCp2 = lane.DistanceBetween(vehicleState.Front, oppAw.Position); // check along distance < 5.0m if (Math.Abs(distanceToCp1 - distanceToCp2) < 5.0) { // set cp cp2 = new ArbiterCheckpoint(oppAw.CheckpointId, oppAw.WaypointId, CheckpointType.Inserted); } } } // check close cp exists if (cp2 != null) { // remove cp1 and replace with cp2 ArbiterOutput.Output("inserting checkpoint: " + cp2.WaypointId.ToString() + " before checkpoint: " + cp1.WaypointId.ToString() + " as can be replaced with adjacent opposing cp2"); //CoreCommon.Mission.MissionCheckpoints.Dequeue(); // get current checkpoints ArbiterCheckpoint[] updatedAcs = new ArbiterCheckpoint[CoreCommon.Mission.MissionCheckpoints.Count + 1]; updatedAcs[0] = cp2; CoreCommon.Mission.MissionCheckpoints.CopyTo(updatedAcs, 1); updatedAcs[1].Type = CheckpointType.Blocked; CoreCommon.Mission.MissionCheckpoints = new Queue<ArbiterCheckpoint>(new List<ArbiterCheckpoint>(updatedAcs)); } // otherwise remove cp1 else { // remove cp ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue(); ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane"); } } } // otherwise we remove the checkpoint else { // remove cp ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue(); ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane"); } } #endregion #region Plan Uturn // notify ArbiterOutput.Output("Segment blocked, uturn available"); // nav penalties ArbiterLanePartition alpClose = lane.GetClosestPartition(vehicleState.Front); CoreCommon.Navigation.AddHarshBlockageAcrossSegment(alpClose, vehicleState.Front); // uturn LinePath lpTmp = new LinePath(new Coordinates[] { vehicleState.Front, opp.LanePath().GetClosestPoint(vehicleState.Front).Location }); Coordinates vector = lpTmp[1] - lpTmp[0]; lpTmp[1] = lpTmp[1] + vector.Normalize(opp.Width / 2.0); lpTmp[0] = lpTmp[0] - vector.Normalize(lane.Width / 2.0); LinePath lb = lpTmp.ShiftLateral(15.0); LinePath rb = lpTmp.ShiftLateral(-15.0); List<Coordinates> uturnPolyCOords = new List<Coordinates>(); uturnPolyCOords.AddRange(lb); uturnPolyCOords.AddRange(rb); uTurnState uts = new uTurnState(opp, Polygon.GrahamScan(uturnPolyCOords)); // reset blockages BlockageHandler.SetDefaultBlockageCooldown(); // reset the timers this.Reset(); // go to uturn return new Maneuver(uts.Resume(vehicleState, 2.0), uts, TurnDecorators.NoDecorators, vehicleState.Timestamp); #endregion } // uturn timer not enough else { // check timer running if(!uTurnTimer.IsRunning) { uTurnTimer.Stop(); uTurnTimer.Reset(); uTurnTimer.Start(); } // if gets here, need to wait double time = uTurnTimer.ElapsedMilliseconds / 1000.0; ArbiterOutput.Output("uTurn behavior evaluated to success, cooling down for: " + time.ToString("f2") + " out of 2"); waitForUTurn = true; return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } else { waitForUTurn = false; this.Reset(); } } else { waitForUTurn = false; this.Reset(); } #endregion #region Recovery Escalation // plan forward reasoning this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardManeuver(lane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>()); // check clear on right, or if it does nto exist here ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning; bool rightClear = !rightLateral.Exists || !rightLateral.ExistsExactlyHere(vehicleState) || rightLateral.AdjacentAndRearClear(vehicleState); // check clear on left, or if it does nto exist here ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning; bool leftClear = !leftLateral.Exists || !leftLateral.ExistsExactlyHere(vehicleState) || leftLateral.AdjacentAndRearClear(vehicleState); if(leftClear && leftLateral is OpposingLateralReasoning) leftClear = leftLateral.ForwardClear(vehicleState, TahoeParams.VL * 3.0, 2.24, new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null), lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), TahoeParams.VL * 3.0).Location); // check both clear to widen bool widenOk = rightClear && leftClear; // Notify ArbiterOutput.Output("Blockage tactical recovery escalation: rightClear: " + rightClear.ToString() + ", leftCler: " + leftClear.ToString()); // if we can't widen for some reason just go through with no widen StayInLaneBehavior silb = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, brs.EncounteredState); silb.TimeStamp = vehicleState.Timestamp; // check widen if (widenOk) { // output ArbiterOutput.Output("Lane Blockage Recovery: Adjacent areas clear"); // mini icrease width silb.LaneWidth = silb.LaneWidth + TahoeParams.T; // check execute none saudi CompletionReport l0Cr; bool l0TestOk = CoreCommon.Communications.TestExecute(silb, out l0Cr); // check mini ok if (l0TestOk) { // notify ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen ok"); // update the current state BlockageRecoveryState brsL0 = new BlockageRecoveryState( silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); return new Maneuver(silb, brsL0, TurnDecorators.NoDecorators, vehicleState.Timestamp); } else { // notify ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen failed, moving to large widen"); #region Change Lanes // check not in change lanes if (brs.Defcon != BlockageRecoveryDEFCON.CHANGELANES_FORWARD && brs.Defcon != BlockageRecoveryDEFCON.WIDENBOUNDS) { // check normal change lanes reasoning bool shouldWait; IState laneChangeCompletionState; ChangeLaneBehavior changeLanesBehavior = this.ChangeLanesRecoveryBehavior(lane, vehicleState, out shouldWait, out laneChangeCompletionState); // check change lanes behaviore exists if (changeLanesBehavior != null) { ArbiterOutput.Output("Lane Blockage Recovery: Found adjacent lane available change lanes beahvior: " + changeLanesBehavior.ToShortString() + ", " + changeLanesBehavior.ShortBehaviorInformation()); // update the current state BlockageRecoveryState brsCL = new BlockageRecoveryState( changeLanesBehavior, laneChangeCompletionState, sils, BlockageRecoveryDEFCON.CHANGELANES_FORWARD, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); return new Maneuver(changeLanesBehavior, brsCL, changeLanesBehavior.ChangeLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, vehicleState.Timestamp); } // check should wait else if (shouldWait) { ArbiterOutput.Output("Lane Blockage Recovery: Should wait for the forward lane, waiting"); return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } #endregion // notify ArbiterOutput.Output("Lane Blockage Recovery: Fell Through Forward Change Lanes"); // increase width silb.LaneWidth = silb.LaneWidth * 3.0; // check execute l1 CompletionReport l1Cr; bool l1TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr); // check ok if (l1TestOk) { // notify ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen ok"); // update the current state BlockageRecoveryState brsL1 = new BlockageRecoveryState( silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); return new Maneuver(silb, brsL1, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // go to l2 for all the marbles else { // notify ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen failed, moving to 3LW, L1 Saudi"); ShutUpAndDoItDecorator saudi2 = new ShutUpAndDoItDecorator(SAUDILevel.L1); List<BehaviorDecorator> saudi2bds = new List<BehaviorDecorator>(new BehaviorDecorator[] { saudi2 }); silb.Decorators = saudi2bds; BlockageRecoveryState brsL2 = new BlockageRecoveryState( silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); return new Maneuver(silb, brsL2, saudi2bds, vehicleState.Timestamp); } } } #endregion // fallout goes back to lane state return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); }
/// <summary> /// Maneuver for recovering from a lane blockage, used for lane changes as well /// </summary> /// <param name="lane"></param> /// <param name="vehicleState"></param> /// <param name="vehicleSpeed"></param> /// <param name="defcon"></param> /// <param name="saudi"></param> /// <returns></returns> public Maneuver OpposingLaneRecoveryManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, INavigationalPlan plan, BlockageRecoveryState brs, bool failedVehiclesPersistentlyOnly) { // get the blockage ITacticalBlockage laneBlockageReport = brs.EncounteredState.TacticalBlockage; // get the turn state OpposingLanesState sils = (OpposingLanesState)brs.EncounteredState.PlanningState; #region Reverse // check the state of the recovery for being the initial state if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL) { // determine if the reverse behavior is recommended if (laneBlockageReport.BlockageReport.ReverseRecommended) { // notify ArbiterOutput.Output("Initial encounter, reverse recommended, reversing"); // path LinePath revPath = lane.LanePath().Clone(); revPath.Reverse(); // dist to reverse double distToReverse; if (double.IsNaN(laneBlockageReport.BlockageReport.DistanceToBlockage)) distToReverse = TahoeParams.VL * 1.5; else if (laneBlockageReport.BlockageReport.DistanceToBlockage < 0 || laneBlockageReport.BlockageReport.DistanceToBlockage > 2 * TahoeParams.VL) distToReverse = TahoeParams.VL * 1.5; else distToReverse = TahoeParams.VL - laneBlockageReport.BlockageReport.DistanceToBlockage; // get reverse behavior StayInLaneBehavior reverseRecovery = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List<int>(), revPath, lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false)); // get recovery state BlockageRecoveryState brsT = new BlockageRecoveryState( reverseRecovery, CoreCommon.CorePlanningState, new OpposingLanesState(lane, false, CoreCommon.CorePlanningState, vehicleState), brs.Defcon = BlockageRecoveryDEFCON.REVERSE, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); brsT.CompletionState = brsT; // reset blockages BlockageHandler.SetDefaultBlockageCooldown(); // maneuver return new Maneuver(reverseRecovery, brsT, TurnDecorators.HazardDecorator, vehicleState.Timestamp); } } #endregion #region Recovery Escalation // plan forward reasoning this.tacticalUmbrella.roadTactical.opposingReasoning.ForwardManeuver(lane, ((OpposingLanesState)brs.EncounteredState.PlanningState).ClosestGoodLane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>()); // check clear on right, or if it does nto exist here ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning; bool rightClear = !rightLateral.Exists || !rightLateral.ExistsExactlyHere(vehicleState) || rightLateral.AdjacentAndRearClear(vehicleState); // check clear on left, or if it does nto exist here ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning; bool leftClear = !leftLateral.Exists || !leftLateral.ExistsExactlyHere(vehicleState) || leftLateral.AdjacentAndRearClear(vehicleState); if (leftClear && leftLateral is OpposingLateralReasoning) leftClear = leftLateral.ForwardClear(vehicleState, TahoeParams.VL * 3.0, 2.24, new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null), lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), TahoeParams.VL * 3.0).Location); // check both clear to widen bool widenOk = rightClear && leftClear; // Notify ArbiterOutput.Output("Blockage tactical recovery escalation: rightClear: " + rightClear.ToString() + ", leftClear: " + leftClear.ToString()); // path LinePath revPath2 = lane.LanePath().Clone(); revPath2.Reverse(); // check widen if (widenOk) { // output ArbiterOutput.Output("Lane Blockage Recovery: Adjacent areas clear"); if (brs.Defcon != BlockageRecoveryDEFCON.CHANGELANES_FORWARD) { // check change lanes back to good ChangeLaneBehavior clb = new ChangeLaneBehavior( lane.LaneId, sils.ClosestGoodLane.LaneId, false, TahoeParams.VL * 3.0, new StopAtDistSpeedCommand(TahoeParams.VL * 3.0), new List<int>(), revPath2, sils.ClosestGoodLane.LanePath(), lane.Width, sils.ClosestGoodLane.Width, lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false)); // change lanes brs.Defcon = BlockageRecoveryDEFCON.CHANGELANES_FORWARD; // cehck ok CompletionReport clROk; bool clCheck = CoreCommon.Communications.TestExecute(clb, out clROk); // check change lanes ok if (clCheck) { ArbiterOutput.Output("Change lanes behaviro ok, executing"); // return manevuer return new Maneuver(clb, brs, TurnDecorators.RightTurnDecorator, vehicleState.Timestamp); } else { ArbiterOutput.Output("Change lanes behaviro not ok"); } } // if we can't widen for some reason just go through with no widen StayInLaneBehavior silb = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(TahoeParams.VL * 2.0), new List<int>(), revPath2, lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false)); // mini icrease width silb.LaneWidth = silb.LaneWidth + TahoeParams.T; // check execute none saudi CompletionReport l0Cr; bool l0TestOk = CoreCommon.Communications.TestExecute(silb, out l0Cr); // check mini ok if (l0TestOk) { // notify ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen ok"); // update the current state BlockageRecoveryState brsL0 = new BlockageRecoveryState( silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); return new Maneuver(silb, brsL0, TurnDecorators.NoDecorators, vehicleState.Timestamp); } else { // notify ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen failed, moving to large widen"); // increase width silb.LaneWidth = silb.LaneWidth * 3.0; // check execute l1 CompletionReport l1Cr; bool l1TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr); // check ok if (l1TestOk) { // notify ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen ok"); // update the current state BlockageRecoveryState brsL1 = new BlockageRecoveryState( silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); return new Maneuver(silb, brsL1, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // go to l2 for all the marbles else { // notify ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen failed, moving to 3LW, L1 Saudi"); ShutUpAndDoItDecorator saudi2 = new ShutUpAndDoItDecorator(SAUDILevel.L1); List<BehaviorDecorator> saudi2bds = new List<BehaviorDecorator>(new BehaviorDecorator[] { saudi2 }); silb.Decorators = saudi2bds; BlockageRecoveryState brsL2 = new BlockageRecoveryState( silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); // check execute l1 CompletionReport l2Cr; bool l2TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr); // go return new Maneuver(silb, brsL2, saudi2bds, vehicleState.Timestamp); } } } #endregion // fallout goes back to lane state return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); }
/// <summary> /// Maneuver if the reverse maneuver is blocked /// </summary> /// <param name="lane"></param> /// <param name="vehicleState"></param> /// <param name="vehicleSpeed"></param> /// <param name="defcon"></param> /// <param name="saudi"></param> /// <param name="laneOpposing"></param> /// <param name="currentBlockage"></param> /// <param name="ebs"></param> /// <returns></returns> private Maneuver LaneReverseBlockedManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, BlockageRecoveryState brs, EncounteredBlockageState ebs, INavigationalPlan plan) { // get closest partition ArbiterLanePartition alp = lane.GetClosestPartition(vehicleState.Front); // check type if (alp.Type == PartitionType.Sparse) { #region Get Recovery Behavior // get the recovery behavior StayInLaneBehavior testForwards = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, ebs); // up the saudi level SAUDILevel sl = brs.EncounteredState.Saudi < SAUDILevel.L2 ? brs.EncounteredState.Saudi++ : SAUDILevel.L2; testForwards.Decorators = new List<BehaviorDecorator>(new BehaviorDecorator[] { new ShutUpAndDoItDecorator(sl) }); // return the behavior brs.EncounteredState.Saudi = sl; brs.RecoveryStatus = BlockageRecoverySTATUS.EXECUTING; return new Maneuver(testForwards, brs, TurnDecorators.NoDecorators, vehicleState.Timestamp); #endregion } else { return new Maneuver(new NullBehavior(), brs.AbortState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } }
/// <summary> /// What to do when we complete a reverse maneuver /// </summary> /// <param name="lane"></param> /// <param name="vehicleState"></param> /// <param name="vehicleSpeed"></param> /// <param name="defcon"></param> /// <param name="saudi"></param> /// <param name="laneOpposing"></param> /// <param name="currentBlockage"></param> /// <param name="ebs"></param> /// <returns></returns> private Maneuver LaneReverseCompleteManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, BlockageRecoveryState brs, EncounteredBlockageState ebs, INavigationalPlan plan) { // get the lane recovery behavior StayInLaneBehavior testForwards = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, ebs); #region Test and Return // notify ArbiterOutput.Output("Attempting to test execute recovery complete behavior: " + testForwards.ToString()); // test the test behavior CompletionReport testForwardsReport; bool canCompleteTestForwards = CoreCommon.Communications.TestExecute(testForwards, out testForwardsReport); // notify ArbiterOutput.Output("Received completion result ok: " + canCompleteTestForwards.ToString() + " with completion result: " + testForwardsReport.Result.ToString()); // check completion ok of stop at distance if (canCompleteTestForwards) { // switch to the completion state brs.RecoveryStatus = BlockageRecoverySTATUS.EXECUTING; return new Maneuver(testForwards, brs, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // not ok, report this blockage else { // return the blocked reversal maneuver return this.LaneReverseBlockedManeuver(lane, vehicleState, vehicleSpeed, brs, ebs, plan); } #endregion }
/// <summary> /// Get the default recovery behavior /// </summary> /// <param name="lane"></param> /// <param name="vehicleState"></param> /// <param name="vehicleSpeed"></param> /// <param name="brs"></param> /// <param name="ebs"></param> /// <returns></returns> private StayInLaneBehavior LaneRecoveryBehavior(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, INavigationalPlan plan, BlockageRecoveryState brs, EncounteredBlockageState ebs) { #region Get the Recovery Behavior // set the default distance to go forwards double distanceForwards = TahoeParams.VL * 3.0; // check distance to next lane point this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.Primary(lane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>() , new List<ArbiterWaypoint>(), false); // get navigation distance to go double navDistanceToGo = this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.NavigationParameters.DistanceToGo; distanceForwards = navDistanceToGo < distanceForwards ? navDistanceToGo : distanceForwards; // check if there is a forward vehicle we should follow normally if (this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker) { // fv distance double fvDistance = this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardMonitor.ForwardVehicle.ForwardControl.xSeparation; // check distance forwards to vehicle if (fvDistance < distanceForwards) { // distance modification distanceForwards = fvDistance > 2.0 ? fvDistance - 2.0 : fvDistance; } } // test behavior StayInLaneBehavior testForwards = new StayInLaneBehavior( lane.LaneId, new StopAtDistSpeedCommand(distanceForwards), new List<int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true)); // set the specifiec saudi level testForwards.Decorators = new List<BehaviorDecorator>(new BehaviorDecorator[] { new ShutUpAndDoItDecorator(brs.EncounteredState.Saudi) }); // return the test return testForwards; #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 }
/// <summary> /// Plans over the zone /// </summary> /// <param name="planningState"></param> /// <param name="navigationalPlan"></param> /// <param name="vehicleState"></param> /// <param name="vehicles"></param> /// <param name="obstacles"></param> /// <param name="blockages"></param> /// <returns></returns> public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, List<ITacticalBlockage> blockages) { #region Zone Travelling State if (planningState is ZoneTravelingState) { // check blockages /*if (blockages != null && blockages.Count > 0 && blockages[0] is ZoneBlockage) { // create the blockage state EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState); // go to a blockage handling tactical return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp); }*/ // cast state ZoneState zs = (ZoneState)planningState; // plan over state and zone ZonePlan zp = (ZonePlan)navigationalPlan; // check zone path does not exist if (zp.RecommendedPath.Count < 2) { // zone startup again ZoneStartupState zss = new ZoneStartupState(zs.Zone, true); return new Maneuver(new HoldBrakeBehavior(), zss, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // final path seg LinePath.PointOnPath endBack = zp.RecommendedPath.AdvancePoint(zp.RecommendedPath.EndPoint, -TahoeParams.VL); LinePath lp = zp.RecommendedPath.SubPath(endBack, zp.RecommendedPath.EndPoint); LinePath lB = lp.ShiftLateral(TahoeParams.T); LinePath rB = lp.ShiftLateral(-TahoeParams.T); // add to info CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound)); // get speed command ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24); // Behavior Behavior b = new ZoneTravelingBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, zp.Zone.StayOutAreas.ToArray(), sc, zp.RecommendedPath, lB, rB); // maneuver return new Maneuver(b, CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } #endregion #region Parking State else if (planningState is ParkingState) { // get state ParkingState ps = (ParkingState)planningState; // determine stay out areas to use List<Polygon> stayOuts = new List<Polygon>(); foreach (Polygon p in ps.Zone.StayOutAreas) { if (!p.IsInside(ps.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(ps.ParkingSpot.Checkpoint.Position)) stayOuts.Add(p); } LinePath rB = ps.ParkingSpot.GetRightBound(); LinePath lB = ps.ParkingSpot.GetLeftBound(); // add to info CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound)); // create behavior ZoneParkingBehavior zpb = new ZoneParkingBehavior(ps.Zone.ZoneId, ps.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(), new ScalarSpeedCommand(2.24), ps.ParkingSpot.GetSpotPath(), lB, rB, ps.ParkingSpot.SpotId, 1.0); // maneuver return new Maneuver(zpb, ps, TurnDecorators.NoDecorators, vehicleState.Timestamp); } #endregion #region Pulling Out State else if (planningState is PullingOutState) { // get state PullingOutState pos = (PullingOutState)planningState; // plan over state and zone ZonePlan zp = (ZonePlan)navigationalPlan; // final path seg Coordinates endVec = zp.RecommendedPath[0] - zp.RecommendedPath[1]; Coordinates endBack = zp.RecommendedPath[0] + endVec.Normalize(TahoeParams.VL * 2.0); LinePath rp = new LinePath(new Coordinates[]{pos.ParkingSpot.Checkpoint.Position, pos.ParkingSpot.NormalWaypoint.Position, zp.RecommendedPath[0], endBack}); LinePath lp = new LinePath(new Coordinates[]{zp.RecommendedPath[0], endBack}); LinePath lB = lp.ShiftLateral(TahoeParams.T * 2.0); LinePath rB = lp.ShiftLateral(-TahoeParams.T * 2.0); // add to info CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rp, ArbiterInformationDisplayObjectType.leftBound)); // determine stay out areas to use List<Polygon> stayOuts = new List<Polygon>(); foreach (Polygon p in pos.Zone.StayOutAreas) { if (!p.IsInside(pos.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(pos.ParkingSpot.Checkpoint.Position)) stayOuts.Add(p); } // get speed command ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24); // Behavior Behavior b = new ZoneParkingPullOutBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(), sc, pos.ParkingSpot.GetSpotPath(), pos.ParkingSpot.GetLeftBound(), pos.ParkingSpot.GetRightBound(), pos.ParkingSpot.SpotId, rp, lB, rB); // maneuver return new Maneuver(b, pos, TurnDecorators.NoDecorators, vehicleState.Timestamp); } #endregion #region Zone Startup State else if (planningState is ZoneStartupState) { // state ZoneStartupState zss = (ZoneStartupState)planningState; // get the zone ArbiterZone az = zss.Zone; // navigational edge INavigableNode inn = CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]; // check over all the parking spaces foreach (ArbiterParkingSpot aps in az.ParkingSpots) { // inside both parts of spot if ((vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position) && vehicleState.VehiclePolygon.IsInside(aps.Checkpoint.Position)) || (vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position))) { // want to just park in it again return new Maneuver(new HoldBrakeBehavior(), new ParkingState(az, aps), TurnDecorators.NoDecorators, vehicleState.Timestamp); } } Polygon forwardPolygon = vehicleState.ForwardPolygon; Polygon rearPolygon = vehicleState.RearPolygon; Navigator nav = CoreCommon.Navigation; List<ZoneNavigationEdgeSort> forwardForward = new List<ZoneNavigationEdgeSort>(); List<ZoneNavigationEdgeSort> reverseReverse = new List<ZoneNavigationEdgeSort>(); List<ZoneNavigationEdgeSort> perpendicularPerpendicular = new List<ZoneNavigationEdgeSort>(); List<ZoneNavigationEdgeSort> allEdges = new List<ZoneNavigationEdgeSort>(); List<ZoneNavigationEdgeSort> allEdgesNoLimits = new List<ZoneNavigationEdgeSort>(); foreach (NavigableEdge ne in az.NavigableEdges) { if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint)) { // get distance to edge LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position }); double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front); // get direction along segment DirectionAlong da = vehicleState.DirectionAlongSegment(lp); // check dist reasonable if (distTmp > TahoeParams.VL) { // zone navigation edge sort item ZoneNavigationEdgeSort znes = new ZoneNavigationEdgeSort(distTmp, ne, lp); // add to lists if (da == DirectionAlong.Forwards && (forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position))) forwardForward.Add(znes); /*else if (da == DirectionAlong.Perpendicular && !(forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position)) && !(rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position))) perpendicularPerpendicular.Add(znes); else if (rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position)) reverseReverse.Add(znes);*/ // add to all edges allEdges.Add(znes); } } } // sort by distance forwardForward.Sort(); reverseReverse.Sort(); perpendicularPerpendicular.Sort(); allEdges.Sort(); ZoneNavigationEdgeSort bestZnes = null; for (int i = 0; i < allEdges.Count; i++) { // get line to initial Line toInitial = new Line(vehicleState.Front, allEdges[i].edge.Start.Position); Line toFinal = new Line(vehicleState.Front, allEdges[i].edge.End.Position); bool intersects = false; Coordinates[] interPts; foreach (Polygon sop in az.StayOutAreas) { if (!intersects && (sop.Intersect(toInitial, out interPts) && sop.Intersect(toFinal, out interPts))) intersects = true; } if (!intersects) { allEdges[i].zp = nav.PlanZone(az, allEdges[i].edge.End, inn); allEdges[i].zp.Time += vehicleState.Front.DistanceTo(allEdges[i].lp.GetClosestPoint(vehicleState.Front).Location) / 2.24; ZoneNavigationEdgeSort tmpZnes = allEdges[i]; if ((bestZnes == null && tmpZnes.zp.RecommendedPath.Count > 1) || (bestZnes != null && tmpZnes.zp.RecommendedPath.Count > 1 && tmpZnes.zp.Time < bestZnes.zp.Time)) bestZnes = tmpZnes; } if (i > allEdges.Count / 2 && bestZnes != null) break; } if (bestZnes != null) { ArbiterOutput.Output("Found good edge to start in zone"); return new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, bestZnes.edge), TurnDecorators.NoDecorators, vehicleState.Timestamp); } else { ArbiterOutput.Output("Could not find good edge to start, choosing random not blocked"); List<ZoneNavigationEdgeSort> okZnes = new List<ZoneNavigationEdgeSort>(); foreach (NavigableEdge tmpOk in az.NavigableEdges) { // get line to initial LinePath edgePath = new LinePath(new Coordinates[] { tmpOk.Start.Position, tmpOk.End.Position }); double dist = edgePath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front); ZoneNavigationEdgeSort tmpZnes = new ZoneNavigationEdgeSort(dist, tmpOk, edgePath); tmpZnes.zp = nav.PlanZone(az, tmpZnes.edge.End, inn); tmpZnes.zp.Time += vehicleState.Front.DistanceTo(tmpZnes.lp.GetClosestPoint(vehicleState.Front).Location) / 2.24; if (tmpZnes.zp.RecommendedPath.Count >= 2) okZnes.Add(tmpZnes); } if (okZnes.Count == 0) okZnes = allEdges; else okZnes.Sort(); // get random close edge Random rand = new Random(); int chosen = rand.Next(Math.Min(5, okZnes.Count)); // get closest edge not part of a parking spot, get on it NavigableEdge closest = okZnes[chosen].edge;//null; //double distance = Double.MaxValue; /*foreach (NavigableEdge ne in az.NavigableEdges) { if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint)) { // get distance to edge LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position }); double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front); if (closest == null || distTmp < distance) { closest = ne; distance = distTmp; } } }*/ return new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, closest), TurnDecorators.NoDecorators, vehicleState.Timestamp); } } #endregion #region Unknown else { // non-handled state throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState"); } #endregion }
/// <summary> /// Plans over the zone /// </summary> /// <param name="planningState"></param> /// <param name="navigationalPlan"></param> /// <param name="vehicleState"></param> /// <param name="vehicles"></param> /// <param name="obstacles"></param> /// <param name="blockages"></param> /// <returns></returns> public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, List <ITacticalBlockage> blockages) { #region Zone Travelling State if (planningState is ZoneTravelingState) { // check blockages /*if (blockages != null && blockages.Count > 0 && blockages[0] is ZoneBlockage) * { * // create the blockage state * EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState); * * // go to a blockage handling tactical * return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp); * }*/ // cast state ZoneState zs = (ZoneState)planningState; // plan over state and zone ZonePlan zp = (ZonePlan)navigationalPlan; // check zone path does not exist if (zp.RecommendedPath.Count < 2) { // zone startup again ZoneStartupState zss = new ZoneStartupState(zs.Zone, true); return(new Maneuver(new HoldBrakeBehavior(), zss, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } // final path seg LinePath.PointOnPath endBack = zp.RecommendedPath.AdvancePoint(zp.RecommendedPath.EndPoint, -TahoeParams.VL); LinePath lp = zp.RecommendedPath.SubPath(endBack, zp.RecommendedPath.EndPoint); LinePath lB = lp.ShiftLateral(TahoeParams.T); LinePath rB = lp.ShiftLateral(-TahoeParams.T); // add to info CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound)); // get speed command ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24); // Behavior Behavior b = new ZoneTravelingBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, zp.Zone.StayOutAreas.ToArray(), sc, zp.RecommendedPath, lB, rB); // maneuver return(new Maneuver(b, CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } #endregion #region Parking State else if (planningState is ParkingState) { // get state ParkingState ps = (ParkingState)planningState; // determine stay out areas to use List <Polygon> stayOuts = new List <Polygon>(); foreach (Polygon p in ps.Zone.StayOutAreas) { if (!p.IsInside(ps.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(ps.ParkingSpot.Checkpoint.Position)) { stayOuts.Add(p); } } LinePath rB = ps.ParkingSpot.GetRightBound(); LinePath lB = ps.ParkingSpot.GetLeftBound(); // add to info CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound)); // create behavior ZoneParkingBehavior zpb = new ZoneParkingBehavior(ps.Zone.ZoneId, ps.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(), new ScalarSpeedCommand(2.24), ps.ParkingSpot.GetSpotPath(), lB, rB, ps.ParkingSpot.SpotId, 1.0); // maneuver return(new Maneuver(zpb, ps, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } #endregion #region Pulling Out State else if (planningState is PullingOutState) { // get state PullingOutState pos = (PullingOutState)planningState; // plan over state and zone ZonePlan zp = (ZonePlan)navigationalPlan; // final path seg Coordinates endVec = zp.RecommendedPath[0] - zp.RecommendedPath[1]; Coordinates endBack = zp.RecommendedPath[0] + endVec.Normalize(TahoeParams.VL * 2.0); LinePath rp = new LinePath(new Coordinates[] { pos.ParkingSpot.Checkpoint.Position, pos.ParkingSpot.NormalWaypoint.Position, zp.RecommendedPath[0], endBack }); LinePath lp = new LinePath(new Coordinates[] { zp.RecommendedPath[0], endBack }); LinePath lB = lp.ShiftLateral(TahoeParams.T * 2.0); LinePath rB = lp.ShiftLateral(-TahoeParams.T * 2.0); // add to info CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound)); CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rp, ArbiterInformationDisplayObjectType.leftBound)); // determine stay out areas to use List <Polygon> stayOuts = new List <Polygon>(); foreach (Polygon p in pos.Zone.StayOutAreas) { if (!p.IsInside(pos.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(pos.ParkingSpot.Checkpoint.Position)) { stayOuts.Add(p); } } // get speed command ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24); // Behavior Behavior b = new ZoneParkingPullOutBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(), sc, pos.ParkingSpot.GetSpotPath(), pos.ParkingSpot.GetLeftBound(), pos.ParkingSpot.GetRightBound(), pos.ParkingSpot.SpotId, rp, lB, rB); // maneuver return(new Maneuver(b, pos, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } #endregion #region Zone Startup State else if (planningState is ZoneStartupState) { // state ZoneStartupState zss = (ZoneStartupState)planningState; // get the zone ArbiterZone az = zss.Zone; // navigational edge INavigableNode inn = CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]; // check over all the parking spaces foreach (ArbiterParkingSpot aps in az.ParkingSpots) { // inside both parts of spot if ((vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position) && vehicleState.VehiclePolygon.IsInside(aps.Checkpoint.Position)) || (vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position))) { // want to just park in it again return(new Maneuver(new HoldBrakeBehavior(), new ParkingState(az, aps), TurnDecorators.NoDecorators, vehicleState.Timestamp)); } } Polygon forwardPolygon = vehicleState.ForwardPolygon; Polygon rearPolygon = vehicleState.RearPolygon; Navigator nav = CoreCommon.Navigation; List <ZoneNavigationEdgeSort> forwardForward = new List <ZoneNavigationEdgeSort>(); List <ZoneNavigationEdgeSort> reverseReverse = new List <ZoneNavigationEdgeSort>(); List <ZoneNavigationEdgeSort> perpendicularPerpendicular = new List <ZoneNavigationEdgeSort>(); List <ZoneNavigationEdgeSort> allEdges = new List <ZoneNavigationEdgeSort>(); List <ZoneNavigationEdgeSort> allEdgesNoLimits = new List <ZoneNavigationEdgeSort>(); foreach (NavigableEdge ne in az.NavigableEdges) { if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint)) { // get distance to edge LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position }); double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front); // get direction along segment DirectionAlong da = vehicleState.DirectionAlongSegment(lp); // check dist reasonable if (distTmp > TahoeParams.VL) { // zone navigation edge sort item ZoneNavigationEdgeSort znes = new ZoneNavigationEdgeSort(distTmp, ne, lp); // add to lists if (da == DirectionAlong.Forwards && (forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position))) { forwardForward.Add(znes); } /*else if (da == DirectionAlong.Perpendicular && * !(forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position)) && * !(rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position))) * perpendicularPerpendicular.Add(znes); * else if (rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position)) * reverseReverse.Add(znes);*/ // add to all edges allEdges.Add(znes); } } } // sort by distance forwardForward.Sort(); reverseReverse.Sort(); perpendicularPerpendicular.Sort(); allEdges.Sort(); ZoneNavigationEdgeSort bestZnes = null; for (int i = 0; i < allEdges.Count; i++) { // get line to initial Line toInitial = new Line(vehicleState.Front, allEdges[i].edge.Start.Position); Line toFinal = new Line(vehicleState.Front, allEdges[i].edge.End.Position); bool intersects = false; Coordinates[] interPts; foreach (Polygon sop in az.StayOutAreas) { if (!intersects && (sop.Intersect(toInitial, out interPts) && sop.Intersect(toFinal, out interPts))) { intersects = true; } } if (!intersects) { allEdges[i].zp = nav.PlanZone(az, allEdges[i].edge.End, inn); allEdges[i].zp.Time += vehicleState.Front.DistanceTo(allEdges[i].lp.GetClosestPoint(vehicleState.Front).Location) / 2.24; ZoneNavigationEdgeSort tmpZnes = allEdges[i]; if ((bestZnes == null && tmpZnes.zp.RecommendedPath.Count > 1) || (bestZnes != null && tmpZnes.zp.RecommendedPath.Count > 1 && tmpZnes.zp.Time < bestZnes.zp.Time)) { bestZnes = tmpZnes; } } if (i > allEdges.Count / 2 && bestZnes != null) { break; } } if (bestZnes != null) { ArbiterOutput.Output("Found good edge to start in zone"); return(new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, bestZnes.edge), TurnDecorators.NoDecorators, vehicleState.Timestamp)); } else { ArbiterOutput.Output("Could not find good edge to start, choosing random not blocked"); List <ZoneNavigationEdgeSort> okZnes = new List <ZoneNavigationEdgeSort>(); foreach (NavigableEdge tmpOk in az.NavigableEdges) { // get line to initial LinePath edgePath = new LinePath(new Coordinates[] { tmpOk.Start.Position, tmpOk.End.Position }); double dist = edgePath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front); ZoneNavigationEdgeSort tmpZnes = new ZoneNavigationEdgeSort(dist, tmpOk, edgePath); tmpZnes.zp = nav.PlanZone(az, tmpZnes.edge.End, inn); tmpZnes.zp.Time += vehicleState.Front.DistanceTo(tmpZnes.lp.GetClosestPoint(vehicleState.Front).Location) / 2.24; if (tmpZnes.zp.RecommendedPath.Count >= 2) { okZnes.Add(tmpZnes); } } if (okZnes.Count == 0) { okZnes = allEdges; } else { okZnes.Sort(); } // get random close edge Random rand = new Random(); int chosen = rand.Next(Math.Min(5, okZnes.Count)); // get closest edge not part of a parking spot, get on it NavigableEdge closest = okZnes[chosen].edge; //null; //double distance = Double.MaxValue; /*foreach (NavigableEdge ne in az.NavigableEdges) * { * if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint)) * { * // get distance to edge * LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position }); * double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front); * if (closest == null || distTmp < distance) * { * closest = ne; * distance = distTmp; * } * } * }*/ return(new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, closest), TurnDecorators.NoDecorators, vehicleState.Timestamp)); } } #endregion #region Unknown else { // non-handled state throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState"); } #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 }