/// <summary>
/// Updates the ai with a new mission if can
/// </summary>
/// <param name="mission"></param>
/// <returns></returns>
public override bool UpdateMission(ArbiterMissionDescription mission)
{
try
{
ArbiterOutput.Output("Setting new mission");
CarMode carMode = CoreCommon.Communications.GetCarMode();
if (carMode == CarMode.Pause || carMode == CarMode.Human)
{
if (mission != null)
{
// create roads and mission
CoreCommon.RoadNetwork.SetSpeedLimits(mission.SpeedLimits);
this.arbiterMissionDescription = mission;
CoreCommon.Mission = mission;
return(true);
}
else
{
ArbiterOutput.Output("Mission must have value");
}
}
else
{
ArbiterOutput.Output("Cannot set mission when car is in CarMode: " + carMode.ToString());
}
}
catch (Exception ex)
{
ArbiterOutput.Output("UpdateMission(ArbiterMissionDescription mission) Failed", ex);
}
return(false);
}
/// <summary>
/// Checks if it is clear to go into the other lane
/// </summary>
/// <param name="state"></param>
/// <returns></returns>
public bool IsClear(VehicleState state, double vUs)
{
// temp get
bool b = this.CheckClear(state, vUs);
if (!b)
{
this.ResetTiming();
return(false);
}
// if none found, tiemr not running start timer
if (!this.RearClearStopwatch.IsRunning)
{
this.ResetTiming();
this.RearClearStopwatch.Start();
ArbiterOutput.Output("Forward Rear: " + this.VehicleSide.ToString() + " Clear, starting cooldown");
return(false);
}
// enough time
else if (this.RearClearStopwatch.IsRunning && this.RearClearStopwatch.ElapsedMilliseconds / 1000.0 > 0.5)
{
ArbiterOutput.Output("Forward Rear: " + this.VehicleSide.ToString() + " Clear, cooldown complete");
return(true);
}
// not enough time
else
{
double timer = this.RearClearStopwatch.ElapsedMilliseconds / 1000.0;
ArbiterOutput.Output("Forward Rear: " + this.VehicleSide.ToString() + " Clear, cooldown timer: " + timer.ToString("F2"));
return(false);
}
}
/// <summary>
/// Sets the ai mode
/// </summary>
/// <param name="mode"></param>
public override void SetAiMode(ArbiterMode mode)
{
try
{
ArbiterOutput.Output("Setting ai mode to ArbiterMode: " + mode.ToString());
switch (mode)
{
case ArbiterMode.Run:
this.intelligenceCore.RunIntelligence();
break;
case ArbiterMode.Pause:
this.intelligenceCore.PauseIntelligence();
break;
case ArbiterMode.Stop:
this.intelligenceCore.DestroyIntelligence();
break;
}
}
catch (Exception ex)
{
ArbiterOutput.Output("SetAiMode(ArbiterMode mode) Failed", ex);
}
}
/// <summary>
/// Shuts down the old ai thread
/// </summary>
public void DestroyIntelligence()
{
// notify
ArbiterOutput.Output("Attemptimg to Destroy Old Core Intelligence, Please Wait");
int count = 0;
// destroy old arbiter intelligence
while (this.CoreIntelligenceThread != null && this.CoreIntelligenceThread.IsAlive)
{
this.arbiterMode = ArbiterMode.Stop;
Thread.Sleep(100);
count++;
if (count > 50)
{
try
{
ArbiterOutput.Output("Cold not normally shutdown old intelligence, Aborting the core intelligence thread");
this.CoreIntelligenceThread.Abort();
}
catch (Exception)
{
}
// chill and wait for intelligence to exit
Thread.Sleep(1000);
}
}
// notify
ArbiterOutput.Output("No Intelligence Running, Destruction Successful");
}
/// <summary>
/// Check if a side sick blockng obstacle is detected
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
private bool SideSickObstacleDetected(ArbiterLane lane, VehicleState state)
{
try
{
// get distance from vehicle to lane opp side
Coordinates vec = state.Front - state.Position;
vec = this.VehicleSide == SideObstacleSide.Driver ? vec.Rotate90() : vec.RotateM90();
SideObstacles sobs = CoreCommon.Communications.GetSideObstacles(this.VehicleSide);
if (sobs != null && sobs.obstacles != null)
{
foreach (SideObstacle so in sobs.obstacles)
{
Coordinates cVec = state.Position + vec.Normalize(so.distance);
if (so.height > 0.7 && lane.LanePolygon.IsInside(cVec))
{
return(true);
}
}
}
}
catch (Exception ex)
{
ArbiterOutput.Output("opposing side sick obstacle exception: " + ex.ToString());
}
return(false);
}
/// <summary>
/// Begins all functions for maintaining the arbiter
/// </summary>
public void BeginArbiterCore()
{
// Console begin
Console.WriteLine("");
Console.WriteLine("Cornell University Darpa Urban Challenge, 2006-2007");
Art.WriteImpossible();
// begin communications
ArbiterOutput.Output("Creating Communications Watchdog");
this.communicator.BeginCommunications();
CoreCommon.Communications = this.communicator;
// begin watchdog
ArbiterOutput.Output("Creating Intelligence Watchdog");
watchdogThread = new Thread(this.Watchdog);
watchdogThread.IsBackground = true;
watchdogThread.Priority = ThreadPriority.BelowNormal;
watchdogThread.Start();
// Notify ready
Console.WriteLine("");
ArbiterOutput.Output("Ready To Begin, Good Luck!");
Console.WriteLine("");
// begin command line interface
this.Command();
}
/// <summary>
/// Keeps watch over the ai and determines if any components need to be restarted
/// </summary>
private void Watchdog()
{
while (true)
{
try
{
// wait for a second or so at a time
Thread.Sleep(1000);
// check state of intelligence
if ((intelligenceCore.CoreIntelligenceThread == null || (intelligenceCore.CoreIntelligenceThread != null && !intelligenceCore.CoreIntelligenceThread.IsAlive)) && intelligenceCore.arbiterMode == ArbiterMode.Run)
{
// critical error
ArbiterOutput.Output("Critical error found by intelligence watchdog! intelligence thread not running");
// restart
ArbiterOutput.Output("Restarting ai by watchdog");
// begin intelligence thread
ArbiterOutput.Output("Spooling Up Arbiter Core Intelligence");
intelligenceCore.Jumpstart();
}
}
catch (Exception e)
{
ArbiterOutput.Output("Error in intelligence watchdog: \n" + e.ToString());
}
}
}
public override void RemoveNextCheckpoint()
{
try
{
if (CoreCommon.Mission != null)
{
if (CoreCommon.Mission.MissionCheckpoints.Count > 0)
{
this.intelligenceCore.DestroyIntelligence();
Thread.Sleep(300);
ArbiterOutput.Output("Removing checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
this.Reset();
}
else
{
ArbiterOutput.Output("No checkpoint to remove");
}
}
}
catch (Exception e)
{
ArbiterOutput.Output(e.ToString());
}
}
/// <summary>
/// Pause the vehicle using an ai defined pause
/// </summary>
public void ExecuteAiDefinedPause()
{
this.PauseIntelligence();
Thread.Sleep(500);
CoreCommon.Communications.Execute(new EmergencyStop(true, true, true));
ArbiterOutput.Output("Executed Ai Defined Pause, Core Intelligence Paused");
}
/// <summary>
/// Start updating blockages
/// </summary>
public void JumpstartBlockageThread()
{
ArbiterOutput.Output("Preprocessing Blockage Costs: Simple Version");
// need to calculate blockage costs
foreach (ArbiterSegment asg in CoreCommon.RoadNetwork.ArbiterSegments.Values)
{
//ArbiterOutput.Output(" Segment: " + asg.SegmentId.ToString());
foreach (ArbiterWay aw in asg.Ways.Values)
{
foreach (ArbiterLane al in aw.Lanes.Values)
{
foreach (ArbiterLanePartition alp in al.Partitions)
{
this.SetBlockageCost(alp);
}
}
}
}
ArbiterOutput.Output("Preprocessing of blockage costs completed");
// blockage thread stuff
blockageUpdateThread = new Thread(BlockageUpdate);
blockageUpdateThread.IsBackground = true;
blockageUpdateThread.Priority = ThreadPriority.Normal;
blockageUpdateThread.Start();
}
/// <summary>
/// Executes an emergency stop and kills core intelligence
/// </summary>
public void ExecuteEmergencyStop()
{
// execute stop
CoreCommon.Communications.Execute(new EmergencyStop(false, false, false));
this.DestroyIntelligence();
CoreCommon.Communications.Execute(new EmergencyStop(false, false, false));
ArbiterOutput.Output("Executed Emergency Stop, Destroyed Core Intelligence");
}
/// <summary>
/// Quit
/// </summary>
private void ShutDown()
{
// notify shut down nominally
ArbiterOutput.WriteToLog("Shutting down nominally");
// close comms
this.communicator.Shutdown();
// close output
ArbiterOutput.ShutDownOutput();
}
/// <summary>
/// Emergency stop the vehicle
/// </summary>
public override void EmergencyStop()
{
try
{
ArbiterOutput.Output("Emergency Stop");
this.intelligenceCore.ExecuteEmergencyStop();
}
catch (Exception ex)
{
ArbiterOutput.Output("EmergencyStop() Failed", ex);
}
}
/// <summary>
/// Resets the intelligence
/// </summary>
public override void Reset()
{
try
{
ArbiterOutput.Output("Resetting Arbiter Intelligence");
this.intelligenceCore.Restart();
}
catch (Exception ex)
{
ArbiterOutput.Output("Reset() Failed", ex);
}
}
/// <summary>
/// Pauses the ai, direting it to forece the vehicle to pause
/// </summary>
public override void PauseFromAi()
{
try
{
ArbiterOutput.Output("Directing ai to pause vehicle");
this.intelligenceCore.PauseIntelligence();
}
catch (Exception ex)
{
ArbiterOutput.Output("PauseFromAi() Failed", ex);
}
}
/// <summary>
/// Starts a new log
/// </summary>
public override void BeginNewLog()
{
try
{
ArbiterOutput.Output("Starting new log");
ArbiterOutput.ShutDownOutput();
ArbiterOutput.BeginLog();
}
catch (Exception ex)
{
ArbiterOutput.Output("BeginNewLog Failed", ex);
}
}
/// <summary>
/// sets the current blockage report
/// </summary>
/// <param name="tbr"></param>
public void OnBlockageReport(TrajectoryBlockedReport tbr)
{
// set the report
this.recentReport = tbr;
// output
ArbiterOutput.Output("Blockage report: Behavior: " + tbr.BehaviorType.ToString() + ", Type: " + tbr.BlockageType.ToString() + ", Dist: " + tbr.DistanceToBlockage.ToString("f2") + ", Result: " + tbr.Result.ToString() + ", Reverse Rec: " + tbr.ReverseRecommended.ToString() + ", Saudi: " + tbr.SAUDILevel.ToString() + ", Track: " + tbr.TrackID.ToString() + "\n");
// lock the timer
lock (timeoutTimer)
{
this.timeoutTimer.Stop();
this.timeoutTimer.Reset();
this.timeoutTimer.Start();
}
}
/// <summary>
/// Makes the thread wait for the entry data
/// </summary>
private void WaitForEntryData()
{
// wait for entry data
ArbiterOutput.Output("Waiting for entry data");
// flag to wait
bool canStart = false;
// chack while we can't start
while (!canStart && (this.arbiterMode == ArbiterMode.Run || this.arbiterMode == ArbiterMode.Pause))
{
// check data ready
canStart =
(CoreCommon.RoadNetwork != null &&
CoreCommon.Mission != null &&
CoreCommon.Communications.GetVehicleState() != null &&
CoreCommon.Communications.GetVehicleSpeed() != null &&
CoreCommon.Communications.GetObservedVehicles() != null &&
CoreCommon.Communications.GetObservedObstacles() != null);
if (canStart)
{
ArbiterOutput.Output("Entry data nominal, starting");
}
else
{
bool roads = CoreCommon.RoadNetwork != null;
bool mission = CoreCommon.Mission != null;
bool vs = CoreCommon.Communications.GetVehicleState() != null;
bool vspeed = CoreCommon.Communications.GetVehicleSpeed() != null;
bool obsV = CoreCommon.Communications.GetObservedVehicles() != null;
bool obsO = CoreCommon.Communications.GetObservedObstacles() != null;
ArbiterOutput.Output("");
ArbiterOutput.Output("Waiting for data, currently received:");
ArbiterOutput.Output(" Roads: " + roads.ToString() + ", Mission: " + mission.ToString() + ", Vehicle State " + vs.ToString());
ArbiterOutput.Output(" Speed: " + vspeed.ToString() + ", Vehicles: " + obsV.ToString() + ", Obstacles " + obsO.ToString());
}
Thread.Sleep(1000);
}
// nominal
ArbiterOutput.Output("All inputs nominal, car in Run, starting");
}
/// <summary>
/// Jumpstarts the intelligence core
/// </summary>
public void Jumpstart()
{
// make sure to remove old
this.DestroyIntelligence();
// start new
ArbiterOutput.Output("Starting New Core Intelligence Thread");
this.arbiterMode = ArbiterMode.Run;
// start intelligence thread
CoreIntelligenceThread = new Thread(CoreIntelligence);
CoreIntelligenceThread.Priority = ThreadPriority.AboveNormal;
CoreIntelligenceThread.IsBackground = true;
CoreIntelligenceThread.Start();
// notify
ArbiterOutput.Output("Core Intelligence Thread Jumpstarted");
}
/// <summary>
/// Constructor
/// </summary>
public ArbiterCore()
{
// begin the log
ArbiterOutput.BeginLog();
// constructor
this.communicator = new Communicator(this);
// set intelligence core
this.intelligenceCore = new CoreIntelligence.Core();
#region Handle Events
this.OnRoadNetworkChanged += new RoadNetworkChangedDelegate(ArbiterCore_OnRoadNetworkChanged);
this.OnMissionDescriptionChanged += new MissionDescriptionChangedDelegate(ArbiterCore_OnMissionDescriptionChanged);
#endregion
}
/// <summary>
/// Runs intelligence
/// </summary>
public void RunIntelligence()
{
ArbiterOutput.Output("Attempting to switch Core Intelligence to run mode");
if (this.CoreIntelligenceThread != null)
{
if (this.CoreIntelligenceThread.ThreadState != System.Threading.ThreadState.Running)
{
ArbiterOutput.Output("Core Intelligence set to run mode");
this.arbiterMode = ArbiterMode.Run;
}
else
{
ArbiterOutput.Output("Intelligence already running");
}
}
else
{
ArbiterOutput.Output("Intelligence Not Initialized");
}
}
/// <summary>
/// Jumpstarts the ai with a new road network and mission if we can
/// </summary>
/// <param name="roadNetwork"></param>
/// <param name="mission"></param>
public override void JumpstartArbiter(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission)
{
try
{
ArbiterOutput.Output("Jumpstarting Arbiter");
// warn if carmode not correct
CarMode carMode = CoreCommon.Communications.GetCarMode();
if (carMode != CarMode.Pause && carMode != CarMode.Human)
{
ArbiterOutput.Output("Warning: Vehicle is in CarMode: " + carMode.ToString());
}
if (roadNetwork != null && mission != null)
{
// destroy intelligence if exists
this.intelligenceCore.DestroyIntelligence();
// create roads and mission
roadNetwork.SetSpeedLimits(mission.SpeedLimits);
roadNetwork.GenerateVehicleAreas();
this.arbiterRoadNetwork = roadNetwork;
this.arbiterMissionDescription = mission;
CoreCommon.RoadNetwork = roadNetwork;
CoreCommon.Mission = mission;
// startup ai
this.intelligenceCore.Jumpstart();
}
else
{
ArbiterOutput.Output("RoadNetwork and Mission must both have value");
}
}
catch (Exception ex)
{
ArbiterOutput.Output("JumpstartArbiter(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission) Failed", ex);
}
}
/// <summary>
/// Filter the estiamtes
/// </summary>
public IState UpdateFilter()
{
// forward filter
this.FilteredEstimate.Add(this.Forward());
// check again if we should use lane agent for state
if (CoreCommon.CorePlanningState.UseLaneAgent)
{
if (FilterSteady)
{
return(CoreCommon.CorePlanningState);
}
else
{
ArbiterOutput.Output("Lane Agent Filter Diverged, Resetting Planning State");
return(new StartUpState());
}
}
else
{
return(CoreCommon.CorePlanningState);
}
}
/// <summary>
/// Updates blockages every minute
/// </summary>
public void BlockageUpdate()
{
while (true)
{
try
{
foreach (ArbiterSegment asg in CoreCommon.RoadNetwork.ArbiterSegments.Values)
{
foreach (ArbiterLane al in asg.Lanes.Values)
{
foreach (ArbiterLanePartition alp in al.Partitions)
{
if (alp.Blockage.ComputeBlockageExists())
{
alp.Blockage.SecondsSinceObserved += 60.0;
}
}
}
}
foreach (ArbiterInterconnect ai in CoreCommon.RoadNetwork.ArbiterInterconnects.Values)
{
if (ai.Blockage.ComputeBlockageExists())
{
ai.Blockage.SecondsSinceObserved += 60.0;
}
}
}
catch (Exception e)
{
ArbiterOutput.Output("Error in blockage update");
ArbiterOutput.WriteToLog("Error: " + e.ToString() + "\n Stack:" + e.StackTrace.ToString());
}
Thread.Sleep(60000);
}
}
/// <summary>
/// Makes new parameterization for nav
/// </summary>
/// <param name="lane"></param>
/// <param name="lanePlan"></param>
/// <param name="speed"></param>
/// <param name="distance"></param>
/// <param name="stopType"></param>
/// <returns></returns>
public TravelingParameters NavStopParameterization(IFQMPlanable lane, RoadPlan roadPlan, double speed, double distance,
ArbiterWaypoint stopWaypoint, StopType stopType, VehicleState state)
{
// get min dist
double distanceCutOff = stopType == StopType.StopLine ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance;
#region Get Decorators
// turn direction default
ArbiterTurnDirection atd = ArbiterTurnDirection.Straight;
List <BehaviorDecorator> decorators = TurnDecorators.NoDecorators;
// check if need decorators
if (lane is ArbiterLane &&
stopWaypoint.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest) &&
roadPlan.BestPlan.laneWaypointOfInterest.IsExit &&
distance < 40.0)
{
if (roadPlan.BestPlan.laneWaypointOfInterest.BestExit == null)
{
ArbiterOutput.Output("NAV BUG: lanePlan.laneWaypointOfInterest.BestExit: FQM NavStopParameterization");
}
else
{
switch (roadPlan.BestPlan.laneWaypointOfInterest.BestExit.TurnDirection)
{
case ArbiterTurnDirection.Left:
decorators = TurnDecorators.LeftTurnDecorator;
atd = ArbiterTurnDirection.Left;
break;
case ArbiterTurnDirection.Right:
atd = ArbiterTurnDirection.Right;
decorators = TurnDecorators.RightTurnDecorator;
break;
case ArbiterTurnDirection.Straight:
atd = ArbiterTurnDirection.Straight;
decorators = TurnDecorators.NoDecorators;
break;
case ArbiterTurnDirection.UTurn:
atd = ArbiterTurnDirection.UTurn;
decorators = TurnDecorators.LeftTurnDecorator;
break;
}
}
}
else if (lane is SupraLane)
{
SupraLane sl = (SupraLane)lane;
double distToInterconnect = sl.DistanceBetween(state.Front, sl.Interconnect.InitialGeneric.Position);
if ((distToInterconnect > 0 && distToInterconnect < 40.0) || sl.ClosestComponent(state.Front) == SLComponentType.Interconnect)
{
switch (sl.Interconnect.TurnDirection)
{
case ArbiterTurnDirection.Left:
decorators = TurnDecorators.LeftTurnDecorator;
atd = ArbiterTurnDirection.Left;
break;
case ArbiterTurnDirection.Right:
atd = ArbiterTurnDirection.Right;
decorators = TurnDecorators.RightTurnDecorator;
break;
case ArbiterTurnDirection.Straight:
atd = ArbiterTurnDirection.Straight;
decorators = TurnDecorators.NoDecorators;
break;
case ArbiterTurnDirection.UTurn:
atd = ArbiterTurnDirection.UTurn;
decorators = TurnDecorators.LeftTurnDecorator;
break;
}
}
}
#endregion
#region Get Maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
SpeedCommand sc = new StopAtDistSpeedCommand(distance);
#region Distance Cutoff
// check if distance is less than cutoff
if (distance < distanceCutOff && stopType != StopType.EndOfLane)
{
// default behavior
Behavior b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtDistSpeedCommand(distance), new List <int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true));
// stopping so not using speed param
usingSpeed = false;
// exit is next
if (stopType == StopType.Exit)
{
// exit means stopping at a good exit in our current lane
IState nextState = new StoppingAtExitState(stopWaypoint.Lane, stopWaypoint, atd, true, roadPlan.BestPlan.laneWaypointOfInterest.BestExit, state.Timestamp, state.Front);
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
// stop line is left
else if (stopType == StopType.StopLine)
{
// determine if hte stop line is the best exit
bool isNavExit = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Equals(stopWaypoint);
// get turn direction
atd = isNavExit ? atd : ArbiterTurnDirection.Straight;
// predetermine interconnect if best exit
ArbiterInterconnect desired = null;
if (isNavExit)
{
desired = roadPlan.BestPlan.laneWaypointOfInterest.BestExit;
}
else if (stopWaypoint.NextPartition != null && state.Front.DistanceTo(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > 25)
{
desired = stopWaypoint.NextPartition.ToInterconnect;
}
// set decorators
decorators = isNavExit ? decorators : TurnDecorators.NoDecorators;
// stop at the stop
IState nextState = new StoppingAtStopState(stopWaypoint.Lane, stopWaypoint, atd, isNavExit, desired);
b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtLineSpeedCommand(), new List <int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true));
m = new Maneuver(b, nextState, decorators, state.Timestamp);
sc = new StopAtLineSpeedCommand();
}
else if (stopType == StopType.LastGoal)
{
// stop at the last goal
IState nextState = new StayInLaneState(stopWaypoint.Lane, CoreCommon.CorePlanningState);
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// set speed
sc = new ScalarSpeedCommand(speed);
// check if lane
if (lane is ArbiterLane)
{
// get lane
ArbiterLane al = (ArbiterLane)lane;
// default behavior
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List <int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), decorators, state.Timestamp);
}
// check if supra lane
else if (lane is SupraLane)
{
// get lane
SupraLane sl = (SupraLane)lane;
// get sl state
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
// get default beheavior
Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List <int>());
// standard behavior is fine for maneuver
m = new Maneuver(b, sisls, decorators, state.Timestamp);
}
}
#endregion
#endregion
#region Parameterize
// create new params
TravelingParameters tp = new TravelingParameters();
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = distance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = speed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.SpeedCommand = sc;
tp.VehiclesToIgnore = new List <int>();
// return navigation params
return(tp);
#endregion
}
/// <summary>
/// Reconnect to stuff
/// </summary>
public override void Reconnect()
{
ArbiterOutput.Output("Attempting to reconnect to stuff");
CoreCommon.Communications.TryReconnect();
}
public static INavigableNode FilterGoal(VehicleState state)
{
// get goal
INavigableNode goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ?
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null;
if (waitRemoveLastGoal && CoreCommon.Mission.MissionCheckpoints.Count != 1)
{
waitRemoveLastGoal = false;
}
// id
IArbiterWaypoint goalWp = null;
if (goal != null)
{
goalWp = CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId];
}
// check lane change or opposing
if (goal != null &&
(CoreCommon.CorePlanningState is OpposingLanesState && ((OpposingLanesState)CoreCommon.CorePlanningState).HitGoal(state, goal.Position, goalWp.AreaSubtypeWaypointId)) ||
(CoreCommon.CorePlanningState is ChangeLanesState && ((ChangeLanesState)CoreCommon.CorePlanningState).HitGoal(state, goal.Position, goalWp.AreaSubtypeWaypointId)))
{
if (CoreCommon.Mission.MissionCheckpoints.Count == 1)
{
waitRemoveLastGoal = true;
ArbiterOutput.Output("Waiting to remove last Checkpoint: " + goal.ToString());
}
else
{
// set hit
ArbiterOutput.Output("Reached Checkpoint: " + goal.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// update goal
goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ?
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null;
}
}
else if (goal != null && CoreCommon.Mission.MissionCheckpoints.Count == 1 && waitRemoveLastGoal &&
(CoreCommon.CorePlanningState is StayInLaneState || CoreCommon.CorePlanningState is StayInSupraLaneState))
{
// set hit
ArbiterOutput.Output("Wait over, Reached Checkpoint: " + goal.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// update goal
goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ?
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null;
}
// TODO implement full version of hit test
// check if we have hit the goal (either by being in opposing lane or going to opposing and next to it or in lane and pass over it
else if (goal != null)
{
bool reachedCp = false;
if (CoreCommon.CorePlanningState is StayInLaneState)
{
StayInLaneState sils = (StayInLaneState)CoreCommon.CorePlanningState;
if (goal is ArbiterWaypoint && ((ArbiterWaypoint)goal).Lane.Equals(sils.Lane))
{
if (CoreCommon.Mission.MissionCheckpoints.Count != 1)
{
double distanceAlong = sils.Lane.DistanceBetween(state.Front, goal.Position);
if (Math.Abs(distanceAlong) < 1.5 + (1.5 * CoreCommon.Communications.GetVehicleSpeed().Value) / 5.0)
{
reachedCp = true;
}
}
else
{
double distanceAlong = sils.Lane.DistanceBetween(state.Front, goal.Position);
double distanceAlong2 = sils.Lane.DistanceBetween(state.Position, goal.Position);
if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.005 && Math.Abs(distanceAlong) < 0.3 ||
CoreCommon.Communications.GetVehicleState().VehiclePolygon.IsInside(goal.Position) ||
(distanceAlong <= 0.0 && distanceAlong2 >= 0))
{
reachedCp = true;
}
}
}
}
else if (CoreCommon.CorePlanningState is ChangeLanesState)
{
ChangeLanesState cls = (ChangeLanesState)CoreCommon.CorePlanningState;
if (cls.Parameters.Initial.Way.Equals(cls.Parameters.Target.Way) &&
goal is ArbiterWaypoint && ((ArbiterWaypoint)goal).Lane.Equals(cls.Parameters.Target))
{
double distanceAlong = cls.Parameters.Target.DistanceBetween(state.Front, goal.Position);
if (Math.Abs(distanceAlong) < 1.5 + (1.5 * CoreCommon.Communications.GetVehicleSpeed().Value) / 5.0)
{
reachedCp = true;
ArbiterOutput.Output("Removed goal changing lanes");
}
}
}
if (reachedCp)
{
// set hit
ArbiterOutput.Output("Reached Checkpoint: " + goal.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// update goal
goal = CoreCommon.Mission.MissionCheckpoints.Count > 0 ?
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId] : null;
}
}
// set goal info
CoreCommon.CurrentInformation.RouteCheckpoint = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? goal.ToString() : "NONE";
CoreCommon.CurrentInformation.GoalsRemaining = CoreCommon.Mission.MissionCheckpoints.Count.ToString();
CoreCommon.CurrentInformation.RouteCheckpointId = CoreCommon.Mission.MissionCheckpoints.Count > 0 ? CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() : "NONE";
// return current
return(goal);
}
/// <summary>
/// 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>
/// Populates mapping of areas to vehicles
/// </summary>
public void PopulateAreaVehicles()
{
TacticalDirector.VehicleAreas = new Dictionary <IVehicleArea, List <VehicleAgent> >();
VehicleState vs = CoreCommon.Communications.GetVehicleState();
Circle circ = new Circle(TahoeParams.T + 0.3, new Coordinates());
Polygon conv = circ.ToPolygon(32);
Circle circ1 = new Circle(TahoeParams.T + 0.6, new Coordinates());
Polygon conv1 = circ1.ToPolygon(32);
Circle circ2 = new Circle(TahoeParams.T + 1.4, new Coordinates());
Polygon conv2 = circ2.ToPolygon(32);
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
#region Standard Areas
List <AreaProbability> AreaProbabilities = new List <AreaProbability>();
#region Add up probablities
for (int i = 0; i < va.StateMonitor.Observed.closestPartitions.Length; i++)
{
SceneEstimatorClusterPartition secp = va.StateMonitor.Observed.closestPartitions[i];
if (CoreCommon.RoadNetwork.VehicleAreaMap.ContainsKey(secp.partitionID))
{
IVehicleArea iva = CoreCommon.RoadNetwork.VehicleAreaMap[secp.partitionID];
bool found = false;
for (int j = 0; j < AreaProbabilities.Count; j++)
{
AreaProbability ap = AreaProbabilities[j];
if (ap.Key.Equals(iva))
{
ap.Value = ap.Value + secp.probability;
found = true;
}
}
if (!found)
{
AreaProbabilities.Add(new AreaProbability(iva, secp.probability));
}
}
else
{
ArbiterOutput.Output("Core Intelligence thread caught exception, partition: " + secp.partitionID + " not found in Vehicle Area Map");
}
}
#endregion
#region Assign
if (AreaProbabilities.Count > 0)
{
double rP = 0.0;
foreach (AreaProbability ap in AreaProbabilities)
{
if (ap.Key is ArbiterLane)
{
rP += ap.Value;
}
}
if (rP > 0.1)
{
foreach (AreaProbability ap in AreaProbabilities)
{
if (ap.Key is ArbiterLane)
{
// get lane
ArbiterLane al = (ArbiterLane)ap.Key;
// probability says in area
double vP = ap.Value / rP;
if (vP > 0.3)
{
#region Check if obstacle enough in area
bool ok = false;
if (ap.Key is ArbiterLane)
{
Coordinates closest = va.ClosestPointToLine(al.LanePath(), vs).Value;
// dist to closest
double distanceToClosest = vs.Front.DistanceTo(closest);
// get our dist to closest
if (30.0 < distanceToClosest && distanceToClosest < (30.0 + ((5.0 / 2.24) * Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value))))
{
if (al.LanePolygon != null)
{
ok = this.VehicleExistsInsidePolygon(va, al.LanePolygon, vs);
}
else
{
ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
}
}
else if (distanceToClosest <= 30.0)
{
if (al.LanePolygon != null)
{
#warning Darpa is an asshole
if (!va.IsStopped && this.VehicleAllInsidePolygon(va, al.LanePolygon, vs))
{
ok = true;
}
else
{
if (va.IsStopped)
{
// check vehicle is in a safety zone
bool isInSafety = false;
try
{
foreach (ArbiterIntersection ai in CoreCommon.RoadNetwork.ArbiterIntersections.Values)
{
if (ai.IntersectionPolygon.IsInside(va.ClosestPosition))
{
isInSafety = true;
}
}
foreach (ArbiterSafetyZone asz in al.SafetyZones)
{
if (asz.IsInSafety(va.ClosestPosition))
{
isInSafety = true;
}
}
}
catch (Exception) { }
if (isInSafety)
{
if (!this.VehiclePassableInPolygon(al, va, al.LanePolygon, vs, conv1))
{
ok = true;
}
}
else
{
if (!this.VehiclePassableInPolygon(al, va, al.LanePolygon, vs, conv))
{
ok = true;
}
}
}
else
{
if (!this.VehiclePassableInPolygon(al, va, al.LanePolygon, vs, conv2))
{
ok = true;
}
}
}
}
else
{
ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
}
}
else
{
ok = true;
}
}
#endregion
if (ok)
{
if (TacticalDirector.VehicleAreas.ContainsKey(ap.Key))
{
TacticalDirector.VehicleAreas[ap.Key].Add(va);
}
else
{
List <VehicleAgent> vas = new List <VehicleAgent>();
vas.Add(va);
TacticalDirector.VehicleAreas.Add(ap.Key, vas);
}
}
}
}
}
}
}
#endregion
#endregion
#region Interconnect Area Mappings
foreach (ArbiterInterconnect ai in CoreCommon.RoadNetwork.ArbiterInterconnects.Values)
{
if (ai.TurnPolygon.IsInside(va.StateMonitor.Observed.closestPoint))
{
if (TacticalDirector.VehicleAreas.ContainsKey(ai) && !TacticalDirector.VehicleAreas[ai].Contains(va))
{
TacticalDirector.VehicleAreas[ai].Add(va);
}
else
{
List <VehicleAgent> vas = new List <VehicleAgent>();
vas.Add(va);
TacticalDirector.VehicleAreas.Add(ai, vas);
}
// check if uturn
if (ai.TurnDirection == ArbiterTurnDirection.UTurn &&
ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint)
{
// get the lanes the uturn is a part of
ArbiterLane initialLane = ((ArbiterWaypoint)ai.InitialGeneric).Lane;
ArbiterLane targetLane = ((ArbiterWaypoint)ai.FinalGeneric).Lane;
if (TacticalDirector.VehicleAreas.ContainsKey(initialLane))
{
if (!TacticalDirector.VehicleAreas[initialLane].Contains(va))
{
TacticalDirector.VehicleAreas[initialLane].Add(va);
}
}
else
{
List <VehicleAgent> vas = new List <VehicleAgent>();
vas.Add(va);
TacticalDirector.VehicleAreas.Add(initialLane, vas);
}
if (TacticalDirector.VehicleAreas.ContainsKey(targetLane))
{
if (!TacticalDirector.VehicleAreas[targetLane].Contains(va))
{
TacticalDirector.VehicleAreas[targetLane].Add(va);
}
}
else
{
List <VehicleAgent> vas = new List <VehicleAgent>();
vas.Add(va);
TacticalDirector.VehicleAreas.Add(targetLane, vas);
}
}
}
}
#endregion
}
}
/// <summary>
/// The command line for simple information adn control functions
/// </summary>
private void Command()
{
// command string
string command = "";
// run while not supposed to quit
while (command != "exit")
{
// command prompt
Console.Write("Arbiter > ");
// wait for entry
command = Console.ReadLine();
// exit
if (command == "exit")
{
// shutdown
this.ShutDown();
}
else if (command == "resetArbiter")
{
try
{
this.intelligenceCore.Restart();
}
catch (Exception e)
{
ArbiterOutput.Output("Attempted to reset arbiter, error: " + e.ToString());
}
}
else if (command == "disableOutputLogging")
{
ArbiterOutput.Output("Disabled Output Logging");
ArbiterOutput.DefaultOutputLoggingEnabled = false;
}
else if (command == "disableOutputMessaging")
{
ArbiterOutput.Output("Disabled Output Messaging");
ArbiterOutput.DefaultOutputMessagingEnabled = false;
}
else if (command == "enableOutputLogging")
{
ArbiterOutput.DefaultOutputLoggingEnabled = true;
ArbiterOutput.Output("Enabled Output Logging");
}
else if (command == "enableOutputMessaging")
{
ArbiterOutput.DefaultOutputMessagingEnabled = true;
ArbiterOutput.Output("Enabled Output Logging");
}
else if (command == "removeCheckpoint")
{
if (CoreCommon.Mission != null)
{
if (CoreCommon.Mission.MissionCheckpoints.Count > 0)
{
this.intelligenceCore.DestroyIntelligence();
Thread.Sleep(300);
ArbiterOutput.Output("Removing checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
this.Reset();
}
else
{
ArbiterOutput.Output("No checkpoint to remove");
}
}
else
{
ArbiterOutput.Output("Mission cannot be null to remove checkpoints");
}
}
else if (command == "")
{
}
else
{
Console.WriteLine(
"\n" +
"exit\n" +
"resetArbiter\n" +
"disableOutputMessaging\n" +
"enableOutputMessaging\n" +
"disableOutputLogging\n" +
"enableOutputLogging\n" +
"removeCheckpoint\n");
}
}
}
