public void PauseVehicle() {
this.commanded_engine_torque = 0;
this.commanded_master_cylinder_pressure = 40;
this.commanded_steering_wheel = 0;
carMode = CarMode.Pause;
acceptCommands = false;
}
public void MessageArrived(string channelName, object message)
{
if (channelName == "PositionChannel")
{
this.vehicleState = (VehicleState)message;
}
else if (channelName == "ArbiterInformationChannel")
{
this.ArbiterInformation = (ArbiterInformation)message;
}
else if (channelName == "CarMode")
{
this.CarMode = (CarMode)message;
}
else if (channelName == "ObservedVehicleChannel")
{
this.observedVehicles = (ObservedVehicles)message;
}
else if (channelName == "ObservedObstacleChannel")
{
this.observedObstacles = (ObservedObstacles)message;
}
else if (channelName == "FakeVehicleChannel")
{
this.fakeVehicles = (ObservedVehicles)message;
}
else
{
throw new ArgumentException("Unknown Channel", channelName);
}
}
/// <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);
}
public void SetRunMode(CarMode mode)
{
this.carMode = mode;
if (carMode == CarMode.Run)
{
acceptCommands = true;
}
else
{
acceptCommands = false;
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="currentArbiterState"></param>
/// <param name="internalCarMode"></param>
/// <param name="fullRoute"></param>
/// <param name="routeTime"></param>
/// <param name="actionPoint"></param>
/// <param name="currentGoal"></param>
/// <param name="goals"></param>
/// <param name="behavior"></param>
public ArbiterInformation(string currentArbiterState, CarMode internalCarMode, FullRoute fullRoute, double routeTime,
RndfWaypointID actionPoint, RndfWaypointID currentGoal, Queue <RndfWaypointID> goals, Behavior behavior, VehicleState planningState)
{
this.CurrentArbiterState = currentArbiterState;
this.InternalCarMode = internalCarMode;
this.FullRoute = fullRoute;
this.RouteTime = routeTime;
this.ActionPoint = actionPoint;
this.CurrentGoal = currentGoal;
this.Goals = goals;
this.Behavior = behavior;
this.PlanningState = planningState;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="currentArbiterState"></param>
/// <param name="internalCarMode"></param>
/// <param name="fullRoute"></param>
/// <param name="routeTime"></param>
/// <param name="actionPoint"></param>
/// <param name="currentGoal"></param>
/// <param name="goals"></param>
/// <param name="behavior"></param>
public ArbiterInformation(string currentArbiterState, CarMode internalCarMode, FullRoute fullRoute, double routeTime,
RndfWaypointID actionPoint, RndfWaypointID currentGoal, Queue<RndfWaypointID> goals, Behavior behavior, VehicleState planningState)
{
this.CurrentArbiterState = currentArbiterState;
this.InternalCarMode = internalCarMode;
this.FullRoute = fullRoute;
this.RouteTime = routeTime;
this.ActionPoint = actionPoint;
this.CurrentGoal = currentGoal;
this.Goals = goals;
this.Behavior = behavior;
this.PlanningState = planningState;
}
public void OnCarMode(CarTimestamp ct, CarMode mode)
{
// check if the car mode has changed
if (mode != lastCarMode)
{
Console.WriteLine("car mode changed to " + mode);
lastCarMode = mode;
// invoke the callback asynchronously so we don't block the reading thread
if (CarModeChanged != null)
{
CarModeChanged.BeginInvoke(this, new CarModeChangedEventArgs(mode, ct), OnCarModeChangedCompleted, CarModeChanged);
}
}
}
public OperationalSimVehicleState(Coordinates position, double speed, double heading, double steeringAngle, TransmissionGear transGear, double engineTorque, double brakePressure, double engineRPM, CarMode carMode, double stopLineDistance, bool stoplineFound, CarTimestamp timestamp)
{
this.position = position;
this.speed = speed;
this.heading = heading;
this.steeringAngle = steeringAngle;
this.transGear = transGear;
this.engineTorque = engineTorque;
this.brakePressure = brakePressure;
this.engineRPM = engineRPM;
this.carMode = carMode;
this.stopLineDistance = stopLineDistance;
this.stoplineFound = stoplineFound;
this.timestamp = timestamp;
}
public OperationalSimVehicleState(Coordinates position, double speed, double heading, double steeringAngle, TransmissionGear transGear, double engineTorque, double brakePressure, double engineRPM, CarMode carMode, double stopLineDistance, bool stoplineFound, CarTimestamp timestamp)
{
this.position = position;
this.speed = speed;
this.heading = heading;
this.steeringAngle = steeringAngle;
this.transGear = transGear;
this.engineTorque = engineTorque;
this.brakePressure = brakePressure;
this.engineRPM = engineRPM;
this.carMode = carMode;
this.stopLineDistance = stopLineDistance;
this.stoplineFound = stoplineFound;
this.timestamp = timestamp;
}
/// <summary>
/// Sets the mode of the car
/// </summary>
/// <param name="mode"></param>
public override void SetCarMode(CarMode mode)
{
if (this.ClientArbiter != null)
{
try
{
this.DynamicsVehicle.SetRunMode(mode);
Console.WriteLine("Set dynamics vehicle run mode to: " + mode.ToString());
}
catch (Exception e)
{
Console.WriteLine("Error setting dynamics vehicle run mode to: " + mode.ToString() + "\n" + e.ToString());
}
}
}
public void OnActuationStatus(
CarTimestamp ct,
int version,
CarMode carMode,
bool steeringActive,
bool steeringBypass,
bool transActive,
bool transBypass,
bool throttleActive,
bool throttleBypass,
bool brakeActive,
bool brakeBypass)
{
Services.Dataset.ItemAs <bool>("trans bypassed").Add(transBypass, ct);
}
public void OnCarModeChanged(CarMode newMode)
{
if (!testMode)
{
try {
if (newMode == CarMode.Human)
{
// clear out the current behavior
Execute(new HoldBrake(), null, false);
}
}
catch (Exception ex) {
TraceSource.TraceEvent(TraceEventType.Error, 1, "exception when handling new car mode: {0}", ex);
}
}
}
void OnGUI()
{
if (menuId == 0)
{
GUI.Box(new Rect(Screen.width / 2 - 100, Screen.height / 2 - 100, 200, 220), "Menu");
if (GUI.Button(new Rect(Screen.width / 2 - 90, Screen.height / 2 - 40, 180, 30), "Play"))
{
mode = CarMode.Play;
menuId = 1;
//Application.LoadLevel(1);
}
else if (GUI.Button(new Rect(Screen.width / 2 - 90, Screen.height / 2 - 80, 180, 30), "Record"))
{
mode = CarMode.Record;
Application.LoadLevel(1);
}
else if (GUI.Button(new Rect(Screen.width / 2 - 90, Screen.height / 2, 180, 30), "Save"))
{
mode = CarMode.Save;
//Application.LoadLevel(1);
menuId = 1;
}
else if (GUI.Button(new Rect(Screen.width / 2 - 90, Screen.height / 2 + 80, 180, 30), "Exit"))
{
Application.Quit();
}
}
else
{
GUI.Box(new Rect(Screen.width / 2 - 100, Screen.height / 2 - 100, 200, 220), "Sensors");
if (GUI.Button(new Rect(Screen.width / 2 - 90, Screen.height / 2 - 80, 180, 30), "Good"))
{
sensorsMode = SensorsMode.GoodSensors;
Application.LoadLevel(1);
}
else if(GUI.Button(new Rect(Screen.width / 2 - 90, Screen.height / 2 - 40, 180, 30), "Bad"))
{
sensorsMode = SensorsMode.BadSensors;
Application.LoadLevel(1);
}
else if (GUI.Button(new Rect(Screen.width / 2 - 90, Screen.height / 2 + 80, 180, 30), "Back"))
{
menuId = 0;
}
}
}
public FullSimTransport()
{
// initialize the car mode to unknown so that we raised the changed event immediately
lastCarMode = CarMode.Unknown;
// get the DynamicsSimFacade
dynamicsSim = (DynamicsSimFacade)CommBuilder.GetObject(DynamicsSimFacade.ServiceName);
// set the sim state channel
simStateChannel = CommBuilder.GetChannel(OperationalSimVehicleState.ChannelName);
obstacleChannel = CommBuilder.GetChannel(SceneEstimatorObstacleChannelNames.UntrackedClusterChannelName);
vehicleChannel = CommBuilder.GetChannel(SceneEstimatorObstacleChannelNames.TrackedClusterChannelName);
// add ourself as a listern
simStateChannel.Subscribe(this);
obstacleChannel.Subscribe(this);
vehicleChannel.Subscribe(this);
}
public FullSimTransport()
{
// initialize the car mode to unknown so that we raised the changed event immediately
lastCarMode = CarMode.Unknown;
// get the DynamicsSimFacade
dynamicsSim = (DynamicsSimFacade)CommBuilder.GetObject(DynamicsSimFacade.ServiceName);
// set the sim state channel
simStateChannel = CommBuilder.GetChannel(OperationalSimVehicleState.ChannelName);
obstacleChannel = CommBuilder.GetChannel(SceneEstimatorObstacleChannelNames.UntrackedClusterChannelName);
vehicleChannel = CommBuilder.GetChannel(SceneEstimatorObstacleChannelNames.TrackedClusterChannelName);
// add ourself as a listern
simStateChannel.Subscribe(this);
obstacleChannel.Subscribe(this);
vehicleChannel.Subscribe(this);
}
void transport_CarModeChanged(object sender, CarModeChangedEventArgs e)
{
if (this.carMode != e.Mode && e.Mode != CarMode.Run)
{
// clear out the forced car mode on transition
forcedCarMode = CarMode.Unknown;
}
this.carMode = e.Mode;
//Console.WriteLine("car mode changed to {0}", e.Mode);
if (carModeChannel != null)
{
carModeChannel.PublishUnreliably(carMode);
}
Services.BehaviorManager.OnCarModeChanged(carMode);
}
public void Command()
{
while (true)
{
Console.Write("Mimic > ");
string s = Console.ReadLine();
switch (s)
{
case "exit":
return;
case "run":
carMode = CarMode.Run;
break;
case "pause":
carMode = CarMode.Pause;
break;
case "human":
carMode = CarMode.Human;
break;
case "connect":
this.TryConnect();
break;
case "signal":
this.displaySignals = this.displaySignals ? false : true;
break;
case "man":
Console.WriteLine("exit");
Console.WriteLine("run");
Console.WriteLine("pause");
Console.WriteLine("human");
Console.WriteLine("connect");
Console.WriteLine("signal");
break;
}
}
}
/// <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);
}
}
public void Command()
{
while (true)
{
Console.Write("Mimic > ");
string s = Console.ReadLine();
switch (s)
{
case "exit":
return;
case "run":
carMode = CarMode.Run;
break;
case "pause":
carMode = CarMode.Pause;
break;
case "human":
carMode = CarMode.Human;
break;
case "connect":
this.TryConnect();
break;
case "signal":
this.displaySignals = this.displaySignals ? false : true;
break;
case "man":
Console.WriteLine("exit");
Console.WriteLine("run");
Console.WriteLine("pause");
Console.WriteLine("human");
Console.WriteLine("connect");
Console.WriteLine("signal");
break;
}
}
}
public void OnActuationStatus(
CarTimestamp ct,
int version,
CarMode carMode,
bool steeringActive,
bool steeringBypass,
bool transActive,
bool transBypass,
bool throttleActive,
bool throttleBypass,
bool brakeActive,
bool brakeBypass)
{
Services.Dataset.ItemAs<bool>("trans bypassed").Add(transBypass, ct);
}
/// <summary>
/// Keeps watch over communications components and determines if any need to be restarted
/// </summary>
public void Watchdog()
{
// always loop
while (true)
{
try
{
if (Arbiter.Core.ArbiterSettings.Default.IgnoreVehicles)
{
ArbiterOutput.Output("Ignoring Tracked Clusters");
}
#region Update Completion reports with 3 Sec Timeout
lock (this.recentReports)
{
List <KeyValuePair <CompletionReport, DateTime> > crs = new List <KeyValuePair <CompletionReport, DateTime> >();
foreach (KeyValuePair <CompletionReport, DateTime> cr in this.recentReports)
{
TimeSpan diff = DateTime.Now.Subtract(cr.Value);
ArbiterOutput.WriteToLog("Comm line 263 test diff.ToString: " + diff.ToString());
if (diff.Seconds < 2 && cr.Value.Date.Equals(DateTime.Now.Date))
{
crs.Add(cr);
}
else
{
ArbiterOutput.WriteToLog("Removed Completion Report: " + cr.Key.BehaviorType.ToString() + ", " + cr.Key.Result.ToString());
}
}
this.recentReports = crs;
}
#endregion
#region Check for communications readiness
if (!this.CommunicationsReady)
{
try
{
// configure
this.Configure();
}
catch (Exception e)
{
// notify
ArbiterOutput.Output("Error in communications watchdog, configuration");
ArbiterOutput.WriteToLog(e.ToString());
}
}
if (!this.CommunicationsReady)
{
try
{
// make sure nothing else registered
this.Shutdown();
// register services
this.Register();
}
catch (Exception e)
{
// notify
ArbiterOutput.Output("Error in communications watchdog, registration");
ArbiterOutput.WriteToLog(e.ToString());
}
}
#endregion
#region Get Car Mode (Acts as Operational Ping and know then Comms Ready)
try
{
if (this.operationalFacade != null)
{
// update the car mode
this.carMode = this.operationalFacade.GetCarMode();
// set comms ready as true given success
if (!this.CommunicationsReady)
{
this.CommunicationsReady = true;
}
}
else
{
this.CommunicationsReady = false;
}
}
catch (Exception e)
{
// notify
ArbiterOutput.Output("Error retreiving car mode from operational in watchdog, attempting to reconnect");
// log
ArbiterOutput.WriteToLog(e.ToString());
// set comms ready as false
this.CommunicationsReady = false;
}
#endregion
}
catch (Exception e)
{
ArbiterOutput.Output("Error in communications watchdog", e);
}
// wait for cycle time
Thread.Sleep(1000);
}
}
void IChannelListener.MessageArrived(string channelName, object message)
{
if (message is OperationalSimVehicleState)
{
OperationalTrace.ThreadTraceSource = TraceSource;
Trace.CorrelationManager.StartLogicalOperation("simstate callback");
try {
OperationalSimVehicleState state = (OperationalSimVehicleState)message;
DatasetSource ds = Services.Dataset;
OperationalTrace.WriteVerbose("received operational sim state, t = {0}", state.Timestamp);
Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow);
CarTimestamp now = state.Timestamp;
ds.ItemAs <Coordinates>("xy").Add(state.Position, now);
ds.ItemAs <double>("speed").Add(state.Speed, now);
ds.ItemAs <double>("heading").Add(state.Heading, now);
ds.ItemAs <double>("actual steering").Add(state.SteeringAngle, now);
ds.ItemAs <TransmissionGear>("transmission gear").Add(state.TransmissionGear, now);
ds.ItemAs <double>("engine torque").Add(state.EngineTorque, now);
ds.ItemAs <double>("brake pressure").Add(state.BrakePressure, now);
ds.ItemAs <double>("rpm").Add(state.EngineRPM, now);
if (state.TransmissionGear != lastRecvTransGear)
{
ds.ItemAs <DateTime>("trans gear change time").Add(HighResDateTime.Now, now);
lastRecvTransGear = state.TransmissionGear;
}
lastCarMode = state.CarMode;
if (CarModeChanged != null)
{
CarModeChanged(this, new CarModeChangedEventArgs(lastCarMode, now));
}
// build the current relative pose matrix
Matrix4 relativePose = Matrix4.Translation(state.Position.X, state.Position.Y, 0) * Matrix4.YPR(state.Heading, 0, 0);
relativePose = relativePose.Inverse();
// push on to the relative pose stack
Services.RelativePose.PushTransform(now, relativePose);
// push the current absolute pose entry
AbsolutePose absPose = new AbsolutePose(state.Position, state.Heading, now);
Services.AbsolutePose.PushAbsolutePose(absPose);
}
finally {
Trace.CorrelationManager.StopLogicalOperation();
OperationalTrace.ThreadTraceSource = null;
}
}
else if (message is SceneEstimatorUntrackedClusterCollection)
{
// push to obstacle pipeline
Services.ObstaclePipeline.OnUntrackedClustersReceived(((SceneEstimatorUntrackedClusterCollection)message));
}
else if (message is SceneEstimatorTrackedClusterCollection)
{
Services.ObstaclePipeline.OnTrackedClustersReceived((SceneEstimatorTrackedClusterCollection)message);
}
}
//
// Set and get methods
//
public void StopCar()
{
carMode = CarMode.Stop;
}
public void OnCarModeChanged(CarMode newMode)
{
if (!testMode) {
try {
if (newMode == CarMode.Human) {
// clear out the current behavior
Execute(new HoldBrake(), null, false);
}
}
catch (Exception ex) {
TraceSource.TraceEvent(TraceEventType.Error, 1, "exception when handling new car mode: {0}", ex);
}
}
}
public void MoveCarForward()
{
carMode = CarMode.Forward;
}
void IChannelListener.MessageArrived(string channelName, object message)
{
if (message is OperationalSimVehicleState) {
OperationalTrace.ThreadTraceSource = TraceSource;
Trace.CorrelationManager.StartLogicalOperation("simstate callback");
try {
OperationalSimVehicleState state = (OperationalSimVehicleState)message;
DatasetSource ds = Services.Dataset;
OperationalTrace.WriteVerbose("received operational sim state, t = {0}", state.Timestamp);
Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow);
CarTimestamp now = state.Timestamp;
ds.ItemAs<Coordinates>("xy").Add(state.Position, now);
ds.ItemAs<double>("speed").Add(state.Speed, now);
ds.ItemAs<double>("heading").Add(state.Heading, now);
ds.ItemAs<double>("actual steering").Add(state.SteeringAngle, now);
ds.ItemAs<TransmissionGear>("transmission gear").Add(state.TransmissionGear, now);
ds.ItemAs<double>("engine torque").Add(state.EngineTorque, now);
ds.ItemAs<double>("brake pressure").Add(state.BrakePressure, now);
ds.ItemAs<double>("rpm").Add(state.EngineRPM, now);
if (state.TransmissionGear != lastRecvTransGear) {
ds.ItemAs<DateTime>("trans gear change time").Add(HighResDateTime.Now, now);
lastRecvTransGear = state.TransmissionGear;
}
lastCarMode = state.CarMode;
if (CarModeChanged != null) {
CarModeChanged(this, new CarModeChangedEventArgs(lastCarMode, now));
}
// build the current relative pose matrix
Matrix4 relativePose = Matrix4.Translation(state.Position.X, state.Position.Y, 0)*Matrix4.YPR(state.Heading, 0, 0);
relativePose = relativePose.Inverse();
// push on to the relative pose stack
Services.RelativePose.PushTransform(now, relativePose);
// push the current absolute pose entry
AbsolutePose absPose = new AbsolutePose(state.Position, state.Heading, now);
Services.AbsolutePose.PushAbsolutePose(absPose);
}
finally {
Trace.CorrelationManager.StopLogicalOperation();
OperationalTrace.ThreadTraceSource = null;
}
}
else if (message is SceneEstimatorUntrackedClusterCollection) {
// push to obstacle pipeline
Services.ObstaclePipeline.OnUntrackedClustersReceived(((SceneEstimatorUntrackedClusterCollection)message));
}
else if (message is SceneEstimatorTrackedClusterCollection) {
Services.ObstaclePipeline.OnTrackedClustersReceived((SceneEstimatorTrackedClusterCollection)message);
}
}
public static IState FilterStates(CarMode carMode, BehavioralDirector behavioral)
{
// check run mode
if(carMode == CarMode.Run)
{
// check if goals left
if (CoreCommon.Mission.MissionCheckpoints.Count > 0)
{
// good to go
return CoreCommon.CorePlanningState;
}
// otherwise need to stop
else
{
// no goals left
return new NoGoalsLeftState();
}
}
else if(carMode == CarMode.EStop)
{
// move to the estopped state
return new eStoppedState();
}
else if(carMode == CarMode.Human)
{
// clear out standard (non-intersection) queuing values
TacticalDirector.RefreshQueuingMonitors();
// clear other timings
behavioral.ResetTiming();
// clear intersection timings
if (IntersectionTactical.IntersectionMonitor != null)
IntersectionTactical.IntersectionMonitor.ResetTimers();
// human mode
return new HumanState();
}
else if(carMode == CarMode.Pause || carMode == CarMode.TransitioningFromPause || carMode == CarMode.TransitioningToPause)
{
// check if the state is a paused state
if (CoreCommon.CorePlanningState is PausedState)
{
// stay paused
return CoreCommon.CorePlanningState;
}
// otherwise we need to go into pause
else
{
// clear other timings
behavioral.ResetTiming();
// clear out standard (non-intersection) queuing values
TacticalDirector.RefreshQueuingMonitors();
behavioral.tactical.ResetTimers();
if (IntersectionTactical.IntersectionMonitor != null)
IntersectionTactical.IntersectionMonitor.ResetTimers();
// make a new paused state with the previous state intact
return new PausedState(CoreCommon.CorePlanningState);
}
}
else
{
// notify of unknown state
throw new Exception("Unknown car mode type");
}
}
/// <summary>
/// Startup the vehicle from a certain position, pass the next state back,
/// and initialize the lane agent if possible
/// </summary>
/// <param name="vehicleState"></param>
/// <returns></returns>
public IState Startup(VehicleState vehicleState, CarMode carMode)
{
// check car mode
if (carMode == CarMode.Run)
{
// check areas
ArbiterLane al = CoreCommon.RoadNetwork.ClosestLane(vehicleState.Front);
ArbiterZone az = CoreCommon.RoadNetwork.InZone(vehicleState.Front);
ArbiterIntersection ain = CoreCommon.RoadNetwork.InIntersection(vehicleState.Front);
ArbiterInterconnect ai = CoreCommon.RoadNetwork.ClosestInterconnect(vehicleState.Front, vehicleState.Heading);
if (az != null)
{
// special zone startup
return new ZoneStartupState(az);
}
if (ain != null)
{
if (al != null)
{
// check lane stuff
PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);
// get distance from front of car
double dist = lanePoint.pt.DistanceTo(vehicleState.Front);
// check dist to lane
if (dist < al.Width + 1.0)
{
// check orientation relative to lane
Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// forwards or backwards
bool forwards = true;
// check dist to each other
if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
{
// not going forwards
forwards = false;
}
// stay in lane if forwards
if (forwards)
{
ArbiterOutput.Output("Starting up in lane: " + al.ToString());
return new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState);
}
else
{
// Opposing lane
return new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState);
}
}
}
// startup intersection state
return new IntersectionStartupState(ain);
}
if (al != null)
{
// get a startup chute
ArbiterLanePartition startupChute = this.GetStartupChute(vehicleState);
// check if in a startup chute
if (startupChute != null && !startupChute.IsInside(vehicleState.Front))
{
ArbiterOutput.Output("Starting up in chute: " + startupChute.ToString());
return new StartupOffChuteState(startupChute);
}
// not in a startup chute
else
{
PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);
// get distance from front of car
double dist = lanePoint.pt.DistanceTo(vehicleState.Front);
// check orientation relative to lane
Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// forwards or backwards
bool forwards = true;
// check dist to each other
if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
{
// not going forwards
forwards = false;
}
// stay in lane if forwards
if (forwards)
{
ArbiterOutput.Output("Starting up in lane: " + al.ToString());
return new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState);
}
else
{
// opposing
return new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState);
}
}
}
// fell out
ArbiterOutput.Output("Cannot find area to startup in");
return CoreCommon.CorePlanningState;
}
else
{
return CoreCommon.CorePlanningState;
}
}
/// <summary> /// Sets the mode of the car /// </summary> /// <param name="mode"></param> public abstract void SetCarMode(CarMode mode);
public void MoveCarReverse()
{
carMode = CarMode.Reverse;
}
public void RunVehicle() {
carMode = CarMode.Run;
acceptCommands = true;
}
public void OnCarMode(CarTimestamp ct, CarMode mode)
{
// check if the car mode has changed
if (mode != lastCarMode) {
Console.WriteLine("car mode changed to " + mode);
lastCarMode = mode;
// invoke the callback asynchronously so we don't block the reading thread
if (CarModeChanged != null) {
CarModeChanged.BeginInvoke(this, new CarModeChangedEventArgs(mode, ct), OnCarModeChangedCompleted, CarModeChanged);
}
}
}
/// <summary>
/// Startup the vehicle from a certain position, pass the next state back,
/// and initialize the lane agent if possible
/// </summary>
/// <param name="vehicleState"></param>
/// <returns></returns>
public IState Startup(VehicleState vehicleState, CarMode carMode)
{
// check car mode
if (carMode == CarMode.Run)
{
// check areas
ArbiterLane al = CoreCommon.RoadNetwork.ClosestLane(vehicleState.Front);
ArbiterZone az = CoreCommon.RoadNetwork.InZone(vehicleState.Front);
ArbiterIntersection ain = CoreCommon.RoadNetwork.InIntersection(vehicleState.Front);
ArbiterInterconnect ai = CoreCommon.RoadNetwork.ClosestInterconnect(vehicleState.Front, vehicleState.Heading);
if (az != null)
{
// special zone startup
return(new ZoneStartupState(az));
}
if (ain != null)
{
if (al != null)
{
// check lane stuff
PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);
// get distance from front of car
double dist = lanePoint.pt.DistanceTo(vehicleState.Front);
// check dist to lane
if (dist < al.Width + 1.0)
{
// check orientation relative to lane
Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// forwards or backwards
bool forwards = true;
// check dist to each other
if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
{
// not going forwards
forwards = false;
}
// stay in lane if forwards
if (forwards)
{
ArbiterOutput.Output("Starting up in lane: " + al.ToString());
return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
}
else
{
// Opposing lane
return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
}
}
}
// startup intersection state
return(new IntersectionStartupState(ain));
}
if (al != null)
{
// get a startup chute
ArbiterLanePartition startupChute = this.GetStartupChute(vehicleState);
// check if in a startup chute
if (startupChute != null && !startupChute.IsInside(vehicleState.Front))
{
ArbiterOutput.Output("Starting up in chute: " + startupChute.ToString());
return(new StartupOffChuteState(startupChute));
}
// not in a startup chute
else
{
PointOnPath lanePoint = al.PartitionPath.GetClosest(vehicleState.Front);
// get distance from front of car
double dist = lanePoint.pt.DistanceTo(vehicleState.Front);
// check orientation relative to lane
Coordinates laneVector = al.GetClosestPartition(vehicleState.Front).Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// forwards or backwards
bool forwards = true;
// check dist to each other
if (laneVector.DistanceTo(ourHeading) > Math.Sqrt(2.0))
{
// not going forwards
forwards = false;
}
// stay in lane if forwards
if (forwards)
{
ArbiterOutput.Output("Starting up in lane: " + al.ToString());
return(new StayInLaneState(al, new Probability(0.7, 0.3), true, CoreCommon.CorePlanningState));
}
else
{
// opposing
return(new OpposingLanesState(al, true, CoreCommon.CorePlanningState, vehicleState));
}
}
}
// fell out
ArbiterOutput.Output("Cannot find area to startup in");
return(CoreCommon.CorePlanningState);
}
else
{
return(CoreCommon.CorePlanningState);
}
}
public void MessageArrived(string channelName, object message)
{
if (channelName == "PositionChannel")
{
this.vehicleState = (VehicleState)message;
}
else if (channelName == "ArbiterInformationChannel")
{
this.ArbiterInformation = (ArbiterInformation)message;
}
else if (channelName == "CarMode")
{
this.CarMode = (CarMode)message;
}
else if (channelName == "ObservedVehicleChannel")
{
this.observedVehicles = (ObservedVehicles)message;
}
else if (channelName == "ObservedObstacleChannel")
{
this.observedObstacles = (ObservedObstacles)message;
}
else if (channelName == "FakeVehicleChannel")
{
this.fakeVehicles = (ObservedVehicles)message;
}
else
{
throw new ArgumentException("Unknown Channel", channelName);
}
}
/// <summary> /// Sets the mode of the car /// </summary> /// <param name="mode"></param> public abstract void SetCarMode(CarMode mode);
/// <summary>
/// Sets the mode of the car
/// </summary>
/// <param name="mode"></param>
public override void SetCarMode(CarMode mode)
{
if (this.ClientArbiter != null)
{
try
{
this.DynamicsVehicle.SetRunMode(mode);
Console.WriteLine("Set dynamics vehicle run mode to: " + mode.ToString());
}
catch (Exception e)
{
Console.WriteLine("Error setting dynamics vehicle run mode to: " + mode.ToString() + "\n" + e.ToString());
}
}
}
/// <summary>
/// Keeps watch over communications components and determines if any need to be restarted
/// </summary>
public void Watchdog()
{
// always loop
while (runCommsWatchdog)
{
try
{
#region Check for communications readiness
if (!this.CommunicationsReady)
{
try
{
// configure
this.Configure();
}
catch (Exception e)
{
// notify
RemoraOutput.WriteLine("Error in communications watchdog, configuration", OutputType.Remora);
Console.WriteLine(e.ToString());
}
}
if (!this.CommunicationsReady)
{
try
{
// make sure nothing else registered
this.Shutdown();
// register services
this.Register();
}
catch (Exception e)
{
// notify
RemoraOutput.WriteLine("Error in communications watchdog, registration", OutputType.Remora);
Console.WriteLine(e.ToString());
}
}
#endregion
#region Get Available Machines if in sim
if (global::RemoraAdvanced.Properties.Settings.Default.SimMode)
{
try
{
// update the machine names
Dictionary <string, int> machineNames = this.simulatorFacade.GetClientMachines();
this.ManchineNameUpdate(this, machineNames);
// set comms ready as true given success
if (!this.CommunicationsReady)
{
this.CommunicationsReady = true;
}
}
catch (Exception e)
{
// notify
RemoraOutput.WriteLine("Error retreiving client machines from simulation in watchdog, attempting to reconnect", OutputType.Remora);
Console.WriteLine(e.ToString());
this.ManchineNameUpdate(this, new Dictionary <string, int>());
// set comms ready as false
this.CommunicationsReady = false;
}
}
else
{
if (!this.CommunicationsReady)
{
try
{
this.ResgisterWithClient();
// notify
if (RemoraAdvanced.Properties.Settings.Default.LogMode || arbiterRemote.Ping())
{
RemoraOutput.WriteLine("Registered with default non-sim client", OutputType.Remora);
this.CommunicationsReady = true;
}
}
catch (Exception ex)
{
RemoraOutput.WriteLine("Error connecting to default non-sim client in watchdog, attempting to reconnect", OutputType.Remora);
Console.WriteLine(ex.ToString());
this.CommunicationsReady = false;
}
}
else
{
// try and ping the ai
try
{
if (!RemoraAdvanced.Properties.Settings.Default.LogMode)
{
this.arbiterRemote.Ping();
}
}
catch (Exception e)
{
RemoraOutput.WriteLine("Error pinging default ai", OutputType.Remora);
this.CommunicationsReady = false;
Console.WriteLine(e.ToString());
}
}
}
#endregion
#region Get Car Mode (Acts as Operational Ping and know then Comms Ready)
try
{
// update the car mode
if (this.operationalFacade != null)
{
this.carMode = this.operationalFacade.GetCarMode();
}
}
catch (Exception e)
{
// notify
Console.WriteLine(e.ToString());
}
#endregion
}
catch (Exception e)
{
// notify
RemoraOutput.WriteLine("Error in comms watchdog: " + e.ToString(), OutputType.Remora);
Console.WriteLine(e.ToString());
}
// wait for cycle time
Thread.Sleep(5000);
}
}
public static IState FilterStates(CarMode carMode, BehavioralDirector behavioral)
{
// check run mode
if (carMode == CarMode.Run)
{
// check if goals left
if (CoreCommon.Mission.MissionCheckpoints.Count > 0)
{
// good to go
return(CoreCommon.CorePlanningState);
}
// otherwise need to stop
else
{
// no goals left
return(new NoGoalsLeftState());
}
}
else if (carMode == CarMode.EStop)
{
// move to the estopped state
return(new eStoppedState());
}
else if (carMode == CarMode.Human)
{
// clear out standard (non-intersection) queuing values
TacticalDirector.RefreshQueuingMonitors();
// clear other timings
behavioral.ResetTiming();
// clear intersection timings
if (IntersectionTactical.IntersectionMonitor != null)
{
IntersectionTactical.IntersectionMonitor.ResetTimers();
}
// human mode
return(new HumanState());
}
else if (carMode == CarMode.Pause || carMode == CarMode.TransitioningFromPause || carMode == CarMode.TransitioningToPause)
{
// check if the state is a paused state
if (CoreCommon.CorePlanningState is PausedState)
{
// stay paused
return(CoreCommon.CorePlanningState);
}
// otherwise we need to go into pause
else
{
// clear other timings
behavioral.ResetTiming();
// clear out standard (non-intersection) queuing values
TacticalDirector.RefreshQueuingMonitors();
behavioral.tactical.ResetTimers();
if (IntersectionTactical.IntersectionMonitor != null)
{
IntersectionTactical.IntersectionMonitor.ResetTimers();
}
// make a new paused state with the previous state intact
return(new PausedState(CoreCommon.CorePlanningState));
}
}
else
{
// notify of unknown state
throw new Exception("Unknown car mode type");
}
}
public void PauseVehicle()
{
lock (lockobj) {
this.commanded_engine_torque = 0;
this.commanded_master_cylinder_pressure = 35;
this.commanded_steering_wheel = 0;
carMode = CarMode.Pause;
acceptCommands = false;
}
}
public void ForceCarMode(CarMode forced)
{
this.forcedCarMode = forced;
}
public void RunVehicle()
{
carMode = CarMode.Run;
acceptCommands = true;
}
/// <summary>
/// Keeps watch over communications components and determines if any need to be restarted
/// </summary>
public void Watchdog()
{
// always loop
while (runCommsWatchdog)
{
try
{
#region Check for communications readiness
if (!this.CommunicationsReady)
{
try
{
// configure
this.Configure();
}
catch (Exception e)
{
// notify
RemoraOutput.WriteLine("Error in communications watchdog, configuration", OutputType.Remora);
Console.WriteLine(e.ToString());
}
}
if (!this.CommunicationsReady)
{
try
{
// make sure nothing else registered
this.Shutdown();
// register services
this.Register();
}
catch (Exception e)
{
// notify
RemoraOutput.WriteLine("Error in communications watchdog, registration", OutputType.Remora);
Console.WriteLine(e.ToString());
}
}
#endregion
#region Get Available Machines if in sim
if (global::RemoraAdvanced.Properties.Settings.Default.SimMode)
{
try
{
// update the machine names
Dictionary<string, int> machineNames = this.simulatorFacade.GetClientMachines();
this.ManchineNameUpdate(this, machineNames);
// set comms ready as true given success
if (!this.CommunicationsReady)
this.CommunicationsReady = true;
}
catch (Exception e)
{
// notify
RemoraOutput.WriteLine("Error retreiving client machines from simulation in watchdog, attempting to reconnect", OutputType.Remora);
Console.WriteLine(e.ToString());
this.ManchineNameUpdate(this, new Dictionary<string, int>());
// set comms ready as false
this.CommunicationsReady = false;
}
}
else
{
if (!this.CommunicationsReady)
{
try
{
this.ResgisterWithClient();
// notify
if (RemoraAdvanced.Properties.Settings.Default.LogMode || arbiterRemote.Ping())
{
RemoraOutput.WriteLine("Registered with default non-sim client", OutputType.Remora);
this.CommunicationsReady = true;
}
}
catch (Exception ex)
{
RemoraOutput.WriteLine("Error connecting to default non-sim client in watchdog, attempting to reconnect", OutputType.Remora);
Console.WriteLine(ex.ToString());
this.CommunicationsReady = false;
}
}
else
{
// try and ping the ai
try
{
if(!RemoraAdvanced.Properties.Settings.Default.LogMode)
this.arbiterRemote.Ping();
}
catch (Exception e)
{
RemoraOutput.WriteLine("Error pinging default ai", OutputType.Remora);
this.CommunicationsReady = false;
Console.WriteLine(e.ToString());
}
}
}
#endregion
#region Get Car Mode (Acts as Operational Ping and know then Comms Ready)
try
{
// update the car mode
if(this.operationalFacade != null)
this.carMode = this.operationalFacade.GetCarMode();
}
catch (Exception e)
{
// notify
Console.WriteLine(e.ToString());
}
#endregion
}
catch (Exception e)
{
// notify
RemoraOutput.WriteLine("Error in comms watchdog: " + e.ToString(), OutputType.Remora);
Console.WriteLine(e.ToString());
}
// wait for cycle time
Thread.Sleep(5000);
}
}
public void SetRunMode(CarMode mode)
{
this.carMode = mode;
if (carMode == CarMode.Run) {
acceptCommands = true;
}
else {
acceptCommands = false;
}
}
public CarModeChangedEventArgs(CarMode mode, CarTimestamp ct)
{
this.mode = mode;
this.timestamp = ct;
}
/// <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
}
