/// <summary> /// Plans over an intersection /// </summary> /// <param name="exitWaypoint"></param> /// <param name="goal"></param> /// <returns></returns> public IntersectionPlan PlanIntersection(ITraversableWaypoint exitWaypoint, INavigableNode goal) { // road plan if the itnersection has a road available to take from it RoadPlan rp = null; // check if road waypoint if (exitWaypoint is ArbiterWaypoint) { // get exit ArbiterWaypoint awp = (ArbiterWaypoint)exitWaypoint; // check if it has lane partition moving outwards if (awp.NextPartition != null) { // road plan ignoring exit List <ArbiterWaypoint> iws = RoadToolkit.WaypointsClose(awp.Lane.Way, awp.Position, awp); rp = this.PlanNavigableArea(awp.Lane, awp.Position, goal, iws); } } // get exit plan IntersectionPlan ip = this.GetIntersectionExitPlan(exitWaypoint, goal); // add road plan if exists ip.SegmentPlan = rp; // return the plan return(ip); }
/// <summary> /// Try to plan the intersection heavily penalizing the interconnect /// </summary> /// <param name="iTraversableWaypoint"></param> /// <param name="iArbiterWaypoint"></param> /// <param name="arbiterInterconnect"></param> /// <returns></returns> public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect) { // save old blockage NavigationBlockage tmpBlockage = interconnect.Blockage; // create new NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue); newerBlockage.BlockageExists = true; interconnect.Blockage = newerBlockage; KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes; this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >(); // plan IntersectionPlan ip = this.PlanIntersection(exit, goal); this.currentTimes = tmpCurrentTimes; // reset interconnect blockage interconnect.Blockage = tmpBlockage; // return plan return(ip); }
/// <summary> /// Plan over all exits /// </summary> /// <param name="exitWaypoint"></param> /// <param name="goal"></param> /// <returns></returns> public IntersectionPlan GetIntersectionExitPlan(ITraversableWaypoint exitWaypoint, INavigableNode goal) { // initialize the intersection plan IntersectionPlan ip = new IntersectionPlan(exitWaypoint, new List <PlanableInterconnect>(), null); ip.ExitWaypoint = exitWaypoint; //check valid exit if (exitWaypoint.IsExit) { // plan over each interconnect foreach (ArbiterInterconnect ai in exitWaypoint.Exits) { // start of plan is the final wp of itner INavigableNode start = (INavigableNode)ai.FinalGeneric; // plan double time; List <INavigableNode> nodes; this.Plan(start, goal, out nodes, out time); time += ai.TimeCost(); // create planned interconnect PlanableInterconnect pi = new PlanableInterconnect(ai, time, nodes); // add planned interconnect to the intersection plan ip.PossibleEntries.Add(pi); } } // return the plan return(ip); }
public WaitingAtIntersectionExitState(ITraversableWaypoint exit, ArbiterTurnDirection turnDirection, IntersectionDescription description, ArbiterInterconnect desired) { this.exitWaypoint = exit; this.turnDirection = turnDirection; this.IntersectionDescription = description; this.desired = desired; this.turnTestState = TurnTestState.Unknown; }
/// <summary> /// Try to plan the intersection heavily penalizing the interconnect /// </summary> /// <param name="iTraversableWaypoint"></param> /// <param name="iArbiterWaypoint"></param> /// <param name="arbiterInterconnect"></param> /// <returns></returns> public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect, bool blockAllRelated) { ITraversableWaypoint entry = (ITraversableWaypoint)interconnect.FinalGeneric; if (!blockAllRelated) { return(this.PlanIntersectionWithoutInterconnect(exit, goal, interconnect)); } else { Dictionary <IConnectAreaWaypoints, NavigationBlockage> saved = new Dictionary <IConnectAreaWaypoints, NavigationBlockage>(); if (entry.IsEntry) { foreach (ArbiterInterconnect ai in entry.Entries) { saved.Add(ai, ai.Blockage); // create new NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue); newerBlockage.BlockageExists = true; ai.Blockage = newerBlockage; } } if (entry is ArbiterWaypoint && ((ArbiterWaypoint)entry).PreviousPartition != null) { ArbiterLanePartition alp = ((ArbiterWaypoint)entry).PreviousPartition; saved.Add(alp, alp.Blockage); // create new NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue); newerBlockage.BlockageExists = true; alp.Blockage = newerBlockage; } KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes; this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >(); // plan IntersectionPlan ip = this.PlanIntersection(exit, goal); this.currentTimes = tmpCurrentTimes; // reset the blockages foreach (KeyValuePair <IConnectAreaWaypoints, NavigationBlockage> savedPair in saved) { savedPair.Key.Blockage = savedPair.Value; } // return plan return(ip); } }
/// <summary> /// Check where turns intersect the subpath furthese along it /// </summary> /// <param name="aiSubpath"></param> /// <param name="iTraversableWaypoint"></param> /// <returns></returns> private Coordinates?TurnIntersects(LinePath aiSubpath, ITraversableWaypoint iTraversableWaypoint, out ArbiterInterconnect interconnect) { Line aiSub = new Line(aiSubpath.StartPoint.Location, aiSubpath.EndPoint.Location); double distance = 0.0; Coordinates?furthest = null; interconnect = null; foreach (ArbiterInterconnect ai in iTraversableWaypoint.OutgoingConnections) { Line iLine = new Line(ai.InterconnectPath.StartPoint.Location, ai.InterconnectPath.EndPoint.Location); Coordinates intersect; bool doesIntersect = aiSub.Intersect(iLine, out intersect); if (doesIntersect && (ai.IsInside(intersect) || ai.InterconnectPath.GetClosestPoint(intersect).Equals(ai.InterconnectPath.EndPoint))) { if (!furthest.HasValue) { furthest = intersect; distance = intersect.DistanceTo(aiSubpath.EndPoint.Location); interconnect = ai; } else { double tmpDist = intersect.DistanceTo(aiSubpath.EndPoint.Location); if (tmpDist < distance) { furthest = intersect; distance = tmpDist; interconnect = ai; } } } } if (furthest.HasValue) { return(furthest.Value); } else { return(null); } }
/// <summary> /// Try to plan the intersection heavily penalizing the interconnect /// </summary> /// <param name="iTraversableWaypoint"></param> /// <param name="iArbiterWaypoint"></param> /// <param name="arbiterInterconnect"></param> /// <returns></returns> public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect) { // save old blockage NavigationBlockage tmpBlockage = interconnect.Blockage; // create new NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue); newerBlockage.BlockageExists = true; interconnect.Blockage = newerBlockage; KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>> tmpCurrentTimes = currentTimes; this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>(); // plan IntersectionPlan ip = this.PlanIntersection(exit, goal); this.currentTimes = tmpCurrentTimes; // reset interconnect blockage interconnect.Blockage = tmpBlockage; // return plan return ip; }
/// <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> /// Generates interconnects into the road network /// </summary> /// <param name="arn"></param> /// <returns></returns> public ArbiterRoadNetwork GenerateInterconnects(ArbiterRoadNetwork arn) { // list of all exit entries in the xy rndf List <SimpleExitEntry> sees = new List <SimpleExitEntry>(); // zones if (xyRndf.Zones != null) { // loop over zones foreach (SimpleZone sz in xyRndf.Zones) { // add all ee's sees.AddRange(sz.Perimeter.ExitEntries); } } // segments if (xyRndf.Segments != null) { // loop over segments foreach (SimpleSegment ss in xyRndf.Segments) { // lanes foreach (SimpleLane sl in ss.Lanes) { // add all ee's sees.AddRange(sl.ExitEntries); } } } // loop over ee's and create interconnects foreach (SimpleExitEntry see in sees) { IArbiterWaypoint initial = arn.LegacyWaypointLookup[see.ExitId]; IArbiterWaypoint final = arn.LegacyWaypointLookup[see.EntryId]; ArbiterInterconnect ai = new ArbiterInterconnect(initial, final); arn.ArbiterInterconnects.Add(ai.InterconnectId, ai); arn.DisplayObjects.Add(ai); if (initial is ITraversableWaypoint) { ITraversableWaypoint initialWaypoint = (ITraversableWaypoint)initial; initialWaypoint.IsExit = true; if (initialWaypoint.Exits == null) { initialWaypoint.Exits = new List <ArbiterInterconnect>(); } initialWaypoint.Exits.Add(ai); } else { throw new Exception("initial wp of ee: " + see.ExitId + " is not ITraversableWaypoint"); } if (final is ITraversableWaypoint) { ITraversableWaypoint finalWaypoint = (ITraversableWaypoint)final; finalWaypoint.IsEntry = true; if (finalWaypoint.Entries == null) { finalWaypoint.Entries = new List <ArbiterInterconnect>(); } finalWaypoint.Entries.Add(ai); } else { throw new Exception("final wp of ee: " + see.EntryId + " is not ITraversableWaypoint"); } // set the turn direction this.SetTurnDirection(ai); // interconnectp olygon stuff this.GenerateInterconnectPolygon(ai); if (ai.TurnPolygon.IsComplex) { Console.WriteLine("Found complex polygon for interconnect: " + ai.ToString()); ai.TurnPolygon = ai.DefaultPoly(); } } return(arn); }
/// <summary> /// finish the wrapping and cretae the intersection /// </summary> public void FinalizeIntersection(Polygon interPoly, ArbiterIntersection ai) { // check poly not null if (interPoly != null) { this.WrappingHelpers = new List<Coordinates>(); Rect r = interPoly.CalculateBoundingRectangle(); Coordinates bl = new Coordinates(r.x, r.y); Coordinates tr = new Coordinates(r.x + r.width, r.y + r.height); Polygon sqPoly = this.CreateSquarePolygon(bl, tr).Inflate(1.0); //interPoly = this.CreateIntersectionPolygon(interPoly); //interPoly = interPoly.Inflate(0.05); // retreive all exits involved in this intersection Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint> exits = this.IntersectionExits(sqPoly);//interPoly); // make sure the inter contains an exit if (exits.Count > 0) { // make stopped exits, necessarily these are arbiter waypoints not perimeter points List<ArbiterStoppedExit> ases = this.CreateStoppedExits(exits.Values); // construct intersection id ITraversableWaypoint[] exitArray = new ITraversableWaypoint[exits.Count]; exits.Values.CopyTo(exitArray, 0); ArbiterIntersectionId aii = new ArbiterIntersectionId(exitArray[0].Exits[0].InterconnectId); // determine incoming lanes Dictionary<ArbiterLane, LinePath.PointOnPath> incoming = this.DetermineIncoming(interPoly); // create the intersection ai.IntersectionPolygon = interPoly; ai.Center = interPoly.BoundingCircle.center; ai.StoppedExits = ases; ai.IncomingLanePoints = incoming; ai.AllExits = exits; ai.AllEntries = this.IntersectionEntries(sqPoly); ai.PriorityLanes = this.DetermineInvolved(exits.Values, incoming); // update poly //this.UpdateIntersectionPolygon(ai); try { List<Polygon> ps = new List<Polygon>(); foreach (ITraversableWaypoint itw in exits.Values) { foreach (ArbiterInterconnect ait in itw.Exits) { ps.Add(ait.TurnPolygon); } } ai.IntersectionPolygon = this.GetIntersectionPolygon(ps, ai.AllExits, ai.AllEntries); if (ai.IntersectionPolygon.IsComplex) { EditorOutput.WriteLine("Intersection polygon complex, defaulting"); throw new Exception("complex polygon exception"); } } catch (Exception) { EditorOutput.WriteLine("Error in union polygon generation, using better default"); try { this.UpdateIntersectionPolygon(ai); } catch (Exception) { EditorOutput.WriteLine("Error in my simple polygon generation, plain default"); List<Coordinates> cs = new List<Coordinates>(); foreach (ITraversableWaypoint itw in ai.AllEntries.Values) cs.Add(itw.Position); foreach (ITraversableWaypoint itw in ai.AllExits.Values) { cs.Add(itw.Position); foreach (ArbiterInterconnect aint in itw.Exits) { cs.AddRange(aint.TurnPolygon); } } ai.IntersectionPolygon = Polygon.GrahamScan(cs); } } // add intersection /*arn.ArbiterIntersections.Add(aii, ai); // add to exit lookup foreach (IAreaSubtypeWaypointId awi in exits.Keys) arn.IntersectionLookup.Add(awi, ai); // add to display objects arn.DisplayObjects.Add(ai); rd.AddDisplayObject(ai);*/ } } }
/// <summary> /// finish the wrapping and cretae the intersection /// </summary> public void FinalizeIntersection() { // finalize the intersection polygon Polygon sq = this.CreateSquarePolygon(this.WrapInitial.Value, this.WrapFinal.Value).Inflate(1.0); Polygon interPoly = this.CreateIntersectionPolygon(sq); // check poly not null if (interPoly != null) { // inflate the inter poly interPoly = interPoly.Inflate(0.05); // retreive all exits involved in this intersection Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint> exits = this.IntersectionExits(sq); // make sure the inter contains an exit if (exits.Count > 0) { // make stopped exits, necessarily these are arbiter waypoints not perimeter points List<ArbiterStoppedExit> ases = this.CreateStoppedExits(exits.Values); // construct intersection id ITraversableWaypoint[] exitArray = new ITraversableWaypoint[exits.Count]; exits.Values.CopyTo(exitArray, 0); ArbiterIntersectionId aii = new ArbiterIntersectionId(exitArray[0].Exits[0].InterconnectId); // determine incoming lanes Dictionary<ArbiterLane, LinePath.PointOnPath> incoming = this.DetermineIncoming(interPoly); // create the intersection ArbiterIntersection ai = new ArbiterIntersection( interPoly, ases, this.DetermineInvolved(exits.Values, incoming), incoming, exits, interPoly.Center, aii, arn, this.IntersectionEntries(sq) ); // create safety zones this.CreateSafetyImplicitZones(ai); // update poly //this.UpdateIntersectionPolygon(ai); /*List<Polygon> ps = new List<Polygon>(); foreach (ITraversableWaypoint itw in exits.Values) { foreach (ArbiterInterconnect ait in itw.Exits) { ps.Add(ait.TurnPolygon); } } ai.IntersectionPolygon = this.GetIntersectionPolygon(ps, ai.AllExits, ai.AllEntries);*/ //ai.IntersectionPolygon = UrbanChallenge.Arbiter.Core.Common.Tools.PolygonToolkit.PolygonUnion(ps); try { List<Polygon> ps = new List<Polygon>(); foreach (ITraversableWaypoint itw in exits.Values) { foreach (ArbiterInterconnect ait in itw.Exits) { ps.Add(ait.TurnPolygon); } } ai.IntersectionPolygon = this.GetIntersectionPolygon(ps, ai.AllExits, ai.AllEntries); if (ai.IntersectionPolygon.IsComplex) { EditorOutput.WriteLine("Intersection polygon complex, defaulting"); throw new Exception("complex polygon exception"); } } catch (Exception) { EditorOutput.WriteLine("Error in union polygon generation, using better default"); try { this.UpdateIntersectionPolygon(ai); } catch (Exception) { EditorOutput.WriteLine("Error in my simple polygon generation, plain default"); List<Coordinates> cs = new List<Coordinates>(); foreach (ITraversableWaypoint itw in ai.AllEntries.Values) cs.Add(itw.Position); foreach (ITraversableWaypoint itw in ai.AllExits.Values) { cs.Add(itw.Position); foreach (ArbiterInterconnect aint in itw.Exits) { cs.AddRange(aint.TurnPolygon); } } ai.IntersectionPolygon = Polygon.GrahamScan(cs); } } try { // add intersection arn.ArbiterIntersections.Add(aii, ai); // add to exit lookup foreach (IAreaSubtypeWaypointId awi in exits.Keys) { if (arn.IntersectionLookup.ContainsKey(awi)) arn.IntersectionLookup[awi] = ai; else arn.IntersectionLookup.Add(awi, ai); } // add to display objects arn.DisplayObjects.Add(ai); rd.AddDisplayObject(ai); } catch (Exception e) { EditorOutput.WriteLine("Error adding intersection: " + aii.ToString()); EditorOutput.WriteLine("Error adding intersection: " + e.ToString()); } } } // reset the tool this.it.ResetIcons(); this.WrapFinal = null; this.WrapInitial = null; }
/// <summary> /// Constructor /// </summary> /// <param name="waypoint"></param> /// <param name="entries"></param> public IntersectionPlan(ITraversableWaypoint waypoint, List<PlanableInterconnect> entries, RoadPlan roadPlan) { this.ExitWaypoint = waypoint; this.PossibleEntries = entries; this.SegmentPlan = roadPlan; }
/// <summary> /// Constructor /// </summary> /// <param name="ourExit"></param> public IntersectionMonitor(ITraversableWaypoint ourExit, ArbiterIntersection ai, VehicleState ourState, IConnectAreaWaypoints desired) { // set intersection this.Intersection = ai; // create monitors this.AllMonitors = new List<IIntersectionQueueable>(); this.PriorityMonitors = new List<IIntersectionQueueable>(); this.IntersectionPriorityQueue = new List<IIntersectionQueueable>(); this.InsideIntersectionVehicles = new List<IntersectionVehicleMonitor>(); this.NonStopPriorityAreas = new List<IDominantMonitor>(); this.PreviouslyWaiting = new List<IDominantMonitor>(); this.AllMonitors = new List<IIntersectionQueueable>(); this.cooldownTimer = new Stopwatch(); #region Stopped Exits // get ours IIntersectionQueueable ourMonitor = null; // get stopped exits foreach(ArbiterStoppedExit ase in ai.StoppedExits) { // add a monitor SubmissiveEntryMonitor sem = new SubmissiveEntryMonitor(this, ase.Waypoint, ase.Waypoint.Equals(ourExit)); // add to monitors PriorityMonitors.Add(sem); // check not our if(!sem.IsOurs) { // initial update sem.Update(ourState); // check if add first if(!sem.Clear(ourState)) IntersectionPriorityQueue.Add(sem); } else { // set ours ourMonitor = sem; this.OurMonitor = ourMonitor; } } // check if our monitor exists if (ourMonitor != null) { // add ours this.IntersectionPriorityQueue.Add(ourMonitor); } #endregion #region Priority Areas Over Interconnect // check contains priority lane for this if (ai.PriorityLanes.ContainsKey(desired.ToInterconnect)) { // loop through all other priority monitors over this interconnect foreach (IntersectionInvolved ii in ai.PriorityLanes[desired.ToInterconnect]) { // check area type if lane if (ii.Area is ArbiterLane) { // add lane to non stop priority areas this.NonStopPriorityAreas.Add(new DominantLaneEntryMonitor(this, ii)); } // otherwise is zone else if (ii.Area is ArbiterZone) { // add lane to non stop priority areas this.NonStopPriorityAreas.Add( new DominantZoneEntryMonitor((ArbiterPerimeterWaypoint)ii.Exit, ((ArbiterInterconnect)desired), false, this, ii)); } else { throw new ArgumentException("unknown intersection involved area", "ii"); } } } // otherwise be like, wtf? else { ArbiterOutput.Output("Exception: intersection: " + this.Intersection.ToString() + " priority lanes does not contain interconnect: " + desired.ToString() + " returning can go"); //ArbiterOutput.Output("Error in intersection monitor!!!!!!!!!@Q!!, desired interconnect: " + desired.ToInterconnect.ToString() + " not found in intersection: " + ai.ToString() + ", not setting any dominant monitors"); } #endregion #region Entry Area if (desired.FinalGeneric is ArbiterWaypoint) { this.EntryAreaMonitor = new LaneEntryAreaMonitor((ArbiterWaypoint)desired.FinalGeneric); } else if (desired.FinalGeneric is ArbiterPerimeterWaypoint) { this.EntryAreaMonitor = new ZoneAreaEntryMonitor((ArbiterPerimeterWaypoint)desired.FinalGeneric, (ArbiterInterconnect)desired, false, this, new IntersectionInvolved(ourExit, ((ArbiterPerimeterWaypoint)desired.FinalGeneric).Perimeter.Zone, ((ArbiterInterconnect)desired).TurnDirection)); } else { throw new Exception("unhandled desired interconnect final type"); } #endregion #region Our Monitor // check ours if (ourExit is ArbiterWaypoint) { this.OurMonitor = new DominantLaneEntryMonitor(this, new IntersectionInvolved(ourExit, ((ArbiterWaypoint)ourExit).Lane, desired is ArbiterInterconnect ? ((ArbiterInterconnect)desired).TurnDirection : ArbiterTurnDirection.Straight)); } else if (ourExit is ArbiterPerimeterWaypoint) { // add lane to non stop priority areas this.OurMonitor = new DominantZoneEntryMonitor((ArbiterPerimeterWaypoint)desired.InitialGeneric, ((ArbiterInterconnect)desired), true, this, new IntersectionInvolved(ourExit, ((ArbiterPerimeterWaypoint)desired.InitialGeneric).Perimeter.Zone, ((ArbiterInterconnect)desired).TurnDirection)); } #endregion // add to all this.AllMonitors.AddRange(this.PriorityMonitors); foreach (IIntersectionQueueable iiq in this.NonStopPriorityAreas) this.AllMonitors.Add(iiq); this.AllMonitors.Add(this.EntryAreaMonitor); // update all this.Update(ourState); // check if we need to populate previously waiting if(this.OurMonitor is IDominantMonitor) { // cast our IDominantMonitor ours = (IDominantMonitor)this.OurMonitor; // loop through to determine those previously waiting foreach (IDominantMonitor idm in this.NonStopPriorityAreas) { if(idm.Waypoint != null && !idm.Waypoint.IsStop && idm.WaitingTimerRunning) this.PreviouslyWaiting.Add(idm); } } }
public IntersectionInvolved(IVehicleArea area) { this.Exit = null; this.Area = area; this.TurnDirection = ArbiterTurnDirection.Unknown; }
/// <summary> /// Constructor /// </summary> /// <param name="ourExit"></param> public IntersectionMonitor(ITraversableWaypoint ourExit, ArbiterIntersection ai, VehicleState ourState, IConnectAreaWaypoints desired) { // set intersection this.Intersection = ai; // create monitors this.AllMonitors = new List <IIntersectionQueueable>(); this.PriorityMonitors = new List <IIntersectionQueueable>(); this.IntersectionPriorityQueue = new List <IIntersectionQueueable>(); this.InsideIntersectionVehicles = new List <IntersectionVehicleMonitor>(); this.NonStopPriorityAreas = new List <IDominantMonitor>(); this.PreviouslyWaiting = new List <IDominantMonitor>(); this.AllMonitors = new List <IIntersectionQueueable>(); this.cooldownTimer = new Stopwatch(); #region Stopped Exits // get ours IIntersectionQueueable ourMonitor = null; // get stopped exits foreach (ArbiterStoppedExit ase in ai.StoppedExits) { // add a monitor SubmissiveEntryMonitor sem = new SubmissiveEntryMonitor(this, ase.Waypoint, ase.Waypoint.Equals(ourExit)); // add to monitors PriorityMonitors.Add(sem); // check not our if (!sem.IsOurs) { // initial update sem.Update(ourState); // check if add first if (!sem.Clear(ourState)) { IntersectionPriorityQueue.Add(sem); } } else { // set ours ourMonitor = sem; this.OurMonitor = ourMonitor; } } // check if our monitor exists if (ourMonitor != null) { // add ours this.IntersectionPriorityQueue.Add(ourMonitor); } #endregion #region Priority Areas Over Interconnect // check contains priority lane for this if (ai.PriorityLanes.ContainsKey(desired.ToInterconnect)) { // loop through all other priority monitors over this interconnect foreach (IntersectionInvolved ii in ai.PriorityLanes[desired.ToInterconnect]) { // check area type if lane if (ii.Area is ArbiterLane) { // add lane to non stop priority areas this.NonStopPriorityAreas.Add(new DominantLaneEntryMonitor(this, ii)); } // otherwise is zone else if (ii.Area is ArbiterZone) { // add lane to non stop priority areas this.NonStopPriorityAreas.Add( new DominantZoneEntryMonitor((ArbiterPerimeterWaypoint)ii.Exit, ((ArbiterInterconnect)desired), false, this, ii)); } else { throw new ArgumentException("unknown intersection involved area", "ii"); } } } // otherwise be like, wtf? else { ArbiterOutput.Output("Exception: intersection: " + this.Intersection.ToString() + " priority lanes does not contain interconnect: " + desired.ToString() + " returning can go"); //ArbiterOutput.Output("Error in intersection monitor!!!!!!!!!@Q!!, desired interconnect: " + desired.ToInterconnect.ToString() + " not found in intersection: " + ai.ToString() + ", not setting any dominant monitors"); } #endregion #region Entry Area if (desired.FinalGeneric is ArbiterWaypoint) { this.EntryAreaMonitor = new LaneEntryAreaMonitor((ArbiterWaypoint)desired.FinalGeneric); } else if (desired.FinalGeneric is ArbiterPerimeterWaypoint) { this.EntryAreaMonitor = new ZoneAreaEntryMonitor((ArbiterPerimeterWaypoint)desired.FinalGeneric, (ArbiterInterconnect)desired, false, this, new IntersectionInvolved(ourExit, ((ArbiterPerimeterWaypoint)desired.FinalGeneric).Perimeter.Zone, ((ArbiterInterconnect)desired).TurnDirection)); } else { throw new Exception("unhandled desired interconnect final type"); } #endregion #region Our Monitor // check ours if (ourExit is ArbiterWaypoint) { this.OurMonitor = new DominantLaneEntryMonitor(this, new IntersectionInvolved(ourExit, ((ArbiterWaypoint)ourExit).Lane, desired is ArbiterInterconnect ? ((ArbiterInterconnect)desired).TurnDirection : ArbiterTurnDirection.Straight)); } else if (ourExit is ArbiterPerimeterWaypoint) { // add lane to non stop priority areas this.OurMonitor = new DominantZoneEntryMonitor((ArbiterPerimeterWaypoint)desired.InitialGeneric, ((ArbiterInterconnect)desired), true, this, new IntersectionInvolved(ourExit, ((ArbiterPerimeterWaypoint)desired.InitialGeneric).Perimeter.Zone, ((ArbiterInterconnect)desired).TurnDirection)); } #endregion // add to all this.AllMonitors.AddRange(this.PriorityMonitors); foreach (IIntersectionQueueable iiq in this.NonStopPriorityAreas) { this.AllMonitors.Add(iiq); } this.AllMonitors.Add(this.EntryAreaMonitor); // update all this.Update(ourState); // check if we need to populate previously waiting if (this.OurMonitor is IDominantMonitor) { // cast our IDominantMonitor ours = (IDominantMonitor)this.OurMonitor; // loop through to determine those previously waiting foreach (IDominantMonitor idm in this.NonStopPriorityAreas) { if (idm.Waypoint != null && !idm.Waypoint.IsStop && idm.WaitingTimerRunning) { this.PreviouslyWaiting.Add(idm); } } } }
/// <summary> /// Constructor /// </summary> /// <param name="waypoint"></param> /// <param name="entries"></param> public IntersectionPlan(ITraversableWaypoint waypoint, List <PlanableInterconnect> entries, RoadPlan roadPlan) { this.ExitWaypoint = waypoint; this.PossibleEntries = entries; this.SegmentPlan = roadPlan; }
/// <summary> /// Plans over an intersection /// </summary> /// <param name="exitWaypoint"></param> /// <param name="goal"></param> /// <returns></returns> public IntersectionPlan PlanIntersection(ITraversableWaypoint exitWaypoint, INavigableNode goal) { // road plan if the itnersection has a road available to take from it RoadPlan rp = null; // check if road waypoint if (exitWaypoint is ArbiterWaypoint) { // get exit ArbiterWaypoint awp = (ArbiterWaypoint)exitWaypoint; // check if it has lane partition moving outwards if (awp.NextPartition != null) { // road plan ignoring exit List<ArbiterWaypoint> iws = RoadToolkit.WaypointsClose(awp.Lane.Way, awp.Position, awp); rp = this.PlanNavigableArea(awp.Lane, awp.Position, goal, iws); } } // get exit plan IntersectionPlan ip = this.GetIntersectionExitPlan(exitWaypoint, goal); // add road plan if exists ip.SegmentPlan = rp; // return the plan return ip; }
/// <summary> /// Plan over all exits /// </summary> /// <param name="exitWaypoint"></param> /// <param name="goal"></param> /// <returns></returns> public IntersectionPlan GetIntersectionExitPlan(ITraversableWaypoint exitWaypoint, INavigableNode goal) { // initialize the intersection plan IntersectionPlan ip = new IntersectionPlan(exitWaypoint, new List<PlanableInterconnect>(), null); ip.ExitWaypoint = exitWaypoint; //check valid exit if (exitWaypoint.IsExit) { // plan over each interconnect foreach (ArbiterInterconnect ai in exitWaypoint.Exits) { // start of plan is the final wp of itner INavigableNode start = (INavigableNode)ai.FinalGeneric; // plan double time; List<INavigableNode> nodes; this.Plan(start, goal, out nodes, out time); time += ai.TimeCost(); // create planned interconnect PlanableInterconnect pi = new PlanableInterconnect(ai, time, nodes); // add planned interconnect to the intersection plan ip.PossibleEntries.Add(pi); } } // return the plan return ip; }
/// <summary> /// Constructor /// </summary> /// <param name="exit"></param> /// <param name="area"></param> /// <param name="turnDirection"></param> public IntersectionInvolved(ITraversableWaypoint exit, IVehicleArea area, ArbiterTurnDirection turnDirection) { this.Exit = exit; this.Area = area; this.TurnDirection = turnDirection; }
/// <summary> /// Try to plan the intersection heavily penalizing the interconnect /// </summary> /// <param name="iTraversableWaypoint"></param> /// <param name="iArbiterWaypoint"></param> /// <param name="arbiterInterconnect"></param> /// <returns></returns> public IntersectionPlan PlanIntersectionWithoutInterconnect(ITraversableWaypoint exit, IArbiterWaypoint goal, ArbiterInterconnect interconnect, bool blockAllRelated) { ITraversableWaypoint entry = (ITraversableWaypoint)interconnect.FinalGeneric; if (!blockAllRelated) return this.PlanIntersectionWithoutInterconnect(exit, goal, interconnect); else { Dictionary<IConnectAreaWaypoints, NavigationBlockage> saved = new Dictionary<IConnectAreaWaypoints, NavigationBlockage>(); if (entry.IsEntry) { foreach (ArbiterInterconnect ai in entry.Entries) { saved.Add(ai, ai.Blockage); // create new NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue); newerBlockage.BlockageExists = true; ai.Blockage = newerBlockage; } } if (entry is ArbiterWaypoint && ((ArbiterWaypoint)entry).PreviousPartition != null) { ArbiterLanePartition alp = ((ArbiterWaypoint)entry).PreviousPartition; saved.Add(alp, alp.Blockage); // create new NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue); newerBlockage.BlockageExists = true; alp.Blockage = newerBlockage; } KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>> tmpCurrentTimes = currentTimes; this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>(); // plan IntersectionPlan ip = this.PlanIntersection(exit, goal); this.currentTimes = tmpCurrentTimes; // reset the blockages foreach (KeyValuePair<IConnectAreaWaypoints, NavigationBlockage> savedPair in saved) savedPair.Key.Blockage = savedPair.Value; // return plan return ip; } }