public void clone(Maneuver other) {
name = other.name;
mod_string = other.mod_string;
move_type = other.move_type;
modifiers = new Dictionary<string, float>(other.modifiers);
}
private void loadManeuvers_Click(object sender, RoutedEventArgs e)
{
if (!File.Exists(saveLoadFileName.Text))
{
MessageBox.Show("File named '" + saveLoadFileName.Text + "' does not exist!");
return;
}
var deser = File.ReadAllText(saveLoadFileName.Text);
var man = JsonConvert.DeserializeObject <ObservableCollection <Maneuver> >(deser);
var newUid = -1;
foreach (var item in man)
{
if (newUid < item.Uid)
{
newUid = item.Uid;
}
}
newUid++;
Maneuver.UpdateUid(newUid);
foreach (var item in man)
{
foreach (var mov in item.Movements)
{
mov.Host = item;
}
this.Maneuvers.Add(item);
}
tryRefresh();
}
private void duplicateBtn_Click(object sender, RoutedEventArgs e)
{
var dc = (sender as Button)?.DataContext as Maneuver;
var newManeuver = new Maneuver(dc, true); // "copy constructor"
Maneuvers.Add(newManeuver);
}
private List <IBasicModule> AddManeuverModules()
{
List <IBasicModule> modules = new List <IBasicModule>();
maneuver = new Maneuver("Maneuver Node");
burnTime = new BurnTime("Burn Time");
maneuverCloseApproach = new ManClosestApproach("Closest Approach");
maneuverCloseRelVel = new ManClosestRelVel("Rel Vel At Appr");
maneuver.IsVisible = settings.showManeuverNode;
maneuver.AlwaysShow = settings.showManeuverNodeAlways;
burnTime.IsVisible = settings.showManeuverBurn;
burnTime.AlwaysShow = settings.showManeuverBurnAlways;
maneuverCloseApproach.IsVisible = settings.showManeuverClosestApproach;
maneuverCloseApproach.AlwaysShow = settings.showManeuverClosestApproachAlways;
maneuverCloseRelVel.IsVisible = settings.showManeuverClosestVel;
maneuverCloseRelVel.AlwaysShow = settings.showManeuverClosestVelAlways;
modules.Add(maneuverCloseRelVel);
modules.Add(maneuverCloseApproach);
modules.Add(burnTime);
modules.Add(maneuver);
return(modules);
}
/// <summary>
/// Initializes a new instance of the <see cref="MainViewModel"/> class.
/// </summary>
public MainViewModel()
{
if (IsDesignMode)
{
// Create a design-time maneuver with fake instructions
_maneuver = new Maneuver
{
Text = "HEAD NORTH ON MAIN ST",
Type = DirectionManeuverType.Straight,
RemainingTime = TimeSpan.FromMinutes(15)
};
}
else
{
// We begin the simulation outside of the Living Coast Discovery Center
Simulation = new SimulatedLocationDataSource();
Simulation.SetLocationsWithPolyline(new Polyline(new[]
{
new MapPoint(-117.1109159, 32.6400714, 0, SpatialReferences.Wgs84)
}));
// And here's an example destination
Address = "LEGOLAND California, One Legoland Dr, Carlsbad, CA 92008";
// Create the maneuver
_maneuver = new Maneuver
{
Text = "ENTER A DESTINATION TO START",
};
}
}
//Will be used for setting some pointers and things
public void Init(GameObject c, Maneuver parent)
{
creator = c;
parent_maneuver = parent;
transform.parent.Find("Test Maneuvers").GetComponent <Button>().interactable = false;
}
public static void handleManeuverSelection(Maneuver selectedManeuver)
{
// Hide (all) previous highlight
foreach (Transform maneuverHolder in GameObject.Find("ShipManeuvers").GetComponentsInChildren <Transform>())
{
if (maneuverHolder.Find("ManeuverOverlay") != null && maneuverHolder.Find("ManeuverOverlay").gameObject.activeSelf)
{
maneuverHolder.Find("ManeuverOverlay").gameObject.SetActive(false);
}
}
// Highlight the selected maneuver
foreach (Transform maneuverHolder in GameObject.Find("ShipManeuvers").GetComponentsInChildren <Transform>())
{
if (maneuverHolder.Find("Image") != null && maneuverHolder.Find("Image").gameObject.GetComponent <ManeuverSelectionEvent>() as ManeuverSelectionEvent != null)
{
Maneuver maneuver = maneuverHolder.Find("Image").gameObject.GetComponent <ManeuverSelectionEvent>().maneuver;
if (maneuver.Speed.Equals(selectedManeuver.Speed) && maneuver.Bearing.Equals(selectedManeuver.Bearing))
{
maneuverHolder.Find("ManeuverOverlay").gameObject.SetActive(true);
break;
}
}
}
}
/// <summary>
/// Initializes a new instance of the <see cref="MainViewModel"/> class.
/// </summary>
public MainViewModel()
{
if (IsDesignMode)
{
// Create a design-time maneuver with fake instructions
_maneuver = new Maneuver
{
Text = "HEAD NORTH ON MAIN ST",
Type = DirectionManeuverType.Straight,
RemainingTime = TimeSpan.FromMinutes(15)
};
}
else
{
// We begin the simulation outside of the Living Coast Discovery Center
//_simulator = new SimulatedLocationDataSource(new MapPoint(-117.1109159, 32.6400714, 0, SpatialReferences.Wgs84));
Simulation = new Simulation(new MapPoint(-117.1109159, 32.6400714, 0, SpatialReferences.Wgs84));
// Create the maneuver
_maneuver = new Maneuver
{
Text = "SELECT A DESTINATION TO START"
};
}
}
private void afterManeuver()
{
MatchHandlerUtil.applyManeuverBonus(target, maneuverToComplete.Difficulty);
// TODO Make player chosen an action (if available)
if (target.transform.GetComponent <ShipProperties>().getLoadedShip().getTokenIdByType(typeof(StressToken)) == 0)
{
target.transform.GetComponent <ShipProperties>().getLoadedShip().setNumOfActions(1);
//Action choser.....
//target.transform.GetComponent<ShipProperties>().getLoadedShip().setBeforeAction(true);
GameObject actionChoserPopup = null;
int actionIndex = 0;
foreach (GameObject go in Resources.FindObjectsOfTypeAll(typeof(GameObject)))
{
if (go.name.Equals("ActionChoserPopup"))
{
actionChoserPopup = go;
}
}
if (actionChoserPopup != null)
{
// TODO outsource this fragment to UI handler!!!!!!!
foreach (String action in MatchDatas.getPlayers()[MatchDatas.getActivePlayerIndex()].getActiveShip().getShip().Actions.Action)
{
Image image = null;
Sprite sprite = Resources.Load <Sprite>(SquadBuilderConstants.IMAGE_FOLDER_NAME + "/" + action.Replace(" ", "-"));
Transform actionIconPrefab = Resources.Load <Transform>(SquadBuilderConstants.PREFABS_FOLDER_NAME + "/" + SquadBuilderConstants.ACTION_ICON);
RectTransform rt = (RectTransform)actionIconPrefab;
float actionIconWidth = rt.rect.width;
Transform actionIcon = (Transform)GameObject.Instantiate(
actionIconPrefab,
new Vector3((actionIndex * actionIconWidth) + SquadBuilderConstants.UPGRADE_IMAGE_X_OFFSET + 15, SquadBuilderConstants.UPGRADE_IMAGE_Y_OFFSET - 30, SquadBuilderConstants.UPGRADE_IMAGE_Z_OFFSET),
Quaternion.identity
);
Image actionIconImage = actionIcon.gameObject.GetComponent <Image>();
actionIcon.gameObject.AddComponent <ActionSelectorEvents>();
actionIcon.gameObject.GetComponent <ActionSelectorEvents>().setActionName(action);
actionIcon.transform.SetParent(actionChoserPopup.transform, false);
actionIconImage.sprite = sprite;
actionIconImage.color = new Color(actionIconImage.color.r, actionIconImage.color.g, actionIconImage.color.b, 1.0f);
actionIndex++;
}
actionChoserPopup.SetActive(true);
PhaseHandlerService.initActionPhase();
}
}
maneuverToComplete = null;
target = null;
}
public void clone(Maneuver other)
{
name = other.name;
mod_string = other.mod_string;
move_type = other.move_type;
modifiers = new Dictionary <string, float>(other.modifiers);
}
private void beforeManeuver(GameObject objectToMove, Maneuver maneuver)
{
if (target == null || maneuver == null)
{
target = objectToMove;
maneuverToComplete = maneuver;
}
}
//public void MoveAircraft()
//{
// // move according to heading and altitude
// int tilesToMove = ManeuverManager.SpeedToMovement(speed);
//
// HexPosition newPosition;
// switch (heading)
// {
// case (Heading.N):
// {
// newPosition = hexPos.goN(tilesToMove);
// break;
// }
// case (Heading.NE):
// {
// newPosition = hexPos.goNE(tilesToMove);
// break;
// }
// case (Heading.SE):
// {
// newPosition = hexPos.goSE(tilesToMove);
// break;
// }
// case (Heading.S):
// {
// newPosition = hexPos.goS(tilesToMove);
// break;
// }
// case (Heading.SW):
// {
// newPosition = hexPos.goSW(tilesToMove);
// break;
// }
// case (Heading.NW):
// {
// newPosition = hexPos.goNW(tilesToMove);
// break;
// }
// default:
// {
// newPosition = new HexPosition();
// break;
// }
// }
// UpdatePositionTo(newPosition);
//}
//public void Left60()
//{
// heading = (heading + 5) % 6;
//}
//public void Left120()
//{
// heading = (heading + 4) % 6;
//}
//public void Right60()
//{
// heading = (heading + 1) % 6;
//}
//public void Right120()
//{
// heading = (heading + 1) % 6;
//}
//public void Climb()
//{
// altitude += 1;
// speed -= 6;
//}
//public void Dive()
//{
// altitude -= 1;
// speed += 6;
//}
//public void Idle()
//{
// // nothing
//}
public void PerformManeuver(Maneuver m)
{
speed += m.deltaSpeed;
altitude += m.deltaAltitude;
heading = (heading + m.headingChange + 6) % 6;
speed += acceleration * m.accelerationMultiplier;
TruncateStats();
}
/// <summary>
/// Add the manuever specified by the intercept to add a course correction
/// at the intercept time to have the spaceship match the target course.
///
/// Maneuvers are passed on to the GravityEngine to be executed on the correct
/// integrator timeslice. The SpaceshipRV maintains a copy so they can be displayed/tracked.
/// </summary>
/// <param name="intercept">Intercept.</param>
public void SetManeuver(TrajectoryData.Intercept intercept)
{
Maneuver m = new Maneuver(nbody, intercept);
m.onExecuted = ManeuverExecutedCallback;
GravityEngine.Instance().AddManeuver(m);
maneuverList.Add(m);
}
public void Remove(Maneuver m)
{
bool removed = maneuvers.Remove(m);
if (!removed)
{
Debug.Log("Could not remove maneuver " + m.LogString());
}
}
/// <summary>
/// Callback to remove markers when the maneuver is executed
/// </summary>
/// <param name="m"></param>
private void RemoveMarker(Maneuver m)
{
if (markers.Count > 0)
{
GameObject marker = markers[0];
Destroy(marker);
markers.RemoveAt(0);
}
}
public void showManeuverSelector(GameObject target)
{
resetManeuverSelector();
// TODO Check why this throws NullReferenceException!!
/*foreach (Transform child in PilotCardPanel.transform.Find("ShipDataManeuvers/ShipManeuvers"))
* {
* if (child.Find("Image").gameObject.GetComponent<ManeuverSelectionEvent>() != null)
* {
* Destroy(child.Find("Image").gameObject.GetComponent<ManeuverSelectionEvent>());
* }
* }*/
// DUPLACTE FRAGMENT!!!!! (also in squadron builder!)
foreach (Maneuver maneuver in MatchDatas.getActiveShip().GetComponent <ShipProperties>().getLoadedShip().getShip().Maneuvers.Maneuver)
{
Image image = null;
Sprite sprite = Resources.Load <Sprite>(SquadBuilderConstants.IMAGE_FOLDER_NAME + "/" + maneuver.Bearing + "_" + maneuver.Difficulty);
string maneuverHolderName = maneuver.Speed + "_" + maneuver.Bearing;
string maneuverHolderPath = "ShipDataManeuvers/ShipManeuvers/Speed" + maneuverHolderName;
// Maneuver overlay not showing!! Check why....
Maneuver selectedManeuver = MatchDatas.getActiveShip().GetComponent <ShipProperties>().getLoadedShip().getPlannedManeuver();
bool isSelected = false;
if (selectedManeuver != null && maneuver.Speed.Equals(selectedManeuver.Speed) && maneuver.Bearing.Equals(selectedManeuver.Bearing))
{
isSelected = true;
}
if (maneuverHolderName.Contains("koiogran") || maneuverHolderName.Contains("segnor") || maneuverHolderName.Contains("tallon"))
{
if (maneuverHolderName.Contains("right"))
{
maneuverHolderPath = "ShipDataManeuvers/ShipManeuvers/Speed" + maneuver.Speed + "_special_right";
}
else
{
maneuverHolderPath = "ShipDataManeuvers/ShipManeuvers/Speed" + maneuver.Speed + "_special_left";
}
}
target.transform.Find(maneuverHolderPath + "/Image").gameObject.GetComponent <Image>().sprite = sprite;
target.transform.Find(maneuverHolderPath + "/Image").gameObject.AddComponent <ManeuverSelectionEvent>();
target.transform.Find(maneuverHolderPath + "/Image").gameObject.GetComponent <ManeuverSelectionEvent>().setManeuver(maneuver);
image = target.transform.Find(maneuverHolderPath + "/Image").gameObject.GetComponent <Image>();
target.transform.Find(maneuverHolderPath + "/ManeuverOverlay").gameObject.SetActive(isSelected);
if (image != null)
{
image.color = new Color(image.color.r, image.color.g, image.color.b, 1.0f);
}
}
target.transform.Find("ShipDataManeuvers").gameObject.SetActive(true);
}
/// <summary>
/// Plans the forward maneuver and secondary maneuver
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="p"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver ForwardManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, RoadPlan roadPlan,
List <ITacticalBlockage> blockages)
{
// get primary maneuver
Maneuver primary = this.OpposingForwardMonitor.PrimaryManeuver(arbiterLane, closestGood, vehicleState, blockages);
// return primary for now
return(primary);
}
public void Executed(Maneuver m, bool isCopy)
{
// maneuvers in a copy of the main state are "what if" projections so do not notify about
// completed maneuver
if (!isCopy && m.onExecuted != null)
{
m.onExecuted(m);
}
Remove(m);
}
public ManeuverMgr(ManeuverMgr copyFrom)
{
Maneuver mForCompare = new Maneuver();
maneuvers = new SortedList <Maneuver, Maneuver>(mForCompare);
foreach (Maneuver m in copyFrom.maneuvers.Keys)
{
maneuvers.Add(m, m);
}
}
/// <summary>
/// Convert abstract command list that just references them by this commander's list of planes into a command list with specific Aircraft
/// </summary>
/// <param name="commander"></param>
/// <param name="commands"></param>
/// <returns></returns>
CommandList CommandListAbstractToConcrete(CommandListAbstract commands, Commander commander)
{
CommandList concreteCommands = new CommandList();
for (int i = 0; i < commands.Count; i++)
{
Maneuver maneuver = Maneuver.BasicManeuvers[(short)commands[i]];
concreteCommands.Add(commander.unitsCommanded[i], maneuver);
}
return(concreteCommands);
}
private List <IBasicModule> AddManeuverModules()
{
List <IBasicModule> modules = new List <IBasicModule>();
maneuver = new Maneuver("Maneuver Node");
maneuverTotal = new ManeuverTotal("All Man. Nodes");
burnTime = new BurnTime("Burn Time");
maneuverApoapsis = new ManApoapsis("Apoapsis");
maneuverPeriapsis = new ManPeriapsis("Periapsis");
maneuverInclination = new ManInclination("Inclination");
maneuverEccentricity = new ManEccentricity("Eccentricity");
maneuverCloseApproach = new ManClosestApproach("Closest Approach");
maneuverCloseRelVel = new ManClosestRelVel("Rel Vel At Appr");
maneuverAngleToPro = new ManAngleToPro("Angle To Pro");
maneuverPhaseAngle = new ManPhaseAngle("Phase Angle");
maneuver.IsVisible = BasicSettings.Instance.showManeuverNode;
maneuver.AlwaysShow = BasicSettings.Instance.showManeuverNodeAlways;
maneuverTotal.IsVisible = BasicSettings.Instance.showManeuverNodeTotal;
maneuverTotal.AlwaysShow = BasicSettings.Instance.showManeuverNodeTotalAlways;
burnTime.IsVisible = BasicSettings.Instance.showManeuverBurn;
burnTime.AlwaysShow = BasicSettings.Instance.showManeuverBurnAlways;
maneuverApoapsis.IsVisible = BasicSettings.Instance.showManeuverApoapsis;
maneuverApoapsis.AlwaysShow = BasicSettings.Instance.showManeuverApoapsisAlways;
maneuverPeriapsis.IsVisible = BasicSettings.Instance.showManeuverPeriapsis;
maneuverPeriapsis.AlwaysShow = BasicSettings.Instance.showManeuverPeriapsisAlways;
maneuverInclination.IsVisible = BasicSettings.Instance.showManeuverInclination;
maneuverInclination.AlwaysShow = BasicSettings.Instance.showManeuverInclinationAlways;
maneuverEccentricity.IsVisible = BasicSettings.Instance.showManeuverEccentricity;
maneuverEccentricity.AlwaysShow = BasicSettings.Instance.showManeuverEccentricityAlways;
maneuverCloseApproach.IsVisible = BasicSettings.Instance.showManeuverClosestApproach;
maneuverCloseApproach.AlwaysShow = BasicSettings.Instance.showManeuverClosestApproachAlways;
maneuverCloseRelVel.IsVisible = BasicSettings.Instance.showManeuverClosestVel;
maneuverCloseRelVel.AlwaysShow = BasicSettings.Instance.showManeuverClosestVelAlways;
maneuverAngleToPro.IsVisible = BasicSettings.Instance.showManeuverAngleToPrograde;
maneuverAngleToPro.AlwaysShow = BasicSettings.Instance.showManeuverAngleToProgradeAlways;
maneuverPhaseAngle.IsVisible = BasicSettings.Instance.showManeuverPhaseAngle;
maneuverPhaseAngle.AlwaysShow = BasicSettings.Instance.showManeuverPhaseAngleAlways;
modules.Add(maneuverAngleToPro);
modules.Add(maneuverPhaseAngle);
modules.Add(maneuverCloseRelVel);
modules.Add(maneuverCloseApproach);
modules.Add(maneuverEccentricity);
modules.Add(maneuverInclination);
modules.Add(maneuverPeriapsis);
modules.Add(maneuverApoapsis);
modules.Add(burnTime);
modules.Add(maneuverTotal);
modules.Add(maneuver);
return(modules);
}
void Start()
{
StateManager = new State_Manager <Wolf_Behavior>(this);
maneuver = new Maneuver(this);
d_Attack = new Direct_Attack(this);
attackpoint = this.transform.FindChild("AttackPoint").gameObject;
blnAlive = true;
nmpPath = new NavMeshPath();
nmaAgent = this.gameObject.GetComponent <NavMeshAgent> ();
intHealth = 50;
intDamage = 2000;
blnExecBehavior = false;
}
public bool CanPerformManeuver(UnitAircraft unit, ref Maneuver man)
{
UnitAircraft copy = unit.Clone();
// try the maneuver ahead of time on a copy
copy.PerformManeuver(man);
copy.StallCheck();
copy.CrashCheck();
bool canDo = board.isTraversibleAir(copy.ProjectAhead());
return(canDo); // unit would move outside range!
}
void Start()
{
StateManager = new State_Manager<Wolf_Behavior>(this);
maneuver = new Maneuver(this);
d_Attack = new Direct_Attack(this);
attackpoint = this.transform.FindChild("AttackPoint").gameObject;
blnAlive = true;
nmpPath = new NavMeshPath ();
nmaAgent = this.gameObject.GetComponent<NavMeshAgent> ();
intHealth = 50;
intDamage = 2000;
blnExecBehavior = false;
}
void Update()
{
if (maneuver != null)
{
maneuver.UpdateState();
gameObject.transform.position = maneuver.CalculateWorldPosition();
gameObject.transform.rotation = maneuver.CalculateWorldRotation();
if (maneuver is LoopThenCircle && maneuver.canInterrupt && !(interruptedManeuver is MakeCircle))
{
maneuver = interruptedManeuver;
maneuver.Resume();
}
}
}
public BiellipticXfer(OrbitData fromOrbit, OrbitData toOrbit, float xferOvershootPercent) : base(fromOrbit, toOrbit)
{
// call this beta until can better tune the deep space xfer.
name = "Bi-Elliptic (beta)";
float r_inner = fromOrbit.a;
float r_outer = toOrbit.a;
if (r_inner > r_outer)
{
return;
}
float xfer_radius = r_outer * (1f + xferOvershootPercent / 100f);
// mass scale applied in Orbit data
float a1 = (r_inner + xfer_radius) / 2f;
float a2 = (r_outer + xfer_radius) / 2f;
float mu = fromOrbit.mu;
float dV1 = Mathf.Sqrt(2f * mu / r_inner - mu / a1) - Mathf.Sqrt(mu / r_inner);
float dV2 = Mathf.Sqrt(2f * mu / xfer_radius - mu / a2) -
Mathf.Sqrt(2f * mu / xfer_radius - mu / a1);
float dV3 = Mathf.Sqrt(2f * mu / r_outer - mu / a2) - Mathf.Sqrt(mu / r_outer);
// transfer time
float t1 = Mathf.PI * Mathf.Sqrt(a1 * a1 * a1 / mu);
float t2 = Mathf.PI * Mathf.Sqrt(a2 * a2 * a2 / mu);
// Build the manuevers required
deltaV = 0f;
float worldTime = GravityEngine.Instance().GetPhysicalTime();
Maneuver[] marray = new Maneuver[3];
for (int i = 0; i < marray.Length; i++)
{
marray[i] = new Maneuver();
marray[i].nbody = fromOrbit.nbody;
marray[i].mtype = Maneuver.Mtype.scalar;
maneuvers.Add(marray[i]);
}
marray[0].worldTime = worldTime;
marray[0].dV = dV1;
marray[1].worldTime = worldTime + t1;
marray[1].dV = dV2;
marray[2].worldTime = worldTime + t1 + t2;
marray[2].dV = -dV3;
deltaV = (dV1 + dV2 + dV3);
deltaT = t1 + t2;
}
public override void ReadXml(XmlReader reader)
{
id = reader.GetAttribute("id") ?? string.Empty;
galaxy = reader.GetAttribute("galaxy") ?? string.Empty;
system = reader.GetAttribute("system") ?? string.Empty;
planet = reader.GetAttribute("planet") ?? string.Empty;
Vector3D.TryParse(reader.GetAttribute("position"), out postion);
Vector3D.TryParse(reader.GetAttribute("orientation"), out orientation);
docked = reader.GetAttribute("docked") ?? string.Empty;
dock = reader.GetAttribute("dock") ?? string.Empty;
if (!Enum.TryParse <Spawn.Maneuver>(reader.GetAttribute("maneuver") ?? string.Empty, out maneuver))
{
maneuver = Spawn.Maneuver.Cruise;
}
}
private void addMan_Click(object sender, RoutedEventArgs e)
{
var wind = new MovementConfigurator()
{
Owner = this,
};
var theManover = new Maneuver("Movement X");
wind.DataContext = theManover;
var res = wind.ShowDialog();
if (res == true)
{
Maneuvers.Add(theManover);
}
}
public CircularizeXfer(OrbitData fromOrbit) : base(fromOrbit)
{
name = "Circularize";
GravityEngine ge = GravityEngine.Instance();
// find velocity vector perpendicular to r for circular orbit
Vector3d r_ship = ge.GetPositionDoubleV3(fromOrbit.nbody);
Vector3d v_ship = ge.GetVelocityDoubleV3(fromOrbit.nbody);
Vector3d r_center = ge.GetPositionDoubleV3(fromOrbit.centralMass);
Vector3d v_center = ge.GetVelocityDoubleV3(fromOrbit.centralMass);
Vector3d r = r_ship - r_center;
// want velocity relative to central mass (it could be moving)
Vector3d v = v_ship - v_center;
// to get axis of orbit, can take r x v
Vector3d axis = Vector3d.Cross(r, v).normalized;
// vis visa for circular orbit
double mu = GravityEngine.Instance().GetMass(centerBody);
double v_mag = Mathd.Sqrt(mu / r.magnitude);
// positive v is counter-clockwise
Vector3d v_dir = Vector3d.Cross(axis, r).normalized;
Vector3d v_circular = v_mag * v_dir;
Maneuver m1;
m1 = new Maneuver();
m1.nbody = fromOrbit.nbody;
m1.mtype = Maneuver.Mtype.vector;
m1.velChange = (v_circular - v).ToVector3();
m1.dV = Vector3.Magnitude(m1.velChange);
// maneuver positions and info for KeplerSeq conversion and velocity directions
Vector3d h_unit = fromOrbit.GetAxis();
m1.physPosition = r_ship;
m1.relativePos = r_ship - r_center;
m1.relativeVel = v_circular.magnitude * Vector3d.Cross(h_unit, m1.relativePos).normalized;
m1.relativeTo = fromOrbit.centralMass;
//Debug.LogFormat("v_ship={0} v_center={1} v_c.x={2}", vel_ship, vel_center, v_center[0]);
//Debug.Log(string.Format("v_ship={0} v_circular={1} axis={2} v_dir={3} velChange={4}",
// vel_ship, v_circular, axis, v_dir, m1.velChange));
m1.worldTime = ge.GetPhysicalTime();
maneuvers.Add(m1);
}
private void ChooseManeuver()
{
if (ManeuverTrust <= 0)
{
double R = Random.NextDouble();
if (R <= Player.Wairness)
{
Maneuver = Strategy.Maneuvers[0];
}
else
{
Maneuver = Strategy.Maneuvers[1];
}
ManeuverTrust = 50 * Player.Pride;
}
}
public void MakePopup(GameObject creator, Maneuver parent) {
if (move_type == MoveTypes.forward || move_type == MoveTypes.reverse) {
GameObject g = (GameObject) Instantiate (popup);
g.transform.parent = transform.parent;
g.GetComponent<RectTransform>().localPosition = Vector3.zero;
g.GetComponent<RectTransform>().localScale = Vector3.one;
ForwardPopup fp = g.GetComponent<ForwardPopup>();
fp.Init(creator, parent);
}
else if (move_type == MoveTypes.left_turn || move_type == MoveTypes.right_turn) {
GameObject g = (GameObject) Instantiate (popup);
g.transform.parent = transform.parent;
g.GetComponent<RectTransform>().localPosition = Vector3.zero;
g.GetComponent<RectTransform>().localScale = Vector3.one;
LeftPopup fp = g.GetComponent<LeftPopup>();
fp.Init(creator, parent);
}
}
private void InitializeManeuverList()
{
for (int i = 0; i < maneuvers.Length; i++)
{
maneuvers[i] = new Maneuver
{
allowed = false,
denied = false,
crewConfirmed = false,
physicalButton = physicalButtons[i]
};
}
maneuvers[0].name = "Turn Left";
maneuvers[1].name = "Turn Right";
maneuvers[2].name = "Throttle Up";
maneuvers[3].name = "Throttle Down";
}
public CircularizeXfer(OrbitData fromOrbit) : base(fromOrbit)
{
name = "Circularize";
// find velocity vector perpendicular to r for circular orbit
double[] r_ship = new double[3];
double[] v_ship = new double[3];
GravityEngine ge = GravityEngine.Instance();
ge.GetPositionVelocityScaled(fromOrbit.nbody, ref r_ship, ref v_ship);
double[] r_center = new double[3];
double[] v_center = new double[3];
ge.GetPositionVelocityScaled(fromOrbit.centralMass, ref r_center, ref v_center);
Vector3 pos_ship = new Vector3((float)r_ship[0], (float)r_ship[1], (float)r_ship[2]);
Vector3 vel_ship = new Vector3((float)v_ship[0], (float)v_ship[1], (float)v_ship[2]);
Vector3 pos_center = new Vector3((float)r_center[0], (float)r_center[1], (float)r_center[2]);
Vector3 vel_center = new Vector3((float)v_center[0], (float)v_center[1], (float)v_center[2]);
Vector3 r = pos_ship - pos_center;
// want velocity relative to central mass (it could be moving)
vel_ship = vel_ship - vel_center;
// to get axis of orbit, can take r x v
Vector3 axis = Vector3.Normalize(Vector3.Cross(pos_ship, vel_ship));
// vis visa for circular orbit
float mu = fromOrbit.centralMass.mass * ge.massScale;
float v_mag = Mathf.Sqrt(mu / Vector3.Magnitude(r));
// positive v is counter-clockwise
Vector3 v_dir = Vector3.Normalize(Vector3.Cross(axis, r));
Vector3 v_circular = v_mag * v_dir;
Maneuver m1;
m1 = new Maneuver();
m1.nbody = fromOrbit.nbody;
m1.mtype = Maneuver.Mtype.vector;
m1.velChange = v_circular - vel_ship;
m1.dV = Vector3.Magnitude(m1.velChange);
Debug.Log(string.Format("v_ship={0} v_circular={1} axis={2}", vel_ship, v_circular, axis));
m1.worldTime = GravityEngine.Instance().GetPhysicalTime();
maneuvers.Add(m1);
}
/// <summary>
/// Distinctly want to make lane change, parameters for doing so
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="left"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? LaneChangeManeuver(ArbiterLane lane, bool left, ArbiterWaypoint goal, VehicleState vehicleState,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, LaneChangeInformation laneChangeInformation, Maneuver? secondary,
out LaneChangeParameters parameters)
{
// distance until the change is complete
double distanceToUpperBound = lane.DistanceBetween(vehicleState.Front, goal.Position);
double neededDistance = (1.5 * TahoeParams.VL * Math.Max(Math.Abs(goal.Lane.LaneId.Number - lane.LaneId.Number), 1)) +
(-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));
parameters = new LaneChangeParameters();
if (distanceToUpperBound < neededDistance)
return null;
Coordinates upperBound = new Coordinates();
Coordinates upperReturnBound = new Coordinates();
Coordinates minimumReturnBound = new Coordinates();
Coordinates defaultReturnBound = new Coordinates();
if (laneChangeInformation.Reason == LaneChangeReason.FailedForwardVehicle)
{
double distToForwards = Math.Min(neededDistance, lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) - 2.0);
upperBound = lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), distToForwards).Location;
defaultReturnBound = lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition), TahoeParams.VL * 4.0).Location;
}
// get params for lane change
LaneChangeParameters? changeParams = this.LaneChangeParameterization(
laneChangeInformation,
lane, left ? lane.LaneOnLeft : lane.LaneOnRight, false, goal.Position, upperBound,
new Coordinates(), new Coordinates(), defaultReturnBound, blockages, ignorable,
vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);
// set lane change params
parameters = changeParams.HasValue ? changeParams.Value : parameters = new LaneChangeParameters();
// check if the lane change is available or recommended
if (changeParams != null && changeParams.Value.Feasible)
{
// minimize parameterizations
List<TravelingParameters> tps = new List<TravelingParameters>();
tps.Add(this.ForwardMonitor.LaneParameters);
tps.Add(changeParams.Value.Parameters);
if(this.ForwardMonitor.FollowingParameters.HasValue)
tps.Add(this.ForwardMonitor.FollowingParameters.Value);
tps.Sort();
// check if possible to make lane change
if (changeParams.Value.Available)
{
// get traveling params from FQM to make sure we stopped for vehicle, behind vehicle
double v = CoreCommon.Communications.GetVehicleSpeed().Value;
// just use other params with shorted distance bound
TravelingParameters final = tps[0];
// final behavior
ChangeLaneBehavior clb = new ChangeLaneBehavior(
lane.LaneId,
parameters.Target.LaneId,
left,
final.DistanceToGo,
final.SpeedCommand,
final.VehiclesToIgnore,
lane.LanePath(),
parameters.Target.LanePath(),
lane.Width,
parameters.Target.Width,
lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true));
// final state
ChangeLanesState cls = new ChangeLanesState(changeParams.Value);
// return maneuver
return new Maneuver(clb, cls, left ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, vehicleState.Timestamp);
}
// otherwise plan for requirements of change coming up
else
{
// check if secondary exists
if (secondary != null)
{
return secondary;
}
// otherwise plan for upcoming
else
{
// get params
TravelingParameters final = tps[0];
// return maneuver
return new Maneuver(tps[0].Behavior, tps[0].NextState, this.ForwardMonitor.NavigationParameters.Decorators, vehicleState.Timestamp);
}
}
}
// return null over fallout
return null;
}
/// <summary>
/// Generate the traveling parameterization for the desired behaivor
/// </summary>
/// <param name="lane"></param>
/// <param name="navStopSpeed"></param>
/// <param name="navStopDistance"></param>
/// <param name="navStop"></param>
/// <param name="navStopType"></param>
/// <param name="state"></param>
/// <returns></returns>
private TravelingParameters NavStopParameterization(ArbiterLane lane, double navStopSpeed, double navStopDistance, ArbiterWaypoint navStop, StopType navStopType, VehicleState state)
{
// get min dist
double distanceCutOff = CoreCommon.OperationalStopDistance;
// turn direction default
List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators;
// create new params
TravelingParameters tp = new TravelingParameters();
#region Get Maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
// get lane path
LinePath lp = lane.LanePath().Clone();
lp.Reverse();
#region Distance Cutoff
// check if distance is less than cutoff
if (navStopDistance < distanceCutOff)
{
// default behavior
tp.SpeedCommand = new StopAtDistSpeedCommand(navStopDistance);
Behavior b = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(navStopDistance), new List<int>(), lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
// stopping so not using speed param
usingSpeed = false;
IState nextState = CoreCommon.CorePlanningState;
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// get lane
ArbiterLane al = lane;
// default behavior
tp.SpeedCommand = new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24));
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)), new List<int>(), lp, al.Width, al.NumberOfLanesRight(state.Front, false), al.NumberOfLanesLeft(state.Front, false));
// standard behavior is fine for maneuver
m = new Maneuver(b, CoreCommon.CorePlanningState, decorators, state.Timestamp);
}
#endregion
#endregion
#region Parameterize
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = navStopDistance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = navStopSpeed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.VehiclesToIgnore = new List<int>();
// return navigation params
return tp;
#endregion
}
/// <summary>
/// Behavior given we stay in the current lane
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <param name="downstreamPoint"></param>
/// <returns></returns>
public TravelingParameters Primary(IFQMPlanable lane, VehicleState state, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, bool log)
{
// possible parameterizations
List<TravelingParameters> tps = new List<TravelingParameters>();
#region Lane Major Parameterizations with Current Lane Goal Params, If Best Goal Exists in Current Lane
// check if the best goal is in the current lane
ArbiterWaypoint lanePoint = null;
if (lane.AreaComponents.Contains(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Lane))
lanePoint = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest;
// get the next thing we need to stop at no matter what and parameters for stopping at it
ArbiterWaypoint laneNavStop;
double laneNavStopSpeed;
double laneNavStopDistance;
StopType laneNavStopType;
this.NextNavigationalStop(lane, lanePoint, state.Front, state.ENCovariance, ignorable,
out laneNavStopSpeed, out laneNavStopDistance, out laneNavStopType, out laneNavStop);
// create parameterization of the stop
TravelingParameters laneNavParams = this.NavStopParameterization(lane, roadPlan, laneNavStopSpeed, laneNavStopDistance, laneNavStop, laneNavStopType, state);
this.navigationParameters = laneNavParams;
this.laneParameters = laneNavParams;
tps.Add(laneNavParams);
#region Log
if (log)
{
// add to current parames to arbiter information
CoreCommon.CurrentInformation.FQMBehavior = laneNavParams.Behavior.ToShortString();
CoreCommon.CurrentInformation.FQMBehaviorInfo = laneNavParams.Behavior.ShortBehaviorInformation();
CoreCommon.CurrentInformation.FQMSpeedCommand = laneNavParams.Behavior.SpeedCommandString();
CoreCommon.CurrentInformation.FQMDistance = laneNavParams.DistanceToGo.ToString("F6");
CoreCommon.CurrentInformation.FQMSpeed = laneNavParams.RecommendedSpeed.ToString("F6");
CoreCommon.CurrentInformation.FQMState = laneNavParams.NextState.ShortDescription();
CoreCommon.CurrentInformation.FQMStateInfo = laneNavParams.NextState.StateInformation();
CoreCommon.CurrentInformation.FQMStopType = laneNavStopType.ToString();
CoreCommon.CurrentInformation.FQMWaypoint = laneNavStop.ToString();
CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = lane.CurrentMaximumSpeed(state.Position).ToString("F1");
}
#endregion
#endregion
#region Forward Vehicle Parameterization
// forward vehicle update
this.ForwardVehicle.Update(lane, state);
// clear current params
this.followingParameters = null;
// check not in a sparse area
bool sparseArea = lane is ArbiterLane ?
((ArbiterLane)lane).GetClosestPartition(state.Front).Type == PartitionType.Sparse :
((SupraLane)lane).ClosestComponent(state.Front) == SLComponentType.Initial && ((SupraLane)lane).Initial.GetClosestPartition(state.Front).Type == PartitionType.Sparse;
// exists forward vehicle
if (!sparseArea && this.ForwardVehicle.ShouldUseForwardTracker)
{
// get forward vehicle params, set lane decorators
TravelingParameters vehicleParams = this.ForwardVehicle.Follow(lane, state, ignorable);
vehicleParams.Behavior.Decorators.AddRange(this.laneParameters.Decorators);
this.FollowingParameters = vehicleParams;
#region Log
if (log)
{
// add to current parames to arbiter information
CoreCommon.CurrentInformation.FVTBehavior = vehicleParams.Behavior.ToShortString();
CoreCommon.CurrentInformation.FVTSpeed = this.ForwardVehicle.FollowingParameters.RecommendedSpeed.ToString("F3");
CoreCommon.CurrentInformation.FVTSpeedCommand = vehicleParams.Behavior.SpeedCommandString();
CoreCommon.CurrentInformation.FVTDistance = vehicleParams.DistanceToGo.ToString("F2");
CoreCommon.CurrentInformation.FVTState = vehicleParams.NextState.ShortDescription();
CoreCommon.CurrentInformation.FVTStateInfo = vehicleParams.NextState.StateInformation();
// set xSeparation
CoreCommon.CurrentInformation.FVTXSeparation = this.ForwardVehicle.ForwardControl.xSeparation.ToString("F0");
}
#endregion
// check if we're stopped behind fv, allow wait timer if true, stop wait timer if not behind fv
bool forwardVehicleStopped = this.ForwardVehicle.CurrentVehicle.IsStopped;
bool forwardSeparationGood = this.ForwardVehicle.ForwardControl.xSeparation < TahoeParams.VL * 2.5;
bool wereStopped = CoreCommon.Communications.GetVehicleSpeed().Value < 0.1;
bool forwardDistanceToGo = vehicleParams.DistanceToGo < 3.5;
if (forwardVehicleStopped && forwardSeparationGood && wereStopped && forwardDistanceToGo)
this.ForwardVehicle.StoppedBehindForwardVehicle = true;
else
{
this.ForwardVehicle.StoppedBehindForwardVehicle = false;
this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop();
this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset();
}
// add vehicle param
tps.Add(vehicleParams);
}
else
{
// no forward vehicle
this.followingParameters = null;
this.ForwardVehicle.StoppedBehindForwardVehicle = false;
}
#endregion
#region Sparse Waypoint Parameterization
// check for sparse waypoints downstream
bool sparseDownstream;
bool sparseNow;
double sparseDistance;
lane.SparseDetermination(state.Front, out sparseDownstream, out sparseNow, out sparseDistance);
// check if sparse areas downstream
if (sparseDownstream)
{
// set the distance to the sparse area
if (sparseNow)
sparseDistance = 0.0;
// get speed
double speed = SpeedTools.GenerateSpeed(sparseDistance, 2.24, lane.CurrentMaximumSpeed(state.Front));
// maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
SpeedCommand sc = new ScalarSpeedCommand(speed);
#region Parameterize Given Speed Command
// 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));
b.Decorators = this.laneParameters.Decorators;
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), this.laneParameters.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>());
b.Decorators = this.laneParameters.Decorators;
// standard behavior is fine for maneuver
m = new Maneuver(b, sisls, this.laneParameters.Decorators, state.Timestamp);
}
#endregion
#region Parameterize
// create new params
TravelingParameters tp = new TravelingParameters();
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = this.laneParameters.Decorators;
tp.DistanceToGo = Double.MaxValue;
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
tps.Add(tp);
#endregion
}
#endregion
// sort params by most urgent
tps.Sort();
// set current params
this.currentParameters = tps[0];
// get behavior to check add vehicles to ignore
if (this.currentParameters.Behavior is StayInLaneBehavior)
((StayInLaneBehavior)this.currentParameters.Behavior).IgnorableObstacles = this.ForwardVehicle.VehiclesToIgnore;
// out of navigation, blockages, and vehicle following determine the actual primary parameters for this lane
return tps[0];
}
/// <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>
/// Gets parameterization
/// </summary>
/// <param name="lane"></param>
/// <param name="speed"></param>
/// <param name="distance"></param>
/// <param name="state"></param>
/// <returns></returns>
public TravelingParameters GetParameters(ArbiterLane lane, double speed, double distance, VehicleState state)
{
double distanceCutOff = CoreCommon.OperationslStopLineSearchDistance;
Maneuver m = new Maneuver();
SpeedCommand sc;
bool usingSpeed = true;
#region Distance Cutoff
// check if distance is less than cutoff
if (distance < distanceCutOff)
{
// default behavior
sc = new StopAtDistSpeedCommand(distance);
Behavior b = new StayInLaneBehavior(lane.LaneId, sc, new List<int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(state.Front, true), lane.NumberOfLanesRight(state.Front, true));
// stopping so not using speed param
usingSpeed = false;
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(lane, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp);
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// get lane
ArbiterLane al = lane;
// default behavior
sc = new ScalarSpeedCommand(speed);
Behavior b = new StayInLaneBehavior(al.LaneId, sc, 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), TurnDecorators.NoDecorators, state.Timestamp);
}
#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
}
private void Shooting()
{
if (Time.time >= maneuverTimer)
{
maneuverTimer = Time.time + Random.Range(5, 10);
currentManeuver = (Maneuver)Random.Range(0, 2);
print(string.Format("currentManeuver: {0}", currentManeuver));
if (currentManeuver == Maneuver.Sniping)
{
isStrafingRight = RandomBoolean();
}
}
else
{
switch (currentManeuver)
{
case Maneuver.Attack:
PerformAttackManeuver();
break;
case Maneuver.Sniping:
PerformSnipingManeuver(isStrafingRight);
break;
}
}
}
/// <summary>
/// Distinctly want to make lane change, parameters for doing so
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="left"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? AdvancedDesiredLaneChangeManeuver(ArbiterLane lane, bool left, ArbiterWaypoint goal, RoadPlan rp, VehicleState vehicleState,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, LaneChangeInformation laneChangeInformation, Maneuver? secondary,
out LaneChangeParameters parameters)
{
// set aprams
parameters = new LaneChangeParameters();
// get final maneuver
Maneuver? final = null;
// check partition is not a startup chute
if (lane.GetClosestPartition(vehicleState.Front).Type != PartitionType.Startup)
{
// get the lane goal distance
double distanceToLaneGoal = lane.DistanceBetween(vehicleState.Front, goal.Position);
// check if our distance is less than 50m to the goal
if (distanceToLaneGoal < 50.0)
{
// use old
final = this.LaneChangeManeuver(lane, left, goal, vehicleState, blockages, ignorable, laneChangeInformation, secondary, out parameters);
try
{
// check final null
if (final == null)
{
// check for checkpoint within 4VL of front of failed vehicle
ArbiterCheckpoint acCurrecnt = CoreCommon.Mission.MissionCheckpoints.Peek();
if (acCurrecnt.WaypointId is ArbiterWaypointId)
{
// get waypoint
ArbiterWaypoint awCheckpoint = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[acCurrecnt.WaypointId];
// check way
if (awCheckpoint.Lane.Way.Equals(lane.Way))
{
// distance to wp
double distToWp = lane.DistanceBetween(vehicleState.Front, awCheckpoint.Position);
// check close to waypoint and stopped
if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 && distToWp < TahoeParams.VL * 1.0)
{
ArbiterOutput.Output("Removing checkpoint: " + acCurrecnt.WaypointId.ToString() + " Stopped next to it");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
}
}
catch (Exception) { }
}
// no forward vehicle
else if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle == null)
{
// adjacent monitor
LateralReasoning adjacent = null;
if (left && this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists)
{
// update
adjacent = (LateralReasoning)this.leftLateralReasoning;
}
else if (!left && this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists)
{
// update
adjacent = (LateralReasoning)this.rightLateralReasoning;
}
// check adj
if (adjacent != null)
{
// update
adjacent.ForwardMonitor.Primary(adjacent.LateralLane, vehicleState, rp, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), false);
if (adjacent.ForwardMonitor.ForwardVehicle.CurrentVehicle == null && adjacent.AdjacentAndRearClear(vehicleState))
{
// use old
final = this.LaneChangeManeuver(lane, left, goal, vehicleState, blockages, ignorable, laneChangeInformation, secondary, out parameters);
}
}
}
}
if (!final.HasValue)
{
if (!secondary.HasValue)
{
List<TravelingParameters> falloutParams = new List<TravelingParameters>();
TravelingParameters t1 = this.ForwardMonitor.ParameterizationHelper(lane, lane, goal.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
falloutParams.Add(t1);
falloutParams.Add(this.ForwardMonitor.LaneParameters);
if (this.ForwardMonitor.FollowingParameters.HasValue)
falloutParams.Add(this.ForwardMonitor.FollowingParameters.Value);
falloutParams.Sort();
TravelingParameters tpCatch = falloutParams[0];
return new Maneuver(tpCatch.Behavior, tpCatch.NextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
return secondary;
}
}
else
{
return final;
}
}
/// <summary>
/// Plan the maneuver
/// </summary>
/// <param name="planningState"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, VehicleState vehicleState, double vehicleSpeed, List<ITacticalBlockage> blockages, INavigationalPlan plan)
{
#region Encountered a Blockage
// something is blocked
if (CoreCommon.CorePlanningState is EncounteredBlockageState)
{
// cast blockage state
EncounteredBlockageState ebs = (EncounteredBlockageState)CoreCommon.CorePlanningState;
// get the saudi level
SAUDILevel saudi = ebs.Saudi;
// get the defcon level
BlockageRecoveryDEFCON defcon = ebs.Defcon;
#region Stay In Lane Blockage
if (ebs.PlanningState is StayInLaneState)
{
// get the recovery state
BlockageRecoveryState brs = new BlockageRecoveryState(null, ebs.PlanningState, ebs.PlanningState, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED);
// set cooldown
BlockageHandler.SetDefaultBlockageCooldown();
// return
return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#endregion
#region Supra Lane Blockage
else if (ebs.PlanningState is StayInSupraLaneState)
{
// get all supra lane info
StayInSupraLaneState sisls = (StayInSupraLaneState)ebs.PlanningState;
SupraLane sl = sisls.Lane;
// the closest component of the supra lane to us is the initial lane
if (sl.ClosestComponent(vehicleState.Front) == SLComponentType.Initial)
{
// get stay in lane state
StayInLaneState sils = new StayInLaneState(sl.Initial, CoreCommon.CorePlanningState);
ebs.PlanningState = sils;
// get the recovery state
BlockageRecoveryState brs = new BlockageRecoveryState(null, sils, sils, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED);
// set cooldown
BlockageHandler.SetDefaultBlockageCooldown();
// return
return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// check if closest part of the supra lane is the final lane
else if (sl.ClosestComponent(vehicleState.Front) == SLComponentType.Final)
{
// get stay in lane state
StayInLaneState sils = new StayInLaneState(sl.Final, CoreCommon.CorePlanningState);
ebs.PlanningState = sils;
// get the recovery state
BlockageRecoveryState brs = new BlockageRecoveryState(null, sils, sils, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED);
// set cooldown
BlockageHandler.SetDefaultBlockageCooldown();
// return
return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// the closest component of the supra lane to us is the interconnect otherwise
else
{
// get turn info
LinePath fP;
LineList lB;
LineList rB;
IntersectionToolkit.TurnInfo(((ArbiterWaypoint)sl.Interconnect.FinalGeneric), out fP, out lB, out rB);
// make sure to update the turn reasoning
this.tacticalUmbrella.intersectionTactical.TurnReasoning =
new TurnReasoning(sl.Interconnect, null);
// get turn state
TurnState ts = new TurnState(sl.Interconnect, sl.Interconnect.TurnDirection, sl.Final, fP, lB, rB, new ScalarSpeedCommand(sl.Interconnect.MaximumDefaultSpeed));
ebs.PlanningState = ts;
// get final lane state
StayInLaneState sils = new StayInLaneState(sl.Final, CoreCommon.CorePlanningState);
// get the recovery state
BlockageRecoveryState brs = new BlockageRecoveryState(null, sils, ts, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED);
// set cooldown
BlockageHandler.SetDefaultBlockageCooldown();
// return
return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Change Lanes Blockage
else if (ebs.PlanningState is ChangeLanesState)
{
// get lane change state
ChangeLanesState cls = (ChangeLanesState)ebs.PlanningState;
// figure out which lane we are closest to being inside
ArbiterLane goLane = cls.Parameters.Initial.LanePolygon.IsInside(vehicleState.Front) ?
cls.Parameters.Initial : cls.Parameters.Target;
// check if lane is oncoming
bool goLaneOncoming = cls.Parameters.Initial.LanePolygon.IsInside(vehicleState.Front) ?
cls.Parameters.InitialOncoming : cls.Parameters.TargetOncoming;
// normal
if (!goLaneOncoming)
{
// get lane state
StayInLaneState sils = new StayInLaneState(goLane, CoreCommon.CorePlanningState);
ebs.PlanningState = sils;
// get the recovery state
BlockageRecoveryState brs = new BlockageRecoveryState(null, sils, sils, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED);
// set cooldown
BlockageHandler.SetDefaultBlockageCooldown();
// return
return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// opposing
else
{
// get lane state
OpposingLanesState ols = new OpposingLanesState(goLane, true, CoreCommon.CorePlanningState, vehicleState);
ebs.PlanningState = ols;
// get the recovery state
BlockageRecoveryState brs = new BlockageRecoveryState(null, ols, ols, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED);
// set cooldown
BlockageHandler.SetDefaultBlockageCooldown();
// return
return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Turn Blockage
else if (ebs.PlanningState is TurnState)
{
// get turn state
TurnState ts = (TurnState)ebs.PlanningState;
// notify
ArbiterOutput.Output("Encountered turn blockage, entering blockage recovery state");
// get the recovery state
BlockageRecoveryState brs = new BlockageRecoveryState(null, ts, ts, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED);
// set cooldown
BlockageHandler.SetDefaultBlockageCooldown();
// return
return new Maneuver(new HoldBrakeBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#endregion
#region Opposing Lanes Blockage
else if (ebs.PlanningState is OpposingLanesState)
{
// get the recovery state
BlockageRecoveryState brs = new BlockageRecoveryState(null, ebs.PlanningState, ebs.PlanningState, defcon, ebs, BlockageRecoverySTATUS.ENCOUNTERED);
// set cooldown
BlockageHandler.SetDefaultBlockageCooldown();
// return
return new Maneuver(new NullBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#endregion
#region Fallout
// plan through the blockage state
Maneuver final = new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
// return the final maneuver
return final;
#endregion
}
#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 Blockage Encountered
if (brs.RecoveryStatus == BlockageRecoverySTATUS.ENCOUNTERED)
{
// check encoutnered state for a turn
if (brs.EncounteredState.PlanningState is TurnState)
{
// reset timing
this.Reset();
// return the turn recovery maneuver
TurnState ts = (TurnState)brs.EncounteredState.PlanningState;
return this.TurnRecoveryManeuver(ts.Interconnect, vehicleState, vehicleSpeed, brs);
}
// check encountered state for a lane
else if (brs.EncounteredState.PlanningState is StayInLaneState)
{
// reset execution timer
ExecutionTimer.Stop();
ExecutionTimer.Reset();
// lane blockage recovery
bool fakeWait;
StayInLaneState sils = (StayInLaneState)brs.EncounteredState.PlanningState;
return this.LaneRecoveryManeuver(sils.Lane, vehicleState, vehicleSpeed,
plan, brs, false, out fakeWait);
}
// check encountered state for opposing lane
else if (brs.EncounteredState.PlanningState is OpposingLanesState)
{
// reset timing
this.Reset();
// reset timing
OpposingLanesState ols = (OpposingLanesState)brs.EncounteredState.PlanningState;
return this.OpposingLaneRecoveryManeuver(ols.OpposingLane, vehicleState, vehicleSpeed, plan, brs, false);
}
}
#endregion
#region Blockage Recovery Executing
else if (brs.RecoveryStatus == BlockageRecoverySTATUS.EXECUTING)
{
// reset uturn timer
uTurnTimer.Stop();
uTurnTimer.Reset();
// check if lane change
if (brs.RecoveryBehavior is ChangeLaneBehavior)
{
// reset timing
this.Reset();
// bypass
ArbiterOutput.Output("Executing behavior of type: " + brs.RecoveryBehavior.GetType().ToString() + ", waiting for completion");
return new Maneuver(brs.RecoveryBehavior, CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// check execution timer
if (!ExecutionTimer.IsRunning)
{
ExecutionTimer.Stop();
ExecutionTimer.Reset();
ExecutionTimer.Start();
}
// check too long
if (ExecutionTimer.ElapsedMilliseconds / 1000.0 > 25)
{
// reset execution timer
ExecutionTimer.Stop();
ExecutionTimer.Reset();
// bypass
// notify
ArbiterOutput.Output("Blockage recovery execution timeout, bypassing to send hold brake");
try
{
CoreCommon.Communications.Execute(new HoldBrakeBehavior());
Thread.Sleep(100);
}
catch (Exception) { }
// notify
ArbiterOutput.Output("Blockage recovery execution timeout, resending behavior");
// resend recovery behavior
return new Maneuver(brs.RecoveryBehavior, CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// bypass
ArbiterOutput.Output("Executing behavior of type: " + brs.RecoveryBehavior.GetType().ToString() + ", Currently Sending Null Behavior, waiting for completion");
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Blockage Recovery Behavior Completed
else if (brs.RecoveryStatus == BlockageRecoverySTATUS.COMPLETED)
{
// reset timing
this.Reset();
// check encoutnered state for a turn
if (brs.EncounteredState.PlanningState is TurnState)
{
// return the turn recovery maneuver
TurnState ts = (TurnState)brs.EncounteredState.PlanningState;
return this.TurnRecoveryManeuver(ts.Interconnect, vehicleState, vehicleSpeed, brs);
}
// check lane state
else if (brs.EncounteredState.PlanningState is StayInLaneState)
{
// return the completion state
brs.RecoveryStatus = BlockageRecoverySTATUS.ENCOUNTERED;
return new Maneuver(new HoldBrakeBehavior(), brs.CompletionState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// check opposing
else if(brs.EncounteredState.PlanningState is OpposingLanesState)
{
// return the completion state
brs.RecoveryStatus = BlockageRecoverySTATUS.ENCOUNTERED;
return new Maneuver(new HoldBrakeBehavior(), brs.CompletionState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Fallout Of Recovery
// reset timing
this.Reset();
ArbiterOutput.Output("Fell out of blockage recovery state: recovery status: " + brs.RecoveryStatus.ToString());
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, brs.RecoveryBehavior != null ? brs.RecoveryBehavior.Decorators : TurnDecorators.NoDecorators, vehicleState.Timestamp);
#endregion
}
#endregion
#region Unknown
else
{
throw new Exception("Unknown blockage planning state type");
}
#endregion
}
/// <summary>
/// Maneuver for recovering from a turn blockage
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="defcon"></param>
/// <param name="saudi"></param>
/// <returns></returns>
private Maneuver TurnRecoveryManeuver(ArbiterInterconnect ai, VehicleState vehicleState,
double vehicleSpeed, BlockageRecoveryState brs)
{
// get the blockage
ITacticalBlockage turnBlockageReport = brs.EncounteredState.TacticalBlockage;
// get the turn state
TurnState turnState = (TurnState)brs.EncounteredState.PlanningState;
// check the state of the recovery for being the initial state
if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL)
{
// determine if the reverse behavior is recommended
if (turnBlockageReport.BlockageReport.ReverseRecommended)
{
// get the path of the interconnect
LinePath interconnectPath = ai.InterconnectPath;
// check how much room there is to the start of the interconnect
double distanceToStart = interconnectPath.DistanceBetween(interconnectPath.StartPoint, interconnectPath.GetClosestPoint(vehicleState.Rear));
// check distance to start from the rear bumper is large enough
if (distanceToStart > 0)
{
// notify
ArbiterOutput.Output("Initial encounter of turn blockage with reverse recommended, reversing");
// get hte reverse path
LinePath reversePath = vehicleState.VehicleLinePath;
// generate the reverse recovery behavior
StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(TahoeParams.VL, true);
StayInLaneBehavior silb = new StayInLaneBehavior(null, sadsc, new List<int>(), reversePath, TahoeParams.T * 2.0, 0, 0);
// update the state
BlockageRecoveryState brsUpdated = new BlockageRecoveryState(
silb, brs.CompletionState, brs.AbortState, BlockageRecoveryDEFCON.REVERSE,
brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
// chill out
BlockageHandler.SetDefaultBlockageCooldown();
// send the maneuver
Maneuver recoveryManeuver = new Maneuver(
silb, brsUpdated, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
return recoveryManeuver;
}
}
}
// get the default turn behavior
TurnBehavior defaultTurnBehavior = (TurnBehavior)turnState.Resume(vehicleState, vehicleSpeed);
// check that we are not already ignoring small obstacles
if (turnState.Saudi == SAUDILevel.None)
{
// check the turn ignoring small obstacles
ShutUpAndDoItDecorator saudiDecorator = new ShutUpAndDoItDecorator(SAUDILevel.L1);
defaultTurnBehavior.Decorators.Add(saudiDecorator);
turnState.Saudi = SAUDILevel.L1;
// notify
ArbiterOutput.Output("Attempting test of turn behavior ignoring small obstacles");
// test execute the turn
CompletionReport saudiCompletionReport;
bool completedSaudiTurn = CoreCommon.Communications.TestExecute(defaultTurnBehavior, out saudiCompletionReport);
// if the turn worked with ignorimg small obstacles, execute that
if (completedSaudiTurn)
{
// notify
ArbiterOutput.Output("Test of turn behavior ignoring small obstacles successful");
// chill out
BlockageHandler.SetDefaultBlockageCooldown();
// return the recovery maneuver
return new Maneuver(defaultTurnBehavior, turnState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
// notify
ArbiterOutput.Output("Turn behavior reached last level of using no turn boundaries");
// when ignoring small obstacles does not work, send the turn without boundaries and ignore small obstacles
turnState.UseTurnBounds = false;
turnState.LeftBound = null;
turnState.RightBound = null;
defaultTurnBehavior.RightBound = null;
defaultTurnBehavior.LeftBound = null;
// ignoring small obstacles
ShutUpAndDoItDecorator saudiNoBoundsDecorator = new ShutUpAndDoItDecorator(SAUDILevel.L1);
defaultTurnBehavior.Decorators.Add(saudiNoBoundsDecorator);
turnState.Saudi = SAUDILevel.L1;
// chill out
BlockageHandler.SetDefaultBlockageCooldown();
// go to the turn state
return new Maneuver(defaultTurnBehavior, turnState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
/// <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
}
//Will be used for setting some pointers and things
public void Init(GameObject c, Maneuver parent) {
creator = c;
parent_maneuver = parent;
transform.parent.Find ("Test Maneuvers").GetComponent<Button>().interactable = false;
}
