/// <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 OpposingLaneRecoveryManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed, INavigationalPlan plan, BlockageRecoveryState brs, bool failedVehiclesPersistentlyOnly) { // get the blockage ITacticalBlockage laneBlockageReport = brs.EncounteredState.TacticalBlockage; // get the turn state OpposingLanesState sils = (OpposingLanesState)brs.EncounteredState.PlanningState; #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"); // path LinePath revPath = lane.LanePath().Clone(); revPath.Reverse(); // dist to reverse double distToReverse; if (double.IsNaN(laneBlockageReport.BlockageReport.DistanceToBlockage)) distToReverse = TahoeParams.VL * 1.5; else if (laneBlockageReport.BlockageReport.DistanceToBlockage < 0 || laneBlockageReport.BlockageReport.DistanceToBlockage > 2 * TahoeParams.VL) distToReverse = TahoeParams.VL * 1.5; else distToReverse = TahoeParams.VL - laneBlockageReport.BlockageReport.DistanceToBlockage; // get reverse behavior StayInLaneBehavior reverseRecovery = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List<int>(), revPath, lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false)); // get recovery state BlockageRecoveryState brsT = new BlockageRecoveryState( reverseRecovery, CoreCommon.CorePlanningState, new OpposingLanesState(lane, false, CoreCommon.CorePlanningState, vehicleState), 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 Recovery Escalation // plan forward reasoning this.tacticalUmbrella.roadTactical.opposingReasoning.ForwardManeuver(lane, ((OpposingLanesState)brs.EncounteredState.PlanningState).ClosestGoodLane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>()); // 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() + ", leftClear: " + leftClear.ToString()); // path LinePath revPath2 = lane.LanePath().Clone(); revPath2.Reverse(); // check widen if (widenOk) { // output ArbiterOutput.Output("Lane Blockage Recovery: Adjacent areas clear"); if (brs.Defcon != BlockageRecoveryDEFCON.CHANGELANES_FORWARD) { // check change lanes back to good ChangeLaneBehavior clb = new ChangeLaneBehavior( lane.LaneId, sils.ClosestGoodLane.LaneId, false, TahoeParams.VL * 3.0, new StopAtDistSpeedCommand(TahoeParams.VL * 3.0), new List<int>(), revPath2, sils.ClosestGoodLane.LanePath(), lane.Width, sils.ClosestGoodLane.Width, lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false)); // change lanes brs.Defcon = BlockageRecoveryDEFCON.CHANGELANES_FORWARD; // cehck ok CompletionReport clROk; bool clCheck = CoreCommon.Communications.TestExecute(clb, out clROk); // check change lanes ok if (clCheck) { ArbiterOutput.Output("Change lanes behaviro ok, executing"); // return manevuer return new Maneuver(clb, brs, TurnDecorators.RightTurnDecorator, vehicleState.Timestamp); } else { ArbiterOutput.Output("Change lanes behaviro not ok"); } } // if we can't widen for some reason just go through with no widen StayInLaneBehavior silb = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(TahoeParams.VL * 2.0), new List<int>(), revPath2, lane.Width, lane.NumberOfLanesLeft(vehicleState.Front, false), lane.NumberOfLanesRight(vehicleState.Front, false)); // 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"); // 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); // check execute l1 CompletionReport l2Cr; bool l2TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr); // go 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> /// Plan a lane change /// </summary> /// <param name="cls"></param> /// <param name="initialManeuver"></param> /// <param name="targetManeuver"></param> /// <returns></returns> public Maneuver PlanLaneChange(ChangeLanesState cls, VehicleState vehicleState, RoadPlan roadPlan, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable) { // check blockages if (blockages != null && blockages.Count > 0 && blockages[0] is LaneChangeBlockage) { // 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); } // lanes of the lane change ArbiterLane initial = cls.Parameters.Initial; ArbiterLane target = cls.Parameters.Target; #region Initial Forwards if (!cls.Parameters.InitialOncoming) { ForwardReasoning initialReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), initial); #region Target Forwards if (!cls.Parameters.TargetOncoming) { // target reasoning ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target); #region Navigation if (cls.Parameters.Reason == LaneChangeReason.Navigation) { // parameters to follow List<TravelingParameters> tps = new List<TravelingParameters>(); // vehicles to ignore List<int> ignorableVehicles = new List<int>(); // params for forward lane initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable); TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial, CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ? initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle); ArbiterOutput.Output("initial dist to go: " + initialParams.DistanceToGo.ToString("f3")); if (initialParams.Type == TravellingType.Vehicle && !initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) { tps.Add(initialParams); } else tps.Add(initialReasoning.ForwardMonitor.NavigationParameters); ignorableVehicles.AddRange(initialParams.VehiclesToIgnore); // get params for the final lane targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List<ArbiterWaypoint>()); TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters; tps.Add(targetParams); ignorableVehicles.AddRange(targetParams.VehiclesToIgnore); try { if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 && targetParams.Type == TravellingType.Vehicle && targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle != null && targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed) { return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp); } } catch (Exception) { } ArbiterOutput.Output("target dist to go: " + targetParams.DistanceToGo.ToString("f3")); // decorators List<BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator; // distance double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound); cls.Parameters.DistanceToDepartUpperBound = distanceToGo; // check if need to modify distance to go if (initialParams.Type == TravellingType.Vehicle && initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) { double curDistToUpper = cls.Parameters.DistanceToDepartUpperBound; double newVhcDistToUpper = initial.DistanceBetween(vehicleState.Front, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) - 2.0; if (curDistToUpper > newVhcDistToUpper) { distanceToGo = newVhcDistToUpper; } } // get final tps.Sort(); // get the proper speed command ScalarSpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed); if (sc.Speed < 8.84) sc = new ScalarSpeedCommand(Math.Min(targetParams.RecommendedSpeed, 8.84)); // continue the lane change with the proper speed command ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo, sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true), initial.NumberOfLanesRight(vehicleState.Front, true)); // standard maneuver return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp); } #endregion #region Failed Forwards else if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle) { // parameters to follow List<TravelingParameters> tps = new List<TravelingParameters>(); // vehicles to ignore List<int> ignorableVehicles = new List<int>(); // params for forward lane initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable); TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial, CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ? initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null); tps.Add(initialParams); ignorableVehicles.AddRange(initialParams.VehiclesToIgnore); // get params for the final lane targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List<ArbiterWaypoint>()); TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters; tps.Add(targetParams); ignorableVehicles.AddRange(targetParams.VehiclesToIgnore); // decorators List<BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator; // distance double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound); cls.Parameters.DistanceToDepartUpperBound = distanceToGo; // get final tps.Sort(); // get the proper speed command SpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed); // continue the lane change with the proper speed command ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo, sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true), initial.NumberOfLanesRight(vehicleState.Front, true)); // standard maneuver return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp); } #endregion #region Slow else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle) { // fallout exception throw new Exception("currently unsupported lane change type"); } #endregion else { // fallout exception throw new Exception("currently unsupported lane change type"); } } #endregion #region Target Oncoming else { OpposingReasoning targetReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), target); #region Failed Forward if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle) { // parameters to follow List<TravelingParameters> tps = new List<TravelingParameters>(); // ignore the forward vehicle but keep params for forward lane initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable); TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial, CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ? initial.WaypointList[initial.WaypointList.Count-1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null); tps.Add(initialParams); // get params for the final lane targetReasoning.ForwardManeuver(target, initial, vehicleState, roadPlan, blockages); TravelingParameters targetParams = targetReasoning.OpposingForwardMonitor.CurrentParamters.Value; tps.Add(targetParams); // decorators List<BehaviorDecorator> decorators = cls.Parameters.ToLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator; // distance double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound); cls.Parameters.DistanceToDepartUpperBound = distanceToGo; // get final tps.Sort(); // get the proper speed command SpeedCommand sc = new ScalarSpeedCommand(Math.Min(tps[0].RecommendedSpeed, 2.24)); // check final for stopped failed opposing VehicleAgent forwardVa = targetReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle; if (forwardVa != null) { // dist between double distToFV = -targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition); // check stopped bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5; // check distance bool distOk = distToFV < 2.5 * TahoeParams.VL; // check failed bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed; // notify ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString()); // check all for failed if (stopped && distOk && failed) { // check inside target if (target.LanePolygon.IsInside(vehicleState.Front)) { // blockage recovery StayInLaneState sils = new StayInLaneState(initial, CoreCommon.CorePlanningState); StayInLaneBehavior silb = new StayInLaneBehavior(initial.LaneId, new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List<int>(), initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(vehicleState.Front, false), initial.NumberOfLanesRight(vehicleState.Front, false)); BlockageRecoveryState brs = new BlockageRecoveryState(silb, sils, sils, BlockageRecoveryDEFCON.REVERSE, new EncounteredBlockageState(new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, 4.0, BlockageType.Static, -1, true, silb.GetType())), sils, BlockageRecoveryDEFCON.INITIAL, SAUDILevel.None), BlockageRecoverySTATUS.EXECUTING); return new Maneuver(silb, brs, TurnDecorators.HazardDecorator, vehicleState.Timestamp); } // check which lane we are in else { // return to forward lane return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(initial, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp); } } } // continue the lane change with the proper speed command ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo, sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.ReversePath, initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true), initial.NumberOfLanesRight(vehicleState.Front, true)); // standard maneuver return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp); } #endregion #region Other else if (cls.Parameters.Reason == LaneChangeReason.Navigation) { // fallout exception throw new Exception("currently unsupported lane change type"); } else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle) { // fallout exception throw new Exception("currently unsupported lane change type"); } else { // fallout exception throw new Exception("currently unsupported lane change type"); } #endregion } #endregion } #endregion #region Initial Oncoming else { OpposingReasoning initialReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), initial); #region Target Forwards if (!cls.Parameters.TargetOncoming) { ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target); if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle) { // fallout exception throw new Exception("currently unsupported lane change type"); } #region Navigation else if (cls.Parameters.Reason == LaneChangeReason.Navigation) { // parameters to follow List<TravelingParameters> tps = new List<TravelingParameters>(); // distance to the upper bound of the change double distanceToGo = target.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound); cls.Parameters.DistanceToDepartUpperBound = distanceToGo; // get params for the initial lane initialReasoning.ForwardManeuver(initial, target, vehicleState, roadPlan, blockages); // current params of the fqm TravelingParameters initialParams = initialReasoning.OpposingForwardMonitor.CurrentParamters.Value; if (initialParams.Type == TravellingType.Vehicle) { if(!initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) tps.Add(initialParams); else { tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters); distanceToGo = initial.DistanceBetween(initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, vehicleState.Front) - TahoeParams.VL; } } else tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters); // get params for forward lane targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, ignorable); TravelingParameters targetParams = targetReasoning.ForwardMonitor.ParameterizationHelper(target, target, CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ? target.WaypointList[target.WaypointList.Count-1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle); tps.Add(targetParams); // ignoring vehicles add List<int> ignoreVehicles = initialParams.VehiclesToIgnore; ignoreVehicles.AddRange(targetParams.VehiclesToIgnore); // decorators List<BehaviorDecorator> decorators = !cls.Parameters.ToLeft ? TurnDecorators.RightTurnDecorator : TurnDecorators.LeftTurnDecorator; // get final tps.Sort(); // get the proper speed command SpeedCommand sc = tps[0].SpeedCommand; if (sc is StopAtDistSpeedCommand) { sc = new ScalarSpeedCommand(0.0); } // check final for stopped failed opposing VehicleAgent forwardVa = targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle; if (forwardVa != null) { // dist between double distToFV = targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition); // check stopped bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5; // check distance bool distOk = distToFV < 2.5 * TahoeParams.VL; // check failed bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed; // notify ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString()); // check all for failed if (stopped && distOk && failed) { // check which lane we are in if (initial.LanePolygon.IsInside(vehicleState.Front)) { // return to opposing lane return new Maneuver(new HoldBrakeBehavior(), new OpposingLanesState(initial, true, CoreCommon.CorePlanningState, vehicleState), TurnDecorators.NoDecorators, vehicleState.Timestamp); } else { // lane state return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp); } } } // continue the lane change with the proper speed command ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo, sc, ignoreVehicles, initial.ReversePath, target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, false), initial.NumberOfLanesRight(vehicleState.Front, false)); // standard maneuver return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp); } #endregion else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle) { // fallout exception throw new Exception("currently unsupported lane change type"); } else { // fallout exception throw new Exception("currently unsupported lane change type"); } } #endregion else { // fallout exception throw new Exception("currently unsupported lane change type"); } } #endregion }
/// <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; }
public UrbanChallenge.Behaviors.Behavior Resume(VehicleState currentState, double speed) { double distLeft = Parameters.TargetOncoming ? Parameters.Target.DistanceBetween(Parameters.DepartUpperBound, currentState.Front) : Parameters.Target.DistanceBetween(currentState.Front, Parameters.DepartUpperBound); LaneChangeParameters chosen = this.Parameters; this.Parameters.DistanceToDepartUpperBound = distLeft; // distance to stop from max v given desired acceleration //double stopEnvelopeLength = -Math.Pow(this.Parameters.Target.Way.Segment.SpeedLimits.MaximumSpeed, 2) / // (2.0 * -0.5); //double tmpSpeed = distLeft / this.Parameters.Parameters.DistanceToGo * this.Parameters.Parameters.RecommendedSpeed; // create behavior ChangeLaneBehavior clb = new ChangeLaneBehavior( chosen.Initial.LaneId, chosen.Target.LaneId, chosen.ToLeft, distLeft, new ScalarSpeedCommand(chosen.Parameters.RecommendedSpeed), chosen.Parameters.VehiclesToIgnore, chosen.InitialOncoming ? chosen.Initial.ReversePath : chosen.Initial.LanePath(), chosen.TargetOncoming ? chosen.Target.ReversePath : chosen.Target.LanePath(), chosen.Initial.Width, chosen.Target.Width, chosen.InitialOncoming ? chosen.Initial.NumberOfLanesRight(currentState.Position, true) : chosen.Initial.NumberOfLanesLeft(currentState.Position, true), chosen.InitialOncoming ? chosen.Initial.NumberOfLanesLeft(currentState.Position, true) : chosen.Initial.NumberOfLanesRight(currentState.Position, true)); // return behavior return clb; }
/// <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> /// Makes use of parameterizations made from the initial forward maneuver plan /// </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? SecondaryManeuver(IFQMPlanable arbiterLane, VehicleState vehicleState, RoadPlan roadPlan, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask) { // check if we might be able to pass here bool validArea = arbiterLane is ArbiterLane || (((SupraLane)arbiterLane).ClosestComponent(vehicleState.Front) == SLComponentType.Initial); ArbiterLane ourForwardLane = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial; // check if the forward vehicle exists and we're in a valid area if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && validArea) { // check if we should pass the vehicle ahead LaneChangeInformation lci; bool sp = this.ForwardMonitor.ForwardVehicle.ShouldPass(out lci); // make sure we should do something before processing extras if(sp) { // available parameterizations for the lane change List<LaneChangeParameters> changeParams = new List<LaneChangeParameters>(); // get lane ArbiterLane al = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial; // get the location we need to return by Coordinates absoluteUpperBound = arbiterLane is ArbiterLane ? roadPlan.LanePlans[al.LaneId].laneWaypointOfInterest.PointOfInterest.Position : ((SupraLane)arbiterLane).Interconnect.InitialGeneric.Position; #region Failed Forward // if failed, parameterize ourselved if we're following them if (lci.Reason == LaneChangeReason.FailedForwardVehicle && this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle) { // notify ArbiterOutput.Output("Failed Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.VehicleId.ToString()); // get traveling params from FQM to make sure we stopped for vehicle, behind vehicle double v = CoreCommon.Communications.GetVehicleSpeed().Value; TravelingParameters fqmParams = this.ForwardMonitor.CurrentParameters; double d = this.ForwardMonitor.ForwardVehicle.DistanceToVehicle(arbiterLane, vehicleState.Front); Coordinates departUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d - 3.0).Location; // check stopped behing failed forward try { if (fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle) { // check for checkpoint within 4VL of front of failed vehicle ArbiterCheckpoint acCurrecnt = CoreCommon.Mission.MissionCheckpoints.Peek(); if (acCurrecnt.WaypointId.AreaSubtypeId.Equals(al.LaneId)) { // check distance ArbiterWaypoint awCheckpoint = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[acCurrecnt.WaypointId]; double cpDistacne = Lane.DistanceBetween(vehicleState.Front, awCheckpoint.Position); if (cpDistacne < d || cpDistacne - d < TahoeParams.VL * 4.5) { ArbiterOutput.Output("Removing checkpoint: " + acCurrecnt.WaypointId.ToString() + " as failed vehicle over it"); CoreCommon.Mission.MissionCheckpoints.Dequeue(); return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } } }catch (Exception) { } #region Right Lateral Reasoning Forwards // check right lateral reasoning for existence, if so parametrize if (rightLateralReasoning.Exists && fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle) { // get lane ArbiterLane lane = al; // determine failed vehicle lane change distance params Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location; Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.0)).Location; Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location; // get params for lane change LaneChangeParameters? lcp = this.LaneChangeParameterization( new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null), lane, lane.LaneOnRight, false, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); // check if exists to generate full param if (lcp.HasValue) { // get param LaneChangeParameters tp = lcp.Value; // notify ArbiterOutput.WriteToLog("Failed Forward: Right Lateral Reasoning Forwards: Available: " + tp.Available.ToString() + ", Feasable: " + tp.Feasible.ToString()); // get behavior ChangeLaneBehavior clb = new ChangeLaneBehavior( al.LaneId, al.LaneOnRight.LaneId, false, al.DistanceBetween(vehicleState.Front, departUpperBound), new ScalarSpeedCommand(tp.Parameters.RecommendedSpeed), tp.Parameters.VehiclesToIgnore, al.LanePath(), al.LaneOnRight.LanePath(), al.Width, al.LaneOnRight.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true)); tp.Behavior = clb; tp.Decorators = TurnDecorators.RightTurnDecorator; // next state ChangeLanesState cls = new ChangeLanesState(tp); tp.NextState = cls; // add parameterization to possible changeParams.Add(tp); } } #endregion #region Left Lateral Reasoning // check left lateral reasoning if(leftLateralReasoning.Exists) { #region Left Lateral Opposing // check opposing ArbiterLane closestOpposingLane = this.GetClosestOpposing(ourForwardLane, vehicleState); if(closestOpposingLane != null && (leftLateralReasoning.IsOpposing || !closestOpposingLane.Equals(leftLateralReasoning.LateralLane))) { // check room of initial bool enoughRoom = (arbiterLane is ArbiterLane && (!roadPlan.BestPlan.laneWaypointOfInterest.IsExit || ((ArbiterLane)arbiterLane).DistanceBetween(vehicleState.Front, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > TahoeParams.VL * 5.0)) || (arbiterLane is SupraLane && ((SupraLane)arbiterLane).DistanceBetween(vehicleState.Front, ((SupraLane)arbiterLane).Interconnect.InitialGeneric.Position) > TahoeParams.VL * 5.0); // check opposing enough room bool oppEnough = closestOpposingLane.DistanceBetween(closestOpposingLane.LanePath().StartPoint.Location, vehicleState.Front) > TahoeParams.VL * 5.0; // check if enough room if (enoughRoom && oppEnough) { // check if we're stopped and the current trav params were for a vehicle and we're close to the vehicle bool stoppedBehindFV = fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle; // check that we're stopped behind forward vehicle before attempting to change lanes if (stoppedBehindFV) { #region Check Segment Blockage // check need to make uturn (hack) bool waitForUTurnCooldown; BlockageTactical bt = CoreCommon.BlockageDirector; StayInLaneBehavior tmpBlockBehavior = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(2.0), new List<int>(), al.LanePath(), al.Width, 0, 0); ITacticalBlockage itbTmp = new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, TahoeParams.VL, BlockageType.Static, -1, false, tmpBlockBehavior.GetType())); Maneuver tmpBlockManeuver = bt.LaneRecoveryManeuver(al, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value, roadPlan, new BlockageRecoveryState(tmpBlockBehavior, new StayInLaneState(al, CoreCommon.CorePlanningState), new StayInLaneState(al, CoreCommon.CorePlanningState), BlockageRecoveryDEFCON.REVERSE, new EncounteredBlockageState(itbTmp, CoreCommon.CorePlanningState, BlockageRecoveryDEFCON.REVERSE, SAUDILevel.None), BlockageRecoverySTATUS.ENCOUNTERED), true, out waitForUTurnCooldown); if (!waitForUTurnCooldown && tmpBlockManeuver.PrimaryBehavior is UTurnBehavior) return tmpBlockManeuver; else if (waitForUTurnCooldown) return null; #endregion // 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.WriteToLog("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); } } else { // notify ArbiterOutput.WriteToLog("Secondary: Left Lateral Opposing: Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString()); // determine failed vehicle lane change distance params Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location; Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.5)).Location; Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location; // check if enough room if (al.DistanceBetween(vehicleState.Front, defaultReturnUpperBound) >= d + TahoeParams.VL * 4.5) { // get hte closest oppoing ArbiterLane closestOpposing = this.GetClosestOpposing(al, vehicleState); // check exists if (closestOpposing != null) { // set/check secondary if (this.secondaryLeftLateralReasoning == null || !this.secondaryLeftLateralReasoning.LateralLane.Equals(closestOpposing)) this.secondaryLeftLateralReasoning = new OpposingLateralReasoning(closestOpposing, SideObstacleSide.Driver); // check the state of hte lanes next to us if (this.leftLateralReasoning.LateralLane.Equals(closestOpposing) && this.leftLateralReasoning.ExistsExactlyHere(vehicleState)) { #region Plan // need to make sure that we wait for 3 seconds with the blinker on (resetting with pause) if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds > 3000) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: Wait Timer DONE"); // get parameterization LaneChangeParameters? tp = this.LaneChangeParameterization( new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.ForwardMonitor.ForwardVehicle.CurrentVehicle), al, leftLateralReasoning.LateralLane, true, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); // check if available and feasible if (tp.HasValue && tp.Value.Available && tp.Value.Feasible) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: AVAILABLE & FEASIBLE"); LaneChangeParameters lcp = tp.Value; lcp.Behavior = this.ForwardMonitor.CurrentParameters.Behavior; lcp.Decorators = TurnDecorators.LeftTurnDecorator; lcp.Behavior.Decorators = lcp.Decorators; // next state ChangeLanesState cls = new ChangeLanesState(tp.Value); lcp.NextState = cls; // add parameterization to possible changeParams.Add(lcp); } // check if not available now but still feasible else if (tp.HasValue && !tp.Value.Available && tp.Value.Feasible) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: NOT Available, Still FEASIBLE, WAITING"); // wait and blink maneuver TravelingParameters tp2 = this.ForwardMonitor.CurrentParameters; tp2.Decorators = TurnDecorators.LeftTurnDecorator; tp2.Behavior.Decorators = tp2.Decorators; // create parameterization LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, al.LaneOnLeft, true, true, tp2.Behavior, 0.0, CoreCommon.CorePlanningState, tp2.Decorators, tp2, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle); // add parameterization to possible changeParams.Add(lcp); } } // otherwise timer not running or not been long enough else { // check if timer running if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.IsRunning) this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Start(); double waited = (double)(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds / 1000.0); ArbiterOutput.Output("Waited for failed forwards: " + waited.ToString("F2") + " seconds"); // wait and blink maneuver TravelingParameters tp = this.ForwardMonitor.CurrentParameters; tp.Decorators = TurnDecorators.LeftTurnDecorator; tp.Behavior.Decorators = tp.Decorators; // create parameterization LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, al.LaneOnLeft, true, true, tp.Behavior, 0.0, CoreCommon.CorePlanningState, tp.Decorators, tp, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle); // add parameterization to possible changeParams.Add(lcp); } #endregion } else if (!this.leftLateralReasoning.LateralLane.Equals(closestOpposing) && !this.leftLateralReasoning.ExistsRelativelyHere(vehicleState)) { // set and notify ArbiterOutput.Output("superceeded left lateral reasoning with override for non adjacent left lateral reasoning"); ILateralReasoning tmpReasoning = this.leftLateralReasoning; this.leftLateralReasoning = this.secondaryLeftLateralReasoning; try { #region Plan // need to make sure that we wait for 3 seconds with the blinker on (resetting with pause) if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds > 3000) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: Wait Timer DONE"); // get parameterization LaneChangeParameters? tp = this.LaneChangeParameterization( new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.ForwardMonitor.ForwardVehicle.CurrentVehicle), al, leftLateralReasoning.LateralLane, true, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); // check if available and feasible if (tp.HasValue && tp.Value.Available && tp.Value.Feasible) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: AVAILABLE & FEASIBLE"); LaneChangeParameters lcp = tp.Value; lcp.Behavior = this.ForwardMonitor.CurrentParameters.Behavior; lcp.Decorators = TurnDecorators.LeftTurnDecorator; lcp.Behavior.Decorators = TurnDecorators.LeftTurnDecorator; // next state ChangeLanesState cls = new ChangeLanesState(tp.Value); lcp.NextState = cls; // add parameterization to possible changeParams.Add(lcp); } // check if not available now but still feasible else if (tp.HasValue && !tp.Value.Available && tp.Value.Feasible) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: NOT Available, Still FEASIBLE, WAITING"); // wait and blink maneuver TravelingParameters tp2 = this.ForwardMonitor.CurrentParameters; tp2.Decorators = TurnDecorators.LeftTurnDecorator; tp2.Behavior.Decorators = tp2.Decorators; // create parameterization LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, this.leftLateralReasoning.LateralLane, true, true, tp2.Behavior, 0.0, CoreCommon.CorePlanningState, tp2.Decorators, tp2, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle); // add parameterization to possible changeParams.Add(lcp); } } // otherwise timer not running or not been long enough else { // check if timer running if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.IsRunning) this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Start(); double waited = (double)(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds / 1000.0); ArbiterOutput.Output("Waited for failed forwards: " + waited.ToString("F2") + " seconds"); // wait and blink maneuver TravelingParameters tp = this.ForwardMonitor.CurrentParameters; tp.Decorators = TurnDecorators.LeftTurnDecorator; tp.Behavior.Decorators = tp.Decorators; // create parameterization LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, this.leftLateralReasoning.LateralLane, true, true, tp.Behavior, 0.0, CoreCommon.CorePlanningState, tp.Decorators, tp, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle); // add parameterization to possible changeParams.Add(lcp); } #endregion } catch (Exception ex) { ArbiterOutput.Output("Core intelligence thread caught exception in forward reasoning secondary maneuver when non-standard adjacent left: " + ex.ToString()); } // restore this.leftLateralReasoning = tmpReasoning; } } else { // do nuttin ArbiterOutput.Output("no opposing adjacent"); } } else { // notify ArbiterOutput.Output("Secondary: LeftLatOpp: Stopped Behind FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", but not enough room to pass"); } } } else { this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop(); this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset(); // notify ArbiterOutput.Output("Secondary: Left Lateral Opposing: NOT Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString()); } } else { ArbiterOutput.Output("Secondary Opposing: enough room to pass opposing: initial: " + enoughRoom.ToString() + ", opposing: " + oppEnough.ToString()); } } #endregion #region Left Lateral Forwards // otherwise parameterize else if(fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle) { // get lane ArbiterLane lane = al; // determine failed vehicle lane change distance params Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location; Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.0)).Location; Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location; // get params for lane change LaneChangeParameters? lcp = this.LaneChangeParameterization( new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null), lane, lane.LaneOnLeft, false, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); // check if exists to generate full param if (lcp.HasValue) { // set param LaneChangeParameters tp = lcp.Value; // notify ArbiterOutput.Output("Secondary Failed Forward Reasoning Forwards: Available: " + tp.Available.ToString() + ", Feasible: " + tp.Feasible.ToString()); // get behavior ChangeLaneBehavior clb = new ChangeLaneBehavior( al.LaneId, al.LaneOnLeft.LaneId, true, al.DistanceBetween(vehicleState.Front, departUpperBound), new ScalarSpeedCommand(tp.Parameters.RecommendedSpeed), tp.Parameters.VehiclesToIgnore, al.LanePath(), al.LaneOnLeft.LanePath(), al.Width, al.LaneOnLeft.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true)); tp.Behavior = clb; tp.Decorators = TurnDecorators.LeftTurnDecorator; // next state ChangeLanesState cls = new ChangeLanesState(tp); tp.NextState = cls; // add parameterization to possible changeParams.Add(tp); } } #endregion } #endregion } #endregion #region Slow Forward // if pass, determine if should pass in terms or vehicles adjacent and in front then call lane change function for maneuver else if (lci.Reason == LaneChangeReason.SlowForwardVehicle) { // if left exists and is not opposing, parameterize if (leftLateralReasoning.Exists && !leftLateralReasoning.IsOpposing) { throw new Exception("slow forward vehicle pass not implemented yet"); } // if right exists and is not opposing, parameterize if (rightLateralReasoning.Exists && !rightLateralReasoning.IsOpposing) { throw new Exception("slow forward vehicle pass not implemented yet"); } } #endregion #region Parameterize // check params to see if any are good and available if(changeParams != null && changeParams.Count > 0) { // sort the parameterizations changeParams.Sort(); // get first LaneChangeParameters final = changeParams[0]; // notify ArbiterOutput.Output("Secondary Reasoning Final: Available: " + final.Available.ToString() + ", Feasible: " + final.Feasible.ToString()); // make sure ok if (final.Available && final.Feasible) { // return best param return new Maneuver(changeParams[0].Behavior, changeParams[0].NextState, changeParams[0].Decorators, vehicleState.Timestamp); } } #endregion } } // fallout is null return null; }
private void HandleBehavior(ChangeLaneBehavior cb) { AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(cb.TimeStamp); startingLanePath = cb.BackupStartLanePath.Transform(absTransform); endingLanePath = cb.BackupTargetLanePath.Transform(absTransform); startingNumLanesLeft = cb.StartingNumLanesLeft; startingNumLanesRight = cb.StartingNumLaneRights; startingLaneWidth = cb.StartLaneWidth; endingLaneWidth = cb.TargetLaneWidth; startingLaneID = cb.StartLane; endingLaneID = cb.TargetLane; changeLeft = cb.ChangeLeft; if (cb.MaxDist > -TahoeParams.FL) { dist = cb.MaxDist + TahoeParams.FL; } else { dist = 0; } speedCommand = cb.SpeedCommand; ignorableObstacles = cb.IgnorableObstacles; Services.ObstaclePipeline.LastIgnoredObstacles = cb.IgnorableObstacles; behaviorTimestamp = absTransform.Timestamp; }