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; }