/// <summary> /// Gets parameters to follow vehicle /// </summary> /// <param name="vehicleState"></param> /// <param name="final"></param> /// <returns></returns> private TravelingParameters VehicleParameterization(VehicleState vehicleState, LinePath fullPath, ArbiterWaypoint final) { // get simple following Speed and distance for following double followingSpeed; double distanceToGood; double xSeparation; this.forwardMonitor.Follow(vehicleState, fullPath, final.Lane, this.turn, out followingSpeed, out distanceToGood, out xSeparation); // get parameterization TravelingParameters vehicleParams = this.GetParameters(followingSpeed, distanceToGood, final, vehicleState, false); // add to current parames to arbiter information CoreCommon.CurrentInformation.FVTBehavior = vehicleParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FVTSpeed = vehicleParams.RecommendedSpeed.ToString("F3"); CoreCommon.CurrentInformation.FVTSpeedCommand = vehicleParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FVTDistance = vehicleParams.DistanceToGo.ToString("F2"); CoreCommon.CurrentInformation.FVTState = vehicleParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FVTStateInfo = vehicleParams.NextState.StateInformation(); CoreCommon.CurrentInformation.FVTXSeparation = xSeparation.ToString("F3"); CoreCommon.CurrentInformation.FVTIgnorable = this.forwardMonitor.VehiclesToIgnore.ToArray(); // return the parameterization return(vehicleParams); }
/// <summary> /// Constructor /// </summary> /// <param name="available"></param> /// <param name="feasible"></param> /// <param name="initial"></param> /// <param name="initialOncoming"></param> /// <param name="target"></param> /// <param name="targetOncoming"></param> /// <param name="toLeft"></param> /// <param name="behavior"></param> /// <param name="nextState"></param> /// <param name="decorators"></param> /// <param name="parameters"></param> /// <param name="departUppedBound"></param> /// <param name="defaultReturnLowerBound"></param> /// <param name="minimumReturnComplete"></param> /// <param name="defaultReturnUpperBound"></param> /// <param name="reason"></param> public LaneChangeParameters(bool available, bool feasible, ArbiterLane initial, bool initialOncoming, ArbiterLane target, bool targetOncoming, bool toLeft, Behavior behavior, double distanceToDepartUpperBound, IState nextState, List <BehaviorDecorator> decorators, TravelingParameters parameters, Coordinates departUppedBound, Coordinates defaultReturnLowerBound, Coordinates minimumReturnComplete, Coordinates defaultReturnUpperBound, LaneChangeReason reason) { this.Available = available; this.Feasible = feasible; this.Initial = initial; this.InitialOncoming = initialOncoming; this.Target = target; this.TargetOncoming = targetOncoming; this.ToLeft = toLeft; this.Behavior = behavior; this.NextState = nextState; this.Decorators = decorators; this.Parameters = parameters; this.DistanceToDepartUpperBound = distanceToDepartUpperBound; this.DepartUpperBound = departUppedBound; this.DefaultReturnLowerBound = defaultReturnLowerBound; this.MinimumReturnComplete = minimumReturnComplete; this.DefaultReturnUpperBound = defaultReturnUpperBound; this.Reason = reason; this.ForcedOpposing = false; }
/// <summary> /// Navigation parameterization given point of intereset and our vehicle state /// </summary> /// <param name="vehicleState"></param> /// <param name="dpoi"></param> /// <returns></returns> private TravelingParameters NavigationParameterization(VehicleState vehicleState, DownstreamPointOfInterest dpoi, ArbiterWaypoint final, LinePath fullPath) { // stop waypoint and distance ArbiterWaypoint stopWaypoint = dpoi.PointOfInterest; double distanceToStop = final.Lane.DistanceBetween(final.Position, stopWaypoint.Position) + vehicleState.Front.DistanceTo(this.turn.FinalGeneric.Position); // checks if we need to stop at this point bool isStop = dpoi.IsExit || (dpoi.IsGoal && dpoi.PointOfInterest.IsCheckpoint && dpoi.PointOfInterest.CheckpointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber) && CoreCommon.Mission.MissionCheckpoints.Count == 1); // get next lane type stop List <WaypointType> stopTypes = new List <WaypointType>(); stopTypes.Add(WaypointType.End); stopTypes.Add(WaypointType.Stop); ArbiterWaypoint nextStop = final.Lane.GetNext(final, stopTypes, new List <ArbiterWaypoint>()); // get total distance to lane stop double distanceToLaneStop = final.Lane.DistanceBetween(final.Position, nextStop.Position) + vehicleState.Front.DistanceTo(this.turn.FinalGeneric.Position); // check that we have nearest and correct stop if (!isStop || (isStop && distanceToLaneStop <= distanceToStop)) { stopWaypoint = nextStop; distanceToStop = distanceToLaneStop; } // get speed to stop double stopSpeed = this.StoppingParameters(distanceToStop, final.Lane.Way.Segment.SpeedLimits.MaximumSpeed); // get params TravelingParameters navParams = this.GetParameters(stopSpeed, distanceToStop, final, vehicleState, true); // add to current parames to arbiter information CoreCommon.CurrentInformation.FQMBehavior = navParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FQMBehaviorInfo = navParams.Behavior.ShortBehaviorInformation(); CoreCommon.CurrentInformation.FQMSpeedCommand = navParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FQMDistance = navParams.DistanceToGo.ToString("F6"); CoreCommon.CurrentInformation.FQMSpeed = navParams.RecommendedSpeed.ToString("F6"); CoreCommon.CurrentInformation.FQMState = navParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FQMStateInfo = navParams.NextState.StateInformation(); CoreCommon.CurrentInformation.FQMStopType = "Dpoi: " + dpoi.PointOfInterest.Equals(stopWaypoint) + ", Stop: " + isStop.ToString(); CoreCommon.CurrentInformation.FQMWaypoint = stopWaypoint.ToString(); CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = Math.Min(final.Lane.CurrentMaximumSpeed(vehicleState.Front), this.turn.MaximumDefaultSpeed).ToString("F2"); // return return(navParams); }
/// <summary> /// Gets parameterization /// </summary> /// <param name="lane"></param> /// <param name="speed"></param> /// <param name="distance"></param> /// <param name="state"></param> /// <returns></returns> public TravelingParameters GetParameters(ArbiterLane lane, double speed, double distance, VehicleState state) { double distanceCutOff = CoreCommon.OperationslStopLineSearchDistance; Maneuver m = new Maneuver(); SpeedCommand sc; bool usingSpeed = true; #region Distance Cutoff // check if distance is less than cutoff if (distance < distanceCutOff) { // default behavior sc = new StopAtDistSpeedCommand(distance); Behavior b = new StayInLaneBehavior(lane.LaneId, sc, new List<int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(state.Front, true), lane.NumberOfLanesRight(state.Front, true)); // stopping so not using speed param usingSpeed = false; // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(lane, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp); } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // get lane ArbiterLane al = lane; // default behavior sc = new ScalarSpeedCommand(speed); Behavior b = new StayInLaneBehavior(al.LaneId, sc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp); } #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = m.PrimaryBehavior.Decorators; tp.DistanceToGo = distance; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = speed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.SpeedCommand = sc; tp.VehiclesToIgnore = new List<int>(); // return navigation params return tp; #endregion }
/// <summary> /// Parameters to follow the forward vehicle /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public TravelingParameters Follow(ArbiterLane lane, VehicleState state) { // travelling parameters TravelingParameters tp = new TravelingParameters(); // ignorable obstacles List <int> ignoreVehicle = new List <int>(); ignoreVehicle.Add(CurrentVehicle.VehicleId); // get control parameters ForwardVehicleTrackingControl fvtc = GetControl(lane, state); // init params tp.DistanceToGo = fvtc.xDistanceToGood; tp.NextState = CoreCommon.CorePlanningState; tp.RecommendedSpeed = fvtc.vFollowing; tp.Type = TravellingType.Vehicle; tp.Decorators = TurnDecorators.NoDecorators; // flag to ignore forward vehicle bool ignoreForward = false; // reversed lane path LinePath lp = lane.LanePath().Clone(); lp.Reverse(); #region Following Control #region Immediate Stop // need to stop immediately if (fvtc.vFollowing == 0.0) { // don't ignore forward ignoreForward = false; // speed command SpeedCommand sc = new ScalarSpeedCommand(0.0); tp.UsingSpeed = true; // standard path following behavior Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); tp.Behavior = final; tp.SpeedCommand = sc; } #endregion #region Distance Stop // stop at distance else if (fvtc.vFollowing < 0.7 && CoreCommon.Communications.GetVehicleSpeed().Value <= 2.24 && fvtc.xSeparation > fvtc.xAbsMin) { // ignore forward vehicle ignoreForward = true; // speed command SpeedCommand sc = new StopAtDistSpeedCommand(fvtc.xDistanceToGood); tp.UsingSpeed = false; // standard path following behavior Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); tp.Behavior = final; tp.SpeedCommand = sc; } #endregion #region Normal Following // else normal else { // ignore the forward vehicle as we are tracking properly ignoreForward = true; // speed command SpeedCommand sc = new ScalarSpeedCommand(fvtc.vFollowing); tp.DistanceToGo = fvtc.xDistanceToGood; tp.NextState = CoreCommon.CorePlanningState; tp.RecommendedSpeed = fvtc.vFollowing; tp.Type = TravellingType.Vehicle; tp.UsingSpeed = true; // standard path following behavior Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); tp.Behavior = final; tp.SpeedCommand = sc; } #endregion #endregion // check ignore if (ignoreForward) { List <int> ignorable = new List <int>(); ignorable.Add(this.CurrentVehicle.VehicleId); tp.VehiclesToIgnore = ignorable; } else { tp.VehiclesToIgnore = new List <int>(); } // return parameterization return(tp); }
/// <summary> /// Generate the traveling parameterization for the desired behaivor /// </summary> /// <param name="lane"></param> /// <param name="navStopSpeed"></param> /// <param name="navStopDistance"></param> /// <param name="navStop"></param> /// <param name="navStopType"></param> /// <param name="state"></param> /// <returns></returns> private TravelingParameters NavStopParameterization(ArbiterLane lane, double navStopSpeed, double navStopDistance, ArbiterWaypoint navStop, StopType navStopType, VehicleState state) { // get min dist double distanceCutOff = CoreCommon.OperationalStopDistance; // turn direction default List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators; // create new params TravelingParameters tp = new TravelingParameters(); #region Get Maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; // get lane path LinePath lp = lane.LanePath().Clone(); lp.Reverse(); #region Distance Cutoff // check if distance is less than cutoff if (navStopDistance < distanceCutOff) { // default behavior tp.SpeedCommand = new StopAtDistSpeedCommand(navStopDistance); Behavior b = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(navStopDistance), new List<int>(), lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); // stopping so not using speed param usingSpeed = false; IState nextState = CoreCommon.CorePlanningState; m = new Maneuver(b, nextState, decorators, state.Timestamp); } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // get lane ArbiterLane al = lane; // default behavior tp.SpeedCommand = new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)); Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)), new List<int>(), lp, al.Width, al.NumberOfLanesRight(state.Front, false), al.NumberOfLanesLeft(state.Front, false)); // standard behavior is fine for maneuver m = new Maneuver(b, CoreCommon.CorePlanningState, decorators, state.Timestamp); } #endregion #endregion #region Parameterize tp.Behavior = m.PrimaryBehavior; tp.Decorators = m.PrimaryBehavior.Decorators; tp.DistanceToGo = navStopDistance; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = navStopSpeed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.VehiclesToIgnore = new List<int>(); // return navigation params return tp; #endregion }
/// <summary> /// Behavior given we stay in the current lane /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <param name="downstreamPoint"></param> /// <returns></returns> public Maneuver PrimaryManeuver(ArbiterLane lane, ArbiterLane closestGood, VehicleState state, List<ITacticalBlockage> blockages) { // possible parameterizations List<TravelingParameters> tps = new List<TravelingParameters>(); #region Nav // get the next thing we need to stop at no matter what and parameters for stopping at it ArbiterWaypoint navStop; double navStopSpeed; double navStopDistance; StopType navStopType; // make sure we stop an extra 3VL from any navigational stop double extraDistance = 0.0; // get next stop in this opposing lane this.NextOpposingNavigationalStop(lane, closestGood, state.Front, extraDistance, out navStopSpeed, out navStopDistance, out navStopType, out navStop); // set global stop distance this.NextNavigationStopDistance = navStopDistance; // create parameterization of the stop TravelingParameters navParams = this.NavStopParameterization(lane, navStopSpeed, navStopDistance, navStop, navStopType, state); this.NaviationParameters = navParams; // add to nav parames to arbiter information CoreCommon.CurrentInformation.FQMBehavior = navParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FQMBehaviorInfo = navParams.Behavior.ShortBehaviorInformation(); CoreCommon.CurrentInformation.FQMSpeedCommand = navParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FQMDistance = navParams.DistanceToGo.ToString("F6"); CoreCommon.CurrentInformation.FQMSpeed = navParams.RecommendedSpeed.ToString("F6"); CoreCommon.CurrentInformation.FQMState = navParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FQMStateInfo = navParams.NextState.StateInformation(); CoreCommon.CurrentInformation.FQMStopType = navStopType.ToString(); CoreCommon.CurrentInformation.FQMWaypoint = navStop.ToString(); CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = lane.CurrentMaximumSpeed(state.Position).ToString("F1"); // add nav parameter tps.Add(navParams); #endregion #region Vehicle // forward vehicle update this.ForwardVehicle.Update(lane, state); if (this.ForwardVehicle.ShouldUseForwardTracker) { // get forward vehicle params TravelingParameters vehicleParams = this.ForwardVehicle.Follow(lane, state); // add vehicle param tps.Add(vehicleParams); } #endregion #region Blockages /* // get the blockage stop parameters bool stopAtBlockage; double blockageSpeed; double blockageDistance; this.StopForBlockages(lane, state, List<ITacticalBlockage> blockages, out stopAtBlockage, out blockageSpeed, out blockageDistance); */ #endregion // sort params by most urgent tps.Sort(); // set current this.currentParamters = tps[0]; // out of navigation, blockages, and vehicle following determine the actual primary behavior parameters return new Maneuver(tps[0].Behavior, tps[0].NextState, tps[0].Decorators, state.Timestamp); }
/// <summary> /// Behavior given we stay in the current lane /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <param name="downstreamPoint"></param> /// <returns></returns> public Maneuver PrimaryManeuver(ArbiterLane lane, ArbiterLane closestGood, VehicleState state, List <ITacticalBlockage> blockages) { // possible parameterizations List <TravelingParameters> tps = new List <TravelingParameters>(); #region Nav // get the next thing we need to stop at no matter what and parameters for stopping at it ArbiterWaypoint navStop; double navStopSpeed; double navStopDistance; StopType navStopType; // make sure we stop an extra 3VL from any navigational stop double extraDistance = 0.0; // get next stop in this opposing lane this.NextOpposingNavigationalStop(lane, closestGood, state.Front, extraDistance, out navStopSpeed, out navStopDistance, out navStopType, out navStop); // set global stop distance this.NextNavigationStopDistance = navStopDistance; // create parameterization of the stop TravelingParameters navParams = this.NavStopParameterization(lane, navStopSpeed, navStopDistance, navStop, navStopType, state); this.NaviationParameters = navParams; // add to nav parames to arbiter information CoreCommon.CurrentInformation.FQMBehavior = navParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FQMBehaviorInfo = navParams.Behavior.ShortBehaviorInformation(); CoreCommon.CurrentInformation.FQMSpeedCommand = navParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FQMDistance = navParams.DistanceToGo.ToString("F6"); CoreCommon.CurrentInformation.FQMSpeed = navParams.RecommendedSpeed.ToString("F6"); CoreCommon.CurrentInformation.FQMState = navParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FQMStateInfo = navParams.NextState.StateInformation(); CoreCommon.CurrentInformation.FQMStopType = navStopType.ToString(); CoreCommon.CurrentInformation.FQMWaypoint = navStop.ToString(); CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = lane.CurrentMaximumSpeed(state.Position).ToString("F1"); // add nav parameter tps.Add(navParams); #endregion #region Vehicle // forward vehicle update this.ForwardVehicle.Update(lane, state); if (this.ForwardVehicle.ShouldUseForwardTracker) { // get forward vehicle params TravelingParameters vehicleParams = this.ForwardVehicle.Follow(lane, state); // add vehicle param tps.Add(vehicleParams); } #endregion #region Blockages /* * // get the blockage stop parameters * bool stopAtBlockage; * double blockageSpeed; * double blockageDistance; * this.StopForBlockages(lane, state, List<ITacticalBlockage> blockages, out stopAtBlockage, out blockageSpeed, out blockageDistance); */ #endregion // sort params by most urgent tps.Sort(); // set current this.currentParamters = tps[0]; // out of navigation, blockages, and vehicle following determine the actual primary behavior parameters return(new Maneuver(tps[0].Behavior, tps[0].NextState, tps[0].Decorators, state.Timestamp)); }
/// <summary> /// Simple right lane change /// </summary> /// <param name="arbiterLane"></param> /// <param name="closestGood"></param> /// <param name="vehicleState"></param> /// <param name="blockages"></param> /// <returns></returns> private Maneuver?DefaultRightToGoodChange(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, List <ITacticalBlockage> blockages, Coordinates xUpper, bool forcedOpposing) { bool adjRearClear = this.rightLateralReasoning.AdjacentAndRearClear(vehicleState); if (adjRearClear) { // notify ArbiterOutput.Output("Opposing Secondary: Adjacent and Rear Clear"); LaneChangeParameters lcp = new LaneChangeParameters( true, true, arbiterLane, true, closestGood, false, false, null, 3 * TahoeParams.VL, null, TurnDecorators.RightTurnDecorator, this.OpposingForwardMonitor.CurrentParamters.Value, xUpper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation); lcp.ForcedOpposing = forcedOpposing; ChangeLanesState cls = new ChangeLanesState(lcp); return(new Maneuver(this.OpposingForwardMonitor.CurrentParamters.Value.Behavior, cls, lcp.Decorators, vehicleState.Timestamp)); } else { if (this.rightLateralReasoning.AdjacentVehicle != null && !this.rightLateralReasoning.AdjacentVehicle.IsStopped && this.rightLateralReasoning.AdjacentVehicle.Speed > CoreCommon.Communications.GetVehicleSpeed().Value - 2.0) { // notify ArbiterOutput.Output("Opposing Secondary: Adjacent and Rear NOT Clear, ADJACENT NOT STOPPED, vAdj: " + this.rightLateralReasoning.AdjacentVehicle.Speed.ToString("f1")); TravelingParameters tp = this.OpposingForwardMonitor.CurrentParamters.Value; if (tp.Behavior is StayInLaneBehavior) { StayInLaneBehavior silb = (StayInLaneBehavior)tp.Behavior; silb.SpeedCommand = new ScalarSpeedCommand(0.0); } return(new Maneuver(tp.Behavior, tp.NextState, tp.Decorators, vehicleState.Timestamp)); } else if (this.rightLateralReasoning.AdjacentVehicle != null && this.OpposingForwardMonitor.NaviationParameters.DistanceToGo < TahoeParams.VL * 4.0) { // notify ArbiterOutput.Output("Opposing Secondary: Adjacent and Rear NOT Clear, ADJACENT FILLED, too close to nav stop"); TravelingParameters tp = this.OpposingForwardMonitor.CurrentParamters.Value; if (tp.Behavior is StayInLaneBehavior) { StayInLaneBehavior silb = (StayInLaneBehavior)tp.Behavior; silb.SpeedCommand = new ScalarSpeedCommand(0.0); } return(new Maneuver(tp.Behavior, tp.NextState, tp.Decorators, vehicleState.Timestamp)); } else if (this.rightLateralReasoning.AdjacentVehicle == null && !adjRearClear) { // notify ArbiterOutput.Output("Opposing Secondary: REAR NOT Clear, WAITING"); TravelingParameters tp = this.OpposingForwardMonitor.CurrentParamters.Value; if (tp.Behavior is StayInLaneBehavior) { StayInLaneBehavior silb = (StayInLaneBehavior)tp.Behavior; silb.SpeedCommand = new ScalarSpeedCommand(0.0); } return(new Maneuver(tp.Behavior, tp.NextState, tp.Decorators, vehicleState.Timestamp)); } else { return(null); } } }
/// <summary> /// Generates the lane change parameterization /// </summary> /// <param name="information"></param> /// <param name="initial"></param> /// <param name="final"></param> /// <param name="goal"></param> /// <param name="departUpperBound"></param> /// <param name="defaultReturnLowerBound"></param> /// <param name="minimumReturnComplete"></param> /// <param name="defaultReturnUpperBound"></param> /// <param name="blockages"></param> /// <param name="ignorable"></param> /// <param name="state"></param> /// <param name="speed"></param> /// <returns></returns> public LaneChangeParameters? LaneChangeParameterization(LaneChangeInformation information, ArbiterLane initial, ArbiterLane target, bool targetOncoming, Coordinates goal, Coordinates departUpperBound, Coordinates defaultReturnLowerBound, Coordinates minimumReturnComplete, Coordinates defaultReturnUpperBound, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, VehicleState state, double speed) { // check if the target lane exists here bool validTarget = target.LanePath().GetClosestPoint(state.Front).Location.DistanceTo(state.Front) < 17 && target.IsInside(state.Front); // params bool toLeft = initial.LaneOnLeft != null ? initial.LaneOnLeft.Equals(target) || (targetOncoming && !initial.Way.WayId.Equals(target.Way.WayId)) : false; // get appropriate lateral reasoning ILateralReasoning lateralReasoning = toLeft ? this.leftLateralReasoning : this.rightLateralReasoning; #region Target Lane Valid Here // check if the target is currently valid if (validTarget) { // lane change parameterizations List<LaneChangeParameters> lcps = new List<LaneChangeParameters>(); // distance to the current goal (means different things for all) double xGoal = initial.DistanceBetween(state.Front, goal); // get next stop List<WaypointType> types = new List<WaypointType>(); types.Add(WaypointType.Stop); types.Add(WaypointType.End); ArbiterWaypoint nextMajor = initial.GetNext(state.Front, types, ignorable); double xLaneMajor = initial.DistanceBetween(state.Front, nextMajor.Position); xGoal = Math.Min(xGoal, xLaneMajor); #region Failed Vehicle Lane Change if (information.Reason == LaneChangeReason.FailedForwardVehicle) { #region Target Opposing // check if target lane backwards if (targetOncoming) { // available and feasible bool avail = false; bool feas = false; // check if min return distance < goal distance double xReturnMin = initial.DistanceBetween(state.Front, minimumReturnComplete); double xDepart = initial.DistanceBetween(state.Front, departUpperBound); // dist to upper bound along lane and dist to end of adjacent lane double adjLaneDist = initial.DistanceBetween(state.Front, minimumReturnComplete); // this is feasible feas = xGoal > xReturnMin ? true : false; // check if not feasible that the goal is the current checkpoint if (!feas && CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId].Position.Equals(goal)) feas = true; // check adj and rear clear bool adjRearClear = lateralReasoning.AdjacentAndRearClear(state); // check if forwards clear bool frontClear = lateralReasoning.ForwardClear(state, xReturnMin, 2.24, information, minimumReturnComplete); Console.WriteLine("Adjacent, Rear: " + adjRearClear.ToString() + ", Forward: " + frontClear.ToString()); // if clear if (frontClear && adjRearClear) { // notify ArbiterOutput.Output("Lane Change Params: Target Oncoming Failed Vehicle: Adjacent, Rear, and Front Clear"); // available avail = true; // get lateral parameterization TravelingParameters lateralParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state)); // change into the opposing lane wih opposing forward parameterization LaneChangeParameters lcp = new LaneChangeParameters(avail, feas, initial, false, target, targetOncoming, toLeft, null, xDepart, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, lateralParams, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, information.Reason); // we have been forced lcp.ForcedOpposing = true; // return created params return lcp; } // fell through for some reason, return parameterization explaining why LaneChangeParameters fallThroughParams = new LaneChangeParameters(avail, feas, initial, false, target, targetOncoming, toLeft, null, xDepart, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, this.ForwardMonitor.LaneParameters, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, information.Reason); // return fall through parameters return fallThroughParams; } #endregion #region Target Forwards // otherwise target lane forwards else { // check lateral clear and initial lane does not run out if (lateralReasoning.AdjacentAndRearClear(state) && !initial.GetClosestPoint(defaultReturnUpperBound).Equals(initial.LanePath().EndPoint)) { // notify ArbiterOutput.Output("Lane Change Params: Failed Vehicle Target Forwards: Adjacent and Rear Clear"); // dist to upper bound along lane and dist to end of adjacent lane double distToReturn = initial.DistanceBetween(state.Front, defaultReturnUpperBound); double adjLaneDist = initial.DistanceBetween(state.Front, target.LanePath().EndPoint.Location); // check enough lane room to pass if (distToReturn < adjLaneDist && distToReturn <= initial.DistanceBetween(state.Front, goal)) { // check enough room to change lanes ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable); double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position); // check dist needed to complete double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // check return dist if (distToReturn < xTargetLaneMajor && neededDistance <= xTargetLaneMajor) { // parameters for traveling in current lane to hit other List<TravelingParameters> tps = new List<TravelingParameters>(); // update lateral ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.Update(target, state); // check lateral reasoning for forward vehicle badness if (!((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker || !((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped || initial.DistanceBetween(state.Front, ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) >= distToReturn) { // get parameterization for lateral lane TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state)); tps.Add(navParams); // get and add the vehicle parameterization for our lane if (this.ForwardMonitor.FollowingParameters.HasValue) tps.Add(this.ForwardMonitor.FollowingParameters.Value); // get final tps.Sort(); TravelingParameters final = tps[0]; // check final distance > needed if (navParams.DistanceToGo > neededDistance) { // set ignorable vhcs final.VehiclesToIgnore = this.ForwardMonitor.FollowingParameters.HasValue ? this.ForwardMonitor.FollowingParameters.Value.VehiclesToIgnore : new List<int>(); if (((LateralReasoning)lateralReasoning).ForwardMonitor.FollowingParameters.HasValue) final.VehiclesToIgnore.AddRange(((LateralReasoning)lateralReasoning).ForwardMonitor.FollowingParameters.Value.VehiclesToIgnore); // parameterize LaneChangeParameters lcp = new LaneChangeParameters(); lcp.Decorators = TurnDecorators.RightTurnDecorator; lcp.Available = true; lcp.Feasible = true; lcp.Parameters = final; lcp.Initial = initial; lcp.InitialOncoming = false; lcp.Target = target; lcp.TargetOncoming = false; lcp.Reason = LaneChangeReason.FailedForwardVehicle; lcp.DefaultReturnLowerBound = defaultReturnLowerBound; lcp.DefaultReturnUpperBound = defaultReturnUpperBound; lcp.DepartUpperBound = departUpperBound; lcp.MinimumReturnComplete = minimumReturnComplete; return lcp; } } } } } // otherwise infeasible return null; } #endregion } #endregion #region Navigation Lane Change else if (information.Reason == LaneChangeReason.Navigation) { // parameters for traveling in current lane to hit other List<TravelingParameters> tps = new List<TravelingParameters>(); // get navigation parameterization TravelingParameters lateralParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state)); tps.Add(lateralParams); // get and add the nav parameterization relative to our lane tps.Add(this.ForwardMonitor.NavigationParameters); // check avail bool adjRearAvailable = lateralReasoning.AdjacentAndRearClear(state); // if they are available we are in good shape, need to slow for nav, forward vehicles if (adjRearAvailable) { // notify ArbiterOutput.Output("Lane Change Params: Navigation: Adjacent and Rear Clear"); #region Check Forward and Lateral Vehicles if (this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle && lateralParams.Type == TravellingType.Vehicle) { // check enough room to change lanes ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable); double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position); // distnace to goal double goalDist = initial.DistanceBetween(state.Front, goal); // check dist needed to complete double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // check for proper distances if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance) { // check distance to return (weeds out safety zone crap Coordinates lateralVehicle = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition; double distToReturn = initial.DistanceBetween(state.Front, initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(lateralVehicle), 30.0).Location); // check passing params LaneChangeInformation lci; bool shouldPass = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.ShouldPass(out lci); // check passing params LaneChangeInformation lciInit; bool shouldPassInit = this.ForwardMonitor.ForwardVehicle.ShouldPass(out lciInit); // check forward lateral stopped and enough distance to go around and not vehicles between it and goal close enough to stop if(shouldPass && lci.Reason == LaneChangeReason.FailedForwardVehicle && goalDist > distToReturn && (!shouldPassInit || lciInit.Reason != LaneChangeReason.FailedForwardVehicle || this.ForwardMonitor.CurrentParameters.DistanceToGo > lateralParams.DistanceToGo + TahoeParams.VL * 5)) { // return that we should pass it as normal in the initial lane return null; } // check get distance to upper double xUpper = this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ? Math.Min(goalDist, neededDistance) : this.ForwardMonitor.ForwardVehicle.ForwardControl.xSeparation - 2; Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), xUpper).Location; // add current params if not stopped if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) tps.Add(this.ForwardMonitor.CurrentParameters); // get final tps.Sort(); TravelingParameters final = tps[0]; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft, null, final.DistanceToGo - 3.0, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, final, upper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation); return lcp; } } #endregion #region Check Forward Vehicle else if (this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle) { // check enough room to change lanes ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable); double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position); // distnace to goal double goalDist = initial.DistanceBetween(state.Front, goal); // check dist needed to complete double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // check for proper distances if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance) { // add current params if not stopped if(!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped) tps.Add(this.ForwardMonitor.CurrentParameters); // get final tps.Sort(); TravelingParameters final = tps[0]; // check get distance to upper double xUpper = this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ? neededDistance : this.ForwardMonitor.ForwardVehicle.ForwardControl.xSeparation - 2; Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), xUpper).Location; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft, null, final.DistanceToGo - 3.0, null, toLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, final, upper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation); return lcp; } } #endregion #region Lateral Vehicle // check to see if should use the forward tracker, i.e. forward vehicle exists else if (lateralParams.Type == TravellingType.Vehicle) { // add current params tps.Add(this.ForwardMonitor.CurrentParameters); // check enough room to change lanes ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable); double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position); // distnace to goal double goalDist = initial.DistanceBetween(state.Front, goal); // check dist needed to complete double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // check for proper distances if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance) { // check distance to return (weeds out safety zone crap Coordinates lateralVehicle = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition; double distToReturn = initial.DistanceBetween(state.Front, initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(lateralVehicle), 30.0).Location); // check passing params LaneChangeInformation lci; bool shouldPass = ((LateralReasoning)lateralReasoning).ForwardMonitor.ForwardVehicle.ShouldPass(out lci); // check forward lateral stopped and enough distance to go around and not vehicles between it and goal close enough to stop if (shouldPass && lci.Reason == LaneChangeReason.FailedForwardVehicle && goalDist > distToReturn) { // return that we should pass it as normal in the initial lane return null; } // check if we are already slowed for this vehicle and are at a good distance from it if (speed < lateralParams.RecommendedSpeed + 1.0) { // get final tps.Sort(); TravelingParameters final = tps[0]; // upper bound is nav bound Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), Math.Min(neededDistance, final.DistanceToGo)).Location; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft, null, final.DistanceToGo - 3.0, null, TurnDecorators.LeftTurnDecorator, final, upper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation); return lcp; } // otherwise need to slow for it or other else { // get final tps.Sort(); TravelingParameters final = tps[0]; // upper bound is nav bound Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), final.DistanceToGo).Location; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(false, true, initial, false, target, false, toLeft, null, final.DistanceToGo - 3.0, null, TurnDecorators.LeftTurnDecorator, final, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation); return lcp; } } } #endregion #region No forward or lateral // otherwise just go! else { // add current params tps.Add(this.ForwardMonitor.CurrentParameters); // check enough room to change lanes ArbiterWaypoint nextTargetMajor = target.GetNext(state.Front, types, ignorable); double xTargetLaneMajor = initial.DistanceBetween(state.Front, nextTargetMajor.Position); // distnace to goal double goalDist = initial.DistanceBetween(state.Front, goal); // check dist needed to complete double neededDistance = (1.5 * TahoeParams.VL * Math.Abs(initial.LaneId.Number - target.LaneId.Number)) + (-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration)); // check for proper distances if (xTargetLaneMajor >= neededDistance && goalDist >= neededDistance && this.ForwardMonitor.NavigationParameters.DistanceToGo >= neededDistance) { // get final tps.Sort(); TravelingParameters final = tps[0]; // upper bound is nav bound Coordinates upper = initial.LanePath().AdvancePoint(initial.LanePath().GetClosestPoint(state.Front), Math.Min(neededDistance, final.DistanceToGo)).Location; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(true, true, initial, false, target, false, toLeft, null, final.DistanceToGo, null, TurnDecorators.LeftTurnDecorator, final, upper, new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.Navigation); // return the parameterization return lcp; } } #endregion } // otherwise we need to determine how to make this available else { // notify ArbiterOutput.Output("Lane Change Params: Navigation Adjacent and Rear NOT Clear"); // gets the adjacent vehicle VehicleAgent adjacent = lateralReasoning.AdjacentVehicle; // add current params tps.Add(this.ForwardMonitor.CurrentParameters); #region Pass Adjacent // checks if it is failed for us to pass it if (adjacent != null && (adjacent.IsStopped || adjacent.Speed < 1.5)) { // get final List<TravelingParameters> adjacentTravelingParams = new List<TravelingParameters>(); adjacentTravelingParams.Add(this.ForwardMonitor.CurrentParameters); adjacentTravelingParams.Add(this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, null)); adjacentTravelingParams.Sort(); //tps.Sort(); TravelingParameters final = adjacentTravelingParams[0];// tps[0]; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(); lcp.Available = false; lcp.Feasible = true; lcp.Parameters = final; return lcp; } #endregion #region Follow Adjacent // otherwise we need to follow it, as it is lateral, this means 0.0 speed else if (adjacent != null) { // get and add the vehicle parameterization relative to our lane TravelingParameters tmp = new TravelingParameters(); tmp.Behavior = new StayInLaneBehavior(initial.LaneId, new ScalarSpeedCommand(0.0), this.ForwardMonitor.CurrentParameters.VehiclesToIgnore, initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(state.Front, true), initial.NumberOfLanesRight(state.Front, true)); tmp.NextState = CoreCommon.CorePlanningState; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(); lcp.Available = false; lcp.Feasible = true; lcp.Parameters = tmp; return lcp; } #endregion #region Wait for the rear vehicle else { TravelingParameters tp = new TravelingParameters(); tp.SpeedCommand = new ScalarSpeedCommand(0.0); tp.UsingSpeed = true; tp.DistanceToGo = 0.0; tp.VehiclesToIgnore = new List<int>(); tp.RecommendedSpeed = 0.0; tp.NextState = CoreCommon.CorePlanningState; tp.Behavior = new StayInLaneBehavior(initial.LaneId, tp.SpeedCommand, new List<int>(), initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(state.Front, true), initial.NumberOfLanesRight(state.Front, true)); // parameterize LaneChangeParameters lcp = new LaneChangeParameters(); lcp.Available = false; lcp.Feasible = true; lcp.Parameters = tp; return lcp; } #endregion } } #endregion #region Passing Lane Change else if (information.Reason == LaneChangeReason.SlowForwardVehicle) { throw new Exception("passing slow vehicles not yet supported"); } #endregion // fallout returns null return null; } #endregion #region Target Lane Not Valid, Plan Navigation // otherwise plan for when we approach target if this is a navigational change else if(information.Reason == LaneChangeReason.Navigation) { // parameters for traveling in current lane to hit other List<TravelingParameters> tps = new List<TravelingParameters>(); // add current params tps.Add(this.ForwardMonitor.CurrentParameters); // distance between front of car and start of lane if (target.RelativelyInside(state.Front) || initial.DistanceBetween(state.Front, target.LanePath().StartPoint.Location) > 0) { #region Vehicle and Navigation // check to see if we're not looped around wierd if (lateralReasoning.LateralLane.LanePath().GetClosestPoint(state.Front).Equals(lateralReasoning.LateralLane.LanePath().StartPoint)) { // initialize the forward tracker in the other lane ForwardVehicleTracker fvt = new ForwardVehicleTracker(); fvt.Update(lateralReasoning.LateralLane, state); // check to see if should use the forward tracker if (fvt.ShouldUseForwardTracker) { // get navigation parameterization TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, lateralReasoning.ForwardVehicle(state)); tps.Add(navParams); } else { // get navigation parameterization TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, null); tps.Add(navParams); } } #endregion #region Navigation // check to see that nav point is downstream of us else if (initial.DistanceBetween(state.Front, goal) > 0.0) { // get navigation parameterization TravelingParameters navParams = this.ForwardMonitor.ParameterizationHelper(initial, lateralReasoning.LateralLane, goal, state.Front, CoreCommon.CorePlanningState, state, null); tps.Add(navParams); } #endregion else { return null; } } else return null; // get final tps.Sort(); TravelingParameters final = tps[0]; // parameterize LaneChangeParameters lcp = new LaneChangeParameters(); lcp.Available = false; lcp.Feasible = true; lcp.Parameters = final; return lcp; } #endregion // fallout return null return null; }
/// <summary> /// Parameters to follow the forward vehicle /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public TravelingParameters Follow(ArbiterLane lane, VehicleState state) { // travelling parameters TravelingParameters tp = new TravelingParameters(); // ignorable obstacles List<int> ignoreVehicle = new List<int>(); ignoreVehicle.Add(CurrentVehicle.VehicleId); // get control parameters ForwardVehicleTrackingControl fvtc = GetControl(lane, state); // init params tp.DistanceToGo = fvtc.xDistanceToGood; tp.NextState = CoreCommon.CorePlanningState; tp.RecommendedSpeed = fvtc.vFollowing; tp.Type = TravellingType.Vehicle; tp.Decorators = TurnDecorators.NoDecorators; // flag to ignore forward vehicle bool ignoreForward = false; // reversed lane path LinePath lp = lane.LanePath().Clone(); lp.Reverse(); #region Following Control #region Immediate Stop // need to stop immediately if (fvtc.vFollowing == 0.0) { // don't ignore forward ignoreForward = false; // speed command SpeedCommand sc = new ScalarSpeedCommand(0.0); tp.UsingSpeed = true; // standard path following behavior Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); tp.Behavior = final; tp.SpeedCommand = sc; } #endregion #region Distance Stop // stop at distance else if (fvtc.vFollowing < 0.7 && CoreCommon.Communications.GetVehicleSpeed().Value <= 2.24 && fvtc.xSeparation > fvtc.xAbsMin) { // ignore forward vehicle ignoreForward = true; // speed command SpeedCommand sc = new StopAtDistSpeedCommand(fvtc.xDistanceToGood); tp.UsingSpeed = false; // standard path following behavior Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); tp.Behavior = final; tp.SpeedCommand = sc; } #endregion #region Normal Following // else normal else { // ignore the forward vehicle as we are tracking properly ignoreForward = true; // speed command SpeedCommand sc = new ScalarSpeedCommand(fvtc.vFollowing); tp.DistanceToGo = fvtc.xDistanceToGood; tp.NextState = CoreCommon.CorePlanningState; tp.RecommendedSpeed = fvtc.vFollowing; tp.Type = TravellingType.Vehicle; tp.UsingSpeed = true; // standard path following behavior Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); tp.Behavior = final; tp.SpeedCommand = sc; } #endregion #endregion // check ignore if (ignoreForward) { List<int> ignorable = new List<int>(); ignorable.Add(this.CurrentVehicle.VehicleId); tp.VehiclesToIgnore = ignorable; } else tp.VehiclesToIgnore = new List<int>(); // return parameterization return tp; }
/// <summary> /// Parameters to follow the forward vehicle /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public TravelingParameters Follow(IFQMPlanable lane, VehicleState state, List<ArbiterWaypoint> ignorable) { // travelling parameters TravelingParameters tp = new TravelingParameters(); // get control parameters ForwardVehicleTrackingControl fvtc = GetControl(lane, state, ignorable); this.ForwardControl = fvtc; // initialize the parameters tp.DistanceToGo = fvtc.xDistanceToGood; tp.NextState = CoreCommon.CorePlanningState; tp.RecommendedSpeed = fvtc.vFollowing; tp.Type = TravellingType.Vehicle; tp.Decorators = new List<BehaviorDecorator>(); // ignore the forward vehicles tp.VehiclesToIgnore = this.VehiclesToIgnore; #region Following Control #region Immediate Stop // need to stop immediately if (fvtc.vFollowing == 0.0) { // speed command SpeedCommand sc = new ScalarSpeedCommand(0.0); tp.SpeedCommand = sc; tp.UsingSpeed = true; if (lane is ArbiterLane) { // standard path following behavior ArbiterLane al = ((ArbiterLane)lane); Behavior final = new StayInLaneBehavior(al.LaneId, sc, this.VehiclesToIgnore, al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); final.Decorators = tp.Decorators; tp.Behavior = final; } else { SupraLane sl = (SupraLane)lane; StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; Behavior final = sisls.GetBehavior(sc, state.Front, this.VehiclesToIgnore); final.Decorators = tp.Decorators; tp.Behavior = final; } } #endregion #region Stopping at Distance // stop at distance else if (fvtc.vFollowing < 0.7 && CoreCommon.Communications.GetVehicleSpeed().Value <= 2.24 && fvtc.xSeparation > fvtc.xAbsMin) { // speed command SpeedCommand sc = new StopAtDistSpeedCommand(fvtc.xDistanceToGood); tp.SpeedCommand = sc; tp.UsingSpeed = false; if (lane is ArbiterLane) { ArbiterLane al = (ArbiterLane)lane; // standard path following behavior Behavior final = new StayInLaneBehavior(al.LaneId, sc, this.VehiclesToIgnore, lane.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); final.Decorators = tp.Decorators; tp.Behavior = final; } else { SupraLane sl = (SupraLane)lane; StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; Behavior final = sisls.GetBehavior(sc, state.Front, this.VehiclesToIgnore); final.Decorators = tp.Decorators; tp.Behavior = final; } } #endregion #region Normal Following // else normal else { // speed command SpeedCommand sc = new ScalarSpeedCommand(fvtc.vFollowing); tp.DistanceToGo = fvtc.xDistanceToGood; tp.NextState = CoreCommon.CorePlanningState; tp.RecommendedSpeed = fvtc.vFollowing; tp.Type = TravellingType.Vehicle; tp.UsingSpeed = true; tp.SpeedCommand = sc; if (lane is ArbiterLane) { ArbiterLane al = ((ArbiterLane)lane); // standard path following behavior Behavior final = new StayInLaneBehavior(al.LaneId, sc, this.VehiclesToIgnore, lane.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); final.Decorators = tp.Decorators; tp.Behavior = final; } else { SupraLane sl = (SupraLane)lane; StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; Behavior final = sisls.GetBehavior(sc, state.Front, this.VehiclesToIgnore); final.Decorators = tp.Decorators; tp.Behavior = final; } } #endregion #endregion #region Check for Oncoming Vehicles // check if need to add current lane oncoming vehicle decorator if (false && this.CurrentVehicle.PassedDelayedBirth && fvtc.forwardOncoming && fvtc.xSeparation > TahoeParams.VL && fvtc.xSeparation < 30) { // check valid lane area if (lane is ArbiterLane || ((SupraLane)lane).ClosestComponent(this.CurrentVehicle.ClosestPosition) == SLComponentType.Initial) { // get distance to and speed of the forward vehicle double fvDistance = fvtc.xSeparation; double fvSpeed = fvtc.vTarget; // create the 5mph behavior ScalarSpeedCommand updated = new ScalarSpeedCommand(2.24); // set that we are using speed tp.UsingSpeed = true; tp.RecommendedSpeed = updated.Speed; tp.DistanceToGo = fvtc.xSeparation; // create the decorator OncomingVehicleDecorator ovd = new OncomingVehicleDecorator(updated, fvDistance, fvSpeed); // add the decorator tp.Behavior.Decorators.Add(ovd); tp.Decorators.Add(ovd); } } #endregion // set current this.followingParameters = tp; // return parameterization return tp; }
/// <summary> /// Gets parameterization /// </summary> /// <param name="lane"></param> /// <param name="speed"></param> /// <param name="distance"></param> /// <param name="state"></param> /// <returns></returns> public TravelingParameters GetParameters(double speed, double distance, ArbiterWaypoint final, VehicleState state, bool canUseDistance) { double distanceCutOff = CoreCommon.OperationalStopDistance; SpeedCommand sc; bool usingSpeed = true; Maneuver m; #region Generate Next State // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.TurnInfo(final, out finalPath, out leftLL, out rightLL); TurnState ts = new TurnState(this.turn, this.turn.TurnDirection, final.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(speed)); #endregion #region Distance Cutoff // check if distance is less than cutoff if (distance < distanceCutOff && canUseDistance) { // default behavior sc = new StopAtDistSpeedCommand(distance); // stopping so not using speed param usingSpeed = false; } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // default behavior sc = new ScalarSpeedCommand(speed); } #endregion #region Generate Maneuver if (ts.Interconnect.InitialGeneric is ArbiterWaypoint && CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(((ArbiterWaypoint)ts.Interconnect.InitialGeneric).AreaSubtypeWaypointId)) { Polygon p = CoreCommon.RoadNetwork.IntersectionLookup[((ArbiterWaypoint)ts.Interconnect.InitialGeneric).AreaSubtypeWaypointId].IntersectionPolygon; // behavior Behavior b = new TurnBehavior(ts.TargetLane.LaneId, ts.EndingPath, ts.LeftBound, ts.RightBound, sc, p, this.forwardMonitor.VehiclesToIgnore, ts.Interconnect.InterconnectId); m = new Maneuver(b, ts, ts.DefaultStateDecorators, state.Timestamp); } else { // behavior Behavior b = new TurnBehavior(ts.TargetLane.LaneId, ts.EndingPath, ts.LeftBound, ts.RightBound, sc, null, this.forwardMonitor.VehiclesToIgnore, ts.Interconnect.InterconnectId); m = new Maneuver(b, ts, ts.DefaultStateDecorators, state.Timestamp); } #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = m.PrimaryBehavior.Decorators; tp.DistanceToGo = distance; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = speed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.SpeedCommand = sc; tp.VehiclesToIgnore = this.forwardMonitor.VehiclesToIgnore; #endregion // return navigation params return(tp); }
/// <summary> /// Makes new parameterization for nav /// </summary> /// <param name="lane"></param> /// <param name="lanePlan"></param> /// <param name="speed"></param> /// <param name="distance"></param> /// <param name="stopType"></param> /// <returns></returns> public TravelingParameters NavStopParameterization(IFQMPlanable lane, RoadPlan roadPlan, double speed, double distance, ArbiterWaypoint stopWaypoint, StopType stopType, VehicleState state) { // get min dist double distanceCutOff = stopType == StopType.StopLine ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance; #region Get Decorators // turn direction default ArbiterTurnDirection atd = ArbiterTurnDirection.Straight; List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators; // check if need decorators if (lane is ArbiterLane && stopWaypoint.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest) && roadPlan.BestPlan.laneWaypointOfInterest.IsExit && distance < 40.0) { if (roadPlan.BestPlan.laneWaypointOfInterest.BestExit == null) ArbiterOutput.Output("NAV BUG: lanePlan.laneWaypointOfInterest.BestExit: FQM NavStopParameterization"); else { switch (roadPlan.BestPlan.laneWaypointOfInterest.BestExit.TurnDirection) { case ArbiterTurnDirection.Left: decorators = TurnDecorators.LeftTurnDecorator; atd = ArbiterTurnDirection.Left; break; case ArbiterTurnDirection.Right: atd = ArbiterTurnDirection.Right; decorators = TurnDecorators.RightTurnDecorator; break; case ArbiterTurnDirection.Straight: atd = ArbiterTurnDirection.Straight; decorators = TurnDecorators.NoDecorators; break; case ArbiterTurnDirection.UTurn: atd = ArbiterTurnDirection.UTurn; decorators = TurnDecorators.LeftTurnDecorator; break; } } } else if (lane is SupraLane) { SupraLane sl = (SupraLane)lane; double distToInterconnect = sl.DistanceBetween(state.Front, sl.Interconnect.InitialGeneric.Position); if ((distToInterconnect > 0 && distToInterconnect < 40.0) || sl.ClosestComponent(state.Front) == SLComponentType.Interconnect) { switch (sl.Interconnect.TurnDirection) { case ArbiterTurnDirection.Left: decorators = TurnDecorators.LeftTurnDecorator; atd = ArbiterTurnDirection.Left; break; case ArbiterTurnDirection.Right: atd = ArbiterTurnDirection.Right; decorators = TurnDecorators.RightTurnDecorator; break; case ArbiterTurnDirection.Straight: atd = ArbiterTurnDirection.Straight; decorators = TurnDecorators.NoDecorators; break; case ArbiterTurnDirection.UTurn: atd = ArbiterTurnDirection.UTurn; decorators = TurnDecorators.LeftTurnDecorator; break; } } } #endregion #region Get Maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; SpeedCommand sc = new StopAtDistSpeedCommand(distance); #region Distance Cutoff // check if distance is less than cutoff if (distance < distanceCutOff && stopType != StopType.EndOfLane) { // default behavior Behavior b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtDistSpeedCommand(distance), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true)); // stopping so not using speed param usingSpeed = false; // exit is next if (stopType == StopType.Exit) { // exit means stopping at a good exit in our current lane IState nextState = new StoppingAtExitState(stopWaypoint.Lane, stopWaypoint, atd, true, roadPlan.BestPlan.laneWaypointOfInterest.BestExit, state.Timestamp, state.Front); m = new Maneuver(b, nextState, decorators, state.Timestamp); } // stop line is left else if (stopType == StopType.StopLine) { // determine if hte stop line is the best exit bool isNavExit = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Equals(stopWaypoint); // get turn direction atd = isNavExit ? atd : ArbiterTurnDirection.Straight; // predetermine interconnect if best exit ArbiterInterconnect desired = null; if (isNavExit) desired = roadPlan.BestPlan.laneWaypointOfInterest.BestExit; else if (stopWaypoint.NextPartition != null && state.Front.DistanceTo(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > 25) desired = stopWaypoint.NextPartition.ToInterconnect; // set decorators decorators = isNavExit ? decorators : TurnDecorators.NoDecorators; // stop at the stop IState nextState = new StoppingAtStopState(stopWaypoint.Lane, stopWaypoint, atd, isNavExit, desired); b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtLineSpeedCommand(), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true)); m = new Maneuver(b, nextState, decorators, state.Timestamp); sc = new StopAtLineSpeedCommand(); } else if(stopType == StopType.LastGoal) { // stop at the last goal IState nextState = new StayInLaneState(stopWaypoint.Lane, CoreCommon.CorePlanningState); m = new Maneuver(b, nextState, decorators, state.Timestamp); } } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // set speed sc = new ScalarSpeedCommand(speed); // check if lane if (lane is ArbiterLane) { // get lane ArbiterLane al = (ArbiterLane)lane; // default behavior Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), decorators, state.Timestamp); } // check if supra lane else if (lane is SupraLane) { // get lane SupraLane sl = (SupraLane)lane; // get sl state StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; // get default beheavior Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List<int>()); // standard behavior is fine for maneuver m = new Maneuver(b, sisls, decorators, state.Timestamp); } } #endregion #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = m.PrimaryBehavior.Decorators; tp.DistanceToGo = distance; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = speed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.SpeedCommand = sc; tp.VehiclesToIgnore = new List<int>(); // return navigation params return tp; #endregion }
/// <summary> /// Generate the traveling parameterization for the desired behaivor /// </summary> /// <param name="lane"></param> /// <param name="navStopSpeed"></param> /// <param name="navStopDistance"></param> /// <param name="navStop"></param> /// <param name="navStopType"></param> /// <param name="state"></param> /// <returns></returns> private TravelingParameters NavStopParameterization(ArbiterLane lane, double navStopSpeed, double navStopDistance, ArbiterWaypoint navStop, StopType navStopType, VehicleState state) { // get min dist double distanceCutOff = CoreCommon.OperationalStopDistance; // turn direction default List <BehaviorDecorator> decorators = TurnDecorators.NoDecorators; // create new params TravelingParameters tp = new TravelingParameters(); #region Get Maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; // get lane path LinePath lp = lane.LanePath().Clone(); lp.Reverse(); #region Distance Cutoff // check if distance is less than cutoff if (navStopDistance < distanceCutOff) { // default behavior tp.SpeedCommand = new StopAtDistSpeedCommand(navStopDistance); Behavior b = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(navStopDistance), new List <int>(), lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false)); // stopping so not using speed param usingSpeed = false; IState nextState = CoreCommon.CorePlanningState; m = new Maneuver(b, nextState, decorators, state.Timestamp); } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // get lane ArbiterLane al = lane; // default behavior tp.SpeedCommand = new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)); Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)), new List <int>(), lp, al.Width, al.NumberOfLanesRight(state.Front, false), al.NumberOfLanesLeft(state.Front, false)); // standard behavior is fine for maneuver m = new Maneuver(b, CoreCommon.CorePlanningState, decorators, state.Timestamp); } #endregion #endregion #region Parameterize tp.Behavior = m.PrimaryBehavior; tp.Decorators = m.PrimaryBehavior.Decorators; tp.DistanceToGo = navStopDistance; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = navStopSpeed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.VehiclesToIgnore = new List <int>(); // return navigation params return(tp); #endregion }
/// <summary> /// Behavior given we stay in the current lane /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <param name="downstreamPoint"></param> /// <returns></returns> public TravelingParameters Primary(IFQMPlanable lane, VehicleState state, RoadPlan roadPlan, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, bool log) { // possible parameterizations List<TravelingParameters> tps = new List<TravelingParameters>(); #region Lane Major Parameterizations with Current Lane Goal Params, If Best Goal Exists in Current Lane // check if the best goal is in the current lane ArbiterWaypoint lanePoint = null; if (lane.AreaComponents.Contains(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Lane)) lanePoint = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest; // get the next thing we need to stop at no matter what and parameters for stopping at it ArbiterWaypoint laneNavStop; double laneNavStopSpeed; double laneNavStopDistance; StopType laneNavStopType; this.NextNavigationalStop(lane, lanePoint, state.Front, state.ENCovariance, ignorable, out laneNavStopSpeed, out laneNavStopDistance, out laneNavStopType, out laneNavStop); // create parameterization of the stop TravelingParameters laneNavParams = this.NavStopParameterization(lane, roadPlan, laneNavStopSpeed, laneNavStopDistance, laneNavStop, laneNavStopType, state); this.navigationParameters = laneNavParams; this.laneParameters = laneNavParams; tps.Add(laneNavParams); #region Log if (log) { // add to current parames to arbiter information CoreCommon.CurrentInformation.FQMBehavior = laneNavParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FQMBehaviorInfo = laneNavParams.Behavior.ShortBehaviorInformation(); CoreCommon.CurrentInformation.FQMSpeedCommand = laneNavParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FQMDistance = laneNavParams.DistanceToGo.ToString("F6"); CoreCommon.CurrentInformation.FQMSpeed = laneNavParams.RecommendedSpeed.ToString("F6"); CoreCommon.CurrentInformation.FQMState = laneNavParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FQMStateInfo = laneNavParams.NextState.StateInformation(); CoreCommon.CurrentInformation.FQMStopType = laneNavStopType.ToString(); CoreCommon.CurrentInformation.FQMWaypoint = laneNavStop.ToString(); CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = lane.CurrentMaximumSpeed(state.Position).ToString("F1"); } #endregion #endregion #region Forward Vehicle Parameterization // forward vehicle update this.ForwardVehicle.Update(lane, state); // clear current params this.followingParameters = null; // check not in a sparse area bool sparseArea = lane is ArbiterLane ? ((ArbiterLane)lane).GetClosestPartition(state.Front).Type == PartitionType.Sparse : ((SupraLane)lane).ClosestComponent(state.Front) == SLComponentType.Initial && ((SupraLane)lane).Initial.GetClosestPartition(state.Front).Type == PartitionType.Sparse; // exists forward vehicle if (!sparseArea && this.ForwardVehicle.ShouldUseForwardTracker) { // get forward vehicle params, set lane decorators TravelingParameters vehicleParams = this.ForwardVehicle.Follow(lane, state, ignorable); vehicleParams.Behavior.Decorators.AddRange(this.laneParameters.Decorators); this.FollowingParameters = vehicleParams; #region Log if (log) { // add to current parames to arbiter information CoreCommon.CurrentInformation.FVTBehavior = vehicleParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FVTSpeed = this.ForwardVehicle.FollowingParameters.RecommendedSpeed.ToString("F3"); CoreCommon.CurrentInformation.FVTSpeedCommand = vehicleParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FVTDistance = vehicleParams.DistanceToGo.ToString("F2"); CoreCommon.CurrentInformation.FVTState = vehicleParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FVTStateInfo = vehicleParams.NextState.StateInformation(); // set xSeparation CoreCommon.CurrentInformation.FVTXSeparation = this.ForwardVehicle.ForwardControl.xSeparation.ToString("F0"); } #endregion // check if we're stopped behind fv, allow wait timer if true, stop wait timer if not behind fv bool forwardVehicleStopped = this.ForwardVehicle.CurrentVehicle.IsStopped; bool forwardSeparationGood = this.ForwardVehicle.ForwardControl.xSeparation < TahoeParams.VL * 2.5; bool wereStopped = CoreCommon.Communications.GetVehicleSpeed().Value < 0.1; bool forwardDistanceToGo = vehicleParams.DistanceToGo < 3.5; if (forwardVehicleStopped && forwardSeparationGood && wereStopped && forwardDistanceToGo) this.ForwardVehicle.StoppedBehindForwardVehicle = true; else { this.ForwardVehicle.StoppedBehindForwardVehicle = false; this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop(); this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset(); } // add vehicle param tps.Add(vehicleParams); } else { // no forward vehicle this.followingParameters = null; this.ForwardVehicle.StoppedBehindForwardVehicle = false; } #endregion #region Sparse Waypoint Parameterization // check for sparse waypoints downstream bool sparseDownstream; bool sparseNow; double sparseDistance; lane.SparseDetermination(state.Front, out sparseDownstream, out sparseNow, out sparseDistance); // check if sparse areas downstream if (sparseDownstream) { // set the distance to the sparse area if (sparseNow) sparseDistance = 0.0; // get speed double speed = SpeedTools.GenerateSpeed(sparseDistance, 2.24, lane.CurrentMaximumSpeed(state.Front)); // maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; SpeedCommand sc = new ScalarSpeedCommand(speed); #region Parameterize Given Speed Command // check if lane if (lane is ArbiterLane) { // get lane ArbiterLane al = (ArbiterLane)lane; // default behavior Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); b.Decorators = this.laneParameters.Decorators; // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), this.laneParameters.Decorators, state.Timestamp); } // check if supra lane else if (lane is SupraLane) { // get lane SupraLane sl = (SupraLane)lane; // get sl state StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; // get default beheavior Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List<int>()); b.Decorators = this.laneParameters.Decorators; // standard behavior is fine for maneuver m = new Maneuver(b, sisls, this.laneParameters.Decorators, state.Timestamp); } #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = this.laneParameters.Decorators; tp.DistanceToGo = Double.MaxValue; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = speed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.SpeedCommand = sc; tp.VehiclesToIgnore = new List<int>(); // return navigation params tps.Add(tp); #endregion } #endregion // sort params by most urgent tps.Sort(); // set current params this.currentParameters = tps[0]; // get behavior to check add vehicles to ignore if (this.currentParameters.Behavior is StayInLaneBehavior) ((StayInLaneBehavior)this.currentParameters.Behavior).IgnorableObstacles = this.ForwardVehicle.VehiclesToIgnore; // out of navigation, blockages, and vehicle following determine the actual primary parameters for this lane return tps[0]; }
/// <summary> /// Gets primary maneuver given our position and the turn we are traveling upon /// </summary> /// <param name="vehicleState"></param> /// <returns></returns> public Maneuver PrimaryManeuver(VehicleState vehicleState, List <ITacticalBlockage> blockages, TurnState turnState) { #region Check are planning over the correct turn if (CoreCommon.CorePlanningState is TurnState) { TurnState ts = (TurnState)CoreCommon.CorePlanningState; if (this.turn == null || !this.turn.Equals(ts.Interconnect)) { this.turn = ts.Interconnect; this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null); } else if (this.forwardMonitor.turn == null || !this.forwardMonitor.turn.Equals(ts.Interconnect)) { this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null); } } #endregion #region Blockages // check blockages if (blockages != null && blockages.Count > 0 && blockages[0] is TurnBlockage) { // create the blockage state EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState); // check not at highest level already if (turnState.Saudi != SAUDILevel.L1 || turnState.UseTurnBounds) { // check not from a dynamicly moving vehicle if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic || (TacticalDirector.ValidVehicles.ContainsKey(blockages[0].BlockageReport.TrackID) && TacticalDirector.ValidVehicles[blockages[0].BlockageReport.TrackID].IsStopped)) { // go to a blockage handling tactical return(new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } else { ArbiterOutput.Output("Turn blockage reported for moving vehicle, ignoring"); } } else { ArbiterOutput.Output("Turn blockage, but recovery escalation already at highest state, ignoring report"); } } #endregion #region Intersection Check if (!this.CanGo(vehicleState)) { if (turn.FinalGeneric is ArbiterWaypoint) { TravelingParameters tp = this.GetParameters(0.0, 0.0, (ArbiterWaypoint)turn.FinalGeneric, vehicleState, false); return(new Maneuver(tp.Behavior, CoreCommon.CorePlanningState, tp.NextState.DefaultStateDecorators, vehicleState.Timestamp)); } else { // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL); // hold brake IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0)); TurnBehavior b = new TurnBehavior(null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0), this.turn.InterconnectId); return(new Maneuver(b, CoreCommon.CorePlanningState, nextState.DefaultStateDecorators, vehicleState.Timestamp)); } } #endregion #region Final is Lane Waypoint if (turn.FinalGeneric is ArbiterWaypoint) { // final point ArbiterWaypoint final = (ArbiterWaypoint)turn.FinalGeneric; // plan down entry lane RoadPlan rp = navigation.PlanNavigableArea(final.Lane, final.Position, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], new List <ArbiterWaypoint>()); // point of interest downstream DownstreamPointOfInterest dpoi = rp.BestPlan.laneWaypointOfInterest; // get path this represents List <Coordinates> pathCoordinates = new List <Coordinates>(); pathCoordinates.Add(vehicleState.Position); foreach (ArbiterWaypoint aw in final.Lane.WaypointsInclusive(final, final.Lane.WaypointList[final.Lane.WaypointList.Count - 1])) { pathCoordinates.Add(aw.Position); } LinePath lp = new LinePath(pathCoordinates); // list of all parameterizations List <TravelingParameters> parameterizations = new List <TravelingParameters>(); // get lane navigation parameterization TravelingParameters navigationParameters = this.NavigationParameterization(vehicleState, dpoi, final, lp); parameterizations.Add(navigationParameters); // update forward tracker and get vehicle parameterizations if forward vehicle exists this.forwardMonitor.Update(vehicleState, final, lp); if (this.forwardMonitor.ShouldUseForwardTracker()) { // get vehicle parameterization TravelingParameters vehicleParameters = this.VehicleParameterization(vehicleState, lp, final); parameterizations.Add(vehicleParameters); } // sort and return funal parameterizations.Sort(); // get the final behavior TurnBehavior tb = (TurnBehavior)parameterizations[0].Behavior; // get vehicles to ignore tb.VehiclesToIgnore = this.forwardMonitor.VehiclesToIgnore; // add persistent information about saudi level if (turnState.Saudi == SAUDILevel.L1) { tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray()); tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1)); } // add persistent information about turn bounds if (!turnState.UseTurnBounds) { tb.LeftBound = null; tb.RightBound = null; } // return the behavior return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp)); } #endregion #region Final is Zone Waypoint else if (turn.FinalGeneric is ArbiterPerimeterWaypoint) { // get inteconnect path Coordinates entryVec = ((ArbiterPerimeterWaypoint)turn.FinalGeneric).Perimeter.PerimeterPolygon.BoundingCircle.center - turn.FinalGeneric.Position; entryVec = entryVec.Normalize(TahoeParams.VL / 2.0); LinePath ip = new LinePath(new Coordinates[] { turn.InitialGeneric.Position, turn.FinalGeneric.Position, entryVec + this.turn.FinalGeneric.Position }); // get distance from end double d = ip.DistanceBetween( ip.GetClosestPoint(vehicleState.Front), ip.EndPoint); // get speed command SpeedCommand sc = null; if (d < TahoeParams.VL) { sc = new StopAtDistSpeedCommand(d); } else { sc = new ScalarSpeedCommand(SpeedTools.GenerateSpeed(d - TahoeParams.VL, 1.7, turn.MaximumDefaultSpeed)); } // final perimeter waypoint ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)this.turn.FinalGeneric; // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL); // hold brake IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, sc); TurnBehavior tb = new TurnBehavior(null, finalPath, leftLL, rightLL, sc, null, new List <int>(), this.turn.InterconnectId); // add persistent information about saudi level if (turnState.Saudi == SAUDILevel.L1) { tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray()); tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1)); } // add persistent information about turn bounds if (!turnState.UseTurnBounds) { tb.LeftBound = null; tb.RightBound = null; } // return maneuver return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp)); } #endregion #region Unknown else { throw new Exception("unrecognized type: " + turn.FinalGeneric.ToString()); } #endregion }