/// <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"); } } }