/// <summary> /// Determines proper speed commands given we want to stop at a certain waypoint /// </summary> /// <param name="waypoint"></param> /// <param name="lane"></param> /// <param name="position"></param> /// <param name="enCovariance"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public void StoppingParams(Coordinates coordinate, IFQMPlanable lane, Coordinates position, double[] enCovariance, out double stopSpeed, out double stopDistance) { // get dist to waypoint stopDistance = lane.DistanceBetween(position, coordinate); // subtract distance based upon type to help calculate speed double stopSpeedDistance = stopDistance - CoreCommon.OperationalStopDistance; // check if we are positive distance away if (stopSpeedDistance >= 0) { // segment max speed double segmentMaxSpeed = lane.CurrentMaximumSpeed(position); // get speed using constant acceleration stopSpeed = SpeedTools.GenerateSpeed(stopSpeedDistance, CoreCommon.OperationalStopSpeed, segmentMaxSpeed); } else { // inside stop dist stopSpeed = (stopDistance / CoreCommon.OperationalStopDistance) * CoreCommon.OperationalStopSpeed; stopSpeed = stopSpeed < 0 ? 0.0 : stopSpeed; } }
/// <summary> /// Updates the current vehicle /// </summary> /// <param name="lane"></param> /// <param name="state"></param> public void Update(IFQMPlanable lane, VehicleState state) { // get the forward path LinePath p = lane.LanePath(); // get our position Coordinates f = state.Front; // get all vehicles associated with those components List <VehicleAgent> vas = new List <VehicleAgent>(); foreach (IVehicleArea iva in lane.AreaComponents) { if (TacticalDirector.VehicleAreas.ContainsKey(iva)) { vas.AddRange(TacticalDirector.VehicleAreas[iva]); } } // get the closest forward of us double minDistance = Double.MaxValue; VehicleAgent closest = null; // set the vehicles to ignore this.VehiclesToIgnore = new List <int>(); // get clsoest foreach (VehicleAgent va in vas) { // get position of front Coordinates frontPos = va.ClosestPosition; // check relatively inside if (lane.RelativelyInside(frontPos)) { // distance to vehicle double frontDist = lane.DistanceBetween(f, frontPos); // check forward of us if (frontDist > 0) { // add to ignorable this.VehiclesToIgnore.Add(va.VehicleId); // check for closest if (frontDist < minDistance) { minDistance = frontDist; closest = va; } } } } this.CurrentVehicle = closest; this.currentDistance = minDistance; }
/// <summary> /// Distance from coord to vehicle along lane /// </summary> /// <param name="lane"></param> /// <param name="c"></param> /// <returns></returns> public double DistanceToVehicle(IFQMPlanable lane, Coordinates c) { if (CurrentVehicle != null) { return(lane.DistanceBetween(c, CurrentVehicle.ClosestPosition)); } else { return(Double.MaxValue); } }
/// <summary> /// Next point at which to stop /// </summary> /// <param name="lane"></param> /// <param name="position"></param> /// <param name="stopPoint"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public void NextLaneStop(IFQMPlanable lane, Coordinates position, double[] enCovariance, List <ArbiterWaypoint> ignorable, out ArbiterWaypoint stopPoint, out double stopSpeed, out double stopDistance, out StopType stopType) { // get the stop List <WaypointType> wts = new List <WaypointType>(); wts.Add(WaypointType.Stop); wts.Add(WaypointType.End); stopPoint = lane.GetNext(position, wts, ignorable); // set stop type stopType = stopPoint.IsStop ? StopType.StopLine : StopType.EndOfLane; // parameterize this.StoppingParams(stopPoint, lane, position, enCovariance, out stopSpeed, out stopDistance); }
/// <summary> /// Constructor /// </summary> /// <param name="leftLateral"></param> /// <param name="rightLateral"></param> public ForwardReasoning(ILateralReasoning leftLateral, ILateralReasoning rightLateral, IFQMPlanable lane) { this.Lane = lane; this.leftLateralReasoning = leftLateral; this.rightLateralReasoning = rightLateral; this.ForwardMonitor = new ForwardQuadrantMonitor(); this.blockageTimer = new Stopwatch(); if (lane is ArbiterLane) { this.RearMonitor = new RearQuadrantMonitor((ArbiterLane)lane, SideObstacleSide.Driver); } else { this.RearMonitor = new RearQuadrantMonitor(((SupraLane)lane).Initial, SideObstacleSide.Driver); } }
/// <summary> /// Determines the point at which we need to stop in the current lane /// </summary> /// <param name="lanePoint"></param> /// <param name="position"></param> /// <param name="stopRequired"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public void NextNavigationalStop(IFQMPlanable lane, ArbiterWaypoint lanePoint, Coordinates position, double[] enCovariance, List <ArbiterWaypoint> ignorable, out double stopSpeed, out double stopDistance, out StopType stopType, out ArbiterWaypoint stopWaypoint) { // variables for default next stop line or end of lane this.NextLaneStop(lane, position, enCovariance, ignorable, out stopWaypoint, out stopSpeed, out stopDistance, out stopType); if (lanePoint != null) { // check if the point downstream is the last checkpoint if (CoreCommon.Mission.MissionCheckpoints.Count == 1 && lanePoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId)) { ArbiterWaypoint cpStop = lanePoint; double cpStopSpeed; double cpStopDistance; StopType cpStopType = StopType.LastGoal; this.StoppingParams(cpStop, lane, position, enCovariance, out cpStopSpeed, out cpStopDistance); if (cpStopDistance <= stopDistance) { stopSpeed = cpStopSpeed; stopDistance = cpStopDistance; stopType = cpStopType; stopWaypoint = cpStop; } } // check if point is not the checkpoint and is an exit else if (lanePoint.IsExit && !lanePoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId)) { ArbiterWaypoint exitStop = lanePoint; double exitStopSpeed; double exitStopDistance; StopType exitStopType = lanePoint.IsStop ? StopType.StopLine : StopType.Exit; this.StoppingParams(exitStop, lane, position, enCovariance, out exitStopSpeed, out exitStopDistance); if (exitStopDistance <= stopDistance) { stopSpeed = exitStopSpeed; stopDistance = exitStopDistance; stopType = exitStopType; stopWaypoint = exitStop; } } } }
/// <summary> /// Get the vehicle direction along a lane /// </summary> /// <param name="lane"></param> /// <returns></returns> public VehicleDirectionAlongPath VehicleDirectionAlong(IFQMPlanable lane) { if (this.IsStopped || !this.StateMonitor.Observed.headingValid) { return(VehicleDirectionAlongPath.Forwards); } // get point on the lane path LinePath.PointOnPath pop = lane.LanePath().GetClosestPoint(this.ClosestPosition); // get heading of the lane path there Coordinates pathVector = lane.LanePath().GetSegment(pop.Index).UnitVector; // get vehicle heading Coordinates unit = new Coordinates(1, 0); Coordinates headingVector = unit.Rotate(this.StateMonitor.Observed.absoluteHeading); // rotate vehicle heading Coordinates relativeVehicle = headingVector.Rotate(-pathVector.ArcTan); // get path heading double relativeVehicleDegrees = relativeVehicle.ToDegrees() >= 180.0 ? Math.Abs(relativeVehicle.ToDegrees() - 360.0) : Math.Abs(relativeVehicle.ToDegrees()); if (relativeVehicleDegrees < 70) { return(VehicleDirectionAlongPath.Forwards); } else if (relativeVehicleDegrees > 70 && relativeVehicleDegrees < 110) { return(VehicleDirectionAlongPath.Perpendicular); } else { return(VehicleDirectionAlongPath.Reverse); } }
/// <summary> /// Behavior given we stay in the current lane /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <param name="downstreamPoint"></param> /// <returns></returns> public TravelingParameters Primary(IFQMPlanable lane, VehicleState state, RoadPlan roadPlan, List <ITacticalBlockage> blockages, List <ArbiterWaypoint> ignorable, bool log) { // possible parameterizations List <TravelingParameters> tps = new List <TravelingParameters>(); #region Lane Major Parameterizations with Current Lane Goal Params, If Best Goal Exists in Current Lane // check if the best goal is in the current lane ArbiterWaypoint lanePoint = null; if (lane.AreaComponents.Contains(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Lane)) { lanePoint = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest; } // get the next thing we need to stop at no matter what and parameters for stopping at it ArbiterWaypoint laneNavStop; double laneNavStopSpeed; double laneNavStopDistance; StopType laneNavStopType; this.NextNavigationalStop(lane, lanePoint, state.Front, state.ENCovariance, ignorable, out laneNavStopSpeed, out laneNavStopDistance, out laneNavStopType, out laneNavStop); // create parameterization of the stop TravelingParameters laneNavParams = this.NavStopParameterization(lane, roadPlan, laneNavStopSpeed, laneNavStopDistance, laneNavStop, laneNavStopType, state); this.navigationParameters = laneNavParams; this.laneParameters = laneNavParams; tps.Add(laneNavParams); #region Log if (log) { // add to current parames to arbiter information CoreCommon.CurrentInformation.FQMBehavior = laneNavParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FQMBehaviorInfo = laneNavParams.Behavior.ShortBehaviorInformation(); CoreCommon.CurrentInformation.FQMSpeedCommand = laneNavParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FQMDistance = laneNavParams.DistanceToGo.ToString("F6"); CoreCommon.CurrentInformation.FQMSpeed = laneNavParams.RecommendedSpeed.ToString("F6"); CoreCommon.CurrentInformation.FQMState = laneNavParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FQMStateInfo = laneNavParams.NextState.StateInformation(); CoreCommon.CurrentInformation.FQMStopType = laneNavStopType.ToString(); CoreCommon.CurrentInformation.FQMWaypoint = laneNavStop.ToString(); CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = lane.CurrentMaximumSpeed(state.Position).ToString("F1"); } #endregion #endregion #region Forward Vehicle Parameterization // forward vehicle update this.ForwardVehicle.Update(lane, state); // clear current params this.followingParameters = null; // check not in a sparse area bool sparseArea = lane is ArbiterLane ? ((ArbiterLane)lane).GetClosestPartition(state.Front).Type == PartitionType.Sparse : ((SupraLane)lane).ClosestComponent(state.Front) == SLComponentType.Initial && ((SupraLane)lane).Initial.GetClosestPartition(state.Front).Type == PartitionType.Sparse; // exists forward vehicle if (!sparseArea && this.ForwardVehicle.ShouldUseForwardTracker) { // get forward vehicle params, set lane decorators TravelingParameters vehicleParams = this.ForwardVehicle.Follow(lane, state, ignorable); vehicleParams.Behavior.Decorators.AddRange(this.laneParameters.Decorators); this.FollowingParameters = vehicleParams; #region Log if (log) { // add to current parames to arbiter information CoreCommon.CurrentInformation.FVTBehavior = vehicleParams.Behavior.ToShortString(); CoreCommon.CurrentInformation.FVTSpeed = this.ForwardVehicle.FollowingParameters.RecommendedSpeed.ToString("F3"); CoreCommon.CurrentInformation.FVTSpeedCommand = vehicleParams.Behavior.SpeedCommandString(); CoreCommon.CurrentInformation.FVTDistance = vehicleParams.DistanceToGo.ToString("F2"); CoreCommon.CurrentInformation.FVTState = vehicleParams.NextState.ShortDescription(); CoreCommon.CurrentInformation.FVTStateInfo = vehicleParams.NextState.StateInformation(); // set xSeparation CoreCommon.CurrentInformation.FVTXSeparation = this.ForwardVehicle.ForwardControl.xSeparation.ToString("F0"); } #endregion // check if we're stopped behind fv, allow wait timer if true, stop wait timer if not behind fv bool forwardVehicleStopped = this.ForwardVehicle.CurrentVehicle.IsStopped; bool forwardSeparationGood = this.ForwardVehicle.ForwardControl.xSeparation < TahoeParams.VL * 2.5; bool wereStopped = CoreCommon.Communications.GetVehicleSpeed().Value < 0.1; bool forwardDistanceToGo = vehicleParams.DistanceToGo < 3.5; if (forwardVehicleStopped && forwardSeparationGood && wereStopped && forwardDistanceToGo) { this.ForwardVehicle.StoppedBehindForwardVehicle = true; } else { this.ForwardVehicle.StoppedBehindForwardVehicle = false; this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop(); this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset(); } // add vehicle param tps.Add(vehicleParams); } else { // no forward vehicle this.followingParameters = null; this.ForwardVehicle.StoppedBehindForwardVehicle = false; } #endregion #region Sparse Waypoint Parameterization // check for sparse waypoints downstream bool sparseDownstream; bool sparseNow; double sparseDistance; lane.SparseDetermination(state.Front, out sparseDownstream, out sparseNow, out sparseDistance); // check if sparse areas downstream if (sparseDownstream) { // set the distance to the sparse area if (sparseNow) { sparseDistance = 0.0; } // get speed double speed = SpeedTools.GenerateSpeed(sparseDistance, 2.24, lane.CurrentMaximumSpeed(state.Front)); // maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; SpeedCommand sc = new ScalarSpeedCommand(speed); #region Parameterize Given Speed Command // check if lane if (lane is ArbiterLane) { // get lane ArbiterLane al = (ArbiterLane)lane; // default behavior Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List <int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); b.Decorators = this.laneParameters.Decorators; // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), this.laneParameters.Decorators, state.Timestamp); } // check if supra lane else if (lane is SupraLane) { // get lane SupraLane sl = (SupraLane)lane; // get sl state StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; // get default beheavior Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List <int>()); b.Decorators = this.laneParameters.Decorators; // standard behavior is fine for maneuver m = new Maneuver(b, sisls, this.laneParameters.Decorators, state.Timestamp); } #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = this.laneParameters.Decorators; tp.DistanceToGo = Double.MaxValue; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = speed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.SpeedCommand = sc; tp.VehiclesToIgnore = new List <int>(); // return navigation params tps.Add(tp); #endregion } #endregion // sort params by most urgent tps.Sort(); // set current params this.currentParameters = tps[0]; // get behavior to check add vehicles to ignore if (this.currentParameters.Behavior is StayInLaneBehavior) { ((StayInLaneBehavior)this.currentParameters.Behavior).IgnorableObstacles = this.ForwardVehicle.VehiclesToIgnore; } // out of navigation, blockages, and vehicle following determine the actual primary parameters for this lane return(tps[0]); }
/// <summary> /// Makes new parameterization for nav /// </summary> /// <param name="lane"></param> /// <param name="lanePlan"></param> /// <param name="speed"></param> /// <param name="distance"></param> /// <param name="stopType"></param> /// <returns></returns> public TravelingParameters NavStopParameterization(IFQMPlanable lane, RoadPlan roadPlan, double speed, double distance, ArbiterWaypoint stopWaypoint, StopType stopType, VehicleState state) { // get min dist double distanceCutOff = stopType == StopType.StopLine ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance; #region Get Decorators // turn direction default ArbiterTurnDirection atd = ArbiterTurnDirection.Straight; List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators; // check if need decorators if (lane is ArbiterLane && stopWaypoint.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest) && roadPlan.BestPlan.laneWaypointOfInterest.IsExit && distance < 40.0) { if (roadPlan.BestPlan.laneWaypointOfInterest.BestExit == null) ArbiterOutput.Output("NAV BUG: lanePlan.laneWaypointOfInterest.BestExit: FQM NavStopParameterization"); else { switch (roadPlan.BestPlan.laneWaypointOfInterest.BestExit.TurnDirection) { case ArbiterTurnDirection.Left: decorators = TurnDecorators.LeftTurnDecorator; atd = ArbiterTurnDirection.Left; break; case ArbiterTurnDirection.Right: atd = ArbiterTurnDirection.Right; decorators = TurnDecorators.RightTurnDecorator; break; case ArbiterTurnDirection.Straight: atd = ArbiterTurnDirection.Straight; decorators = TurnDecorators.NoDecorators; break; case ArbiterTurnDirection.UTurn: atd = ArbiterTurnDirection.UTurn; decorators = TurnDecorators.LeftTurnDecorator; break; } } } else if (lane is SupraLane) { SupraLane sl = (SupraLane)lane; double distToInterconnect = sl.DistanceBetween(state.Front, sl.Interconnect.InitialGeneric.Position); if ((distToInterconnect > 0 && distToInterconnect < 40.0) || sl.ClosestComponent(state.Front) == SLComponentType.Interconnect) { switch (sl.Interconnect.TurnDirection) { case ArbiterTurnDirection.Left: decorators = TurnDecorators.LeftTurnDecorator; atd = ArbiterTurnDirection.Left; break; case ArbiterTurnDirection.Right: atd = ArbiterTurnDirection.Right; decorators = TurnDecorators.RightTurnDecorator; break; case ArbiterTurnDirection.Straight: atd = ArbiterTurnDirection.Straight; decorators = TurnDecorators.NoDecorators; break; case ArbiterTurnDirection.UTurn: atd = ArbiterTurnDirection.UTurn; decorators = TurnDecorators.LeftTurnDecorator; break; } } } #endregion #region Get Maneuver Maneuver m = new Maneuver(); bool usingSpeed = true; SpeedCommand sc = new StopAtDistSpeedCommand(distance); #region Distance Cutoff // check if distance is less than cutoff if (distance < distanceCutOff && stopType != StopType.EndOfLane) { // default behavior Behavior b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtDistSpeedCommand(distance), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true)); // stopping so not using speed param usingSpeed = false; // exit is next if (stopType == StopType.Exit) { // exit means stopping at a good exit in our current lane IState nextState = new StoppingAtExitState(stopWaypoint.Lane, stopWaypoint, atd, true, roadPlan.BestPlan.laneWaypointOfInterest.BestExit, state.Timestamp, state.Front); m = new Maneuver(b, nextState, decorators, state.Timestamp); } // stop line is left else if (stopType == StopType.StopLine) { // determine if hte stop line is the best exit bool isNavExit = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Equals(stopWaypoint); // get turn direction atd = isNavExit ? atd : ArbiterTurnDirection.Straight; // predetermine interconnect if best exit ArbiterInterconnect desired = null; if (isNavExit) desired = roadPlan.BestPlan.laneWaypointOfInterest.BestExit; else if (stopWaypoint.NextPartition != null && state.Front.DistanceTo(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > 25) desired = stopWaypoint.NextPartition.ToInterconnect; // set decorators decorators = isNavExit ? decorators : TurnDecorators.NoDecorators; // stop at the stop IState nextState = new StoppingAtStopState(stopWaypoint.Lane, stopWaypoint, atd, isNavExit, desired); b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtLineSpeedCommand(), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true)); m = new Maneuver(b, nextState, decorators, state.Timestamp); sc = new StopAtLineSpeedCommand(); } else if(stopType == StopType.LastGoal) { // stop at the last goal IState nextState = new StayInLaneState(stopWaypoint.Lane, CoreCommon.CorePlanningState); m = new Maneuver(b, nextState, decorators, state.Timestamp); } } #endregion #region Outisde Distance Envelope // not inside distance envalope else { // set speed sc = new ScalarSpeedCommand(speed); // check if lane if (lane is ArbiterLane) { // get lane ArbiterLane al = (ArbiterLane)lane; // default behavior Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true)); // standard behavior is fine for maneuver m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), decorators, state.Timestamp); } // check if supra lane else if (lane is SupraLane) { // get lane SupraLane sl = (SupraLane)lane; // get sl state StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; // get default beheavior Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List<int>()); // standard behavior is fine for maneuver m = new Maneuver(b, sisls, decorators, state.Timestamp); } } #endregion #endregion #region Parameterize // create new params TravelingParameters tp = new TravelingParameters(); tp.Behavior = m.PrimaryBehavior; tp.Decorators = m.PrimaryBehavior.Decorators; tp.DistanceToGo = distance; tp.NextState = m.PrimaryState; tp.RecommendedSpeed = speed; tp.Type = TravellingType.Navigation; tp.UsingSpeed = usingSpeed; tp.SpeedCommand = sc; tp.VehiclesToIgnore = new List<int>(); // return navigation params return tp; #endregion }
/// <summary> /// Figure out standard control to follow /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public ForwardVehicleTrackingControl GetControl(IFQMPlanable lane, VehicleState state, List<ArbiterWaypoint> ignorable) { // check exists a vehicle to track if (this.CurrentVehicle != null) { // get the maximum velocity of the segment we're closest to double segV = lane.CurrentMaximumSpeed(state.Front); double segMaxV = segV; // flag if the forward vehicles is in a stop waypoint type safety zone bool safetyZone = false; ArbiterWaypoint nextStop = lane.GetNext(state.Front, WaypointType.Stop, ignorable); double distance = nextStop != null ? lane.DistanceBetween(state.Front, nextStop.Position) : Double.MaxValue; // check if no stop exists or distance > 30 if (nextStop == null || distance > 30 || distance < 0) { // there is no next stop or the forward vehcile is not in a safety zone safetyZone = false; } // otherwise the tracked vehicle is in a safety zone else { // set the forward vehicle as being in a safety zone safetyZone = true; } // minimum distance double xAbsMin; // if we're in a safety zone if (safetyZone) { // set minimum to 1 times the tahoe's vehicle length xAbsMin = TahoeParams.VL * 1.5; } // otherwise we're not in a safety zone else { // set minimum to 2 times the tahoe's vehicle length xAbsMin = TahoeParams.VL * 2.0; } // retrieve the tracked vehicle's scalar absolute speed double vTarget = CurrentVehicle.StateMonitor.Observed.isStopped ? 0.0 : this.CurrentVehicle.Speed; // check if vehicle is moving away from us VehicleDirectionAlongPath vdap = CurrentVehicle.VehicleDirectionAlong(lane); // if moving perpendic, set speed to 0.0 if (vdap == VehicleDirectionAlongPath.Perpendicular) vTarget = 0.0; #region Forward // can use normal tracker if vehicle not oncoming if (true)//vdap != VehicleDirectionAlongPath.Reverse) { // get the good distance behind the target, is xAbsMin if vehicle velocity is small enough double xGood = xAbsMin + (1.5 * (TahoeParams.VL / 4.4704) * vTarget); // get our current separation double xSeparation = this.currentDistance; // determine the envelope to reason about the vehicle, that is, to slow from vMax to vehicle speed double xEnvelope = (Math.Pow(vTarget, 2.0) - Math.Pow(segMaxV, 2.0)) / (2.0 * -0.5); // the distance to the good double xDistanceToGood; // determine the velocity to follow the vehicle ahead double vFollowing; // check if we are basically stopped in the right place behind another vehicle if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // check if we are within the minimum distance else if (xSeparation <= xAbsMin) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // determine if our separation is less than the good distance but not inside minimum else if (xAbsMin < xSeparation && xSeparation < xGood) { // get the distance we are from xMin double xAlong = xSeparation - xAbsMin; // get the total distance from xGood to xMin double xGoodToMin = xGood - xAbsMin; // slow to 0 by min vFollowing = (((xGoodToMin - xAlong) / xGoodToMin) * (-vTarget)) + vTarget; // good distance xDistanceToGood = 0; } // our separation is greater than the good distance else { // good distance xDistanceToGood = xSeparation - xGood; // get differences in max and target velocities vFollowing = SpeedTools.GenerateSpeed(xDistanceToGood, vTarget, segMaxV); } // return return new ForwardVehicleTrackingControl(true, safetyZone, vFollowing, xSeparation, xDistanceToGood, vTarget, xAbsMin, xGood,false); } #endregion #region Oncoming /* else { // determine the distance for the other vehicle to stop double xTargetEnvelope = -Math.Pow(vTarget, 2.0) / (2.0 * -0.5); // determine the distance for our vehicle to stop double xOurEnvelope = -Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2.0) / (2.0 * -0.5); // get the good distance behind the target, is xAbsMin if vehicle velocity is small enough double xGood = xAbsMin + (1.5 * (TahoeParams.VL / 4.4704) * vTarget) + xOurEnvelope + xTargetEnvelope; // get our current separation double xSeparation = this.currentDistance; // determine the envelope for us to slow to 0 by the good distance double xEnvelope = -Math.Pow(segMaxV, 2.0) / (2.0 * -0.5); // the distance to the good double xDistanceToGood; // determine the velocity to follow the vehicle ahead double vFollowing; // check if we are basically stopped in the right place behind another vehicle if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // check if we are within the minimum distance else if (xSeparation <= xGood) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // our separation is greater than the good distance but within the envelope else if (xGood <= xSeparation && xSeparation <= xEnvelope + xGood) { // get differences in max and target velocities double vDifference = segMaxV; // get the distance we are along the speed envolope from xGood double xAlong = xEnvelope - (xSeparation - xGood); // slow to vTarget by good vFollowing = (((xEnvelope - xAlong) / xEnvelope) * (vDifference)); // good distance xDistanceToGood = xSeparation - xGood; } // otherwise xSeparation > xEnvelope else { // can go max speed vFollowing = segMaxV; // good distance xDistanceToGood = xSeparation - xGood; } // return return new ForwardVehicleTrackingControl(true, safetyZone, vFollowing, xSeparation, xDistanceToGood, vTarget, xAbsMin, xGood, true); } */ #endregion } else { // return return new ForwardVehicleTrackingControl(false, false, Double.MaxValue, Double.MaxValue, Double.MaxValue, Double.MaxValue, 0.0, Double.MaxValue, false); } }
/// <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> /// 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? AdvancedSecondary(IFQMPlanable arbiterLane, VehicleState vehicleState, RoadPlan roadPlan, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask) { // check if we might be able to pass here bool validArea = arbiterLane is ArbiterLane || (((SupraLane)arbiterLane).ClosestComponent(vehicleState.Front) == SLComponentType.Initial); // check normal valid area if (validArea) { // get lane we are in ArbiterLane ourForwardLane = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial; // check sl if (arbiterLane is SupraLane) { Dictionary<ArbiterLaneId, LanePlan> tmpPlans = new Dictionary<ArbiterLaneId,LanePlan>(); tmpPlans.Add(ourForwardLane.LaneId, roadPlan.LanePlans[ourForwardLane.LaneId]); RoadPlan tmp = new RoadPlan(tmpPlans); roadPlan = tmp; } // return the advanced plan ArbiterWaypoint goalWp = arbiterLane is SupraLane ? (ArbiterWaypoint)((SupraLane)arbiterLane).Interconnect.InitialGeneric : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest; return this.AdvancedSecondaryNextCheck(ourForwardLane, goalWp, vehicleState, roadPlan, blockages, ignorable, bestTask); } // fall through return null; }
/// <summary> /// Updates the current vehicle /// </summary> /// <param name="lane"></param> /// <param name="state"></param> public void Update(IFQMPlanable lane, VehicleState state) { // get the forward path LinePath p = lane.LanePath(); // get our position Coordinates f = state.Front; // get all vehicles associated with those components List<VehicleAgent> vas = new List<VehicleAgent>(); foreach (IVehicleArea iva in lane.AreaComponents) { if (TacticalDirector.VehicleAreas.ContainsKey(iva)) vas.AddRange(TacticalDirector.VehicleAreas[iva]); } // get the closest forward of us double minDistance = Double.MaxValue; VehicleAgent closest = null; // set the vehicles to ignore this.VehiclesToIgnore = new List<int>(); // get clsoest foreach (VehicleAgent va in vas) { // get position of front Coordinates frontPos = va.ClosestPosition; // check relatively inside if (lane.RelativelyInside(frontPos)) { // distance to vehicle double frontDist = lane.DistanceBetween(f, frontPos); // check forward of us if (frontDist > 0) { // add to ignorable this.VehiclesToIgnore.Add(va.VehicleId); // check for closest if (frontDist < minDistance) { minDistance = frontDist; closest = va; } } } } this.CurrentVehicle = closest; this.currentDistance = minDistance; }
/// <summary> /// Makes use of parameterizations made from the initial forward maneuver plan /// </summary> /// <param name="arbiterLane"></param> /// <param name="vehicleState"></param> /// <param name="roadPlan"></param> /// <param name="blockages"></param> /// <param name="ignorable"></param> /// <returns></returns> public Maneuver? SecondaryManeuver(IFQMPlanable arbiterLane, VehicleState vehicleState, RoadPlan roadPlan, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask) { // check if we might be able to pass here bool validArea = arbiterLane is ArbiterLane || (((SupraLane)arbiterLane).ClosestComponent(vehicleState.Front) == SLComponentType.Initial); ArbiterLane ourForwardLane = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial; // check if the forward vehicle exists and we're in a valid area if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && validArea) { // check if we should pass the vehicle ahead LaneChangeInformation lci; bool sp = this.ForwardMonitor.ForwardVehicle.ShouldPass(out lci); // make sure we should do something before processing extras if(sp) { // available parameterizations for the lane change List<LaneChangeParameters> changeParams = new List<LaneChangeParameters>(); // get lane ArbiterLane al = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial; // get the location we need to return by Coordinates absoluteUpperBound = arbiterLane is ArbiterLane ? roadPlan.LanePlans[al.LaneId].laneWaypointOfInterest.PointOfInterest.Position : ((SupraLane)arbiterLane).Interconnect.InitialGeneric.Position; #region Failed Forward // if failed, parameterize ourselved if we're following them if (lci.Reason == LaneChangeReason.FailedForwardVehicle && this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle) { // notify ArbiterOutput.Output("Failed Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.VehicleId.ToString()); // get traveling params from FQM to make sure we stopped for vehicle, behind vehicle double v = CoreCommon.Communications.GetVehicleSpeed().Value; TravelingParameters fqmParams = this.ForwardMonitor.CurrentParameters; double d = this.ForwardMonitor.ForwardVehicle.DistanceToVehicle(arbiterLane, vehicleState.Front); Coordinates departUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d - 3.0).Location; // check stopped behing failed forward try { if (fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle) { // check for checkpoint within 4VL of front of failed vehicle ArbiterCheckpoint acCurrecnt = CoreCommon.Mission.MissionCheckpoints.Peek(); if (acCurrecnt.WaypointId.AreaSubtypeId.Equals(al.LaneId)) { // check distance ArbiterWaypoint awCheckpoint = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[acCurrecnt.WaypointId]; double cpDistacne = Lane.DistanceBetween(vehicleState.Front, awCheckpoint.Position); if (cpDistacne < d || cpDistacne - d < TahoeParams.VL * 4.5) { ArbiterOutput.Output("Removing checkpoint: " + acCurrecnt.WaypointId.ToString() + " as failed vehicle over it"); CoreCommon.Mission.MissionCheckpoints.Dequeue(); return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } } }catch (Exception) { } #region Right Lateral Reasoning Forwards // check right lateral reasoning for existence, if so parametrize if (rightLateralReasoning.Exists && fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle) { // get lane ArbiterLane lane = al; // determine failed vehicle lane change distance params Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location; Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.0)).Location; Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location; // get params for lane change LaneChangeParameters? lcp = this.LaneChangeParameterization( new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null), lane, lane.LaneOnRight, false, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); // check if exists to generate full param if (lcp.HasValue) { // get param LaneChangeParameters tp = lcp.Value; // notify ArbiterOutput.WriteToLog("Failed Forward: Right Lateral Reasoning Forwards: Available: " + tp.Available.ToString() + ", Feasable: " + tp.Feasible.ToString()); // get behavior ChangeLaneBehavior clb = new ChangeLaneBehavior( al.LaneId, al.LaneOnRight.LaneId, false, al.DistanceBetween(vehicleState.Front, departUpperBound), new ScalarSpeedCommand(tp.Parameters.RecommendedSpeed), tp.Parameters.VehiclesToIgnore, al.LanePath(), al.LaneOnRight.LanePath(), al.Width, al.LaneOnRight.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true)); tp.Behavior = clb; tp.Decorators = TurnDecorators.RightTurnDecorator; // next state ChangeLanesState cls = new ChangeLanesState(tp); tp.NextState = cls; // add parameterization to possible changeParams.Add(tp); } } #endregion #region Left Lateral Reasoning // check left lateral reasoning if(leftLateralReasoning.Exists) { #region Left Lateral Opposing // check opposing ArbiterLane closestOpposingLane = this.GetClosestOpposing(ourForwardLane, vehicleState); if(closestOpposingLane != null && (leftLateralReasoning.IsOpposing || !closestOpposingLane.Equals(leftLateralReasoning.LateralLane))) { // check room of initial bool enoughRoom = (arbiterLane is ArbiterLane && (!roadPlan.BestPlan.laneWaypointOfInterest.IsExit || ((ArbiterLane)arbiterLane).DistanceBetween(vehicleState.Front, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > TahoeParams.VL * 5.0)) || (arbiterLane is SupraLane && ((SupraLane)arbiterLane).DistanceBetween(vehicleState.Front, ((SupraLane)arbiterLane).Interconnect.InitialGeneric.Position) > TahoeParams.VL * 5.0); // check opposing enough room bool oppEnough = closestOpposingLane.DistanceBetween(closestOpposingLane.LanePath().StartPoint.Location, vehicleState.Front) > TahoeParams.VL * 5.0; // check if enough room if (enoughRoom && oppEnough) { // check if we're stopped and the current trav params were for a vehicle and we're close to the vehicle bool stoppedBehindFV = fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle; // check that we're stopped behind forward vehicle before attempting to change lanes if (stoppedBehindFV) { #region Check Segment Blockage // check need to make uturn (hack) bool waitForUTurnCooldown; BlockageTactical bt = CoreCommon.BlockageDirector; StayInLaneBehavior tmpBlockBehavior = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(2.0), new List<int>(), al.LanePath(), al.Width, 0, 0); ITacticalBlockage itbTmp = new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, TahoeParams.VL, BlockageType.Static, -1, false, tmpBlockBehavior.GetType())); Maneuver tmpBlockManeuver = bt.LaneRecoveryManeuver(al, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value, roadPlan, new BlockageRecoveryState(tmpBlockBehavior, new StayInLaneState(al, CoreCommon.CorePlanningState), new StayInLaneState(al, CoreCommon.CorePlanningState), BlockageRecoveryDEFCON.REVERSE, new EncounteredBlockageState(itbTmp, CoreCommon.CorePlanningState, BlockageRecoveryDEFCON.REVERSE, SAUDILevel.None), BlockageRecoverySTATUS.ENCOUNTERED), true, out waitForUTurnCooldown); if (!waitForUTurnCooldown && tmpBlockManeuver.PrimaryBehavior is UTurnBehavior) return tmpBlockManeuver; else if (waitForUTurnCooldown) return null; #endregion // distance to forward vehicle too small double distToForwards = al.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition); double distToReverse = Math.Max(1.0, 8.0 - distToForwards); if (distToForwards < 8.0) { // notify ArbiterOutput.WriteToLog("Secondary: NOT Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " distance: " + distToForwards.ToString("f2")); this.RearMonitor = new RearQuadrantMonitor(al, SideObstacleSide.Driver); this.RearMonitor.Update(vehicleState); if (this.RearMonitor.CurrentVehicle != null) { double distToRearVehicle = al.DistanceBetween(this.RearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Position) - TahoeParams.RL; double distNeedClear = distToReverse + 2.0; if (distToRearVehicle < distNeedClear) { // notify ArbiterOutput.Output("Secondary: Rear: Not enough room to clear in rear: " + distToRearVehicle.ToString("f2") + " < " + distNeedClear.ToString("f2")); return null; } } double distToLaneStart = al.DistanceBetween(al.LanePath().StartPoint.Location, vehicleState.Position) - TahoeParams.RL; if (distToReverse > distToLaneStart) { // notify ArbiterOutput.Output("Secondary: Rear: Not enough room in lane to reverse in rear: " + distToLaneStart.ToString("f2") + " < " + distToReverse.ToString("f2")); return null; } else { // notify ArbiterOutput.Output("Secondary: Reversing to pass Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " reversing distance: " + distToReverse.ToString("f2")); StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(distToReverse, true); StayInLaneBehavior silb = new StayInLaneBehavior(al.LaneId, sadsc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true)); return new Maneuver(silb, CoreCommon.CorePlanningState, TurnDecorators.HazardDecorator, vehicleState.Timestamp); } } else { // notify ArbiterOutput.WriteToLog("Secondary: Left Lateral Opposing: Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString()); // determine failed vehicle lane change distance params Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location; Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.5)).Location; Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location; // check if enough room if (al.DistanceBetween(vehicleState.Front, defaultReturnUpperBound) >= d + TahoeParams.VL * 4.5) { // get hte closest oppoing ArbiterLane closestOpposing = this.GetClosestOpposing(al, vehicleState); // check exists if (closestOpposing != null) { // set/check secondary if (this.secondaryLeftLateralReasoning == null || !this.secondaryLeftLateralReasoning.LateralLane.Equals(closestOpposing)) this.secondaryLeftLateralReasoning = new OpposingLateralReasoning(closestOpposing, SideObstacleSide.Driver); // check the state of hte lanes next to us if (this.leftLateralReasoning.LateralLane.Equals(closestOpposing) && this.leftLateralReasoning.ExistsExactlyHere(vehicleState)) { #region Plan // need to make sure that we wait for 3 seconds with the blinker on (resetting with pause) if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds > 3000) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: Wait Timer DONE"); // get parameterization LaneChangeParameters? tp = this.LaneChangeParameterization( new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.ForwardMonitor.ForwardVehicle.CurrentVehicle), al, leftLateralReasoning.LateralLane, true, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); // check if available and feasible if (tp.HasValue && tp.Value.Available && tp.Value.Feasible) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: AVAILABLE & FEASIBLE"); LaneChangeParameters lcp = tp.Value; lcp.Behavior = this.ForwardMonitor.CurrentParameters.Behavior; lcp.Decorators = TurnDecorators.LeftTurnDecorator; lcp.Behavior.Decorators = lcp.Decorators; // next state ChangeLanesState cls = new ChangeLanesState(tp.Value); lcp.NextState = cls; // add parameterization to possible changeParams.Add(lcp); } // check if not available now but still feasible else if (tp.HasValue && !tp.Value.Available && tp.Value.Feasible) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: NOT Available, Still FEASIBLE, WAITING"); // wait and blink maneuver TravelingParameters tp2 = this.ForwardMonitor.CurrentParameters; tp2.Decorators = TurnDecorators.LeftTurnDecorator; tp2.Behavior.Decorators = tp2.Decorators; // create parameterization LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, al.LaneOnLeft, true, true, tp2.Behavior, 0.0, CoreCommon.CorePlanningState, tp2.Decorators, tp2, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle); // add parameterization to possible changeParams.Add(lcp); } } // otherwise timer not running or not been long enough else { // check if timer running if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.IsRunning) this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Start(); double waited = (double)(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds / 1000.0); ArbiterOutput.Output("Waited for failed forwards: " + waited.ToString("F2") + " seconds"); // wait and blink maneuver TravelingParameters tp = this.ForwardMonitor.CurrentParameters; tp.Decorators = TurnDecorators.LeftTurnDecorator; tp.Behavior.Decorators = tp.Decorators; // create parameterization LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, al.LaneOnLeft, true, true, tp.Behavior, 0.0, CoreCommon.CorePlanningState, tp.Decorators, tp, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle); // add parameterization to possible changeParams.Add(lcp); } #endregion } else if (!this.leftLateralReasoning.LateralLane.Equals(closestOpposing) && !this.leftLateralReasoning.ExistsRelativelyHere(vehicleState)) { // set and notify ArbiterOutput.Output("superceeded left lateral reasoning with override for non adjacent left lateral reasoning"); ILateralReasoning tmpReasoning = this.leftLateralReasoning; this.leftLateralReasoning = this.secondaryLeftLateralReasoning; try { #region Plan // need to make sure that we wait for 3 seconds with the blinker on (resetting with pause) if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds > 3000) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: Wait Timer DONE"); // get parameterization LaneChangeParameters? tp = this.LaneChangeParameterization( new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.ForwardMonitor.ForwardVehicle.CurrentVehicle), al, leftLateralReasoning.LateralLane, true, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); // check if available and feasible if (tp.HasValue && tp.Value.Available && tp.Value.Feasible) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: AVAILABLE & FEASIBLE"); LaneChangeParameters lcp = tp.Value; lcp.Behavior = this.ForwardMonitor.CurrentParameters.Behavior; lcp.Decorators = TurnDecorators.LeftTurnDecorator; lcp.Behavior.Decorators = TurnDecorators.LeftTurnDecorator; // next state ChangeLanesState cls = new ChangeLanesState(tp.Value); lcp.NextState = cls; // add parameterization to possible changeParams.Add(lcp); } // check if not available now but still feasible else if (tp.HasValue && !tp.Value.Available && tp.Value.Feasible) { // notify ArbiterOutput.Output("Scondary: Left Lateral Opposing: NOT Available, Still FEASIBLE, WAITING"); // wait and blink maneuver TravelingParameters tp2 = this.ForwardMonitor.CurrentParameters; tp2.Decorators = TurnDecorators.LeftTurnDecorator; tp2.Behavior.Decorators = tp2.Decorators; // create parameterization LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, this.leftLateralReasoning.LateralLane, true, true, tp2.Behavior, 0.0, CoreCommon.CorePlanningState, tp2.Decorators, tp2, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle); // add parameterization to possible changeParams.Add(lcp); } } // otherwise timer not running or not been long enough else { // check if timer running if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.IsRunning) this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Start(); double waited = (double)(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds / 1000.0); ArbiterOutput.Output("Waited for failed forwards: " + waited.ToString("F2") + " seconds"); // wait and blink maneuver TravelingParameters tp = this.ForwardMonitor.CurrentParameters; tp.Decorators = TurnDecorators.LeftTurnDecorator; tp.Behavior.Decorators = tp.Decorators; // create parameterization LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, this.leftLateralReasoning.LateralLane, true, true, tp.Behavior, 0.0, CoreCommon.CorePlanningState, tp.Decorators, tp, new Coordinates(), new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle); // add parameterization to possible changeParams.Add(lcp); } #endregion } catch (Exception ex) { ArbiterOutput.Output("Core intelligence thread caught exception in forward reasoning secondary maneuver when non-standard adjacent left: " + ex.ToString()); } // restore this.leftLateralReasoning = tmpReasoning; } } else { // do nuttin ArbiterOutput.Output("no opposing adjacent"); } } else { // notify ArbiterOutput.Output("Secondary: LeftLatOpp: Stopped Behind FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", but not enough room to pass"); } } } else { this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop(); this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset(); // notify ArbiterOutput.Output("Secondary: Left Lateral Opposing: NOT Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString()); } } else { ArbiterOutput.Output("Secondary Opposing: enough room to pass opposing: initial: " + enoughRoom.ToString() + ", opposing: " + oppEnough.ToString()); } } #endregion #region Left Lateral Forwards // otherwise parameterize else if(fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle) { // get lane ArbiterLane lane = al; // determine failed vehicle lane change distance params Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location; Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.0)).Location; Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location; // get params for lane change LaneChangeParameters? lcp = this.LaneChangeParameterization( new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null), lane, lane.LaneOnLeft, false, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value); // check if exists to generate full param if (lcp.HasValue) { // set param LaneChangeParameters tp = lcp.Value; // notify ArbiterOutput.Output("Secondary Failed Forward Reasoning Forwards: Available: " + tp.Available.ToString() + ", Feasible: " + tp.Feasible.ToString()); // get behavior ChangeLaneBehavior clb = new ChangeLaneBehavior( al.LaneId, al.LaneOnLeft.LaneId, true, al.DistanceBetween(vehicleState.Front, departUpperBound), new ScalarSpeedCommand(tp.Parameters.RecommendedSpeed), tp.Parameters.VehiclesToIgnore, al.LanePath(), al.LaneOnLeft.LanePath(), al.Width, al.LaneOnLeft.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true)); tp.Behavior = clb; tp.Decorators = TurnDecorators.LeftTurnDecorator; // next state ChangeLanesState cls = new ChangeLanesState(tp); tp.NextState = cls; // add parameterization to possible changeParams.Add(tp); } } #endregion } #endregion } #endregion #region Slow Forward // if pass, determine if should pass in terms or vehicles adjacent and in front then call lane change function for maneuver else if (lci.Reason == LaneChangeReason.SlowForwardVehicle) { // if left exists and is not opposing, parameterize if (leftLateralReasoning.Exists && !leftLateralReasoning.IsOpposing) { throw new Exception("slow forward vehicle pass not implemented yet"); } // if right exists and is not opposing, parameterize if (rightLateralReasoning.Exists && !rightLateralReasoning.IsOpposing) { throw new Exception("slow forward vehicle pass not implemented yet"); } } #endregion #region Parameterize // check params to see if any are good and available if(changeParams != null && changeParams.Count > 0) { // sort the parameterizations changeParams.Sort(); // get first LaneChangeParameters final = changeParams[0]; // notify ArbiterOutput.Output("Secondary Reasoning Final: Available: " + final.Available.ToString() + ", Feasible: " + final.Feasible.ToString()); // make sure ok if (final.Available && final.Feasible) { // return best param return new Maneuver(changeParams[0].Behavior, changeParams[0].NextState, changeParams[0].Decorators, vehicleState.Timestamp); } } #endregion } } // fallout is null return null; }
/// <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> /// Figure out standard control to follow /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public ForwardVehicleTrackingControl GetControl(IFQMPlanable lane, VehicleState state, List <ArbiterWaypoint> ignorable) { // check exists a vehicle to track if (this.CurrentVehicle != null) { // get the maximum velocity of the segment we're closest to double segV = lane.CurrentMaximumSpeed(state.Front); double segMaxV = segV; // flag if the forward vehicles is in a stop waypoint type safety zone bool safetyZone = false; ArbiterWaypoint nextStop = lane.GetNext(state.Front, WaypointType.Stop, ignorable); double distance = nextStop != null?lane.DistanceBetween(state.Front, nextStop.Position) : Double.MaxValue; // check if no stop exists or distance > 30 if (nextStop == null || distance > 30 || distance < 0) { // there is no next stop or the forward vehcile is not in a safety zone safetyZone = false; } // otherwise the tracked vehicle is in a safety zone else { // set the forward vehicle as being in a safety zone safetyZone = true; } // minimum distance double xAbsMin; // if we're in a safety zone if (safetyZone) { // set minimum to 1 times the tahoe's vehicle length xAbsMin = TahoeParams.VL * 1.5; } // otherwise we're not in a safety zone else { // set minimum to 2 times the tahoe's vehicle length xAbsMin = TahoeParams.VL * 2.0; } // retrieve the tracked vehicle's scalar absolute speed double vTarget = CurrentVehicle.StateMonitor.Observed.isStopped ? 0.0 : this.CurrentVehicle.Speed; // check if vehicle is moving away from us VehicleDirectionAlongPath vdap = CurrentVehicle.VehicleDirectionAlong(lane); // if moving perpendic, set speed to 0.0 if (vdap == VehicleDirectionAlongPath.Perpendicular) { vTarget = 0.0; } #region Forward // can use normal tracker if vehicle not oncoming if (true) //vdap != VehicleDirectionAlongPath.Reverse) { // get the good distance behind the target, is xAbsMin if vehicle velocity is small enough double xGood = xAbsMin + (1.5 * (TahoeParams.VL / 4.4704) * vTarget); // get our current separation double xSeparation = this.currentDistance; // determine the envelope to reason about the vehicle, that is, to slow from vMax to vehicle speed double xEnvelope = (Math.Pow(vTarget, 2.0) - Math.Pow(segMaxV, 2.0)) / (2.0 * -0.5); // the distance to the good double xDistanceToGood; // determine the velocity to follow the vehicle ahead double vFollowing; // check if we are basically stopped in the right place behind another vehicle if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // check if we are within the minimum distance else if (xSeparation <= xAbsMin) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // determine if our separation is less than the good distance but not inside minimum else if (xAbsMin < xSeparation && xSeparation < xGood) { // get the distance we are from xMin double xAlong = xSeparation - xAbsMin; // get the total distance from xGood to xMin double xGoodToMin = xGood - xAbsMin; // slow to 0 by min vFollowing = (((xGoodToMin - xAlong) / xGoodToMin) * (-vTarget)) + vTarget; // good distance xDistanceToGood = 0; } // our separation is greater than the good distance else { // good distance xDistanceToGood = xSeparation - xGood; // get differences in max and target velocities vFollowing = SpeedTools.GenerateSpeed(xDistanceToGood, vTarget, segMaxV); } // return return(new ForwardVehicleTrackingControl(true, safetyZone, vFollowing, xSeparation, xDistanceToGood, vTarget, xAbsMin, xGood, false)); } #endregion #region Oncoming /* * else * { * // determine the distance for the other vehicle to stop * double xTargetEnvelope = -Math.Pow(vTarget, 2.0) / (2.0 * -0.5); * * // determine the distance for our vehicle to stop * double xOurEnvelope = -Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2.0) / (2.0 * -0.5); * * // get the good distance behind the target, is xAbsMin if vehicle velocity is small enough * double xGood = xAbsMin + (1.5 * (TahoeParams.VL / 4.4704) * vTarget) + xOurEnvelope + xTargetEnvelope; * * // get our current separation * double xSeparation = this.currentDistance; * * // determine the envelope for us to slow to 0 by the good distance * double xEnvelope = -Math.Pow(segMaxV, 2.0) / (2.0 * -0.5); * * // the distance to the good * double xDistanceToGood; * * // determine the velocity to follow the vehicle ahead * double vFollowing; * * // check if we are basically stopped in the right place behind another vehicle * if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1) * { * // stop * vFollowing = 0; * * // good distance * xDistanceToGood = 0; * } * // check if we are within the minimum distance * else if (xSeparation <= xGood) * { * // stop * vFollowing = 0; * * // good distance * xDistanceToGood = 0; * } * // our separation is greater than the good distance but within the envelope * else if (xGood <= xSeparation && xSeparation <= xEnvelope + xGood) * { * // get differences in max and target velocities * double vDifference = segMaxV; * * // get the distance we are along the speed envolope from xGood * double xAlong = xEnvelope - (xSeparation - xGood); * * // slow to vTarget by good * vFollowing = (((xEnvelope - xAlong) / xEnvelope) * (vDifference)); * * // good distance * xDistanceToGood = xSeparation - xGood; * } * // otherwise xSeparation > xEnvelope * else * { * // can go max speed * vFollowing = segMaxV; * * // good distance * xDistanceToGood = xSeparation - xGood; * } * * // return * return new ForwardVehicleTrackingControl(true, safetyZone, vFollowing, xSeparation, * xDistanceToGood, vTarget, xAbsMin, xGood, true); * } * */ #endregion } else { // return return(new ForwardVehicleTrackingControl(false, false, Double.MaxValue, Double.MaxValue, Double.MaxValue, Double.MaxValue, 0.0, Double.MaxValue, false)); } }
/// <summary> /// Distance from coord to vehicle along lane /// </summary> /// <param name="lane"></param> /// <param name="c"></param> /// <returns></returns> public double DistanceToVehicle(IFQMPlanable lane, Coordinates c) { if (CurrentVehicle != null) return lane.DistanceBetween(c, CurrentVehicle.ClosestPosition); else return Double.MaxValue; }
/// <summary> /// Get the vehicle direction along a lane /// </summary> /// <param name="lane"></param> /// <returns></returns> public VehicleDirectionAlongPath VehicleDirectionAlong(IFQMPlanable lane) { if (this.IsStopped || !this.StateMonitor.Observed.headingValid) return VehicleDirectionAlongPath.Forwards; // get point on the lane path LinePath.PointOnPath pop = lane.LanePath().GetClosestPoint(this.ClosestPosition); // get heading of the lane path there Coordinates pathVector = lane.LanePath().GetSegment(pop.Index).UnitVector; // get vehicle heading Coordinates unit = new Coordinates(1, 0); Coordinates headingVector = unit.Rotate(this.StateMonitor.Observed.absoluteHeading); // rotate vehicle heading Coordinates relativeVehicle = headingVector.Rotate(-pathVector.ArcTan); // get path heading double relativeVehicleDegrees = relativeVehicle.ToDegrees() >= 180.0 ? Math.Abs(relativeVehicle.ToDegrees() - 360.0) : Math.Abs(relativeVehicle.ToDegrees()); if (relativeVehicleDegrees < 70) return VehicleDirectionAlongPath.Forwards; else if (relativeVehicleDegrees > 70 && relativeVehicleDegrees < 110) return VehicleDirectionAlongPath.Perpendicular; else return VehicleDirectionAlongPath.Reverse; }
/// <summary> /// Determines proper speed commands given we want to stop at a certain waypoint /// </summary> /// <param name="waypoint"></param> /// <param name="lane"></param> /// <param name="position"></param> /// <param name="enCovariance"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public double RequiredSpeed(ArbiterWaypoint waypoint, double speedAtWaypoint, double currentMax, IFQMPlanable lane, Coordinates position) { // get dist to waypoint double stopSpeedDistance = lane.DistanceBetween(position, waypoint.Position); // segment max speed double segmentMaxSpeed = currentMax; // distance to stop from max v given desired acceleration double stopEnvelopeLength = (Math.Pow(speedAtWaypoint, 2) - Math.Pow(currentMax, 2)) / (2.0 * -0.5); // check if we are within profile if (stopSpeedDistance > 0 && stopSpeedDistance < stopEnvelopeLength) { // get speed along profile double requiredSpeed = speedAtWaypoint + ((stopSpeedDistance / stopEnvelopeLength) * (segmentMaxSpeed - speedAtWaypoint)); return(requiredSpeed); } else { return(segmentMaxSpeed); } }
/// <summary> /// Determines proper speed commands given we want to stop at a certain waypoint /// </summary> /// <param name="waypoint"></param> /// <param name="lane"></param> /// <param name="position"></param> /// <param name="enCovariance"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public double RequiredSpeed(ArbiterWaypoint waypoint, double speedAtWaypoint, double currentMax, IFQMPlanable lane, Coordinates position) { // get dist to waypoint double stopSpeedDistance = lane.DistanceBetween(position, waypoint.Position); // segment max speed double segmentMaxSpeed = currentMax; // distance to stop from max v given desired acceleration double stopEnvelopeLength = (Math.Pow(speedAtWaypoint, 2) - Math.Pow(currentMax, 2)) / (2.0 * -0.5); // check if we are within profile if (stopSpeedDistance > 0 && stopSpeedDistance < stopEnvelopeLength) { // get speed along profile double requiredSpeed = speedAtWaypoint + ((stopSpeedDistance / stopEnvelopeLength) * (segmentMaxSpeed - speedAtWaypoint)); return requiredSpeed; } else { return segmentMaxSpeed; } }
/// <summary> /// Next point at which to stop /// </summary> /// <param name="lane"></param> /// <param name="position"></param> /// <param name="stopPoint"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public void NextLaneStop(IFQMPlanable lane, Coordinates position, double[] enCovariance, List<ArbiterWaypoint> ignorable, out ArbiterWaypoint stopPoint, out double stopSpeed, out double stopDistance, out StopType stopType) { // get the stop List<WaypointType> wts = new List<WaypointType>(); wts.Add(WaypointType.Stop); wts.Add(WaypointType.End); stopPoint = lane.GetNext(position, wts, ignorable); // set stop type stopType = stopPoint.IsStop ? StopType.StopLine : StopType.EndOfLane; // parameterize this.StoppingParams(stopPoint, lane, position, enCovariance, out stopSpeed, out stopDistance); }
/// <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> /// Determines the point at which we need to stop in the current lane /// </summary> /// <param name="lanePoint"></param> /// <param name="position"></param> /// <param name="stopRequired"></param> /// <param name="stopSpeed"></param> /// <param name="stopDistance"></param> public void NextNavigationalStop(IFQMPlanable lane, ArbiterWaypoint lanePoint, Coordinates position, double[] enCovariance, List<ArbiterWaypoint> ignorable, out double stopSpeed, out double stopDistance, out StopType stopType, out ArbiterWaypoint stopWaypoint) { // variables for default next stop line or end of lane this.NextLaneStop(lane, position, enCovariance, ignorable, out stopWaypoint, out stopSpeed, out stopDistance, out stopType); if (lanePoint != null) { // check if the point downstream is the last checkpoint if (CoreCommon.Mission.MissionCheckpoints.Count == 1 && lanePoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId)) { ArbiterWaypoint cpStop = lanePoint; double cpStopSpeed; double cpStopDistance; StopType cpStopType = StopType.LastGoal; this.StoppingParams(cpStop, lane, position, enCovariance, out cpStopSpeed, out cpStopDistance); if (cpStopDistance <= stopDistance) { stopSpeed = cpStopSpeed; stopDistance = cpStopDistance; stopType = cpStopType; stopWaypoint = cpStop; } } // check if point is not the checkpoint and is an exit else if (lanePoint.IsExit && !lanePoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId)) { ArbiterWaypoint exitStop = lanePoint; double exitStopSpeed; double exitStopDistance; StopType exitStopType = lanePoint.IsStop ? StopType.StopLine : StopType.Exit; this.StoppingParams(exitStop, lane, position, enCovariance, out exitStopSpeed, out exitStopDistance); if (exitStopDistance <= stopDistance) { stopSpeed = exitStopSpeed; stopDistance = exitStopDistance; stopType = exitStopType; stopWaypoint = exitStop; } } } }
/// <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> /// Plans the forward maneuver and secondary maneuver /// </summary> /// <param name="arbiterLane"></param> /// <param name="vehicleState"></param> /// <param name="p"></param> /// <param name="blockage"></param> /// <returns></returns> public Maneuver ForwardManeuver(IFQMPlanable arbiterLane, VehicleState vehicleState, RoadPlan roadPlan, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable) { // get primary parameterization TravelingParameters primary = this.ForwardMonitor.Primary(arbiterLane, vehicleState, roadPlan, blockages, ignorable, true); // return primary for now return new Maneuver(primary.Behavior, primary.NextState, primary.Decorators, vehicleState.Timestamp); }