/// <summary> /// Checks if hte opposing lane is clear to pass an opposing vehicle /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public bool ClearForDisabledVehiclePass(ArbiterLane lane, VehicleState state, double vUs, Coordinates minReturn) { // update the forward vehicle this.ForwardVehicle.Update(lane, state); // check if the rear vehicle exists and is moving along with us if (this.ForwardVehicle.ShouldUseForwardTracker && this.ForwardVehicle.CurrentVehicle != null) { // distance from other to us double currentDistance = lane.DistanceBetween(this.ForwardVehicle.CurrentVehicle.ClosestPosition, state.Front) - (2 * TahoeParams.VL); double minChangeDist = lane.DistanceBetween(minReturn, state.Front); // check if he's within min return dist if(currentDistance > minChangeDist) { // params double vOther = this.ForwardVehicle.CurrentVehicle.StateMonitor.Observed.speedValid ? this.ForwardVehicle.CurrentVehicle.Speed : lane.Way.Segment.SpeedLimits.MaximumSpeed; // get distance of envelope for him to slow to our speed double xEnvelope = (Math.Pow(vUs, 2.0) - Math.Pow(vOther, 2.0)) / (2.0 * -0.5); // check to see if vehicle is outside of the envelope to slow down for us after 3 seconds double xSafe = currentDistance - minChangeDist - (xEnvelope + (vOther * 15.0)); return xSafe > 0 ? true : false; } else return false; } else return true; }
/// <summary> /// Helps with parameterizations for lateral reasoning /// </summary> /// <param name="referenceLane"></param> /// <param name="fqmLane"></param> /// <param name="goal"></param> /// <param name="vehicleFront"></param> /// <returns></returns> public TravelingParameters ParameterizationHelper(ArbiterLane referenceLane, ArbiterLane fqmLane, Coordinates goal, Coordinates vehicleFront, IState nextState, VehicleState state, VehicleAgent va) { // get traveling parameterized list List<TravelingParameters> tps = new List<TravelingParameters>(); // get distance to the goal double goalDistance; double goalSpeed; this.StoppingParams(goal, referenceLane, vehicleFront, new double[] { }, out goalSpeed, out goalDistance); tps.Add(this.GetParameters(referenceLane, goalSpeed, goalDistance, state)); // get next stop ArbiterWaypoint stopPoint; double stopSpeed; double stopDistance; StopType stopType; this.NextLaneStop(fqmLane, vehicleFront, new double[] { }, new List<ArbiterWaypoint>(), out stopPoint, out stopSpeed, out stopDistance, out stopType); this.StoppingParams(stopPoint.Position, referenceLane, vehicleFront, new double[] { }, out stopSpeed, out stopDistance); tps.Add(this.GetParameters(referenceLane, stopSpeed, stopDistance, state)); // get vehicle if (va != null) { VehicleAgent tmp = this.ForwardVehicle.CurrentVehicle; double tmpDist = this.ForwardVehicle.currentDistance; this.ForwardVehicle.CurrentVehicle = va; this.ForwardVehicle.currentDistance = referenceLane.DistanceBetween(state.Front, va.ClosestPosition); TravelingParameters tp = this.ForwardVehicle.Follow(referenceLane, state, new List<ArbiterWaypoint>()); tps.Add(tp); this.ForwardVehicle.CurrentVehicle = tmp; this.ForwardVehicle.currentDistance = tmpDist; } // parameterize tps.Sort(); return tps[0]; }
/// <summary> /// Given vehicles and there locations determines if the lane adjacent to us is occupied adjacent to the vehicle /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public bool Occupied(ArbiterLane lane, VehicleState state) { // check stopwatch time for proper elapsed if (this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 10) this.Reset(); if (TacticalDirector.VehicleAreas.ContainsKey(lane)) { // vehicles in the lane List<VehicleAgent> laneVehicles = TacticalDirector.VehicleAreas[lane]; // position of the center of our vehicle Coordinates center = state.Front - state.Heading.Normalize(TahoeParams.VL / 2.0); // cutoff for allowing vehicles outside this range double dCutOff = TahoeParams.VL * 1.5; // loop through vehicles foreach (VehicleAgent va in laneVehicles) { // vehicle high level distance double d = Math.Abs(lane.DistanceBetween(center, va.ClosestPosition)); // check less than distance cutoff if (d < dCutOff) { ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " filled with vehicle: " + va.ToString()); this.CurrentVehicle = va; this.Reset(); return true; } } } // now check the lateral sensor for being occupied if (this.SideSickObstacleDetected(lane, state)) { this.CurrentVehicle = new VehicleAgent(true, true); this.Reset(); ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " SIDE OBSTACLE DETECTED"); return true; } // none found this.CurrentVehicle = null; // if none found, timer not running start timer if (!this.LateralClearStopwatch.IsRunning) { this.CurrentVehicle = new VehicleAgent(true, true); this.Reset(); this.LateralClearStopwatch.Start(); ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, starting cooldown"); return true; } // check enough time else if (this.LateralClearStopwatch.IsRunning && this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 0.5) { this.CurrentVehicle = null; ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown complete"); return false; } // not enough time else { this.CurrentVehicle = new VehicleAgent(true, true); double timer = this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0; ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown timer: " + timer.ToString("F2")); return true; } }
/// <summary> /// Maneuver for recovering from a lane blockage, used for lane changes as well /// </summary> /// <param name="lane"></param> /// <param name="vehicleState"></param> /// <param name="vehicleSpeed"></param> /// <param name="defcon"></param> /// <param name="saudi"></param> /// <returns></returns> public Maneuver LaneRecoveryManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, INavigationalPlan plan, BlockageRecoveryState brs, bool failedVehiclesPersistentlyOnly, out bool waitForUTurn) { // get the blockage ITacticalBlockage laneBlockageReport = brs.EncounteredState.TacticalBlockage; // get the turn state StayInLaneState sils = new StayInLaneState(lane, CoreCommon.CorePlanningState); // set wait false waitForUTurn = false; #region Reverse // check the state of the recovery for being the initial state if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL) { // determine if the reverse behavior is recommended if (laneBlockageReport.BlockageReport.ReverseRecommended) { // notify ArbiterOutput.Output("Initial encounter, reverse recommended, reversing"); // get reverse behavior StayInLaneBehavior reverseRecovery = this.LaneReverseRecovery(lane, vehicleState, vehicleSpeed, brs.EncounteredState.TacticalBlockage.BlockageReport); reverseRecovery.TimeStamp = vehicleState.Timestamp; // get recovery state BlockageRecoveryState brsT = new BlockageRecoveryState( reverseRecovery, null, new StayInLaneState(lane, CoreCommon.CorePlanningState), brs.Defcon = BlockageRecoveryDEFCON.REVERSE, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); brsT.CompletionState = brsT; // reset blockages BlockageHandler.SetDefaultBlockageCooldown(); // maneuver return new Maneuver(reverseRecovery, brsT, TurnDecorators.HazardDecorator, vehicleState.Timestamp); } } #endregion #region uTurn // check if uturn is available ArbiterLane opp = this.tacticalUmbrella.roadTactical.forwardReasoning.GetClosestOpposing(lane, vehicleState); // resoning OpposingLateralReasoning olrTmp = new OpposingLateralReasoning(opp, SideObstacleSide.Driver); if (opp != null && olrTmp.ExistsExactlyHere(vehicleState) && opp.IsInside(vehicleState.Front) && opp.LanePath().GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front) < TahoeParams.VL * 3.0) { // check possible to reach goal given block partition way RoadPlan uTurnNavTest = CoreCommon.Navigation.PlanRoadOppositeWithoutPartition( lane.GetClosestPartition(vehicleState.Front), opp.GetClosestPartition(vehicleState.Front), CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], true, vehicleState.Front, true); // flag to try the uturn bool uturnTry = false; // all polygons to include List<Polygon> allPolys = BlockageTactical.AllForwardPolygons(vehicleState, failedVehiclesPersistentlyOnly);// .AllObstacleAndStoppedVehiclePolygons(vehicleState); // test blockage takes up segment double numLanesLeft = lane.NumberOfLanesLeft(vehicleState.Front, true); double numLanesRight = lane.NumberOfLanesRight(vehicleState.Front, true); LinePath leftBound = lane.LanePath().ShiftLateral((lane.Width * numLanesLeft) + (lane.Width / 2.0)); LinePath rightBound = lane.LanePath().ShiftLateral((lane.Width * numLanesRight) + (lane.Width / 2.0)); bool segmentBlockage = BlockageTester.TestBlockage(allPolys, leftBound, rightBound); uturnTry = segmentBlockage; // check if we should try the uturn if(uturnTry) { // check uturn timer if(uTurnTimer.ElapsedMilliseconds / 1000.0 > 2.0) { #region Determine Checkpoint // check route feasible if (uTurnNavTest.BestPlan.laneWaypointOfInterest.RouteTime < double.MaxValue - 1 && uTurnNavTest.BestPlan.laneWaypointOfInterest.TimeCostToPoint < double.MaxValue - 1) { ArbiterOutput.Output("valid route to checkpoint by rerouting, uturning"); } // otherwise remove the next checkpoint else { // notiify ArbiterOutput.Output("NO valid route to checkpoint by rerouting"); // get the next cp ArbiterCheckpoint cp1 = CoreCommon.Mission.MissionCheckpoints.Peek(); // get distance to blockage double blockageDistance = 15.0; // check cp is in our lane if (cp1.WaypointId.AreaSubtypeId.Equals(lane.LaneId)) { // check distance to cp1 double distanceToCp1 = lane.DistanceBetween(vehicleState.Front, CoreCommon.RoadNetwork.ArbiterWaypoints[cp1.WaypointId].Position); // check that this is an already inserted waypoint if (cp1.Type == CheckpointType.Inserted) { // remove cp ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue(); ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as inserted type of checkpoint"); ArbiterCheckpoint ac2 = CoreCommon.Mission.MissionCheckpoints.Dequeue(); ArbiterOutput.Output("removed checkpoint: " + ac2.WaypointId.ToString() + " as blocked type of checkpoint"); } // closer to us than the blockage else if (distanceToCp1 < blockageDistance) { // remove cp ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue(); ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as between us and the blockage ~ 15m away"); } // very close to blockage on the other side else if (distanceToCp1 < blockageDistance + 5.0) { // remove cp ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue(); ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as on the other side of blockage ~ 15m away"); } // further away from the blockage else { // check over all checkpoints if there exists a checkpoint cp2 in the opposing lane within 5m along our lane ArbiterCheckpoint cp2 = null; foreach (ArbiterWaypoint oppAw in opp.WaypointList) { // check checkpoint if (oppAw.IsCheckpoint) { // distance between us and that waypoint double distanceToCp2 = lane.DistanceBetween(vehicleState.Front, oppAw.Position); // check along distance < 5.0m if (Math.Abs(distanceToCp1 - distanceToCp2) < 5.0) { // set cp cp2 = new ArbiterCheckpoint(oppAw.CheckpointId, oppAw.WaypointId, CheckpointType.Inserted); } } } // check close cp exists if (cp2 != null) { // remove cp1 and replace with cp2 ArbiterOutput.Output("inserting checkpoint: " + cp2.WaypointId.ToString() + " before checkpoint: " + cp1.WaypointId.ToString() + " as can be replaced with adjacent opposing cp2"); //CoreCommon.Mission.MissionCheckpoints.Dequeue(); // get current checkpoints ArbiterCheckpoint[] updatedAcs = new ArbiterCheckpoint[CoreCommon.Mission.MissionCheckpoints.Count + 1]; updatedAcs[0] = cp2; CoreCommon.Mission.MissionCheckpoints.CopyTo(updatedAcs, 1); updatedAcs[1].Type = CheckpointType.Blocked; CoreCommon.Mission.MissionCheckpoints = new Queue<ArbiterCheckpoint>(new List<ArbiterCheckpoint>(updatedAcs)); } // otherwise remove cp1 else { // remove cp ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue(); ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane"); } } } // otherwise we remove the checkpoint else { // remove cp ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue(); ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane"); } } #endregion #region Plan Uturn // notify ArbiterOutput.Output("Segment blocked, uturn available"); // nav penalties ArbiterLanePartition alpClose = lane.GetClosestPartition(vehicleState.Front); CoreCommon.Navigation.AddHarshBlockageAcrossSegment(alpClose, vehicleState.Front); // uturn LinePath lpTmp = new LinePath(new Coordinates[] { vehicleState.Front, opp.LanePath().GetClosestPoint(vehicleState.Front).Location }); Coordinates vector = lpTmp[1] - lpTmp[0]; lpTmp[1] = lpTmp[1] + vector.Normalize(opp.Width / 2.0); lpTmp[0] = lpTmp[0] - vector.Normalize(lane.Width / 2.0); LinePath lb = lpTmp.ShiftLateral(15.0); LinePath rb = lpTmp.ShiftLateral(-15.0); List<Coordinates> uturnPolyCOords = new List<Coordinates>(); uturnPolyCOords.AddRange(lb); uturnPolyCOords.AddRange(rb); uTurnState uts = new uTurnState(opp, Polygon.GrahamScan(uturnPolyCOords)); // reset blockages BlockageHandler.SetDefaultBlockageCooldown(); // reset the timers this.Reset(); // go to uturn return new Maneuver(uts.Resume(vehicleState, 2.0), uts, TurnDecorators.NoDecorators, vehicleState.Timestamp); #endregion } // uturn timer not enough else { // check timer running if(!uTurnTimer.IsRunning) { uTurnTimer.Stop(); uTurnTimer.Reset(); uTurnTimer.Start(); } // if gets here, need to wait double time = uTurnTimer.ElapsedMilliseconds / 1000.0; ArbiterOutput.Output("uTurn behavior evaluated to success, cooling down for: " + time.ToString("f2") + " out of 2"); waitForUTurn = true; return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } else { waitForUTurn = false; this.Reset(); } } else { waitForUTurn = false; this.Reset(); } #endregion #region Recovery Escalation // plan forward reasoning this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardManeuver(lane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>()); // check clear on right, or if it does nto exist here ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning; bool rightClear = !rightLateral.Exists || !rightLateral.ExistsExactlyHere(vehicleState) || rightLateral.AdjacentAndRearClear(vehicleState); // check clear on left, or if it does nto exist here ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning; bool leftClear = !leftLateral.Exists || !leftLateral.ExistsExactlyHere(vehicleState) || leftLateral.AdjacentAndRearClear(vehicleState); if(leftClear && leftLateral is OpposingLateralReasoning) leftClear = leftLateral.ForwardClear(vehicleState, TahoeParams.VL * 3.0, 2.24, new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null), lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), TahoeParams.VL * 3.0).Location); // check both clear to widen bool widenOk = rightClear && leftClear; // Notify ArbiterOutput.Output("Blockage tactical recovery escalation: rightClear: " + rightClear.ToString() + ", leftCler: " + leftClear.ToString()); // if we can't widen for some reason just go through with no widen StayInLaneBehavior silb = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, brs.EncounteredState); silb.TimeStamp = vehicleState.Timestamp; // check widen if (widenOk) { // output ArbiterOutput.Output("Lane Blockage Recovery: Adjacent areas clear"); // mini icrease width silb.LaneWidth = silb.LaneWidth + TahoeParams.T; // check execute none saudi CompletionReport l0Cr; bool l0TestOk = CoreCommon.Communications.TestExecute(silb, out l0Cr); // check mini ok if (l0TestOk) { // notify ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen ok"); // update the current state BlockageRecoveryState brsL0 = new BlockageRecoveryState( silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); return new Maneuver(silb, brsL0, TurnDecorators.NoDecorators, vehicleState.Timestamp); } else { // notify ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen failed, moving to large widen"); #region Change Lanes // check not in change lanes if (brs.Defcon != BlockageRecoveryDEFCON.CHANGELANES_FORWARD && brs.Defcon != BlockageRecoveryDEFCON.WIDENBOUNDS) { // check normal change lanes reasoning bool shouldWait; IState laneChangeCompletionState; ChangeLaneBehavior changeLanesBehavior = this.ChangeLanesRecoveryBehavior(lane, vehicleState, out shouldWait, out laneChangeCompletionState); // check change lanes behaviore exists if (changeLanesBehavior != null) { ArbiterOutput.Output("Lane Blockage Recovery: Found adjacent lane available change lanes beahvior: " + changeLanesBehavior.ToShortString() + ", " + changeLanesBehavior.ShortBehaviorInformation()); // update the current state BlockageRecoveryState brsCL = new BlockageRecoveryState( changeLanesBehavior, laneChangeCompletionState, sils, BlockageRecoveryDEFCON.CHANGELANES_FORWARD, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); return new Maneuver(changeLanesBehavior, brsCL, changeLanesBehavior.ChangeLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, vehicleState.Timestamp); } // check should wait else if (shouldWait) { ArbiterOutput.Output("Lane Blockage Recovery: Should wait for the forward lane, waiting"); return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } #endregion // notify ArbiterOutput.Output("Lane Blockage Recovery: Fell Through Forward Change Lanes"); // increase width silb.LaneWidth = silb.LaneWidth * 3.0; // check execute l1 CompletionReport l1Cr; bool l1TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr); // check ok if (l1TestOk) { // notify ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen ok"); // update the current state BlockageRecoveryState brsL1 = new BlockageRecoveryState( silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); return new Maneuver(silb, brsL1, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // go to l2 for all the marbles else { // notify ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen failed, moving to 3LW, L1 Saudi"); ShutUpAndDoItDecorator saudi2 = new ShutUpAndDoItDecorator(SAUDILevel.L1); List<BehaviorDecorator> saudi2bds = new List<BehaviorDecorator>(new BehaviorDecorator[] { saudi2 }); silb.Decorators = saudi2bds; BlockageRecoveryState brsL2 = new BlockageRecoveryState( silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); return new Maneuver(silb, brsL2, saudi2bds, vehicleState.Timestamp); } } } #endregion // fallout goes back to lane state return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); }
/// <summary> /// Reverse in the lane /// </summary> /// <param name="lane"></param> /// <param name="vehicleState"></param> /// <param name="vehicleSpeed"></param> /// <returns></returns> private StayInLaneBehavior LaneReverseRecovery(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, TrajectoryBlockedReport blockage) { // distance to reverse double distToReverse = double.IsNaN(blockage.DistanceToBlockage) ? TahoeParams.VL * 2.0 : TahoeParams.VL - blockage.DistanceToBlockage; if (distToReverse <= 3.0) distToReverse = TahoeParams.VL; // just reverse and let brian catch it StayInLaneBehavior reverseBehavior = new StayInLaneBehavior( lane.LaneId, new StopAtDistSpeedCommand(distToReverse, true), new List<int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true)); #region Start too close to start of lane // check distance to the start of the lane double laneStartDistanceFromFront = lane.DistanceBetween(lane.WaypointList[0].Position, vehicleState.Front); if (laneStartDistanceFromFront < TahoeParams.VL) { // make sure we're not at the very start of the lane if (laneStartDistanceFromFront < 0.5) { // do nothing return null; } // otherwise back up to the start of the lane else { // output ArbiterOutput.Output("Too close to the start of the lane, setting reverse distance to TP.VL"); // set proper distance distToReverse = TahoeParams.VL; // set behavior speed (no car behind us as too close to start of lane) LinePath bp = vehicleState.VehicleLinePath; reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true); StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0); return silb; } } #endregion #region Sparse // check distance to the start of the lane if (lane.GetClosestPartition(vehicleState.Front).Type == PartitionType.Sparse) { // set behavior speed (no car behind us as too close to start of lane) LinePath bp = vehicleState.VehicleLinePath; reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true); StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0); return silb; } #endregion #region Vehicle Behind us // rear monitor RearQuadrantMonitor rearMonitor = this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor; // update and check for vehicle rearMonitor.Update(vehicleState); if (rearMonitor.CurrentVehicle != null) { // check for not failed forward vehicle if (rearMonitor.CurrentVehicle.QueuingState.Queuing != QueuingState.Failed) { // check if enough room to rear vehicle double vehicleDistance = lane.DistanceBetween(rearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Rear); if (vehicleDistance < distToReverse - 2.0) { // revised distance given vehicle double revisedDistance = vehicleDistance - 2.0; // check enough room if (revisedDistance > TahoeParams.VL) { // set the updated speed command reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(revisedDistance, true); } // not enough room else { // output not enough room because of vehicle ArbiterOutput.Output("Blockage recovery, not enough room in rear because of rear vehicle, waiting for it to clear"); return null; } } } } #endregion // all clear, return reverse maneuver, set cooldown return reverseBehavior; }
/// <summary> /// Reverse because of a blockage /// </summary> /// <param name="lane"></param> /// <param name="vehicleState"></param> /// <param name="vehicleSpeed"></param> /// <param name="defcon"></param> /// <param name="saudi"></param> /// <param name="laneOpposing"></param> /// <param name="currentBlockage"></param> /// <param name="ebs"></param> /// <returns></returns> private Maneuver LaneReverseManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, BlockageRecoveryDEFCON defcon, SAUDILevel saudi, bool laneOpposing, ITacticalBlockage currentBlockage, EncounteredBlockageState ebs) { // distance to reverse double distToReverse = TahoeParams.VL * 2.0; // just reverse and let brian catch it StayInLaneBehavior reverseBehavior = new StayInLaneBehavior( lane.LaneId, new StopAtDistSpeedCommand(distToReverse, true), new List<int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, true), lane.NumberOfLanesRight(vehicleState.Front, true)); // get the saudi level List<BehaviorDecorator> decs = new List<BehaviorDecorator>(TurnDecorators.HazardDecorator.ToArray()); decs.Add(new ShutUpAndDoItDecorator(saudi)); reverseBehavior.Decorators = decs; // state IState laneState = new StayInLaneState(lane, CoreCommon.CorePlanningState); BlockageRecoveryState brs = new BlockageRecoveryState( reverseBehavior, laneState, laneState, defcon, ebs, BlockageRecoverySTATUS.EXECUTING); // check enough room in lane to reverse RearQuadrantMonitor rearMonitor = this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor; if (rearMonitor == null || !rearMonitor.lane.Equals(lane)) this.tacticalUmbrella.roadTactical.forwardReasoning.RearMonitor = new RearQuadrantMonitor(lane, SideObstacleSide.Driver); #region Start too close to start of lane // check distance to the start of the lane double laneStartDistanceFromFront = lane.DistanceBetween(lane.WaypointList[0].Position, vehicleState.Front); if (laneStartDistanceFromFront < TahoeParams.VL) { // make sure we're not at the very start of the lane if (laneStartDistanceFromFront < 0.5) { // output ArbiterOutput.Output("Too close to the start of the lane, raising defcon"); // go up chain return new Maneuver(new NullBehavior(), new EncounteredBlockageState(currentBlockage, laneState, BlockageRecoveryDEFCON.WIDENBOUNDS, saudi), TurnDecorators.NoDecorators, vehicleState.Timestamp); } // otherwise back up to the start of the lane else { // output ArbiterOutput.Output("Too close to the start of the lane, setting reverse distance to TP.VL"); // set proper distance distToReverse = TahoeParams.VL; // set behavior speed (no car behind us as too close to start of lane) LinePath bp = vehicleState.VehicleLinePath; reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true); StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0); return new Maneuver(silb, brs, decs, vehicleState.Timestamp); } } #endregion #region Sparse // check distance to the start of the lane if (lane.GetClosestPartition(vehicleState.Front).Type == PartitionType.Sparse) { // set behavior speed (no car behind us as too close to start of lane) LinePath bp = vehicleState.VehicleLinePath; reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(distToReverse, true); StayInLaneBehavior silb = new StayInLaneBehavior(null, reverseBehavior.SpeedCommand, new List<int>(), bp, lane.Width * 1.5, 0, 0); return new Maneuver(silb, brs, decs, vehicleState.Timestamp); } #endregion #region Vehicle Behind us // update and check for vehicle rearMonitor.Update(vehicleState); if (rearMonitor.CurrentVehicle != null) { // check for not failed forward vehicle if (rearMonitor.CurrentVehicle.QueuingState.Queuing != QueuingState.Failed) { // check if enough room to rear vehicle double vehicleDistance = lane.DistanceBetween(rearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Rear); if (vehicleDistance < distToReverse - 2.0) { // revised distance given vehicle double revisedDistance = vehicleDistance - 2.0; // check enough room if (revisedDistance > TahoeParams.VL) { // set the updated speed command reverseBehavior.SpeedCommand = new StopAtDistSpeedCommand(revisedDistance, true); } // not enough room else { // output not enough room because of vehicle ArbiterOutput.Output("Blockage recovery, not enough room in rear because of rear vehicle, waiting for it to clear"); return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } } // check failed rear vehicle, up defcon else { // failed vehicle in rear, go up chain return new Maneuver(new NullBehavior(), new EncounteredBlockageState(currentBlockage, laneState, BlockageRecoveryDEFCON.WIDENBOUNDS, saudi), TurnDecorators.NoDecorators, vehicleState.Timestamp); } } #endregion // all clear, return reverse maneuver, set cooldown BlockageHandler.SetDefaultBlockageCooldown(); return new Maneuver(reverseBehavior, brs, decs, vehicleState.Timestamp); }
/// <summary> /// Determine a change lanes recovery behavior /// </summary> /// <param name="current"></param> /// <param name="vs"></param> /// <param name="shouldWait"></param> /// <returns></returns> private ChangeLaneBehavior ChangeLanesRecoveryBehavior(ArbiterLane current, VehicleState vs, out bool shouldWait, out IState completionState) { // set defaults shouldWait = false; completionState = null; // check partition type to make sure normal if(current.GetClosestPartition(vs.Front).Type != PartitionType.Normal) return null; // make sure not in a safety zone foreach(ArbiterSafetyZone asz in current.SafetyZones) { if(asz.IsInSafety(vs.Front)) return null; } // check not inside an intersection safety zone foreach(ArbiterIntersection aInter in current.Way.Segment.RoadNetwork.ArbiterIntersections.Values) { if(aInter.IntersectionPolygon.IsInside(vs.Front) && (aInter.StoppedExits != null && aInter.StoppedExits.Count > 0)) return null; } // get the distance to the next lane major ArbiterWaypoint nextWaypoint = current.GetClosestPartition(vs.Front).Final; List<WaypointType> majorLaneTypes = new List<WaypointType>(new WaypointType[]{ WaypointType.End, WaypointType.Stop}); double distanceToNextMajor = current.DistanceBetween(vs.Front, current.GetNext(nextWaypoint, majorLaneTypes, new List<ArbiterWaypoint>()).Position); // check distance > 3.0 if(distanceToNextMajor > 30.0) { // check clear on right, or if it does not exist here ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning; bool useRight = rightLateral.Exists && rightLateral.ExistsExactlyHere(vs) && rightLateral is LateralReasoning; // check clear on left, or if it does not exist here ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning; bool useLeft = leftLateral.Exists && leftLateral.ExistsExactlyHere(vs) && leftLateral is LateralReasoning; // notify ArbiterOutput.Output("Blockage recovery: lane change left ok to use: " + useLeft.ToString() + ", use righrt: " + useRight.ToString()); #region Test Right // determine if should use, should wait, or should not use LaneChangeMonitorResult rightLCMR = LaneChangeMonitorResult.DontUse; // check usability of the right lateral maneuver if(useRight && rightLateral is LateralReasoning) { // check adj and rear clear bool adjRearClear = rightLateral.AdjacentAndRearClear(vs); // wait maybe if not clear if(!adjRearClear && (rightLateral.AdjacentVehicle == null || rightLateral.AdjacentVehicle.QueuingState.Queuing != QueuingState.Failed)) rightLCMR = LaneChangeMonitorResult.Wait; else if(adjRearClear) rightLCMR = LaneChangeMonitorResult.Use; else rightLCMR = LaneChangeMonitorResult.DontUse; // check down if (rightLCMR == LaneChangeMonitorResult.Wait) { ArbiterOutput.Output("Blockage lane changes waiting for right lateral lane to clear"); shouldWait = true; return null; } // check use else if (rightLCMR == LaneChangeMonitorResult.Use) { // get the behavior ChangeLaneBehavior rightClb = new ChangeLaneBehavior(current.LaneId, rightLateral.LateralLane.LaneId, false, TahoeParams.VL * 1.5, new ScalarSpeedCommand(2.24), new List<int>(), current.LanePath(), rightLateral.LateralLane.LanePath(), current.Width, rightLateral.LateralLane.Width, current.NumberOfLanesLeft(vs.Front, true), current.NumberOfLanesRight(vs.Front, true)); // test CompletionReport rightClbCr; bool successCL = CoreCommon.Communications.TestExecute(rightClb, out rightClbCr); if (successCL && rightClbCr is SuccessCompletionReport) { ArbiterOutput.Output("Blockage lane changes found good behavior to right lateral lane"); shouldWait = false; completionState = new StayInLaneState(rightLateral.LateralLane, CoreCommon.CorePlanningState); return rightClb; } } } #endregion #region Test Left Forwards if(useLeft && leftLateral is LateralReasoning) { // determine if should use, should wait, or should not use LaneChangeMonitorResult leftLCMR = LaneChangeMonitorResult.DontUse; // check usability of the left lateral maneuver if (useLeft && leftLateral is LateralReasoning) { // check adj and rear clear bool adjRearClear = leftLateral.AdjacentAndRearClear(vs); // wait maybe if not clear if (!adjRearClear && (leftLateral.AdjacentVehicle == null || leftLateral.AdjacentVehicle.QueuingState.Queuing != QueuingState.Failed)) leftLCMR = LaneChangeMonitorResult.Wait; else if (adjRearClear) leftLCMR = LaneChangeMonitorResult.Use; else leftLCMR = LaneChangeMonitorResult.DontUse; ArbiterOutput.Output("Blockage recovery, lane change left: " + leftLCMR.ToString()); // check down if (leftLCMR == LaneChangeMonitorResult.Wait) { ArbiterOutput.Output("Blockage recovery, Blockage lane changes waiting for left lateral lane to clear"); shouldWait = true; return null; } // check use else if (leftLCMR == LaneChangeMonitorResult.Use) { // get the behavior ChangeLaneBehavior leftClb = new ChangeLaneBehavior(current.LaneId, leftLateral.LateralLane.LaneId, false, TahoeParams.VL * 1.5, new ScalarSpeedCommand(2.24), new List<int>(), current.LanePath(), leftLateral.LateralLane.LanePath(), current.Width, leftLateral.LateralLane.Width, current.NumberOfLanesLeft(vs.Front, true), current.NumberOfLanesLeft(vs.Front, true)); // test CompletionReport leftClbCr; bool successCL = CoreCommon.Communications.TestExecute(leftClb, out leftClbCr); if (successCL && leftClbCr is SuccessCompletionReport) { ArbiterOutput.Output("Blockage recovery, Blockage lane changes found good behavior to left lateral lane"); completionState = new StayInLaneState(leftLateral.LateralLane, CoreCommon.CorePlanningState); shouldWait = false; return leftClb; } } } } #endregion } // fallout return null return null; }
/// <summary> /// Determines proper speed commands given we want to stop at a certain waypoint /// </summary> /// <param name="waypoint"></param> /// <param name="lane"></param> /// <param name="position"></param> /// <param name="enCovariance"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public void StoppingParams(ArbiterWaypoint waypoint, ArbiterLane lane, Coordinates position, double extraDistance, out double stopSpeed, out double stopDistance) { // get dist to waypoint stopDistance = lane.DistanceBetween(position, waypoint.Position) - extraDistance; // speed tools stopSpeed = SpeedTools.GenerateSpeed(stopDistance, CoreCommon.OperationalStopSpeed, 2.24); }
/// <summary> /// Gets the next navigational stop relavant to us (stop or end) in the closest good lane or our current opposing lane /// </summary> /// <param name="closestGood"></param> /// <param name="coordinates"></param> /// <param name="ignorable"></param> /// <param name="navStopSpeed"></param> /// <param name="navStopDistance"></param> /// <param name="navStopType"></param> /// <param name="navStop"></param> private void NextOpposingNavigationalStop(ArbiterLane opposing, ArbiterLane closestGood, Coordinates coordinates, double extraDistance, out double navStopSpeed, out double navStopDistance, out StopType navStopType, out ArbiterWaypoint navStop) { ArbiterWaypoint current = null; double minDist = Double.MaxValue; StopType st = StopType.EndOfLane; #region Closest Good Parameterization foreach (ArbiterWaypoint aw in closestGood.WaypointList) { if (aw.IsStop || aw.NextPartition == null) { double dist = closestGood.DistanceBetween(coordinates, aw.Position); if (dist < minDist && dist >= 0) { current = aw; minDist = dist; st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane; } } } #endregion #region Opposing Parameterization ArbiterWaypoint opStart = opposing.GetClosestPartition(coordinates).Initial; int startIndex = opposing.WaypointList.IndexOf(opStart); for (int i = startIndex; i >= 0; i--) { ArbiterWaypoint aw = opposing.WaypointList[i]; if (aw.IsStop || aw.PreviousPartition == null) { double dist = opposing.DistanceBetween(aw.Position, coordinates); if (dist < minDist && dist >= 0) { current = aw; minDist = dist; st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane; } } } #endregion double tmpDistanceIgnore; this.StoppingParams(current, closestGood, coordinates, extraDistance, out navStopSpeed, out tmpDistanceIgnore); navStop = current; navStopDistance = minDist; navStopType = st; }
/// <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> /// Generates the lane change parameterization /// </summary> /// <param name="information"></param> /// <param name="initial"></param> /// <param name="final"></param> /// <param name="goal"></param> /// <param name="departUpperBound"></param> /// <param name="defaultReturnLowerBound"></param> /// <param name="minimumReturnComplete"></param> /// <param name="defaultReturnUpperBound"></param> /// <param name="blockages"></param> /// <param name="ignorable"></param> /// <param name="state"></param> /// <param name="speed"></param> /// <returns></returns> public LaneChangeParameters? LaneChangeParameterization(LaneChangeInformation information, ArbiterLane initial, ArbiterLane target, bool targetOncoming, Coordinates goal, Coordinates departUpperBound, Coordinates defaultReturnLowerBound, Coordinates minimumReturnComplete, Coordinates defaultReturnUpperBound, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, VehicleState state, double speed) { // check if the target lane exists here bool validTarget = target.LanePath().GetClosestPoint(state.Front).Location.DistanceTo(state.Front) < 17 && target.IsInside(state.Front); // params bool toLeft = initial.LaneOnLeft != null ? initial.LaneOnLeft.Equals(target) || (targetOncoming && !initial.Way.WayId.Equals(target.Way.WayId)) : false; // get appropriate lateral reasoning ILateralReasoning lateralReasoning = toLeft ? this.leftLateralReasoning : this.rightLateralReasoning; #region Target Lane Valid Here // check if the target is currently valid if (validTarget) { // lane change parameterizations List<LaneChangeParameters> lcps = new List<LaneChangeParameters>(); // distance to the current goal (means different things for all) double xGoal = initial.DistanceBetween(state.Front, goal); // get next stop List<WaypointType> types = new List<WaypointType>(); types.Add(WaypointType.Stop); types.Add(WaypointType.End); ArbiterWaypoint nextMajor = initial.GetNext(state.Front, types, ignorable); double xLaneMajor = initial.DistanceBetween(state.Front, nextMajor.Position); xGoal = Math.Min(xGoal, xLaneMajor); #region Failed Vehicle Lane Change if (information.Reason == LaneChangeReason.FailedForwardVehicle) { #region Target Opposing // check if target lane backwards if (targetOncoming) { // available and feasible bool avail = false; bool feas = false; // check if min return distance < goal distance double xReturnMin = initial.DistanceBetween(state.Front, minimumReturnComplete); double xDepart = initial.DistanceBetween(state.Front, departUpperBound); // dist to upper bound along lane and dist to end of adjacent lane double adjLaneDist = initial.DistanceBetween(state.Front, minimumReturnComplete); // this is feasible feas = xGoal > xReturnMin ? true : false; // check if not feasible that the goal is the current checkpoint if (!feas && CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId].Position.Equals(goal)) feas = true; // check adj and rear clear bool adjRearClear = lateralReasoning.AdjacentAndRearClear(state); // check if forwards clear bool frontClear = lateralReasoning.ForwardClear(state, xReturnMin, 2.24, information, minimumReturnComplete); Console.WriteLine("Adjacent, Rear: " + adjRearClear.ToString() + ", Forward: " + frontClear.ToString()); // if clear if (frontClear && adjRearClear) { // notify ArbiterOutput.Output("Lane Change Params: Target Oncoming Failed Vehicle: Adjacent, Rear, and Front Clear"); // available avail = true; // get lateral parameterization TravelingParameters lateralParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state)); // change into the opposing lane wih opposing forward parameterization LaneChangeParameters lcp = new LaneChangeParameters(avail, feas, initial, false, target, targetOncoming, toLeft, null, xDepart, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, lateralParams, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, information.Reason); // we have been forced lcp.ForcedOpposing = true; // return created params return lcp; } // fell through for some reason, return parameterization explaining why LaneChangeParameters fallThroughParams = new LaneChangeParameters(avail, feas, initial, false, target, targetOncoming, toLeft, null, xDepart, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, this.ForwardMonitor.LaneParameters, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, information.Reason); // return fall through parameters return fallThroughParams; } #endregion #region Target Forwards // otherwise target lane forwards else { // check lateral clear and initial lane does not run out if (lateralReasoning.AdjacentAndRearClear(state) && !initial.GetClosestPoint(defaultReturnUpperBound).Equals(initial.LanePath().EndPoint)) { // notify ArbiterOutput.Output("Lane Change Params: Failed Vehicle Target Forwards: Adjacent and Rear Clear"); // dist to upper bound along lane and dist to end of adjacent lane double distToReturn = initial.DistanceBetween(state.Front, defaultReturnUpperBound); double adjLaneDist = initial.DistanceBetween(state.Front, target.LanePath().EndPoint.Location); // check enough lane room to pass if (distToReturn < adjLaneDist && distToReturn <= initial.DistanceBetween(state.Front, goal)) { // check enough room to change lanes ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable); double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position); // check dist needed to complete double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // check return dist if (distToReturn < xTargetLaneMajor && neededDistance <= xTargetLaneMajor) { // parameters for traveling in current lane to hit other List<TravelingParameters> tps = new List<TravelingParameters>(); // update lateral ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.Update(target, state); // check lateral reasoning for forward vehicle badness if (!((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker || !((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped || initial.DistanceBetween(state.Front, ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) >= distToReturn) { // get parameterization for lateral lane TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state)); tps.Add(navParams); // get and add the vehicle parameterization for our lane if (this.ForwardMonitor.FollowingParameters.HasValue) tps.Add(this.ForwardMonitor.FollowingParameters.Value); // get final tps.Sort(); TravelingParameters final = tps[0]; // check final distance > needed if (navParams.DistanceToGo > neededDistance) { // set ignorable vhcs final.VehiclesToIgnore = this.ForwardMonitor.FollowingParameters.HasValue ? this.ForwardMonitor.FollowingParameters.Value.VehiclesToIgnore : new List<int>(); if (((LateralReasoning)lateralReasoning).ForwardMonitor.FollowingParameters.HasValue) final.VehiclesToIgnore.AddRange(((LateralReasoning)lateralReasoning).ForwardMonitor.FollowingParameters.Value.VehiclesToIgnore); // parameterize LaneChangeParameters lcp = new LaneChangeParameters(); lcp.Decorators = TurnDecorators.RightTurnDecorator; lcp.Available = true; lcp.Feasible = true; lcp.Parameters = final; lcp.Initial = initial; lcp.InitialOncoming = false; lcp.Target = target; lcp.TargetOncoming = false; lcp.Reason = LaneChangeReason.FailedForwardVehicle; lcp.DefaultReturnLowerBound = defaultReturnLowerBound; lcp.DefaultReturnUpperBound = defaultReturnUpperBound; lcp.DepartUpperBound = departUpperBound; lcp.MinimumReturnComplete = minimumReturnComplete; return lcp; } } } } } // otherwise infeasible return null; } #endregion } #endregion #region Navigation Lane Change else if (information.Reason == LaneChangeReason.Navigation) { // parameters for traveling in current lane to hit other List<TravelingParameters> tps = new List<TravelingParameters>(); // get navigation parameterization TravelingParameters lateralParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state)); tps.Add(lateralParams); // get and add the nav parameterization relative to our lane tps.Add(this.ForwardMonitor.NavigationParameters); // check avail bool adjRearAvailable = lateralReasoning.AdjacentAndRearClear(state); // if they are available we are in good shape, need to slow for nav, forward vehicles if (adjRearAvailable) { // notify ArbiterOutput.Output("Lane Change Params: Navigation: Adjacent and Rear Clear"); #region Check Forward and Lateral Vehicles if (this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle && lateralParams.Type == TravellingType.Vehicle) { // check enough room to change lanes ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable); double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position); // distnace to goal double goalDist = initial.DistanceBetween(state.Front, goal); // check dist needed to complete double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // check for proper distances if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance) { // check distance to return (weeds out safety zone crap Coordinates lateralVehicle = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition; double distToReturn = initial.DistanceBetween(state.Front, initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(lateralVehicle), 30.0).Location); // check passing params LaneChangeInformation lci; bool shouldPass = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.ShouldPass(out lci); // check passing params LaneChangeInformation lciInit; bool shouldPassInit = this.ForwardMonitor.ForwardVehicle.ShouldPass(out lciInit); // check forward lateral stopped and enough distance to go around and not vehicles between it and goal close enough to stop if(shouldPass && lci.Reason == LaneChangeReason.FailedForwardVehicle && goalDist > distToReturn && (!shouldPassInit || lciInit.Reason != LaneChangeReason.FailedForwardVehicle || this.ForwardMonitor.CurrentParameters.DistanceToGo > lateralParams.DistanceToGo + TahoeParams.VL * 5)) { // return that we should pass it as normal in the initial lane return null; } // check get distance to upper double xUpper = this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ? Math.Min(goalDist, neededDistance) : this.ForwardMonitor.ForwardVehicle.ForwardControl.xSeparation - 2; Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), xUpper).Location; // add current params if not stopped if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) tps.Add(this.ForwardMonitor.CurrentParameters); // get final tps.Sort(); TravelingParameters final = tps[0]; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft, null, final.DistanceToGo - 3.0, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, final, upper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation); return lcp; } } #endregion #region Check Forward Vehicle else if (this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle) { // check enough room to change lanes ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable); double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position); // distnace to goal double goalDist = initial.DistanceBetween(state.Front, goal); // check dist needed to complete double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // check for proper distances if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance) { // add current params if not stopped if(!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) tps.Add(this.ForwardMonitor.CurrentParameters); // get final tps.Sort(); TravelingParameters final = tps[0]; // check get distance to upper double xUpper = this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ? neededDistance : this.ForwardMonitor.ForwardVehicle.ForwardControl.xSeparation - 2; Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), xUpper).Location; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft, null, final.DistanceToGo - 3.0, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, final, upper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation); return lcp; } } #endregion #region Lateral Vehicle // check to see if should use the forward tracker, i.e. forward vehicle exists else if (lateralParams.Type == TravellingType.Vehicle) { // add current params tps.Add(this.ForwardMonitor.CurrentParameters); // check enough room to change lanes ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable); double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position); // distnace to goal double goalDist = initial.DistanceBetween(state.Front, goal); // check dist needed to complete double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // check for proper distances if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance) { // check distance to return (weeds out safety zone crap Coordinates lateralVehicle = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition; double distToReturn = initial.DistanceBetween(state.Front, initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(lateralVehicle), 30.0).Location); // check passing params LaneChangeInformation lci; bool shouldPass = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.ShouldPass(out lci); // check forward lateral stopped and enough distance to go around and not vehicles between it and goal close enough to stop if (shouldPass && lci.Reason == LaneChangeReason.FailedForwardVehicle && goalDist > distToReturn) { // return that we should pass it as normal in the initial lane return null; } // check if we are already slowed for this vehicle and are at a good distance from it if (speed < lateralParams.RecommendedSpeed + 1.0) { // get final tps.Sort(); TravelingParameters final = tps[0]; // upper bound is nav bound Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), Math.Min(neededDistance, final.DistanceToGo)).Location; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft, null, final.DistanceToGo - 3.0, null, TurnDecorators.LeftTurnDecorator, final, upper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation); return lcp; } // otherwise need to slow for it or other else { // get final tps.Sort(); TravelingParameters final = tps[0]; // upper bound is nav bound Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), final.DistanceToGo).Location; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(false, true, initial, false, target, false, toLeft, null, final.DistanceToGo - 3.0, null, TurnDecorators.LeftTurnDecorator, final, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation); return lcp; } } } #endregion #region No forward or lateral // otherwise just go! else { // add current params tps.Add(this.ForwardMonitor.CurrentParameters); // check enough room to change lanes ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable); double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position); // distnace to goal double goalDist = initial.DistanceBetween(state.Front, goal); // check dist needed to complete double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // check for proper distances if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance) { // get final tps.Sort(); TravelingParameters final = tps[0]; // upper bound is nav bound Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), Math.Min(neededDistance, final.DistanceToGo)).Location; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft, null, final.DistanceToGo, null, TurnDecorators.LeftTurnDecorator, final, upper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation); // return the parameterization return lcp; } } #endregion } // otherwise we need to determine how to make this available else { // notify ArbiterOutput.Output("Lane Change Params: Navigation Adjacent and Rear NOT Clear"); // gets the adjacent vehicle VehicleAgent adjacent = lateralReasoning.AdjacentVehicle; // add current params tps.Add(this.ForwardMonitor.CurrentParameters); #region Pass Adjacent // checks if it is failed for us to pass it if (adjacent != null && (adjacent.IsStopped || adjacent.Speed < 1.5)) { // get final List<TravelingParameters> adjacentTravelingParams = new List<TravelingParameters>(); adjacentTravelingParams.Add(this.ForwardMonitor.CurrentParameters); adjacentTravelingParams.Add(this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, null)); adjacentTravelingParams.Sort(); //tps.Sort(); TravelingParameters final = adjacentTravelingParams[0];// tps[0]; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(); lcp.Available = false; lcp.Feasible = true; lcp.Parameters = final; return lcp; } #endregion #region Follow Adjacent // otherwise we need to follow it, as it is lateral, this means 0.0 speed else if (adjacent != null) { // get and add the vehicle parameterization relative to our lane TravelingParameters tmp = new TravelingParameters(); tmp.Behavior = new StayInLaneBehavior(initial.LaneId, new ScalarSpeedCommand(0.0), this.ForwardMonitor.CurrentParameters.VehiclesToIgnore, initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(state.Front, true), initial.NumberOfLanesRight(state.Front, true)); tmp.NextState = CoreCommon.CorePlanningState; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(); lcp.Available = false; lcp.Feasible = true; lcp.Parameters = tmp; return lcp; } #endregion #region Wait for the rear vehicle else { TravelingParameters tp = new TravelingParameters(); tp.SpeedCommand = new ScalarSpeedCommand(0.0); tp.UsingSpeed = true; tp.DistanceToGo = 0.0; tp.VehiclesToIgnore = new List<int>(); tp.RecommendedSpeed = 0.0; tp.NextState = CoreCommon.CorePlanningState; tp.Behavior = new StayInLaneBehavior(initial.LaneId, tp.SpeedCommand, new List<int>(), initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(state.Front, true), initial.NumberOfLanesRight(state.Front, true)); // parameterize LaneChangeParameters lcp = new LaneChangeParameters(); lcp.Available = false; lcp.Feasible = true; lcp.Parameters = tp; return lcp; } #endregion } } #endregion #region Passing Lane Change else if (information.Reason == LaneChangeReason.SlowForwardVehicle) { throw new Exception("passing slow vehicles not yet supported"); } #endregion // fallout returns null return null; } #endregion #region Target Lane Not Valid, Plan Navigation // otherwise plan for when we approach target if this is a navigational change else if(information.Reason == LaneChangeReason.Navigation) { // parameters for traveling in current lane to hit other List<TravelingParameters> tps = new List<TravelingParameters>(); // add current params tps.Add(this.ForwardMonitor.CurrentParameters); // distance between front of car and start of lane if (target.RelativelyInside(state.Front) || initial.DistanceBetween(state.Front, target.LanePath().StartPoint.Location) > 0) { #region Vehicle and Navigation // check to see if we're not looped around wierd if (lateralReasoning.LateralLane.LanePath().GetClosestPoint(state.Front).Equals(lateralReasoning.LateralLane.LanePath().StartPoint)) { // initialize the forward tracker in the other lane ForwardVehicleTracker fvt = new ForwardVehicleTracker(); fvt.Update(lateralReasoning.LateralLane, state); // check to see if should use the forward tracker if (fvt.ShouldUseForwardTracker) { // get navigation parameterization TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state)); tps.Add(navParams); } else { // get navigation parameterization TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, null); tps.Add(navParams); } } #endregion #region Navigation // check to see that nav point is downstream of us else if (initial.DistanceBetween(state.Front, goal) > 0.0) { // get navigation parameterization TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, null); tps.Add(navParams); } #endregion else { return null; } } else return null; // get final tps.Sort(); TravelingParameters final = tps[0]; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(); lcp.Available = false; lcp.Feasible = true; lcp.Parameters = final; return lcp; } #endregion // fallout return null return null; }
/// <summary> /// Secondary maneuver outside minimum cap /// </summary> /// <param name="arbiterLane"></param> /// <param name="vehicleState"></param> /// <param name="roadPlan"></param> /// <param name="blockages"></param> /// <param name="ignorable"></param> /// <returns></returns> public Maneuver? AdvancedSecondaryOutsideMinCap(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask) { // get proper reasoning component List<LateralReasoning> adjacentReasonings = new List<LateralReasoning>(); if (bestTask != TypeOfTasks.Left) { if (this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists && this.rightLateralReasoning.ExistsExactlyHere(vehicleState)) adjacentReasonings.Add((LateralReasoning)this.rightLateralReasoning); else if (this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists && this.leftLateralReasoning.ExistsExactlyHere(vehicleState)) adjacentReasonings.Add((LateralReasoning)this.leftLateralReasoning); } else { if (this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists && this.leftLateralReasoning.ExistsExactlyHere(vehicleState)) adjacentReasonings.Add((LateralReasoning)this.leftLateralReasoning); else if (this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists && this.rightLateralReasoning.ExistsExactlyHere(vehicleState)) adjacentReasonings.Add((LateralReasoning)this.rightLateralReasoning); } // loop through possible foreach(LateralReasoning adjacentReasoning in adjacentReasonings) { // check if adjacent reasoning exists if (adjacentReasoning != null && adjacentReasoning.Exists) { // update adjacent reasoning adjacentReasoning.ForwardMonitor.Primary(adjacentReasoning.LateralLane, vehicleState, roadPlan, blockages, ignorable, false); // otherwise check if forward vehicle slow LaneChangeInformation forwardVehicleSecondary; // check if should pass the forward vehicle if (this.ForwardMonitor.ForwardVehicle.ShouldPass(out forwardVehicleSecondary, lane)) { // check if forward vehicle failed if (forwardVehicleSecondary.Reason == LaneChangeReason.FailedForwardVehicle) { ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " failed"); // make sure the failed vehicle is not within 50m of the goal double vehicleDistanceToGoal = lane.DistanceBetween(ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, laneGoal.Position); ArbiterOutput.Output("Failed FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", DistGoal: " + vehicleDistanceToGoal.ToString("f2") + ", speed: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f1")); if ((vehicleDistanceToGoal > 50)) { // check if adjacent has no forward vehicle if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker) { // plan the lane change return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, forwardVehicleSecondary); } // otherwise a forward vehicle exists else { // otherwise check if forward vehicle slow LaneChangeInformation lateralVehicleInformation; // check if lateral vehicle fine if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldPass(out lateralVehicleInformation, adjacentReasoning.LateralLane)) { // plan the lane change return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle)); } // check if lateral vehicle failed or slow else { // check distance to lateral > distance to forward + 25 double distToAdjacent = lane.DistanceBetween(vehicleState.Front, adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition); double distToForward = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition); if (distToAdjacent > distToForward + 25.0) { // plan the lane change return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle)); } } } } } // otherwise check if they are slow else if (forwardVehicleSecondary.Reason == LaneChangeReason.SlowForwardVehicle) { ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " slow"); // make sure the slow vehicle is not within 50m of the goal if velocity is > 5mph double vehicleDistanceToGoal = lane.DistanceBetween(ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, laneGoal.Position); ArbiterOutput.Output("Slow FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", DistGoal: " + vehicleDistanceToGoal.ToString("f2") + ", speed: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f1")); if ((vehicleDistanceToGoal > 50 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 2.24) || (vehicleDistanceToGoal > 75 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 4.48) || (vehicleDistanceToGoal > 100 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 6.72) || (vehicleDistanceToGoal > 125 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 8.96)) { // check if adjacent has no forward vehicle if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker) { // plan the lane change ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: No vehicle in adjacent"); return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle)); } // otherwise a forward vehicle exists else { // otherwise check if forward vehicle slow LaneChangeInformation lateralVehicleInformation; // check if lateral vehicle fine if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldPass(out lateralVehicleInformation, adjacentReasoning.LateralLane)) { // check distance to lateral > distance to forward + 25 double distToAdjacent = lane.DistanceBetween(vehicleState.Front, adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition); double distToForward = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition); ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: Normal vehicle in adjacent: " + adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + "FV Dist: " + distToForward.ToString("f2") + ", Adj Dist: " + adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString()); // check distance greater and adjacent speed greater than forward speed if (distToAdjacent > distToForward + 25.0 && adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed > this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed) { // plan the lane change return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle)); } // plan the lane change //return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle)); } else if ( lane.DistanceBetween(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) > 65.0 && (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped || (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed + 4.48 < adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed))) { // plan the lane change return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle)); } } } } } } } // normal secondary parameterization as a fall through on the naviagation lane changes return this.SecondaryManeuver(lane, vehicleState, roadPlan, blockages, ignorable, bestTask); }
/// <summary> /// Secondary maneuver when current lane is the desired lane /// </summary> /// <param name="arbiterLane"></param> /// <param name="vehicleState"></param> /// <param name="roadPlan"></param> /// <param name="blockages"></param> /// <param name="ignorable"></param> /// <returns></returns> public Maneuver? AdvancedSecondaryNextCheck(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask) { // check if the forward vehicle exists if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.PassedLongDelayedBirth) { // distance to forward vehicle double distanceToForwardVehicle = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition); #region Distance From Forward Not Enough // check distance to forward vehicle if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed && distanceToForwardVehicle < 8.0 && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle) { // set name ArbiterLane al = lane; // distance to forward vehicle too small double distToForwards = al.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition); double distToReverse = Math.Max(1.0, 8.0 - distToForwards); if (distToForwards < 8.0) { // notify ArbiterOutput.Output("Secondary: NOT Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " distance: " + distToForwards.ToString("f2")); this.RearMonitor = new RearQuadrantMonitor(al, SideObstacleSide.Driver); this.RearMonitor.Update(vehicleState); if (this.RearMonitor.CurrentVehicle != null) { double distToRearVehicle = al.DistanceBetween(this.RearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Position) - TahoeParams.RL; double distNeedClear = distToReverse + 2.0; if (distToRearVehicle < distNeedClear) { // notify ArbiterOutput.Output("Secondary: Rear: Not enough room to clear in rear: " + distToRearVehicle.ToString("f2") + " < " + distNeedClear.ToString("f2")); return null; } } double distToLaneStart = al.DistanceBetween(al.LanePath().StartPoint.Location, vehicleState.Position) - TahoeParams.RL; if (distToReverse > distToLaneStart) { // notify ArbiterOutput.Output("Secondary: Rear: Not enough room in lane to reverse in rear: " + distToLaneStart.ToString("f2") + " < " + distToReverse.ToString("f2")); return null; } else { // notify ArbiterOutput.Output("Secondary: Reversing to pass Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " reversing distance: " + distToReverse.ToString("f2")); StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(distToReverse, true); StayInLaneBehavior silb = new StayInLaneBehavior(al.LaneId, sadsc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true)); return new Maneuver(silb, CoreCommon.CorePlanningState, TurnDecorators.HazardDecorator, vehicleState.Timestamp); } } } #endregion // get distance to next lane major List<WaypointType> wts = new List<WaypointType>(new WaypointType[] { WaypointType.Stop, WaypointType.End }); ArbiterWaypoint nextWaypoint = lane.GetNext(lane.GetClosestPartition(vehicleState.Position).Final, wts, new List<ArbiterWaypoint>()); // check if the vehicle occurs before the next major thing double distanceToNextMajor = lane.DistanceBetween(vehicleState.Front, nextWaypoint.Position); // check if vehicle occurs before the next lane major if (distanceToForwardVehicle < distanceToNextMajor) { // check if forward vehicle exists in this lane and is closer then the lane goal if (TacticalDirector.VehicleAreas.ContainsKey(lane) && TacticalDirector.VehicleAreas[lane].Contains(this.ForwardMonitor.ForwardVehicle.CurrentVehicle) && lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) < lane.DistanceBetween(vehicleState.Front, laneGoal.Position)) { // some constants double desiredDistance = 50.0; // get the lane goal distance double distanceToLaneGoal = lane.DistanceBetween(vehicleState.Front, laneGoal.Position); // check if necessary to be in current lane if (distanceToLaneGoal <= desiredDistance) { // default secondary maneuver ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal < 50: " + desiredDistance.ToString("f2")); return this.SecondaryManeuver(lane, vehicleState, roadPlan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), bestTask); } // otherwise would be good but not necessary else { // return the maneuver from the vehicle being outside the min cap ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal > 50: " + desiredDistance.ToString("f2")); return this.AdvancedSecondaryOutsideMinCap(lane, laneGoal, vehicleState, roadPlan, blockages, ignorable, bestTask); } } } } // no secondary return null; }
/// <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> /// Updates the current vehicle /// </summary> /// <param name="lane"></param> /// <param name="state"></param> public void Update(ArbiterLane lane, VehicleState state) { // get the forward path LinePath p = lane.LanePath().Clone(); p.Reverse(); // get our position Coordinates f = state.Front; // get all vehicles associated with those components List<VehicleAgent> vas = new List<VehicleAgent>(); foreach (IVehicleArea iva in lane.AreaComponents) { if (TacticalDirector.VehicleAreas.ContainsKey(iva)) vas.AddRange(TacticalDirector.VehicleAreas[iva]); } // get the closest forward of us double minDistance = Double.MaxValue; VehicleAgent closest = null; // get clsoest foreach (VehicleAgent va in vas) { // get position of front Coordinates frontPos = va.ClosestPosition; // gets distance from other vehicle to us along the lane double frontDist = lane.DistanceBetween(frontPos, f); if (frontDist >= 0 && frontDist < minDistance) { minDistance = frontDist; closest = va; } } this.CurrentVehicle = closest; this.currentDistance = minDistance; }
/// <summary> /// Behavior we would like to do other than going straight /// </summary> /// <param name="arbiterLane"></param> /// <param name="vehicleState"></param> /// <param name="p"></param> /// <param name="blockages"></param> /// <returns></returns> /// <remarks>tries to go right, if not goest left if needs /// to if forward vehicle ahead and we're stopped because of them</remarks> public Maneuver? SecondaryManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, List<ITacticalBlockage> blockages, LaneChangeParameters? entryParameters) { // check blockages if (blockages != null && blockages.Count > 0 && blockages[0] is OpposingLaneBlockage) { // create the blockage state EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState); // go to a blockage handling tactical return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp); } // check dist needed to complete double neededDistance = (Math.Abs(arbiterLane.LaneId.Number - closestGood.LaneId.Number) * 1.5 * TahoeParams.VL) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // get upper bound LinePath.PointOnPath xFront = arbiterLane.LanePath().GetClosestPoint(vehicleState.Front); Coordinates xUpper = arbiterLane.LanePath().AdvancePoint(xFront, -neededDistance).Location; if (entryParameters.HasValue) { // check if we should get back, keep speed nice n' lowi fpassing failed if (entryParameters.Value.Reason == LaneChangeReason.FailedForwardVehicle) { double xToReturn = arbiterLane.DistanceBetween(entryParameters.Value.DefaultReturnLowerBound, vehicleState.Front); if(xToReturn >= 0.0) ArbiterOutput.Output("Distance until must return to lane: " + xToReturn); else ArbiterOutput.Output("Can return to lane from arbitrary upper bound: " + xToReturn); // check can return if (xToReturn < 0) { // check if right lateral exists exactly here if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood)) { ArbiterOutput.Output("Right lateral reasoning good and exists exactly here"); return this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true); } else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood)) { ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here"); if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood)) this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger); if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState)) { ILateralReasoning tmpReasoning = this.rightLateralReasoning; this.rightLateralReasoning = this.secondaryLateralReasoning; Maneuver? tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true); this.rightLateralReasoning = tmpReasoning; return tmp; } else { ArbiterOutput.Output("Cosest good lane does not exist here??"); return null; } } else { ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() + ", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString()); return null; } } else return null; } } // lane change info LaneChangeInformation lci = new LaneChangeInformation(LaneChangeReason.Navigation, null); // notify ArbiterOutput.Output("In Opposing with no Previous state knowledge, attempting to return"); // check if right lateral exists exactly here if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood)) { ArbiterOutput.Output("Right lateral reasoning good and exists exactly here"); return this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false); } else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood)) { ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here"); if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood)) this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger); if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState)) { ILateralReasoning tmpReasoning = this.rightLateralReasoning; this.rightLateralReasoning = this.secondaryLateralReasoning; Maneuver? tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false); this.rightLateralReasoning = tmpReasoning; return tmp; } else { ArbiterOutput.Output("Cosest good lane does not exist here??"); return null; } } else { ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() + ", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString()); return null; } }