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