/// <summary> /// Pupulates all the priority monitors given the information we know right at this moment /// </summary> public void PopulateMonitor(IArbiterWaypoint ourExit) { // 1. Generate dependencies for each lane // 2. Assign stopped vehicles at stops to stop lanes // 3. Given dependencies populate the stop queues for each stopped vehicle // 4. Assign stopped vehicles close to exits in priority lanes to their priority monitor // 5. given our exit assign our dependencies }
/// <summary> /// Constructor /// </summary> /// <param name="initial"></param> /// <param name="final"></param> public ArbiterInterconnect(IArbiterWaypoint initial, IArbiterWaypoint final) : base(false, null, false, null, new List <IConnectAreaWaypoints>(), initial, final) { this.initialWaypoint = initial; this.finalWaypoint = final; this.InterconnectId = new ArbiterInterconnectId(initialWaypoint.AreaSubtypeWaypointId, finalWaypoint.AreaSubtypeWaypointId); // create a path of the partition and get the closest List <Coordinates> ips = new List <Coordinates>(); ips.Add(initial.Position); ips.Add(final.Position); this.InterconnectPath = new LinePath(ips); // nav edge stuff this.Contained.Add(this); this.blockage = new NavigationBlockage(0.0); this.TurnPolygon = this.DefaultPoly(); this.InnerCoordinates = new List <Coordinates>(); }
/// <summary> /// Get a road plan while setting partition costs very high /// </summary> /// <param name="partition"></param> /// <param name="goal"></param> /// <param name="blockAdjacent"></param> /// <param name="c"></param> /// <returns></returns> public RoadPlan PlanRoadOppositeWithoutPartition(ArbiterLanePartition partition, ArbiterLanePartition opposite, IArbiterWaypoint goal, bool blockAdjacent, Coordinates c, bool sameWay) { KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes; this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >(); RoadPlan rp = null; if (!blockAdjacent) { NavigationBlockage nb = partition.Blockage; NavigationBlockage tmp = new NavigationBlockage(double.MaxValue); partition.Blockage = tmp; rp = this.PlanNavigableArea(partition.Lane, c, goal, new List <ArbiterWaypoint>()); partition.Blockage = nb; } else { // save List <KeyValuePair <ArbiterLanePartition, NavigationBlockage> > savedBlockages = new List <KeyValuePair <ArbiterLanePartition, NavigationBlockage> >(); // set savedBlockages.Add(new KeyValuePair <ArbiterLanePartition, NavigationBlockage>(partition, partition.Blockage)); // create new NavigationBlockage anewerBlockage = new NavigationBlockage(Double.MaxValue); anewerBlockage.BlockageExists = true; partition.Blockage = anewerBlockage; foreach (ArbiterLanePartition alp in partition.NonLaneAdjacentPartitions) { if (alp.IsInside(c) && (!sameWay || (sameWay && partition.Lane.Way.Equals(alp.Lane.Way)))) { savedBlockages.Add(new KeyValuePair <ArbiterLanePartition, NavigationBlockage>(alp, alp.Blockage)); // create new NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue); newerBlockage.BlockageExists = true; alp.Blockage = newerBlockage; } } // plan rp = this.PlanNavigableArea(opposite.Lane, c, goal, new List <ArbiterWaypoint>()); // restore foreach (KeyValuePair <ArbiterLanePartition, NavigationBlockage> saved in savedBlockages) { saved.Key.Blockage = saved.Value; } } // restore this.currentTimes = tmpCurrentTimes; // return return(rp); }
/// <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> /// 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); }
public static INavigableNode FilterGoal(VehicleState state) { // get goal INavigableNode goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null; if (waitRemoveLastGoal && CoreCommon.Mission.MissionCheckpoints.Count != 1) { waitRemoveLastGoal = false; } // id IArbiterWaypoint goalWp = null; if (goal != null) { goalWp = CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]; } // check lane change or opposing if (goal != null && (CoreCommon.CorePlanningState is OpposingLanesState && ((OpposingLanesState)CoreCommon.CorePlanningState).HitGoal(state, goal.Position, goalWp.AreaSubtypeWaypointId)) || (CoreCommon.CorePlanningState is ChangeLanesState && ((ChangeLanesState)CoreCommon.CorePlanningState).HitGoal(state, goal.Position, goalWp.AreaSubtypeWaypointId))) { if (CoreCommon.Mission.MissionCheckpoints.Count == 1) { waitRemoveLastGoal = true; ArbiterOutput.Output("Waiting to remove last Checkpoint: " + goal.ToString()); } else { // set hit ArbiterOutput.Output("Reached Checkpoint: " + goal.ToString()); CoreCommon.Mission.MissionCheckpoints.Dequeue(); // update goal goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null; } } else if (goal != null && CoreCommon.Mission.MissionCheckpoints.Count == 1 && waitRemoveLastGoal && (CoreCommon.CorePlanningState is StayInLaneState || CoreCommon.CorePlanningState is StayInSupraLaneState)) { // set hit ArbiterOutput.Output("Wait over, Reached Checkpoint: " + goal.ToString()); CoreCommon.Mission.MissionCheckpoints.Dequeue(); // update goal goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null; } // TODO implement full version of hit test // check if we have hit the goal (either by being in opposing lane or going to opposing and next to it or in lane and pass over it else if (goal != null) { bool reachedCp = false; if (CoreCommon.CorePlanningState is StayInLaneState) { StayInLaneState sils = (StayInLaneState)CoreCommon.CorePlanningState; if (goal is ArbiterWaypoint && ((ArbiterWaypoint)goal).Lane.Equals(sils.Lane)) { if (CoreCommon.Mission.MissionCheckpoints.Count != 1) { double distanceAlong = sils.Lane.DistanceBetween(state.Front, goal.Position); if (Math.Abs(distanceAlong) < 1.5 + (1.5 * CoreCommon.Communications.GetVehicleSpeed().Value) / 5.0) { reachedCp = true; } } else { double distanceAlong = sils.Lane.DistanceBetween(state.Front, goal.Position); double distanceAlong2 = sils.Lane.DistanceBetween(state.Position, goal.Position); if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.005 && Math.Abs(distanceAlong) < 0.3 || CoreCommon.Communications.GetVehicleState().VehiclePolygon.IsInside(goal.Position) || (distanceAlong <= 0.0 && distanceAlong2 >= 0)) { reachedCp = true; } } } } else if (CoreCommon.CorePlanningState is ChangeLanesState) { ChangeLanesState cls = (ChangeLanesState)CoreCommon.CorePlanningState; if (cls.Parameters.Initial.Way.Equals(cls.Parameters.Target.Way) && goal is ArbiterWaypoint && ((ArbiterWaypoint)goal).Lane.Equals(cls.Parameters.Target)) { double distanceAlong = cls.Parameters.Target.DistanceBetween(state.Front, goal.Position); if (Math.Abs(distanceAlong) < 1.5 + (1.5 * CoreCommon.Communications.GetVehicleSpeed().Value) / 5.0) { reachedCp = true; ArbiterOutput.Output("Removed goal changing lanes"); } } } if (reachedCp) { // set hit ArbiterOutput.Output("Reached Checkpoint: " + goal.ToString()); CoreCommon.Mission.MissionCheckpoints.Dequeue(); // update goal goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null; } } // set goal info CoreCommon.CurrentInformation.RouteCheckpoint = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? goal.ToString() : "NONE"; CoreCommon.CurrentInformation.GoalsRemaining = CoreCommon.Mission.MissionCheckpoints.Count.ToString(); CoreCommon.CurrentInformation.RouteCheckpointId = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() : "NONE"; // return current return(goal); }
/// <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> /// Get a road plan while setting partition costs very high /// </summary> /// <param name="partition"></param> /// <param name="goal"></param> /// <param name="blockAdjacent"></param> /// <param name="c"></param> /// <returns></returns> public RoadPlan PlanRoadOppositeWithoutPartition(ArbiterLanePartition partition, ArbiterLanePartition opposite, IArbiterWaypoint goal, bool blockAdjacent, Coordinates c, bool sameWay) { KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>> tmpCurrentTimes = currentTimes; this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>(); RoadPlan rp = null; if (!blockAdjacent) { NavigationBlockage nb = partition.Blockage; NavigationBlockage tmp = new NavigationBlockage(double.MaxValue); partition.Blockage = tmp; rp = this.PlanNavigableArea(partition.Lane, c, goal, new List<ArbiterWaypoint>()); partition.Blockage = nb; } else { // save List<KeyValuePair<ArbiterLanePartition, NavigationBlockage>> savedBlockages = new List<KeyValuePair<ArbiterLanePartition, NavigationBlockage>>(); // set savedBlockages.Add(new KeyValuePair<ArbiterLanePartition,NavigationBlockage>(partition, partition.Blockage)); // create new NavigationBlockage anewerBlockage = new NavigationBlockage(Double.MaxValue); anewerBlockage.BlockageExists = true; partition.Blockage = anewerBlockage; foreach (ArbiterLanePartition alp in partition.NonLaneAdjacentPartitions) { if (alp.IsInside(c) && (!sameWay || (sameWay && partition.Lane.Way.Equals(alp.Lane.Way)))) { savedBlockages.Add(new KeyValuePair<ArbiterLanePartition,NavigationBlockage>(alp, alp.Blockage)); // create new NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue); newerBlockage.BlockageExists = true; alp.Blockage = newerBlockage; } } // plan rp = this.PlanNavigableArea(opposite.Lane, c, goal, new List<ArbiterWaypoint>()); // restore foreach (KeyValuePair<ArbiterLanePartition, NavigationBlockage> saved in savedBlockages) { saved.Key.Blockage = saved.Value; } } // restore this.currentTimes = tmpCurrentTimes; // return return rp; }
/// <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> /// 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> /// Gets priotiy lane determination /// </summary> /// <param name="ii"></param> /// <param name="path"></param> /// <param name="end"></param> /// <returns></returns> private Coordinates?LaneIntersectsPath(IntersectionInvolved ii, LinePath path, IArbiterWaypoint end) { ArbiterLane al = (ArbiterLane)ii.Area; LinePath.PointOnPath current = path.StartPoint; bool go = true; while (go) // && !(current.Location.DistanceTo(path.EndPoint.Location) < 0.1)) { Coordinates alClose = al.LanePath().GetClosestPoint(current.Location).Location; double alDist = alClose.DistanceTo(current.Location); if (alDist <= 0.05) { return(al.LanePath().GetClosestPoint(current.Location).Location); } if (current.Location.Equals(path.EndPoint.Location)) { go = false; } current = path.AdvancePoint(current, 0.1); } /*if (ii.Exit != null) * { * ITraversableWaypoint laneExit = ii.Exit; * foreach (ArbiterInterconnect tmpAi in laneExit.OutgoingConnections) * { * if (tmpAi.FinalGeneric.Equals(end)) * { * return tmpAi.FinalGeneric.Position; * } * } * }*/ return(null); }