/// <summary>
/// Plan between two nodes
/// </summary>
/// <param name="start"></param>
/// <param name="goal"></param>
/// <param name="nodes"></param>
/// <param name="time"></param>
public void Plan(INavigableNode start, INavigableNode goal, out List <INavigableNode> nodes, out double time)
{
aStar astar = new aStar(start, goal, new List <NavigableEdge>());
nodes = astar.Plan();
time = astar.TotalTime;
}
public NavigableEdge GetClosestNavigableEdge(Coordinates c)
{
try
{
int i = this.RecommendedPath.GetClosestPoint(c).Index;
Coordinates rpi = this.RecommendedPath[i];
for (int j = 0; j < PathNodes.Count; j++)
{
INavigableNode inn = PathNodes[j];
if (inn.Position.Equals(rpi))
{
foreach (NavigableEdge ne in inn.OutgoingConnections)
{
if (ne.End.Equals(PathNodes[j + 1]))
{
return(ne);
}
}
}
}
}
catch (Exception)
{
}
return(null);
}
/// <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>
/// Directed edge between two INavigableNodes
/// </summary>
/// <remarks>Can be made up of multiple partitions or zones</remarks>
public NavigableEdge(bool isZone, ArbiterZone zone, bool isSegment, ArbiterSegment segment, List <IConnectAreaWaypoints> contained,
INavigableNode start, INavigableNode end)
{
this.Start = start;
this.End = end;
this.Segment = segment;
this.IsSegment = isSegment;
this.Zone = zone;
this.IsZone = isZone;
this.Contained = contained == null ? new List <IConnectAreaWaypoints>() : contained;
this.standardEdgeDistance = this.Start.Position.DistanceTo(this.End.Position);
}
/// <summary>
/// Seed the planner, making it non blocking
/// </summary>
/// <param name="downstreamPoints"></param>
/// <param name="goal"></param>
/// <param name="way"></param>
private void SeedPlanner(List <DownstreamPointOfInterest> downstreamPoints, INavigableNode goal)
{
seedPlannerThread = new Thread(
delegate()
{
this.RouteTimes(downstreamPoints, goal);
}
);
seedPlannerThread.IsBackground = true;
seedPlannerThread.Priority = ThreadPriority.AboveNormal;
seedPlannerThread.Start();
}
/// <summary>
/// Gets edge going to input node from this exit
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
public ArbiterInterconnect GetEdge(INavigableNode node)
{
foreach (NavigableEdge ne in exit.OutgoingConnections)
{
if (ne is ArbiterInterconnect)
{
ArbiterInterconnect ai = (ArbiterInterconnect)ne;
if (ai.FinalGeneric.Equals(node))
{
return(ai);
}
}
}
return(null);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="start">Node to start plan</param>
/// <param name="goal">Node to end plan</param>
/// <param name="removed">Nodes to not plan over</param>
public aStar(INavigableNode start, INavigableNode goal, List<NavigableEdge> removed)
{
// initialize OPEN and CLOSED lists
this.open = new PriorityHeap();
this.closed = new Dictionary<string, INavigableNode>(CoreCommon.RoadNetwork.ArbiterWaypoints.Count);
// set start and null backpointer
this.start = start;
this.start.Previous = null;
this.start.ResetPlanningCosts();
// set goal and null backpointer
this.goal = goal;
this.goal.Previous = null;
this.goal.ResetPlanningCosts();
// set removed
this.removedEdges = removed;
}
public void Reset(bool resetPrevious)
{
this.WrappingHelpers = new List <Coordinates>();
this.PreviousNode = null;
if (resetPrevious)
{
this.zt.Mode = ZoneToolboxMode.None;
}
this.rightClickNode = null;
this.rightClickEdge = null;
if (this.moveNode != null)
{
this.moveNode.Position = this.moveOriginalCoords;
}
this.moveNode = null;
}
public void BeginMove(Coordinates c)
{
if (this.Mode == ZoneToolboxMode.NavNodes && this.zt.current != null)
{
// nullify
this.Reset(false);
// determine what we selected
NavigableEdge ne;
INavigableNode nn;
this.NavigationHitTest(c, out nn, out ne);
if (nn != null && !(nn is ArbiterPerimeterWaypoint))
{
this.moveNode = nn;
this.moveOriginalCoords = this.moveNode.Position;
}
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="start">Node to start plan</param>
/// <param name="goal">Node to end plan</param>
/// <param name="removed">Nodes to not plan over</param>
public aStar(INavigableNode start, INavigableNode goal, List <NavigableEdge> removed)
{
// initialize OPEN and CLOSED lists
this.open = new PriorityHeap();
this.closed = new Dictionary <string, INavigableNode>(CoreCommon.RoadNetwork.ArbiterWaypoints.Count);
// set start and null backpointer
this.start = start;
this.start.Previous = null;
this.start.ResetPlanningCosts();
// set goal and null backpointer
this.goal = goal;
this.goal.Previous = null;
this.goal.ResetPlanningCosts();
// set removed
this.removedEdges = removed;
}
public void RightClick(Coordinates c)
{
if (this.Mode == ZoneToolboxMode.NavNodes && this.zt.current != null)
{
// nullify
this.Reset(false);
// determine what we selected
NavigableEdge ne;
INavigableNode nn;
this.NavigationHitTest(c, out nn, out ne);
if (nn != null)
{
this.rightClickNode = nn;
}
else if (ne != null)
{
this.rightClickEdge = ne;
}
}
}
public double TimeTo(INavigableNode node)
{
// avg speed of 10mph from place to place
return this.position.DistanceTo(node.Position) / 4.0;
}
/// <summary>
/// Plans the next maneuver
/// </summary>
/// <param name="roads"></param>
/// <param name="mission"></param>
/// <param name="vehicleState"></param>
/// <param name="CoreCommon.CorePlanningState"></param>
/// <param name="observedVehicles"></param>
/// <param name="observedObstacles"></param>
/// <param name="coreState"></param>
/// <param name="carMode"></param>
/// <returns></returns>
public Maneuver Plan(VehicleState vehicleState, double vehicleSpeed,
SceneEstimatorTrackedClusterCollection observedVehicles, SceneEstimatorUntrackedClusterCollection observedObstacles,
CarMode carMode, INavigableNode goal)
{
// set blockages
List<ITacticalBlockage> blockages = this.blockageHandler.DetermineBlockages(CoreCommon.CorePlanningState);
#region Travel State
if (CoreCommon.CorePlanningState is TravelState)
{
#region Stay in Lane State
if (CoreCommon.CorePlanningState is StayInLaneState)
{
// get lane state
StayInLaneState sils = (StayInLaneState)CoreCommon.CorePlanningState;
#region Blockages
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is LaneBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// check not from a dynamicly moving vehicle
if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
{
// go to a blockage handling tactical
return new Maneuver(new HoldBrakeBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
ArbiterOutput.Output("Lane blockage reported for moving vehicle, ignoring");
}
#endregion
// update the total time ignorable have been seen
sils.UpdateIgnoreList();
// nav plan to find poi
RoadPlan rp = navigation.PlanNavigableArea(sils.Lane, vehicleState.Position, goal, sils.WaypointsToIgnore);
// check for unreachable route
if (rp.BestPlan.laneWaypointOfInterest.BestRoute != null &&
rp.BestPlan.laneWaypointOfInterest.BestRoute.Count == 0 &&
rp.BestPlan.laneWaypointOfInterest.RouteTime >= Double.MaxValue - 1.0)
{
ArbiterOutput.Output("Removed Unreachable Checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else if (rp.BestPlan.laneWaypointOfInterest.TimeCostToPoint >= Double.MaxValue - 1.0)
{
ArbiterOutput.Output("Best Lane Waypoint of Interest is END OF LANE WITH NO INTERCONNECTS, LEADING NOWHERE");
ArbiterOutput.Output("Removed Unreachable Checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#region Check Supra Lane Availability
// if the poi is at the end of this lane, is not stop, leads to another lane, and has no overlapping lanes
// or if the poi's best exit is an exit in this lane, is not a stop, has no overlapping lanes and leads to another lane
// create supralane
// check if navigation is corrent in saying we want to continue on the current lane and we're far enough along the lane, 30m for now
if(rp.BestPlan.Lane.Equals(sils.Lane.LaneId))
{
// get navigation poi
DownstreamPointOfInterest dpoi = rp.BestPlan.laneWaypointOfInterest;
// check that the poi is not stop and is not the current checkpoint
if(!dpoi.PointOfInterest.IsStop && !(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(dpoi.PointOfInterest.WaypointId)))
{
// get the best exit or the poi
ArbiterInterconnect ai = dpoi.BestExit;
// check if exit goes into a lane and not a uturn
if(ai != null && ai.FinalGeneric is ArbiterWaypoint && ai.TurnDirection != ArbiterTurnDirection.UTurn)
{
// final lane or navigation poi interconnect
ArbiterLane al = ((ArbiterWaypoint)ai.FinalGeneric).Lane;
// check not same lane
if (!al.Equals(sils.Lane))
{
// check if enough room to start
bool enoughRoom = !sils.Lane.Equals(al) || sils.Lane.LanePath(sils.Lane.WaypointList[0].Position, vehicleState.Front).PathLength > 30;
if (enoughRoom)
{
// try to get intersection associated with the exit
ArbiterIntersection aInter = CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(dpoi.PointOfInterest.WaypointId) ?
CoreCommon.RoadNetwork.IntersectionLookup[dpoi.PointOfInterest.WaypointId] : null;
// check no intersection or no overlapping lanes
if (aInter == null || !aInter.PriorityLanes.ContainsKey(ai) || aInter.PriorityLanes[ai].Count == 0)
{
// create the supra lane
SupraLane sl = new SupraLane(sils.Lane, ai, al);
// switch to the supra lane state
StayInSupraLaneState sisls = new StayInSupraLaneState(sl, CoreCommon.CorePlanningState);
sisls.UpdateState(vehicleState.Front);
// set
return new Maneuver(new NullBehavior(), sisls, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
}
}
}
#endregion
// plan final tactical maneuver
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
#endregion
#region Stay in Supra Lane State
else if (CoreCommon.CorePlanningState is StayInSupraLaneState)
{
// state
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
#region Blockages
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is LaneBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// check not from a dynamicly moving vehicle
if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
{
// go to a blockage handling tactical
return new Maneuver(new HoldBrakeBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
ArbiterOutput.Output("Lane blockage reported for moving vehicle, ignoring");
}
#endregion
// check if we are in the final lane
if (sisls.Lane.ClosestComponent(vehicleState.Position) == SLComponentType.Final)
{
// go to stay in lane
return new Maneuver(new NullBehavior(), new StayInLaneState(sisls.Lane.Final, sisls), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// update ignorable
sisls.UpdateIgnoreList();
// nav plan to find points
RoadPlan rp = navigation.PlanNavigableArea(sisls.Lane, vehicleState.Position, goal, sisls.WaypointsToIgnore);
// check for unreachable route
if (rp.BestPlan.laneWaypointOfInterest.BestRoute != null &&
rp.BestPlan.laneWaypointOfInterest.BestRoute.Count == 0 &&
rp.BestPlan.laneWaypointOfInterest.RouteTime >= Double.MaxValue - 1.0)
{
ArbiterOutput.Output("Removed Unreachable Checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// update current state
sisls.UpdateState(vehicleState.Front);
// return final maneuver
return final;
}
#endregion
#region Stopping At Stop State
else if (CoreCommon.CorePlanningState is StoppingAtStopState)
{
// get state
StoppingAtStopState sass = (StoppingAtStopState)CoreCommon.CorePlanningState;
// check to see if we're stopped
// check if in other lane
if (CoreCommon.Communications.HasCompleted((new StayInLaneBehavior(null, null, null)).GetType()))
{
// update intersection monitor
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(sass.waypoint.AreaSubtypeWaypointId))
{
// nav plan
IntersectionPlan ip = navigation.PlanIntersection(sass.waypoint, goal);
// update intersection monitor
this.tactical.Update(observedVehicles, vehicleState);
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
sass.waypoint,
CoreCommon.RoadNetwork.IntersectionLookup[sass.waypoint.AreaSubtypeWaypointId],
vehicleState, ip.BestOption);
}
else
{
IntersectionTactical.IntersectionMonitor = null;
}
// check if we've hit goal if stop is cp
if (sass.waypoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
{
ArbiterOutput.Output("Stopped at current goal: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + ", Removing");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
if (CoreCommon.Mission.MissionCheckpoints.Count == 0)
{
return new Maneuver(new HoldBrakeBehavior(), new NoGoalsLeftState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
// move to the intersection
IState next = new WaitingAtIntersectionExitState(sass.waypoint, sass.turnDirection, new IntersectionDescription(), sass.desiredExit);
Behavior b = new HoldBrakeBehavior();
return new Maneuver(b, next, sass.DefaultStateDecorators, vehicleState.Timestamp);
}
else
{
// otherwise update the stop parameters
Behavior b = sass.Resume(vehicleState, vehicleSpeed);
return new Maneuver(b, CoreCommon.CorePlanningState, sass.DefaultStateDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Change Lanes State
else if (CoreCommon.CorePlanningState is ChangeLanesState)
{
// get state
ChangeLanesState cls = (ChangeLanesState)CoreCommon.CorePlanningState;
#region Blockages
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is LaneChangeBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// check not from a dynamicly moving vehicle
if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
{
// go to a blockage handling tactical
return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
ArbiterOutput.Output("Lane Change blockage reported for moving vehicle, ignoring");
}
#endregion
// get a good lane
ArbiterLane goodLane = null;
if(!cls.Parameters.InitialOncoming)
goodLane = cls.Parameters.Initial;
else if(!cls.Parameters.TargetOncoming)
goodLane = cls.Parameters.Target;
else
throw new Exception("not going from or to good lane");
// nav plan to find poi
#warning make goal better if there is none come to stop
RoadPlan rp = navigation.PlanNavigableArea(goodLane, vehicleState.Front,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], new List<ArbiterWaypoint>());
// check current behavior type
bool done = CoreCommon.Communications.HasCompleted((new ChangeLaneBehavior(null, null, false, 0, null, null)).GetType());
if (done)
{
if (cls.Parameters.TargetOncoming)
return new Maneuver(
new StayInLaneBehavior(cls.Parameters.Target.LaneId,
new ScalarSpeedCommand(cls.Parameters.Parameters.RecommendedSpeed),
cls.Parameters.Parameters.VehiclesToIgnore,
cls.Parameters.Target.ReversePath,
cls.Parameters.Target.Width,
cls.Parameters.Target.NumberOfLanesRight(vehicleState.Front, !cls.Parameters.InitialOncoming),
cls.Parameters.Target.NumberOfLanesLeft(vehicleState.Front, !cls.Parameters.InitialOncoming)),
new OpposingLanesState(cls.Parameters.Target, true, cls, vehicleState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
else
return new Maneuver(
new StayInLaneBehavior(cls.Parameters.Target.LaneId,
new ScalarSpeedCommand(cls.Parameters.Parameters.RecommendedSpeed),
cls.Parameters.Parameters.VehiclesToIgnore,
cls.Parameters.Target.LanePath(),
cls.Parameters.Target.Width,
cls.Parameters.Target.NumberOfLanesLeft(vehicleState.Front, !cls.Parameters.InitialOncoming),
cls.Parameters.Target.NumberOfLanesRight(vehicleState.Front, !cls.Parameters.InitialOncoming)),
new StayInLaneState(cls.Parameters.Target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
return tactical.Plan(cls, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
}
}
#endregion
#region Opposing Lanes State
else if (CoreCommon.CorePlanningState is OpposingLanesState)
{
// get state
OpposingLanesState ols = (OpposingLanesState)CoreCommon.CorePlanningState;
ols.SetClosestGood(vehicleState);
#region Blockages
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is OpposingLaneBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// check not from a dynamicly moving vehicle
if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
{
// go to a blockage handling tactical
return new Maneuver(new HoldBrakeBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
ArbiterOutput.Output("Opposing Lane blockage reported for moving vehicle, ignoring");
}
#endregion
// check closest good null
if (ols.ClosestGoodLane != null)
{
// nav plan to find poi
RoadPlan rp = navigation.PlanNavigableArea(ols.ClosestGoodLane, vehicleState.Position, goal, new List<ArbiterWaypoint>());
// plan final tactical maneuver
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
// otherwise need to make a uturn
else
{
ArbiterOutput.Output("in opposing lane with no closest good, making a uturn");
ArbiterLanePartition alp = ols.OpposingLane.GetClosestPartition(vehicleState.Front);
Coordinates c1 = vehicleState.Front + alp.Vector().Normalize(8.0);
Coordinates c2 = vehicleState.Front - alp.Vector().Normalize(8.0);
LinePath lpTmp = new LinePath(new Coordinates[] { c1, c2 });
List<Coordinates> pCoords = new List<Coordinates>();
pCoords.AddRange(lpTmp.ShiftLateral(ols.OpposingLane.Width)); //* 1.5));
pCoords.AddRange(lpTmp.ShiftLateral(-ols.OpposingLane.Width));// / 2.0));
Polygon uturnPoly = Polygon.GrahamScan(pCoords);
uTurnState uts = new uTurnState(ols.OpposingLane, uturnPoly, true);
uts.Interconnect = alp.ToInterconnect;
// plan final tactical maneuver
Maneuver final = new Maneuver(new NullBehavior(), uts, TurnDecorators.LeftTurnDecorator, vehicleState.Timestamp);
// return final maneuver
return final;
}
}
#endregion
#region Starting up off of chute state
else if (CoreCommon.CorePlanningState is StartupOffChuteState)
{
// cast the type
StartupOffChuteState socs = (StartupOffChuteState)CoreCommon.CorePlanningState;
// check if in lane part of chute
if (CoreCommon.Communications.HasCompleted((new TurnBehavior(null, null, null, null, null, null)).GetType()))
{
// go to lane state
return new Maneuver(new NullBehavior(), new StayInLaneState(socs.Final.Lane, new Probability(0.8, 0.2), true, socs), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// otherwise continue
else
{
// simple maneuver generation
TurnBehavior tb = (TurnBehavior)socs.Resume(vehicleState, 1.4);
// add bounds to observable
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(tb.LeftBound, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(tb.RightBound, ArbiterInformationDisplayObjectType.rightBound));
// final maneuver
return new Maneuver(tb, socs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
}
#endregion
}
#endregion
#region Intersection State
else if (CoreCommon.CorePlanningState is IntersectionState)
{
#region Waiting at Intersection Exit State
if (CoreCommon.CorePlanningState is WaitingAtIntersectionExitState)
{
// get state
WaitingAtIntersectionExitState waies = (WaitingAtIntersectionExitState)CoreCommon.CorePlanningState;
// nav plan
IntersectionPlan ip = navigation.PlanIntersection(waies.exitWaypoint, goal);
// plan
Maneuver final = tactical.Plan(waies, ip, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
#endregion
#region Stopping At Exit State
else if (CoreCommon.CorePlanningState is StoppingAtExitState)
{
// get state
StoppingAtExitState saes = (StoppingAtExitState)CoreCommon.CorePlanningState;
// check to see if we're stopped
if (CoreCommon.Communications.HasCompleted((new StayInLaneBehavior(null, null, null)).GetType()))
{
// check if we've hit goal if stop is cp
if (saes.waypoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
{
ArbiterOutput.Output("Stopped at current goal: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + ", Removing");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
if (CoreCommon.Mission.MissionCheckpoints.Count == 0)
{
return new Maneuver(new HoldBrakeBehavior(), new NoGoalsLeftState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
// move to the intersection
IState next = new WaitingAtIntersectionExitState(saes.waypoint, saes.turnDirection, new IntersectionDescription(), saes.desiredExit);
Behavior b = new HoldBrakeBehavior();
return new Maneuver(b, next, saes.DefaultStateDecorators, vehicleState.Timestamp);
}
else
{
// nav plan
IntersectionPlan ip = navigation.PlanIntersection(saes.waypoint, goal);
// update the intersection monitor
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(saes.waypoint.AreaSubtypeWaypointId))
{
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
saes.waypoint,
CoreCommon.RoadNetwork.IntersectionLookup[saes.waypoint.AreaSubtypeWaypointId],
vehicleState, ip.BestOption);
}
else
IntersectionTactical.IntersectionMonitor = null;
// plan final tactical maneuver
Maneuver final = tactical.Plan(saes, ip, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
}
#endregion
#region Turn State
else if (CoreCommon.CorePlanningState is TurnState)
{
// get state
TurnState ts = (TurnState)CoreCommon.CorePlanningState;
// check if in other lane
if (CoreCommon.Communications.HasCompleted((new TurnBehavior(null, null, null, null, null, null)).GetType()))
{
if (ts.Interconnect.FinalGeneric is ArbiterWaypoint)
{
// get final wp, and if next cp, remove
ArbiterWaypoint final = (ArbiterWaypoint)ts.Interconnect.FinalGeneric;
if (CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(final.AreaSubtypeWaypointId))
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// stay in target lane
IState nextState = new StayInLaneState(ts.TargetLane, new Probability(0.8, 0.2), true, CoreCommon.CorePlanningState);
Behavior b = new NullBehavior();
return new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else if (ts.Interconnect.FinalGeneric is ArbiterPerimeterWaypoint)
{
// stay in target lane
IState nextState = new ZoneTravelingState(((ArbiterPerimeterWaypoint)ts.Interconnect.FinalGeneric).Perimeter.Zone, (INavigableNode)ts.Interconnect.FinalGeneric);
Behavior b = new NullBehavior();
return new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
throw new Exception("unhandled unterconnect final wp type");
}
// get interconnect
if (ts.Interconnect.FinalGeneric is ArbiterWaypoint)
{
// nav plan
IntersectionPlan ip = navigation.PlanIntersection((ITraversableWaypoint)ts.Interconnect.InitialGeneric, goal);
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, ip, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
// else to zone
else if (ts.Interconnect.FinalGeneric is ArbiterPerimeterWaypoint)
{
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
else
{
throw new Exception("method not imp");
}
}
#endregion
#region uTurn State
else if (CoreCommon.CorePlanningState is uTurnState)
{
// get state
uTurnState uts = (uTurnState)CoreCommon.CorePlanningState;
// plan over the target segment, ignoring the initial waypoint of the target lane
ArbiterWaypoint initial = uts.TargetLane.GetClosestPartition(vehicleState.Position).Initial;
List<ArbiterWaypoint> iws = RoadToolkit.WaypointsClose(initial.Lane.Way, vehicleState.Front, initial);
RoadPlan rp = navigation.PlanRoads(uts.TargetLane, vehicleState.Front, goal, iws);
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
#endregion
#region Intersection Startup State
else if (CoreCommon.CorePlanningState is IntersectionStartupState)
{
// get startup state
IntersectionStartupState iss = (IntersectionStartupState)CoreCommon.CorePlanningState;
// get intersection
ArbiterIntersection ai = iss.Intersection;
// get plan
IEnumerable<ITraversableWaypoint> entries = ai.AllEntries.Values;
IntersectionStartupPlan isp = navigation.PlanIntersectionStartup(entries, goal);
// plan tac
Maneuver final = tactical.Plan(iss, isp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return
return final;
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
}
#endregion
}
#endregion
#region Zone State
else if (CoreCommon.CorePlanningState is ZoneState)
{
#region Zone Travelling State
if (CoreCommon.CorePlanningState is ZoneTravelingState)
{
// set state
ZoneTravelingState zts = (ZoneTravelingState)CoreCommon.CorePlanningState;
// check to see if we're stopped
if (CoreCommon.Communications.HasCompleted((new ZoneTravelingBehavior(null, null, new Polygon[0], null, null, null, null)).GetType()))
{
// plan over state and zone
ZonePlan zp = this.navigation.PlanZone(zts.Zone, zts.Start, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]);
if (zp.ZoneGoal is ArbiterParkingSpotWaypoint)
{
// move to parking state
ParkingState ps = new ParkingState(zp.Zone, ((ArbiterParkingSpotWaypoint)zp.ZoneGoal).ParkingSpot);
return new Maneuver(new HoldBrakeBehavior(), ps, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else if(zp.ZoneGoal is ArbiterPerimeterWaypoint)
{
// get plan
IntersectionPlan ip = navigation.GetIntersectionExitPlan((ITraversableWaypoint)zp.ZoneGoal, goal);
// move to exit
WaitingAtIntersectionExitState waies = new WaitingAtIntersectionExitState((ITraversableWaypoint)zp.ZoneGoal, ip.BestOption.ToInterconnect.TurnDirection, new IntersectionDescription(), ip.BestOption.ToInterconnect);
return new Maneuver(new HoldBrakeBehavior(), waies, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
else
{
// plan over state and zone
ZonePlan zp = this.navigation.PlanZone(zts.Zone, zts.Start, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]);
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, zp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
}
#endregion
#region Parking State
else if (CoreCommon.CorePlanningState is ParkingState)
{
// set state
ParkingState ps = (ParkingState)CoreCommon.CorePlanningState;
// check to see if we're stopped
if (CoreCommon.Communications.HasCompleted((new ZoneParkingBehavior(null, null, new Polygon[0], null, null, null, null, null, 0.0)).GetType()))
{
if (ps.ParkingSpot.Checkpoint.CheckpointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber))
{
ArbiterOutput.Output("Reached Goal, cp: " + ps.ParkingSpot.Checkpoint.CheckpointId.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
}
// pull out of the space
PullingOutState pos = new PullingOutState(ps.Zone, ps.ParkingSpot);
return new Maneuver(new HoldBrakeBehavior(), pos, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
}
#endregion
#region Pulling Out State
else if (CoreCommon.CorePlanningState is PullingOutState)
{
// set state
PullingOutState pos = (PullingOutState)CoreCommon.CorePlanningState;
// plan over state and zone
ZonePlan zp = this.navigation.PlanZone(pos.Zone, pos.ParkingSpot.Checkpoint, goal);
// check to see if we're stopped
if (CoreCommon.Communications.HasCompleted((new ZoneParkingPullOutBehavior(null, null, new Polygon[0], null, null, null, null, null, null, null, null)).GetType()))
{
// maneuver to next place to go
return new Maneuver(new HoldBrakeBehavior(), new ZoneTravelingState(pos.Zone, pos.ParkingSpot.Checkpoint), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, zp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
}
#endregion
#region Zone Startup State
else if (CoreCommon.CorePlanningState is ZoneStartupState)
{
// feed through the plan from the zone tactical
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
#endregion
#region Zone Orientation
else if (CoreCommon.CorePlanningState is ZoneOrientationState)
{
ZoneOrientationState zos = (ZoneOrientationState)CoreCommon.CorePlanningState;
// add bounds to observable
LinePath lp = new LinePath(new Coordinates[] { zos.final.Start.Position, zos.final.End.Position });
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lp.ShiftLateral(TahoeParams.T), ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lp.ShiftLateral(-TahoeParams.T), ArbiterInformationDisplayObjectType.rightBound));
// check to see if we're stopped
//if (CoreCommon.Communications.HasCompleted((new UTurnBehavior(null, null, null, null)).GetType()))
//{
// maneuver to next place to go
return new Maneuver(new HoldBrakeBehavior(), new ZoneTravelingState(zos.Zone, zos.final.End), TurnDecorators.NoDecorators, vehicleState.Timestamp);
//}
// not stopped doing hte maneuver
//else
// return new Maneuver(zos.Resume(vehicleState, 1.4), zos, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
}
#endregion
}
#endregion
#region Other State
else if (CoreCommon.CorePlanningState is OtherState)
{
#region Start Up State
if (CoreCommon.CorePlanningState is StartUpState)
{
// get state
StartUpState sus = (StartUpState)CoreCommon.CorePlanningState;
// make a new startup agent
StartupReasoning sr = new StartupReasoning(this.laneAgent);
// get final state
IState nextState = sr.Startup(vehicleState, carMode);
// return no op ad zero all decorators
Behavior nextBehavior = sus.Resume(vehicleState, vehicleSpeed);
// return maneuver
return new Maneuver(nextBehavior, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#endregion
#region Paused State
else if (CoreCommon.CorePlanningState is PausedState)
{
// if switch back to run
if (carMode == CarMode.Run)
{
// get state
PausedState ps = (PausedState)CoreCommon.CorePlanningState;
// get what we were previously doing
IState previousState = ps.PreviousState();
// check if can resume
if (previousState != null && previousState.CanResume())
{
// resume state is next
return new Maneuver(new HoldBrakeBehavior(), new ResumeState(previousState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// otherwise go to startup
else
{
// next state is startup
IState nextState = new StartUpState();
// return no op
Behavior nextBehavior = new HoldBrakeBehavior();
// return maneuver
return new Maneuver(nextBehavior, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
// otherwise stay stopped
else
{
// stay stopped and paused
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Human State
else if (CoreCommon.CorePlanningState is HumanState)
{
// change to startup
if (carMode == CarMode.Run)
{
// next is startup
IState next = new StartUpState();
// next behavior just stay iin place for now
Behavior behavior = new HoldBrakeBehavior();
// return startup maneuver
return new Maneuver(behavior, next, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// in human mode still
else
{
// want to remove old behavior stuff
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Resume State
else if (CoreCommon.CorePlanningState is ResumeState)
{
// get state
ResumeState rs = (ResumeState)CoreCommon.CorePlanningState;
// make sure can resume (this is simple action)
if (rs.StateToResume != null && rs.StateToResume.CanResume())
{
// return old behavior
Behavior nextBehavior = rs.Resume(vehicleState, vehicleSpeed);
// return maneuver
return new Maneuver(nextBehavior, rs.StateToResume, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// otherwise just startup
else
{
// startup
return new Maneuver(new HoldBrakeBehavior(), new StartUpState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region No Goals Left State
else if (CoreCommon.CorePlanningState is NoGoalsLeftState)
{
// check if goals available
if (CoreCommon.Mission.MissionCheckpoints.Count > 0)
{
// startup
return new Maneuver(new HoldBrakeBehavior(), new StartUpState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// stay paused
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region eStopped State
else if (CoreCommon.CorePlanningState is eStoppedState)
{
// change to startup
if (carMode == CarMode.Run)
{
// next is startup
IState next = new StartUpState();
// next behavior just stay iin place for now
Behavior behavior = new HoldBrakeBehavior();
// return startup maneuver
return new Maneuver(behavior, next, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// in human mode still
else
{
// want to remove old behavior stuff
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown OtherState type", "CoreCommon.CorePlanningState");
}
#endregion
}
#endregion
#region Blockage State
else if (CoreCommon.CorePlanningState is BlockageState)
{
#region Blockage State
// something is blocked, in the encountered state we want to filter to base components of state
if (CoreCommon.CorePlanningState is EncounteredBlockageState)
{
// cast blockage state
EncounteredBlockageState bs = (EncounteredBlockageState)CoreCommon.CorePlanningState;
// plan through the blockage state with no road plan as just a quick filter
Maneuver final = tactical.Plan(bs, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return the final maneuver
return final;
}
#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 Check Various Statuses of Completion
// check successful completion report of behavior
if (brs.RecoveryBehavior != null && CoreCommon.Communications.HasCompleted(brs.RecoveryBehavior.GetType()))
{
// set updated status
ArbiterOutput.Output("Successfully received completion of behavior: " + brs.RecoveryBehavior.ToShortString() + ", " + brs.RecoveryBehavior.ShortBehaviorInformation());
brs.RecoveryStatus = BlockageRecoverySTATUS.COMPLETED;
// move to the tactical plan
return this.tactical.Plan(brs, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
}
// check operational startup
else if (CoreCommon.Communications.HasCompleted((new OperationalStartupBehavior()).GetType()))
{
// check defcon types
if (brs.Defcon == BlockageRecoveryDEFCON.REVERSE)
{
// abort maneuver as operational has no state information
return new Maneuver(new NullBehavior(), brs.AbortState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Information
// set recovery information
CoreCommon.CurrentInformation.FQMState = brs.EncounteredState.ShortDescription();
CoreCommon.CurrentInformation.FQMStateInfo = brs.EncounteredState.StateInformation();
CoreCommon.CurrentInformation.FQMBehavior = brs.RecoveryBehavior != null ? brs.RecoveryBehavior.ToShortString() : "NONE";
CoreCommon.CurrentInformation.FQMBehaviorInfo = brs.RecoveryBehavior != null ? brs.RecoveryBehavior.ShortBehaviorInformation() : "NONE";
CoreCommon.CurrentInformation.FQMSpeed = brs.RecoveryBehavior != null ? brs.RecoveryBehavior.SpeedCommandString() : "NONE";
#endregion
#region Blocked
if (brs.RecoveryStatus == BlockageRecoverySTATUS.BLOCKED)
{
if (brs.RecoveryBehavior is ChangeLaneBehavior)
{
brs.RecoveryStatus = BlockageRecoverySTATUS.ENCOUNTERED;
brs.Defcon = BlockageRecoveryDEFCON.CHANGELANES_FORWARD;
return new Maneuver(new HoldBrakeBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
ArbiterOutput.Output("Recovery behavior blocked, reverting to abort state: " + brs.AbortState.ToString());
return new Maneuver(new HoldBrakeBehavior(), brs.AbortState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Navigational Plan
// set navigational plan
INavigationalPlan navPlan = null;
#region Encountered
// blockage
if (brs.RecoveryStatus == BlockageRecoverySTATUS.ENCOUNTERED)
{
// get state
if (brs.AbortState is StayInLaneState)
{
// lane state
StayInLaneState sils = (StayInLaneState)brs.AbortState;
navPlan = navigation.PlanNavigableArea(sils.Lane, vehicleState.Position, goal, sils.WaypointsToIgnore);
}
}
#endregion
#region Completion
// blockage
if (brs.RecoveryStatus == BlockageRecoverySTATUS.COMPLETED)
{
// get state
if (brs.CompletionState is StayInLaneState)
{
// lane state
StayInLaneState sils = (StayInLaneState)brs.CompletionState;
navPlan = navigation.PlanNavigableArea(sils.Lane, vehicleState.Position, goal, sils.WaypointsToIgnore);
}
}
#endregion
#endregion
// move to the tactical plan
Maneuver final = this.tactical.Plan(brs, navPlan, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return the final maneuver
return final;
}
#endregion
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
}
// for now, return null
return new Maneuver();
#endregion
}
public double TimeTo(INavigableNode node)
{
return(exit.TimeTo(node));
}
public bool EqualsNode(INavigableNode node)
{
return exit.EqualsNode(node);
}
/// <summary>
/// Route ploan for downstream exits
/// </summary>
/// <param name="downstreamPoints"></param>
/// <param name="goal"></param>
private void RouteTimes(List<DownstreamPointOfInterest> downstreamPoints, INavigableNode goal)
{
// check if we are planning over the correct goal
if (this.currentTimes.Key != CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber
|| this.currentTimes.Value == null)
{
// create new lookup
this.currentTimes = new KeyValuePair<int, Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>>(
CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber, new Dictionary<ArbiterWaypointId, DownstreamPointOfInterest>());
}
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
foreach (DownstreamPointOfInterest dpoi in downstreamPoints)
{
// container flag
bool contains = this.currentTimes.Value.ContainsKey(dpoi.PointOfInterest.WaypointId);
// check if exit
if (dpoi.IsExit && !contains)
{
// fake node
FakeExitNode fen = new FakeExitNode(dpoi.PointOfInterest);
// init fields
double timeCost;
List<INavigableNode> routeNodes;
// plan
this.Plan(fen, goal, out routeNodes, out timeCost);
// set best
dpoi.RouteTime = timeCost;
dpoi.BestRoute = routeNodes;
dpoi.BestExit = routeNodes.Count > 1 ? fen.GetEdge(routeNodes[1]) : null;
// add to keepers
this.currentTimes.Value.Add(dpoi.PointOfInterest.WaypointId, dpoi.Clone());
}
else if (dpoi.IsExit && contains)
{
DownstreamPointOfInterest tmp = this.currentTimes.Value[dpoi.PointOfInterest.WaypointId];
if (tmp.BestExit == null)
{
ArbiterOutput.Output("NAV RouteTimes: Removing exit with no valid route: " + dpoi.PointOfInterest.WaypointId.ToString());
// remove
this.currentTimes.Value.Remove(dpoi.PointOfInterest.WaypointId);
dpoi.PointOfInterest = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[dpoi.PointOfInterest.WaypointId];
// fake node
FakeExitNode fen = new FakeExitNode(dpoi.PointOfInterest);
// init fields
double timeCost;
List<INavigableNode> routeNodes;
// plan
this.Plan(fen, goal, out routeNodes, out timeCost);
// set best
dpoi.RouteTime = timeCost;
dpoi.BestRoute = routeNodes;
dpoi.BestExit = routeNodes.Count > 1 ? fen.GetEdge(routeNodes[1]) : null;
// add to keepers
this.currentTimes.Value.Add(dpoi.PointOfInterest.WaypointId, dpoi);
}
else
{
dpoi.RouteTime = tmp.RouteTime;
dpoi.BestRoute = tmp.BestRoute;
dpoi.BestExit = tmp.BestExit;
}
}
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="zone"></param>
public ZoneTravelingState(ArbiterZone zone, INavigableNode start)
: base(zone)
{
this.Start = start;
}
/// <summary>
/// Gets exits downstream, or the goal we are currently reaching
/// </summary>
/// <param name="currentPosition"></param>
/// <param name="ignorable"></param>
/// <param name="goal"></param>
/// <returns></returns>
public List <DownstreamPointOfInterest> Downstream(Coordinates currentPosition, List <ArbiterWaypoint> ignorable, INavigableNode goal)
{
// downstream final
List <DownstreamPointOfInterest> waypoints = new List <DownstreamPointOfInterest>();
foreach (ArbiterLane al in Way.Lanes.Values)
{
if (al.Equals(this) || (al.GetClosestPartition(currentPosition).Type != PartitionType.Startup &&
((this.LaneOnLeft != null && this.LaneOnLeft.Equals(al) && this.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) ||
(this.LaneOnRight != null && this.LaneOnRight.Equals(al) && this.BoundaryRight != ArbiterLaneBoundary.SolidWhite))))
{
// get starting waypoint
ArbiterWaypoint waypoint = null;
// get closest partition
ArbiterLanePartition alp = al.GetClosestPartition(currentPosition);
if (alp.Initial.Position.DistanceTo(currentPosition) < TahoeParams.VL - 2)
{
waypoint = alp.Initial;
}
else if (alp.IsInside(currentPosition) || alp.Final.Position.DistanceTo(currentPosition) < TahoeParams.VL)
{
waypoint = alp.Final;
}
else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition))
{
waypoint = alp.Initial;
}
else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition) && alp.Final.NextPartition == null)
{
waypoint = null;
}
else
{
waypoint = null;
}
// check waypoint exists
if (waypoint != null)
{
// save start
ArbiterWaypoint initial = waypoint;
// initial cost
double initialCost = 0.0;
if (al.Equals(this))
{
initialCost = currentPosition.DistanceTo(waypoint.Position) / waypoint.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
}
else if (waypoint.WaypointId.Number != 1)
{
// get closest partition
ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue);
initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number);
initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
}
else
{
// get closest partition
ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue);
ArbiterWaypoint tmpF = this.GetClosestWaypoint(waypoint.Position, Double.MaxValue);
initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number);
initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
initialCost += this.TimeCostInLane(tmpI, tmpF, new List <ArbiterWaypoint>());
}
// loop while waypoint not null
while (waypoint != null)
{
if (waypoint.IsCheckpoint && (goal is ArbiterWaypoint) && ((ArbiterWaypoint)goal).WaypointId.Equals(waypoint.WaypointId))
{
double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable);
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
dpoi.IsExit = false;
dpoi.IsGoal = true;
dpoi.PointOfInterest = waypoint;
dpoi.TimeCostToPoint = timeCost;
waypoints.Add(dpoi);
}
else if (waypoint.IsExit && !ignorable.Contains(waypoint))
{
double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable);
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
dpoi.IsExit = true;
dpoi.IsGoal = false;
dpoi.PointOfInterest = waypoint;
dpoi.TimeCostToPoint = timeCost;
waypoints.Add(dpoi);
}
else if (waypoint.NextPartition == null && !ignorable.Contains(waypoint))
{
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
dpoi.IsExit = false;
dpoi.IsGoal = false;
dpoi.PointOfInterest = waypoint;
dpoi.TimeCostToPoint = Double.MaxValue;
waypoints.Add(dpoi);
}
waypoint = waypoint.NextPartition != null ? waypoint.NextPartition.Final : null;
}
}
}
}
return(waypoints);
}
/// <summary>
/// Plan given that we are starting on a road
/// </summary>
/// <param name="currentLane"></param>
/// <param name="currentPosition"></param>
/// <param name="goal"></param>
/// <returns></returns>
public RoadPlan PlanNavigableArea(INavigableTravelArea currentArea, Coordinates currentPosition, INavigableNode goal, List<ArbiterWaypoint> ignorable)
{
// get all downstream points of interest as well as the goal point of interest
List<DownstreamPointOfInterest> downstreamPoints = currentArea.Downstream(currentPosition, ignorable, goal);
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
this.RouteTimes(downstreamPoints, goal);
// get road plan
RoadPlan rp = this.GetRoadPlan(downstreamPoints);
#region Output
// update arbiter information
List<RouteInformation> routeInfo = rp.RouteInformation(currentPosition);
// make sure we're in a road state
if (CoreCommon.CorePlanningState == null || CoreCommon.CorePlanningState is UrbanChallenge.Arbiter.Core.Common.State.TravelState)
{
// check route 1
if (routeInfo.Count > 0)
{
RouteInformation ri = routeInfo[0];
CoreCommon.CurrentInformation.Route1 = ri;
CoreCommon.CurrentInformation.Route1Time = ri.RouteTimeCost.ToString("F6");
CoreCommon.CurrentInformation.Route1Wp = ri.Waypoint;
}
}
#endregion
// return road plan
return rp;
}
/// <summary>
/// Plan given that we are starting on a road
/// </summary>
/// <param name="currentLane"></param>
/// <param name="currentPosition"></param>
/// <param name="goal"></param>
/// <returns></returns>
public RoadPlan PlanRoads(ArbiterLane currentLane, Coordinates currentPosition, INavigableNode goal, List<ArbiterWaypoint> ignorable)
{
// get all downstream points of interest
List<DownstreamPointOfInterest> downstreamPoints = new List<DownstreamPointOfInterest>();
// get exits downstream from this current position in the way
downstreamPoints.AddRange(this.Downstream(currentPosition, currentLane.Way, true, ignorable));
// determine if goal is downstream in a specific lane, add to possible route times to consider
DownstreamPointOfInterest goalDownstream = this.IsGoalDownStream(currentLane.Way, currentPosition, goal);
// add goal to points downstream if it exists
if (goalDownstream != null)
downstreamPoints.Add(goalDownstream);
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
this.DetermineDownstreamPointRouteTimes(downstreamPoints, goal, currentLane.Way);
// get road plan
RoadPlan rp = this.GetRoadPlan(downstreamPoints, currentLane.Way);
// update arbiter information
List<RouteInformation> routeInfo = rp.RouteInformation(currentPosition);
// make sure we're in a road state
if (CoreCommon.CorePlanningState == null ||
CoreCommon.CorePlanningState is TravelState ||
CoreCommon.CorePlanningState is TurnState)
{
// check route 1
if (routeInfo.Count > 0)
{
RouteInformation ri = routeInfo[0];
CoreCommon.CurrentInformation.Route1 = ri;
CoreCommon.CurrentInformation.Route1Time = ri.RouteTimeCost.ToString("F6");
CoreCommon.CurrentInformation.Route1Wp = ri.Waypoint;
}
// check route 2
if (routeInfo.Count > 1)
{
RouteInformation ri = routeInfo[1];
CoreCommon.CurrentInformation.Route2 = ri;
CoreCommon.CurrentInformation.Route2Time = ri.RouteTimeCost.ToString("F6");
CoreCommon.CurrentInformation.Route2Wp = ri.Waypoint;
}
}
// return road plan
return rp;
}
/// <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>
/// Determine intersection startup costs
/// </summary>
/// <param name="entries"></param>
/// <param name="goal"></param>
/// <returns></returns>
public IntersectionStartupPlan PlanIntersectionStartup(IEnumerable<ITraversableWaypoint> entries, INavigableNode goal)
{
Dictionary<ITraversableWaypoint, double> entryCosts = new Dictionary<ITraversableWaypoint, double>();
foreach (ITraversableWaypoint start in entries)
{
// given start and goal, get plan
double time;
List<INavigableNode> nodes;
this.Plan(start, goal, out nodes, out time);
entryCosts.Add(start, time);
}
return new IntersectionStartupPlan(entryCosts);
}
/// <summary>
/// Plan between two nodes
/// </summary>
/// <param name="start"></param>
/// <param name="goal"></param>
/// <param name="nodes"></param>
/// <param name="time"></param>
public void Plan(INavigableNode start, INavigableNode goal, out List<INavigableNode> nodes, out double time)
{
aStar astar = new aStar(start, goal, new List<NavigableEdge>());
nodes = astar.Plan();
time = astar.TotalTime;
}
/// <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>
/// Seed the planner, making it non blocking
/// </summary>
/// <param name="downstreamPoints"></param>
/// <param name="goal"></param>
/// <param name="way"></param>
private void SeedPlanner(List<DownstreamPointOfInterest> downstreamPoints, INavigableNode goal)
{
seedPlannerThread = new Thread(
delegate()
{
this.RouteTimes(downstreamPoints, goal);
}
);
seedPlannerThread.IsBackground = true;
seedPlannerThread.Priority = ThreadPriority.AboveNormal;
seedPlannerThread.Start();
}
/// <summary>
/// Checks if a goal is downstream on the current road
/// </summary>
/// <param name="way"></param>
/// <param name="currentPosition"></param>
/// <param name="goal"></param>
/// <returns></returns>
private DownstreamPointOfInterest IsGoalDownStream(ArbiterWay way, Coordinates currentPosition, INavigableNode goal)
{
if (goal is ArbiterWaypoint)
{
ArbiterWaypoint goalWaypoint = (ArbiterWaypoint)goal;
if (goalWaypoint.Lane.Way.Equals(way))
{
foreach (ArbiterLane lane in way.Lanes.Values)
{
if (lane.Equals(goalWaypoint.Lane))
{
Coordinates current = lane.GetClosest(currentPosition);
Coordinates goalPoint = lane.GetClosest(goal.Position);
if (lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0)
{
double distToGoal = lane.DistanceBetween(current, goalPoint);
if (distToGoal > 0)
{
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = distToGoal;
dpoi.IsGoal = true;
dpoi.IsExit = false;
dpoi.PointOfInterest = goalWaypoint;
dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint);
dpoi.RouteTime = 0.0;
return(dpoi);
}
}
}
else
{
Coordinates current = lane.GetClosest(currentPosition);
Coordinates goalPoint = lane.GetClosest(goal.Position);
if ((lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0) && lane.IsInside(goalPoint))
{
double distToGoal = lane.DistanceBetween(current, goalPoint);
if (distToGoal > 0)
{
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = distToGoal;
dpoi.IsGoal = true;
dpoi.IsExit = false;
dpoi.PointOfInterest = goalWaypoint;
dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint);
dpoi.RouteTime = 0.0;
//return dpoi;
}
}
}
}
}
}
return(null);
}
/// <summary>
/// Plan in a zone
/// </summary>
/// <param name="az"></param>
/// <param name="goal"></param>
/// <returns></returns>
public ZonePlan PlanZone(ArbiterZone az, INavigableNode start, INavigableNode goal)
{
// zone plan
ZonePlan zp = new ZonePlan();
zp.Zone = az;
zp.Start = start;
// given start and goal, get plan
double time;
List<INavigableNode> nodes;
this.Plan(start, goal, out nodes, out time);
zp.Time = time;
// final path
LinePath recommendedPath = new LinePath();
List<INavigableNode> pathNodes = new List<INavigableNode>();
// start and end counts
int startCount = 0;
int endCount = 0;
// check start type
if(start is ArbiterParkingSpotWaypoint)
startCount = 2;
// check end type
if(goal is ArbiterParkingSpotWaypoint &&
((ArbiterParkingSpotWaypoint)goal).ParkingSpot.Zone.Equals(az))
endCount = -2;
// loop through nodes
for(int i = startCount; i < nodes.Count + endCount; i++)
{
// go to parking spot or endpoint
if(nodes[i] is ArbiterParkingSpotWaypoint)
{
// set zone goal
zp.ZoneGoal = ((ArbiterParkingSpotWaypoint)nodes[i]).ParkingSpot.Checkpoint;
// set path, return
zp.RecommendedPath = recommendedPath;
zp.PathNodes = pathNodes;
// set route info
RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString());
CoreCommon.CurrentInformation.Route1 = ri;
// return the plan
return zp;
}
// go to perimeter waypoint if this is one
else if(nodes[i] is ArbiterPerimeterWaypoint && ((ArbiterPerimeterWaypoint)nodes[i]).IsExit)
{
// add
recommendedPath.Add(nodes[i].Position);
// set zone goal
zp.ZoneGoal = nodes[i];
// set path, return
zp.RecommendedPath = recommendedPath;
zp.PathNodes = pathNodes;
// set route info
RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString());
CoreCommon.CurrentInformation.Route1 = ri;
// return the plan
return zp;
}
// otherwise just add
else
{
// add
recommendedPath.Add(nodes[i].Position);
pathNodes.Add(nodes[i]);
}
}
// set zone goal
zp.ZoneGoal = goal;
// set path, return
zp.RecommendedPath = recommendedPath;
// set route info
CoreCommon.CurrentInformation.Route1 = new RouteInformation(recommendedPath, time, goal.ToString());
// return the plan
return zp;
}
/// <summary>
/// Plan given that we are starting on a road
/// </summary>
/// <param name="currentLane"></param>
/// <param name="currentPosition"></param>
/// <param name="goal"></param>
/// <returns></returns>
public RoadPlan PlanNavigableArea(INavigableTravelArea currentArea, Coordinates currentPosition, INavigableNode goal, List <ArbiterWaypoint> ignorable)
{
// get all downstream points of interest as well as the goal point of interest
List <DownstreamPointOfInterest> downstreamPoints = currentArea.Downstream(currentPosition, ignorable, goal);
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
this.RouteTimes(downstreamPoints, goal);
// get road plan
RoadPlan rp = this.GetRoadPlan(downstreamPoints);
#region Output
// update arbiter information
List <RouteInformation> routeInfo = rp.RouteInformation(currentPosition);
// make sure we're in a road state
if (CoreCommon.CorePlanningState == null || CoreCommon.CorePlanningState is UrbanChallenge.Arbiter.Core.Common.State.TravelState)
{
// check route 1
if (routeInfo.Count > 0)
{
RouteInformation ri = routeInfo[0];
CoreCommon.CurrentInformation.Route1 = ri;
CoreCommon.CurrentInformation.Route1Time = ri.RouteTimeCost.ToString("F6");
CoreCommon.CurrentInformation.Route1Wp = ri.Waypoint;
}
}
#endregion
// return road plan
return(rp);
}
/// <summary>
/// Seed the road planner
/// </summary>
/// <param name="seedLane"></param>
/// <param name="seedPosition"></param>
/// <param name="goal"></param>
/// <param name="ignorable"></param>
public void SeedRoadPlanner(INavigableTravelArea seed, Coordinates seedPosition, INavigableNode goal, List<ArbiterWaypoint> ignorable)
{
// get all downstream points of interest
List<DownstreamPointOfInterest> downstreamPoints = seed.Downstream(seedPosition, ignorable, goal);
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
this.SeedPlanner(downstreamPoints, goal);
}
/// <summary>
/// Plan in a zone
/// </summary>
/// <param name="az"></param>
/// <param name="goal"></param>
/// <returns></returns>
public ZonePlan PlanZone(ArbiterZone az, INavigableNode start, INavigableNode goal)
{
// zone plan
ZonePlan zp = new ZonePlan();
zp.Zone = az;
zp.Start = start;
// given start and goal, get plan
double time;
List <INavigableNode> nodes;
this.Plan(start, goal, out nodes, out time);
zp.Time = time;
// final path
LinePath recommendedPath = new LinePath();
List <INavigableNode> pathNodes = new List <INavigableNode>();
// start and end counts
int startCount = 0;
int endCount = 0;
// check start type
if (start is ArbiterParkingSpotWaypoint)
{
startCount = 2;
}
// check end type
if (goal is ArbiterParkingSpotWaypoint &&
((ArbiterParkingSpotWaypoint)goal).ParkingSpot.Zone.Equals(az))
{
endCount = -2;
}
// loop through nodes
for (int i = startCount; i < nodes.Count + endCount; i++)
{
// go to parking spot or endpoint
if (nodes[i] is ArbiterParkingSpotWaypoint)
{
// set zone goal
zp.ZoneGoal = ((ArbiterParkingSpotWaypoint)nodes[i]).ParkingSpot.Checkpoint;
// set path, return
zp.RecommendedPath = recommendedPath;
zp.PathNodes = pathNodes;
// set route info
RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString());
CoreCommon.CurrentInformation.Route1 = ri;
// return the plan
return(zp);
}
// go to perimeter waypoint if this is one
else if (nodes[i] is ArbiterPerimeterWaypoint && ((ArbiterPerimeterWaypoint)nodes[i]).IsExit)
{
// add
recommendedPath.Add(nodes[i].Position);
// set zone goal
zp.ZoneGoal = nodes[i];
// set path, return
zp.RecommendedPath = recommendedPath;
zp.PathNodes = pathNodes;
// set route info
RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString());
CoreCommon.CurrentInformation.Route1 = ri;
// return the plan
return(zp);
}
// otherwise just add
else
{
// add
recommendedPath.Add(nodes[i].Position);
pathNodes.Add(nodes[i]);
}
}
// set zone goal
zp.ZoneGoal = goal;
// set path, return
zp.RecommendedPath = recommendedPath;
// set route info
CoreCommon.CurrentInformation.Route1 = new RouteInformation(recommendedPath, time, goal.ToString());
// return the plan
return(zp);
}
/// <summary>
/// Route ploan for downstream exits
/// </summary>
/// <param name="downstreamPoints"></param>
/// <param name="goal"></param>
private void DetermineDownstreamPointRouteTimes(List<DownstreamPointOfInterest> downstreamPoints, INavigableNode goal, ArbiterWay aw)
{
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
foreach (DownstreamPointOfInterest dpoi in downstreamPoints)
{
// check if exit
if (dpoi.IsExit)
{
// current best time
double bestCurrent = Double.MaxValue;
List<INavigableNode> bestRoute = null;
ArbiterInterconnect bestInterconnect = null;
// fake node
FakeExitNode fen = new FakeExitNode(dpoi.PointOfInterest);
// init fields
double timeCost;
List<INavigableNode> routeNodes;
// plan
this.Plan(fen, goal, out routeNodes, out timeCost);
bestCurrent = timeCost;
bestRoute = routeNodes;
bestInterconnect = routeNodes.Count > 1 ? fen.GetEdge(routeNodes[1]) : null;
// plan from each interconnect to find the best time from exit
/*foreach (ArbiterInterconnect ai in dpoi.PointOfInterest.Exits)
{
// init fields
double timeCost;
List<INavigableNode> routeNodes;
// plan
this.Plan(ai.End, goal, out routeNodes, out timeCost);
// check
if (timeCost < bestCurrent)
{
bestCurrent = timeCost;
bestRoute = routeNodes;
bestInterconnect = ai;
}
}*/
// set best
dpoi.RouteTime = bestCurrent;
dpoi.BestRoute = bestRoute;
dpoi.BestExit = bestInterconnect;
}
}
}
/// <summary>
/// Plan given that we are starting on a road
/// </summary>
/// <param name="currentLane"></param>
/// <param name="currentPosition"></param>
/// <param name="goal"></param>
/// <returns></returns>
public RoadPlan PlanRoads(ArbiterLane currentLane, Coordinates currentPosition, INavigableNode goal, List <ArbiterWaypoint> ignorable)
{
// get all downstream points of interest
List <DownstreamPointOfInterest> downstreamPoints = new List <DownstreamPointOfInterest>();
// get exits downstream from this current position in the way
downstreamPoints.AddRange(this.Downstream(currentPosition, currentLane.Way, true, ignorable));
// determine if goal is downstream in a specific lane, add to possible route times to consider
DownstreamPointOfInterest goalDownstream = this.IsGoalDownStream(currentLane.Way, currentPosition, goal);
// add goal to points downstream if it exists
if (goalDownstream != null)
{
downstreamPoints.Add(goalDownstream);
}
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
this.DetermineDownstreamPointRouteTimes(downstreamPoints, goal, currentLane.Way);
// get road plan
RoadPlan rp = this.GetRoadPlan(downstreamPoints, currentLane.Way);
// update arbiter information
List <RouteInformation> routeInfo = rp.RouteInformation(currentPosition);
// make sure we're in a road state
if (CoreCommon.CorePlanningState == null ||
CoreCommon.CorePlanningState is TravelState ||
CoreCommon.CorePlanningState is TurnState)
{
// check route 1
if (routeInfo.Count > 0)
{
RouteInformation ri = routeInfo[0];
CoreCommon.CurrentInformation.Route1 = ri;
CoreCommon.CurrentInformation.Route1Time = ri.RouteTimeCost.ToString("F6");
CoreCommon.CurrentInformation.Route1Wp = ri.Waypoint;
}
// check route 2
if (routeInfo.Count > 1)
{
RouteInformation ri = routeInfo[1];
CoreCommon.CurrentInformation.Route2 = ri;
CoreCommon.CurrentInformation.Route2Time = ri.RouteTimeCost.ToString("F6");
CoreCommon.CurrentInformation.Route2Wp = ri.Waypoint;
}
}
// return road plan
return(rp);
}
public void BeginMove(Coordinates c)
{
if (this.Mode == ZoneToolboxMode.NavNodes && this.zt.current != null)
{
// nullify
this.Reset(false);
// determine what we selected
NavigableEdge ne;
INavigableNode nn;
this.NavigationHitTest(c, out nn, out ne);
if (nn != null && !(nn is ArbiterPerimeterWaypoint))
{
this.moveNode = nn;
this.moveOriginalCoords = this.moveNode.Position;
}
}
}
public double TimeTo(INavigableNode node)
{
return exit.TimeTo(node);
}
public void Click(Coordinates c)
{
if (this.zt.Mode == ZoneToolboxMode.Selection)
{
this.zt.SelectZone(c);
}
else if (this.Mode == ZoneToolboxMode.NavNodes && this.zt.current != null)
{
// save undo point
this.ed.SaveUndoPoint();
// check if we hit any of hte edges or nodes part of the zone
ArbiterZone az = this.zt.current;
// check if hit node or edge
NavigableEdge ne;
INavigableNode nn;
this.NavigationHitTest(c, out nn, out ne);
if (nn != null)
{
// create new node
INavigableNode aznn = nn;
if (this.PreviousNode != null)
{
// create new edges
NavigableEdge newE1 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), this.PreviousNode, aznn);
this.PreviousNode.OutgoingConnections.Add(newE1);
this.zt.current.NavigableEdges.Add(newE1);
}
this.PreviousNode = aznn;
}
else if (ne != null)
{
// remove old
this.zt.current.NavigableEdges.Remove(ne);
// remove all references
ne.Start.OutgoingConnections.Remove(ne);
// create new node
ArbiterZoneNavigableNode aznn = new ArbiterZoneNavigableNode(c);
// create new edges
NavigableEdge newE1 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), ne.Start, aznn);
NavigableEdge newE2 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), aznn, ne.End);
// add edges
ne.Start.OutgoingConnections.Add(newE1);
aznn.OutgoingConnections.Add(newE2);
// add all to lists
this.zt.current.NavigableEdges.Add(newE1);
this.zt.current.NavigableEdges.Add(newE2);
this.zt.current.NavigationNodes.Add(aznn);
if (this.PreviousNode != null)
{
NavigableEdge newE3 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), this.PreviousNode, aznn);
this.PreviousNode.OutgoingConnections.Add(newE3);
this.zt.current.NavigableEdges.Add(newE3);
}
this.PreviousNode = aznn;
}
else
{
// create new node
ArbiterZoneNavigableNode aznn = new ArbiterZoneNavigableNode(c);
if (this.PreviousNode != null)
{
// create new edges
NavigableEdge newE1 = new NavigableEdge(true, this.zt.current, false, null, new List<IConnectAreaWaypoints>(), this.PreviousNode, aznn);
this.PreviousNode.OutgoingConnections.Add(newE1);
this.zt.current.NavigableEdges.Add(newE1);
}
this.PreviousNode = aznn;
this.zt.current.NavigationNodes.Add(aznn);
}
}
else if (this.zt.Mode == ZoneToolboxMode.StayOut && this.zt.current != null)
{
if (this.WrappingHelpers.Count == 0)
{
this.WrappingHelpers.Add(c);
}
else
{
// Determine size of bounding box
float scaled_offset = 1 / this.rd.WorldTransform.Scale;
// invert the scale
float scaled_size = DrawingUtility.cp_large_size;
// assume that the world transform is currently applied correctly to the graphics
RectangleF rect = new RectangleF((float)c.X - scaled_size / 2, (float)c.Y - scaled_size / 2, scaled_size, scaled_size);
if (rect.Contains(DrawingUtility.ToPointF(this.WrappingHelpers[0])) &&
c.DistanceTo(this.WrappingHelpers[0]) < 1)
{
ed.SaveUndoPoint();
Polygon p = new Polygon(this.WrappingHelpers);
this.zt.current.StayOutAreas.Add(p);
this.WrappingHelpers = new List<Coordinates>();
}
else
this.WrappingHelpers.Add(c);
}
this.rd.Invalidate();
}
}
public bool EqualsNode(INavigableNode node)
{
return(exit.EqualsNode(node));
}
public void NavigationHitTest(Coordinates c, out INavigableNode node, out NavigableEdge edge)
{
if (this.zt.current != null)
{
// Determine size of bounding box
float scaled_offset = 1 / this.rd.WorldTransform.Scale;
// invert the scale
float scaled_size = DrawingUtility.cp_large_size;
// assume that the world transform is currently applied correctly to the graphics
RectangleF rect = new RectangleF((float)c.X - scaled_size / 2, (float)c.Y - scaled_size / 2, scaled_size, scaled_size);
foreach (ArbiterParkingSpotWaypoint apsw in this.zt.current.ParkingSpotWaypoints.Values)
{
if (apsw.Position.DistanceTo(c) < 1.0)
{
node = apsw;
edge = null;
return;
}
}
foreach (ArbiterPerimeterWaypoint apw in this.zt.current.Perimeter.PerimeterPoints.Values)
{
if ((apw.IsExit || apw.IsEntry) && rect.Contains(DrawingUtility.ToPointF(apw.Position)) &&
(this.PreviousNode == null || !this.PreviousNode.Equals(apw)) &&
apw.Position.DistanceTo(c) < 1.0)
{
node = apw;
edge = null;
return;
}
}
foreach (INavigableNode inn in this.zt.current.NavigationNodes)
{
if (rect.Contains(DrawingUtility.ToPointF(inn.Position)) &&
(this.PreviousNode == null || !this.PreviousNode.Equals(inn)) &&
inn.Position.DistanceTo(c) < 1.0)
{
node = inn;
edge = null;
return;
}
}
NavigableEdge closest = null;
double distance = double.MaxValue;
foreach (NavigableEdge ne in this.zt.current.NavigableEdges)
{
LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position });
double dist = lp.GetClosestPoint(c).Location.DistanceTo(c);
if (dist < rect.Width && (dist < distance || closest == null) && dist < 1.0)
{
closest = ne;
distance = dist;
}
}
if (closest != null)
{
node = null;
edge = closest;
return;
}
}
node = null;
edge = null;
return;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="exit"></param>
public FakeExitNode(INavigableNode exit)
{
this.exit = exit;
}
public void Reset(bool resetPrevious)
{
this.WrappingHelpers = new List<Coordinates>();
this.PreviousNode = null;
if (resetPrevious)
this.zt.Mode = ZoneToolboxMode.None;
this.rightClickNode = null;
this.rightClickEdge = null;
if (this.moveNode != null)
this.moveNode.Position = this.moveOriginalCoords;
this.moveNode = null;
}
public bool EqualsNode(INavigableNode node)
{
return this.Equals(node);
}
public void RightClick(Coordinates c)
{
if (this.Mode == ZoneToolboxMode.NavNodes && this.zt.current != null)
{
// nullify
this.Reset(false);
// determine what we selected
NavigableEdge ne;
INavigableNode nn;
this.NavigationHitTest(c, out nn, out ne);
if (nn != null)
{
this.rightClickNode = nn;
}
else if (ne != null)
{
this.rightClickEdge = ne;
}
}
}
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);
}
public double TimeTo(INavigableNode node)
{
// avg speed of 10mph from place to place
return this.position.DistanceTo(node.Position) / (this.Lane.Way.Segment.RoadNetwork.MissionAverageMaxSpeed / 2.0);
}
public double TimeTo(INavigableNode node)
{
// avg speed of 10mph from place to place
return(this.position.DistanceTo(node.Position) / (this.Lane.Way.Segment.RoadNetwork.MissionAverageMaxSpeed / 2.0));
}
public bool EqualsNode(INavigableNode node)
{
return(this.Equals(node));
}
/// <summary>
/// Route ploan for downstream exits
/// </summary>
/// <param name="downstreamPoints"></param>
/// <param name="goal"></param>
private void RouteTimes(List <DownstreamPointOfInterest> downstreamPoints, INavigableNode goal)
{
// check if we are planning over the correct goal
if (this.currentTimes.Key != CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber ||
this.currentTimes.Value == null)
{
// create new lookup
this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >(
CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber, new Dictionary <ArbiterWaypointId, DownstreamPointOfInterest>());
}
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
foreach (DownstreamPointOfInterest dpoi in downstreamPoints)
{
// container flag
bool contains = this.currentTimes.Value.ContainsKey(dpoi.PointOfInterest.WaypointId);
// check if exit
if (dpoi.IsExit && !contains)
{
// fake node
FakeExitNode fen = new FakeExitNode(dpoi.PointOfInterest);
// init fields
double timeCost;
List <INavigableNode> routeNodes;
// plan
this.Plan(fen, goal, out routeNodes, out timeCost);
// set best
dpoi.RouteTime = timeCost;
dpoi.BestRoute = routeNodes;
dpoi.BestExit = routeNodes.Count > 1 ? fen.GetEdge(routeNodes[1]) : null;
// add to keepers
this.currentTimes.Value.Add(dpoi.PointOfInterest.WaypointId, dpoi.Clone());
}
else if (dpoi.IsExit && contains)
{
DownstreamPointOfInterest tmp = this.currentTimes.Value[dpoi.PointOfInterest.WaypointId];
if (tmp.BestExit == null)
{
ArbiterOutput.Output("NAV RouteTimes: Removing exit with no valid route: " + dpoi.PointOfInterest.WaypointId.ToString());
// remove
this.currentTimes.Value.Remove(dpoi.PointOfInterest.WaypointId);
dpoi.PointOfInterest = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[dpoi.PointOfInterest.WaypointId];
// fake node
FakeExitNode fen = new FakeExitNode(dpoi.PointOfInterest);
// init fields
double timeCost;
List <INavigableNode> routeNodes;
// plan
this.Plan(fen, goal, out routeNodes, out timeCost);
// set best
dpoi.RouteTime = timeCost;
dpoi.BestRoute = routeNodes;
dpoi.BestExit = routeNodes.Count > 1 ? fen.GetEdge(routeNodes[1]) : null;
// add to keepers
this.currentTimes.Value.Add(dpoi.PointOfInterest.WaypointId, dpoi);
}
else
{
dpoi.RouteTime = tmp.RouteTime;
dpoi.BestRoute = tmp.BestRoute;
dpoi.BestExit = tmp.BestExit;
}
}
}
}
/// <summary>
/// Plans a path from start to goal
/// </summary>
/// <returns>List of INavigableNode nodes in order of travel</returns>
public List <INavigableNode> Plan()
{
// 1. Add Start to Open list
open.Push(start);
// 2. While Open list not empty
// Console.WriteLine("2");
while (open.Count != 0)
{
// 3. Return and Remove node with lowest Cost from Open list
INavigableNode current = (INavigableNode)open.Pop();
// 4. If Current == Goal then we have found solution, terminate
if (current.EqualsNode(goal))
{
goal = current;
break;
}
// 5. Otherwise get Successors of Current
List <NavigableEdge> successors = current.OutgoingConnections;
// 6. For each successor
foreach (NavigableEdge successor in successors)
{
// 6.5 make sure successor is allowed to be planned over
if (this.removedEdges == null || !this.removedEdges.Contains(successor))
{
// 7. Get the cost estimate for the successor
double timeToHere = current.TimeToHere + current.TimeThroughAdjacent(successor);
double timeToGoal = successor.End.TimeTo(goal);
double time = timeToHere + timeToGoal;
// flag to skip this successor
bool skip = false;
// 8. Skip successor if node is closed and we've not found a better node
if (closed.ContainsKey(successor.End.Name))
{
// 9. Get the matching node on the closed list
INavigableNode closedCopy = closed[successor.End.Name];
// 10. If we did not find a shorter route, skip; else remove from closed
if (closedCopy.Time <= time)
{
skip = true;
}
else
{
closed.Remove(successor.End.Name);
}
}
// 11. Skip if node is in open and we've not found a better node
if (!skip && open.Contains(successor.End.Name))
{
// 12. Find record in open list
INavigableNode openCopy = (INavigableNode)open.Find(successor.End.Name);
// 13. If we did not find a shorter route, skip; else remove from Open
if (openCopy.Time <= time)
{
skip = true;
}
else
{
open.Remove(successor.End.Name);
}
}
// 14. If we're not to skip, it's an unvisited node or we've found a better path to a node we've previously visited
if (!skip)
{
// 15. Set costs for successor
successor.End.Previous = current;
successor.End.TimeToHere = timeToHere;
successor.End.TimeToGoal = timeToGoal;
successor.End.Time = time;
// 16. Add successor to open list
open.Push(successor.End);
}
}
}
// 17. Add current to closed list
closed.Add(current.Name, current);
} // end while
// check that we have planned to here
if (goal.Previous != null)
{
// 18. Set total time to get to the goal
totalTime = goal.TimeToHere;
// 19. Reverse the route here TESTING purposes for the moment
List <INavigableNode> route = new List <INavigableNode>();
INavigableNode temp = goal;
while (temp.Previous != null)
{
route.Add(temp);
temp = temp.Previous;
}
route.Add(temp);
route.Reverse();
return(route);
}
else
{
if (start.Equals(goal))
{
this.totalTime = 0.0;
return(new List <INavigableNode>());
}
else
{
this.totalTime = Double.MaxValue;
return(new List <INavigableNode>());
}
}
} // end Plan()
/// <summary>
/// Seed the road planner
/// </summary>
/// <param name="seedLane"></param>
/// <param name="seedPosition"></param>
/// <param name="goal"></param>
/// <param name="ignorable"></param>
public void SeedRoadPlanner(INavigableTravelArea seed, Coordinates seedPosition, INavigableNode goal, List <ArbiterWaypoint> ignorable)
{
// get all downstream points of interest
List <DownstreamPointOfInterest> downstreamPoints = seed.Downstream(seedPosition, ignorable, goal);
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
this.SeedPlanner(downstreamPoints, goal);
}
/// <summary>
/// Intelligence thread
/// </summary>
public void CoreIntelligence()
{
// wait for entry data
this.WaitForEntryData();
// jumpstart behavioral
this.Behavioral.Jumpstart();
// timer, run at 10Hz
MMWaitableTimer cycleTimer = new MMWaitableTimer(100);
// stopwatch
Stopwatch stopwatch = new Stopwatch();
Stopwatch stopwatch2 = new Stopwatch();
// set initial state
CoreCommon.CorePlanningState = new StartUpState();
// send the projection to the operational components
CoreCommon.Communications.TrySendProjection();
CoreCommon.Communications.TryOperationalTestFacadeConnect();
// always run
while (this.arbiterMode == ArbiterMode.Run || this.arbiterMode == ArbiterMode.Pause)
{
try
{
// make sure we run at 10Hz
cycleTimer.WaitEvent.WaitOne();
if (this.arbiterMode == ArbiterMode.Run)
{
// start stopwatch
stopwatch.Reset();
stopwatch.Start();
// reset ai information
CoreCommon.CurrentInformation = new ArbiterInformation();
// check for null current state
if (CoreCommon.CorePlanningState == null)
{
CoreCommon.CorePlanningState = new StartUpState();
throw new Exception("CoreCommon.CorePlanningState == null, returning to startup state");
}
// get goal
INavigableNode goal = StateReasoning.FilterGoal(CoreCommon.Communications.GetVehicleState());
// filter the state for needed changes
CoreCommon.CorePlanningState = StateReasoning.FilterStates(CoreCommon.Communications.GetCarMode(), Behavioral);
// set current state
CoreCommon.CurrentInformation.CurrentState = CoreCommon.CorePlanningState.ShortDescription();
CoreCommon.CurrentInformation.CurrentStateInfo = CoreCommon.CorePlanningState.StateInformation();
// plan the maneuver
Maneuver m = Behavioral.Plan(
CoreCommon.Communications.GetVehicleState(),
CoreCommon.Communications.GetVehicleSpeed().Value,
CoreCommon.Communications.GetObservedVehicles(),
CoreCommon.Communications.GetObservedObstacles(),
CoreCommon.Communications.GetCarMode(),
goal);
// set next state
CoreCommon.CorePlanningState = m.PrimaryState;
CoreCommon.CurrentInformation.NextState = CoreCommon.CorePlanningState.ShortDescription();
CoreCommon.CurrentInformation.NextStateInfo = CoreCommon.CorePlanningState.StateInformation();
// get ignorable
List <int> toIgnore = new List <int>();
if (m.PrimaryBehavior is StayInLaneBehavior)
{
StayInLaneBehavior silb = (StayInLaneBehavior)m.PrimaryBehavior;
if (silb.IgnorableObstacles != null)
{
toIgnore.AddRange(silb.IgnorableObstacles);
}
else
{
ArbiterOutput.Output("stay in lane ignorable obstacles null");
}
}
else if (m.PrimaryBehavior is SupraLaneBehavior)
{
SupraLaneBehavior slb = (SupraLaneBehavior)m.PrimaryBehavior;
if (slb.IgnorableObstacles != null)
{
toIgnore.AddRange(slb.IgnorableObstacles);
}
else
{
ArbiterOutput.Output("Supra lane ignorable obstacles null");
}
}
CoreCommon.CurrentInformation.FVTIgnorable = toIgnore.ToArray();
// reset the execution stopwatch
this.executionStopwatch.Stop();
this.executionStopwatch.Reset();
this.executionStopwatch.Start();
// send behavior to communications
CoreCommon.Communications.Execute(m.PrimaryBehavior);
CoreCommon.CurrentInformation.NextBehavior = m.PrimaryBehavior.ToShortString();
CoreCommon.CurrentInformation.NextBehaviorInfo = m.PrimaryBehavior.ShortBehaviorInformation();
CoreCommon.CurrentInformation.NextSpeedCommand = m.PrimaryBehavior.SpeedCommandString();
CoreCommon.CurrentInformation.NextBehaviorTimestamp = m.PrimaryBehavior.TimeStamp.ToString("F6");
#region Turn Decorators
// set turn signal decorators
if (m.PrimaryBehavior.Decorators != null)
{
bool foundDec = false;
foreach (BehaviorDecorator bd in m.PrimaryBehavior.Decorators)
{
if (bd is TurnSignalDecorator)
{
if (!foundDec)
{
TurnSignalDecorator tsd = (TurnSignalDecorator)bd;
foundDec = true;
CoreCommon.CurrentInformation.NextBehaviorTurnSignals = tsd.Signal.ToString();
}
else
{
CoreCommon.CurrentInformation.NextBehaviorTurnSignals = "Multiple!";
}
}
}
}
#endregion
// filter the lane state
if (CoreCommon.CorePlanningState.UseLaneAgent)
{
this.coreLaneAgent.UpdateInternal(CoreCommon.CorePlanningState.InternalLaneState, CoreCommon.CorePlanningState.ResetLaneAgent);
this.coreLaneAgent.UpdateEvidence(CoreCommon.Communications.GetVehicleState().Area);
CoreCommon.CorePlanningState = this.coreLaneAgent.UpdateFilter();
}
// log and send information to remote listeners
CoreCommon.Communications.UpdateInformation(CoreCommon.CurrentInformation);
// check cycle time
stopwatch.Stop();
if (stopwatch.ElapsedMilliseconds > 100 || global::UrbanChallenge.Arbiter.Core.ArbiterSettings.Default.PrintCycleTimesAlways)
{
ArbiterOutput.Output("Cycle t: " + stopwatch.ElapsedMilliseconds.ToString());
}
}
}
catch (Exception e)
{
// notify exception made its way up to the core thread
ArbiterOutput.Output("\n\n");
ArbiterOutput.Output("Core Intelligence Thread caught exception!! \n");
ArbiterOutput.Output(" Exception type: " + e.GetType().ToString());
ArbiterOutput.Output(" Exception thrown by: " + e.TargetSite + "\n");
ArbiterOutput.Output(" Stack Trace: " + e.StackTrace + "\n");
if (e is NullReferenceException)
{
NullReferenceException nre = (NullReferenceException)e;
ArbiterOutput.Output("Null reference exception from: " + nre.Source);
}
ArbiterOutput.Output("\n");
if (this.executionStopwatch.ElapsedMilliseconds / 1000.0 > 3.0)
{
ArbiterOutput.Output(" Time since last execution more then 3 seconds");
ArbiterOutput.Output(" Resetting and Restarting Intelligence");
try
{
Thread tmp = new Thread(ResetThread);
tmp.IsBackground = true;
this.arbiterMode = ArbiterMode.Stop;
tmp.Start();
}
catch (Exception ex)
{
ArbiterOutput.Output("\n\n");
ArbiterOutput.Output("Core Intelligence Thread caught exception attempting to restart itself1!!!!HOLYCRAP!! \n");
ArbiterOutput.Output(" Exception thrown by: " + ex.TargetSite + "\n");
ArbiterOutput.Output(" Stack Trace: " + ex.StackTrace + "\n");
ArbiterOutput.Output(" Resetting planning state to startup");
ArbiterOutput.Output("\n");
CoreCommon.CorePlanningState = new StartUpState();
}
}
}
}
}
/// <summary>
/// Gets exit or goal downstream
/// </summary>
/// <returns></returns>
public List<DownstreamPointOfInterest> Downstream(Coordinates currentPosition, List<ArbiterWaypoint> ignorable, INavigableNode goal)
{
List<DownstreamPointOfInterest> downstream = Initial.Downstream(currentPosition, ignorable, goal);
double addedDist = this.DistanceBetween(currentPosition, Interconnect.FinalGeneric.Position);
//double addedTime = this.Inside(currentPosition).Equals(Interconnect) ? 0.0 : Initial.TimeCostInLane(Initial.GetClosestPartition(currentPosition).Final, (ArbiterWaypoint)Interconnect.InitialGeneric) + Interconnect.ExtraCost;
List<DownstreamPointOfInterest> secondary = Final.Downstream(Interconnect.FinalGeneric.Position, ignorable, goal);
foreach (DownstreamPointOfInterest dpoi in secondary)
{
//dpoi.TimeCostToPoint += addedTime;
dpoi.DistanceToPoint += addedDist;
}
downstream.AddRange(secondary);
return downstream;
}
public double TimeTo(INavigableNode node)
{
// avg speed of 10mph from place to place
return(this.position.DistanceTo(node.Position) / 2.24);
}
/// <summary>
/// Determine intersection startup costs
/// </summary>
/// <param name="entries"></param>
/// <param name="goal"></param>
/// <returns></returns>
public IntersectionStartupPlan PlanIntersectionStartup(IEnumerable <ITraversableWaypoint> entries, INavigableNode goal)
{
Dictionary <ITraversableWaypoint, double> entryCosts = new Dictionary <ITraversableWaypoint, double>();
foreach (ITraversableWaypoint start in entries)
{
// given start and goal, get plan
double time;
List <INavigableNode> nodes;
this.Plan(start, goal, out nodes, out time);
entryCosts.Add(start, time);
}
return(new IntersectionStartupPlan(entryCosts));
}
/// <summary>
/// Plans a path from start to goal
/// </summary>
/// <returns>List of INavigableNode nodes in order of travel</returns>
public List<INavigableNode> Plan()
{
// 1. Add Start to Open list
open.Push(start);
// 2. While Open list not empty
// Console.WriteLine("2");
while (open.Count != 0)
{
// 3. Return and Remove node with lowest Cost from Open list
INavigableNode current = (INavigableNode)open.Pop();
// 4. If Current == Goal then we have found solution, terminate
if (current.EqualsNode(goal))
{
goal = current;
break;
}
// 5. Otherwise get Successors of Current
List<NavigableEdge> successors = current.OutgoingConnections;
// 6. For each successor
foreach (NavigableEdge successor in successors)
{
// 6.5 make sure successor is allowed to be planned over
if (this.removedEdges == null || !this.removedEdges.Contains(successor))
{
// 7. Get the cost estimate for the successor
double timeToHere = current.TimeToHere + current.TimeThroughAdjacent(successor);
double timeToGoal = successor.End.TimeTo(goal);
double time = timeToHere + timeToGoal;
// flag to skip this successor
bool skip = false;
// 8. Skip successor if node is closed and we've not found a better node
if (closed.ContainsKey(successor.End.Name))
{
// 9. Get the matching node on the closed list
INavigableNode closedCopy = closed[successor.End.Name];
// 10. If we did not find a shorter route, skip; else remove from closed
if (closedCopy.Time <= time)
{
skip = true;
}
else
{
closed.Remove(successor.End.Name);
}
}
// 11. Skip if node is in open and we've not found a better node
if (!skip && open.Contains(successor.End.Name))
{
// 12. Find record in open list
INavigableNode openCopy = (INavigableNode)open.Find(successor.End.Name);
// 13. If we did not find a shorter route, skip; else remove from Open
if (openCopy.Time <= time)
{
skip = true;
}
else
{
open.Remove(successor.End.Name);
}
}
// 14. If we're not to skip, it's an unvisited node or we've found a better path to a node we've previously visited
if (!skip)
{
// 15. Set costs for successor
successor.End.Previous = current;
successor.End.TimeToHere = timeToHere;
successor.End.TimeToGoal = timeToGoal;
successor.End.Time = time;
// 16. Add successor to open list
open.Push(successor.End);
}
}
}
// 17. Add current to closed list
closed.Add(current.Name, current);
} // end while
// check that we have planned to here
if (goal.Previous != null)
{
// 18. Set total time to get to the goal
totalTime = goal.TimeToHere;
// 19. Reverse the route here TESTING purposes for the moment
List<INavigableNode> route = new List<INavigableNode>();
INavigableNode temp = goal;
while (temp.Previous != null)
{
route.Add(temp);
temp = temp.Previous;
}
route.Add(temp);
route.Reverse();
return route;
}
else
{
if (start.Equals(goal))
{
this.totalTime = 0.0;
return new List<INavigableNode>();
}
else
{
this.totalTime = Double.MaxValue;
return new List<INavigableNode>();
}
}
}
/// <summary>
/// Route ploan for downstream exits
/// </summary>
/// <param name="downstreamPoints"></param>
/// <param name="goal"></param>
private void DetermineDownstreamPointRouteTimes(List <DownstreamPointOfInterest> downstreamPoints, INavigableNode goal, ArbiterWay aw)
{
// so, for each exit downstream we need to plan from the end of each interconnect to the goal
foreach (DownstreamPointOfInterest dpoi in downstreamPoints)
{
// check if exit
if (dpoi.IsExit)
{
// current best time
double bestCurrent = Double.MaxValue;
List <INavigableNode> bestRoute = null;
ArbiterInterconnect bestInterconnect = null;
// fake node
FakeExitNode fen = new FakeExitNode(dpoi.PointOfInterest);
// init fields
double timeCost;
List <INavigableNode> routeNodes;
// plan
this.Plan(fen, goal, out routeNodes, out timeCost);
bestCurrent = timeCost;
bestRoute = routeNodes;
bestInterconnect = routeNodes.Count > 1 ? fen.GetEdge(routeNodes[1]) : null;
// plan from each interconnect to find the best time from exit
/*foreach (ArbiterInterconnect ai in dpoi.PointOfInterest.Exits)
* {
* // init fields
* double timeCost;
* List<INavigableNode> routeNodes;
*
* // plan
* this.Plan(ai.End, goal, out routeNodes, out timeCost);
*
* // check
* if (timeCost < bestCurrent)
* {
* bestCurrent = timeCost;
* bestRoute = routeNodes;
* bestInterconnect = ai;
* }
* }*/
// set best
dpoi.RouteTime = bestCurrent;
dpoi.BestRoute = bestRoute;
dpoi.BestExit = bestInterconnect;
}
}
}
/// <summary>
/// Gets exit or goal downstream
/// </summary>
/// <returns></returns>
public List <DownstreamPointOfInterest> Downstream(Coordinates currentPosition, List <ArbiterWaypoint> ignorable, INavigableNode goal)
{
List <DownstreamPointOfInterest> downstream = Initial.Downstream(currentPosition, ignorable, goal);
double addedDist = this.DistanceBetween(currentPosition, Interconnect.FinalGeneric.Position);
//double addedTime = this.Inside(currentPosition).Equals(Interconnect) ? 0.0 : Initial.TimeCostInLane(Initial.GetClosestPartition(currentPosition).Final, (ArbiterWaypoint)Interconnect.InitialGeneric) + Interconnect.ExtraCost;
List <DownstreamPointOfInterest> secondary = Final.Downstream(Interconnect.FinalGeneric.Position, ignorable, goal);
foreach (DownstreamPointOfInterest dpoi in secondary)
{
//dpoi.TimeCostToPoint += addedTime;
dpoi.DistanceToPoint += addedDist;
}
downstream.AddRange(secondary);
return(downstream);
}
/// <summary>
/// Gets edge going to input node from this exit
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
public ArbiterInterconnect GetEdge(INavigableNode node)
{
foreach (NavigableEdge ne in exit.OutgoingConnections)
{
if (ne is ArbiterInterconnect)
{
ArbiterInterconnect ai = (ArbiterInterconnect)ne;
if (ai.FinalGeneric.Equals(node))
return ai;
}
}
return null;
}
/// <summary>
/// Gets exits downstream, or the goal we are currently reaching
/// </summary>
/// <param name="currentPosition"></param>
/// <param name="ignorable"></param>
/// <param name="goal"></param>
/// <returns></returns>
public List<DownstreamPointOfInterest> Downstream(Coordinates currentPosition, List<ArbiterWaypoint> ignorable, INavigableNode goal)
{
// downstream final
List<DownstreamPointOfInterest> waypoints = new List<DownstreamPointOfInterest>();
foreach (ArbiterLane al in Way.Lanes.Values)
{
if (al.Equals(this) || (al.GetClosestPartition(currentPosition).Type != PartitionType.Startup &&
((this.LaneOnLeft != null && this.LaneOnLeft.Equals(al) && this.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) ||
(this.LaneOnRight != null && this.LaneOnRight.Equals(al) && this.BoundaryRight != ArbiterLaneBoundary.SolidWhite))))
{
// get starting waypoint
ArbiterWaypoint waypoint = null;
// get closest partition
ArbiterLanePartition alp = al.GetClosestPartition(currentPosition);
if (alp.Initial.Position.DistanceTo(currentPosition) < TahoeParams.VL - 2)
waypoint = alp.Initial;
else if (alp.IsInside(currentPosition) || alp.Final.Position.DistanceTo(currentPosition) < TahoeParams.VL)
waypoint = alp.Final;
else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition))
waypoint = alp.Initial;
else if (alp.Initial.Position.DistanceTo(currentPosition) < alp.Final.Position.DistanceTo(currentPosition) && alp.Final.NextPartition == null)
waypoint = null;
else
waypoint = null;
// check waypoint exists
if (waypoint != null)
{
// save start
ArbiterWaypoint initial = waypoint;
// initial cost
double initialCost = 0.0;
if (al.Equals(this))
initialCost = currentPosition.DistanceTo(waypoint.Position) / waypoint.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
else if (waypoint.WaypointId.Number != 1)
{
// get closest partition
ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue);
initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number);
initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
}
else
{
// get closest partition
ArbiterWaypoint tmpI = this.GetClosestWaypoint(currentPosition, Double.MaxValue);
ArbiterWaypoint tmpF = this.GetClosestWaypoint(waypoint.Position, Double.MaxValue);
initialCost = NavigationPenalties.ChangeLanes * Math.Abs(this.LaneId.Number - al.LaneId.Number);
initialCost += currentPosition.DistanceTo(tmpI.Position) / tmpI.Lane.Way.Segment.SpeedLimits.MaximumSpeed;
initialCost += this.TimeCostInLane(tmpI, tmpF, new List<ArbiterWaypoint>());
}
// loop while waypoint not null
while (waypoint != null)
{
if (waypoint.IsCheckpoint && (goal is ArbiterWaypoint) && ((ArbiterWaypoint)goal).WaypointId.Equals(waypoint.WaypointId))
{
double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable);
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
dpoi.IsExit = false;
dpoi.IsGoal = true;
dpoi.PointOfInterest = waypoint;
dpoi.TimeCostToPoint = timeCost;
waypoints.Add(dpoi);
}
else if (waypoint.IsExit && !ignorable.Contains(waypoint))
{
double timeCost = initialCost + this.TimeCostInLane(initial, waypoint, ignorable);
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
dpoi.IsExit = true;
dpoi.IsGoal = false;
dpoi.PointOfInterest = waypoint;
dpoi.TimeCostToPoint = timeCost;
waypoints.Add(dpoi);
}
else if (waypoint.NextPartition == null && !ignorable.Contains(waypoint))
{
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = al.DistanceBetween(currentPosition, waypoint.Position);
dpoi.IsExit = false;
dpoi.IsGoal = false;
dpoi.PointOfInterest = waypoint;
dpoi.TimeCostToPoint = Double.MaxValue;
waypoints.Add(dpoi);
}
waypoint = waypoint.NextPartition != null ? waypoint.NextPartition.Final : null;
}
}
}
}
return waypoints;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="exit"></param>
public FakeExitNode(INavigableNode exit)
{
this.exit = exit;
}
/// <summary>
/// Checks if a goal is downstream on the current road
/// </summary>
/// <param name="way"></param>
/// <param name="currentPosition"></param>
/// <param name="goal"></param>
/// <returns></returns>
private DownstreamPointOfInterest IsGoalDownStream(ArbiterWay way, Coordinates currentPosition, INavigableNode goal)
{
if (goal is ArbiterWaypoint)
{
ArbiterWaypoint goalWaypoint = (ArbiterWaypoint)goal;
if (goalWaypoint.Lane.Way.Equals(way))
{
foreach (ArbiterLane lane in way.Lanes.Values)
{
if (lane.Equals(goalWaypoint.Lane))
{
Coordinates current = lane.GetClosest(currentPosition);
Coordinates goalPoint = lane.GetClosest(goal.Position);
if (lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0)
{
double distToGoal = lane.DistanceBetween(current, goalPoint);
if (distToGoal > 0)
{
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = distToGoal;
dpoi.IsGoal = true;
dpoi.IsExit = false;
dpoi.PointOfInterest = goalWaypoint;
dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint);
dpoi.RouteTime = 0.0;
return dpoi;
}
}
}
else
{
Coordinates current = lane.GetClosest(currentPosition);
Coordinates goalPoint = lane.GetClosest(goal.Position);
if ((lane.IsInside(current) || currentPosition.DistanceTo(lane.LanePath().StartPoint.Location) < 1.0) && lane.IsInside(goalPoint))
{
double distToGoal = lane.DistanceBetween(current, goalPoint);
if (distToGoal > 0)
{
DownstreamPointOfInterest dpoi = new DownstreamPointOfInterest();
dpoi.DistanceToPoint = distToGoal;
dpoi.IsGoal = true;
dpoi.IsExit = false;
dpoi.PointOfInterest = goalWaypoint;
dpoi.TimeCostToPoint = this.TimeCostInLane(lane.GetClosestPartition(currentPosition).Initial, goalWaypoint);
dpoi.RouteTime = 0.0;
//return dpoi;
}
}
}
}
}
}
return null;
}
