/// <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> /// 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 }
public override void Process(object param) { if (!base.BeginProcess()) { return; } if (param is ZoneTravelingBehavior) { HandleBaseBehavior((ZoneTravelingBehavior)param); } extraObstacles = GetPerimeterObstacles(); if (reverseGear) { ProcessReverse(); return; } AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp); // get the vehicle relative path LinePath relRecommendedPath = recommendedPath.Transform(absTransform); LinePath.PointOnPath zeroPoint = relRecommendedPath.ZeroPoint; // get the distance to the end point double distToEnd = relRecommendedPath.DistanceBetween(zeroPoint, relRecommendedPath.EndPoint); // get the planning distance double planningDist = GetPlanningDistance(); planningDist = Math.Max(planningDist, 20); planningDist -= zeroPoint.Location.Length; if (planningDist < 2.5) planningDist = 2.5; if (distToEnd < planningDist) { // make the speed command at stop speed command behaviorTimestamp = curTimestamp; speedCommand = new StopAtDistSpeedCommand(distToEnd - TahoeParams.FL); planningDist = distToEnd; approachSpeed = recommendedSpeed.Speed; settings.endingHeading = relRecommendedPath.EndSegment.UnitVector.ArcTan; settings.endingPositionFixed = true; settings.endingPositionMax = 2; settings.endingPositionMin = -2; } else { speedCommand = new ScalarSpeedCommand(recommendedSpeed.Speed); } // get the distance of the path segment we care about LinePath pathSegment = relRecommendedPath.SubPath(zeroPoint, planningDist); double avoidanceDist = planningDist + 5; avoidanceBasePath = relRecommendedPath.SubPath(zeroPoint, ref avoidanceDist); if (avoidanceDist > 0) { avoidanceBasePath.Add(avoidanceBasePath.EndPoint.Location + avoidanceBasePath.EndSegment.Vector.Normalize(avoidanceDist)); } // test if we should clear out of arc mode if (arcMode) { if (TestNormalModeClear(relRecommendedPath, zeroPoint)) { prevCurvature = double.NaN; arcMode = false; } } if (Math.Abs(zeroPoint.AlongtrackDistance(Coordinates.Zero)) > 1) { pathSegment.Insert(0, Coordinates.Zero); } else { if (pathSegment[0].DistanceTo(pathSegment[1]) < 1) { pathSegment.RemoveAt(0); } pathSegment[0] = Coordinates.Zero; } if (arcMode) { Coordinates relativeGoalPoint = relRecommendedPath.EndPoint.Location; ArcVoteZone(relativeGoalPoint, extraObstacles); return; } double pathLength = pathSegment.PathLength; if (pathLength < 6) { double additionalDist = 6.25 - pathLength; pathSegment.Add(pathSegment.EndPoint.Location + pathSegment.EndSegment.Vector.Normalize(additionalDist)); } // determine if polygons are to the left or right of the path for (int i = 0; i < zoneBadRegions.Length; i++) { Polygon poly = zoneBadRegions[i].Transform(absTransform); int numLeft = 0; int numRight = 0; foreach (LineSegment ls in pathSegment.GetSegmentEnumerator()) { for (int j = 0; j < poly.Count; j++) { if (ls.IsToLeft(poly[j])) { numLeft++; } else { numRight++; } } } if (numLeft > numRight) { // we'll consider this polygon on the left of the path //additionalLeftBounds.Add(new Boundary(poly, 0.1, 0.1, 0)); } else { //additionalRightBounds.Add(new Boundary(poly, 0.1, 0.1, 0)); } } // TODO: add zone perimeter disablePathAngleCheck = false; laneWidthAtPathEnd = 7; settings.Options.w_diff = 3; smootherBasePath = new LinePath(); smootherBasePath.Add(Coordinates.Zero); smootherBasePath.Add(pathSegment.EndPoint.Location); AddTargetPath(pathSegment, 0.005); settings.maxSpeed = recommendedSpeed.Speed; useAvoidancePath = false; SmoothAndTrack(command_label, true); }
/// <summary> /// Maneuver for recovering from a turn blockage /// </summary> /// <param name="lane"></param> /// <param name="vehicleState"></param> /// <param name="vehicleSpeed"></param> /// <param name="defcon"></param> /// <param name="saudi"></param> /// <returns></returns> private Maneuver TurnRecoveryManeuver(ArbiterInterconnect ai, VehicleState vehicleState, double vehicleSpeed, BlockageRecoveryState brs) { // get the blockage ITacticalBlockage turnBlockageReport = brs.EncounteredState.TacticalBlockage; // get the turn state TurnState turnState = (TurnState)brs.EncounteredState.PlanningState; // check the state of the recovery for being the initial state if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL) { // determine if the reverse behavior is recommended if (turnBlockageReport.BlockageReport.ReverseRecommended) { // get the path of the interconnect LinePath interconnectPath = ai.InterconnectPath; // check how much room there is to the start of the interconnect double distanceToStart = interconnectPath.DistanceBetween(interconnectPath.StartPoint, interconnectPath.GetClosestPoint(vehicleState.Rear)); // check distance to start from the rear bumper is large enough if (distanceToStart > 0) { // notify ArbiterOutput.Output("Initial encounter of turn blockage with reverse recommended, reversing"); // get hte reverse path LinePath reversePath = vehicleState.VehicleLinePath; // generate the reverse recovery behavior StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(TahoeParams.VL, true); StayInLaneBehavior silb = new StayInLaneBehavior(null, sadsc, new List<int>(), reversePath, TahoeParams.T * 2.0, 0, 0); // update the state BlockageRecoveryState brsUpdated = new BlockageRecoveryState( silb, brs.CompletionState, brs.AbortState, BlockageRecoveryDEFCON.REVERSE, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING); // chill out BlockageHandler.SetDefaultBlockageCooldown(); // send the maneuver Maneuver recoveryManeuver = new Maneuver( silb, brsUpdated, TurnDecorators.HazardDecorator, vehicleState.Timestamp); return recoveryManeuver; } } } // get the default turn behavior TurnBehavior defaultTurnBehavior = (TurnBehavior)turnState.Resume(vehicleState, vehicleSpeed); // check that we are not already ignoring small obstacles if (turnState.Saudi == SAUDILevel.None) { // check the turn ignoring small obstacles ShutUpAndDoItDecorator saudiDecorator = new ShutUpAndDoItDecorator(SAUDILevel.L1); defaultTurnBehavior.Decorators.Add(saudiDecorator); turnState.Saudi = SAUDILevel.L1; // notify ArbiterOutput.Output("Attempting test of turn behavior ignoring small obstacles"); // test execute the turn CompletionReport saudiCompletionReport; bool completedSaudiTurn = CoreCommon.Communications.TestExecute(defaultTurnBehavior, out saudiCompletionReport); // if the turn worked with ignorimg small obstacles, execute that if (completedSaudiTurn) { // notify ArbiterOutput.Output("Test of turn behavior ignoring small obstacles successful"); // chill out BlockageHandler.SetDefaultBlockageCooldown(); // return the recovery maneuver return new Maneuver(defaultTurnBehavior, turnState, TurnDecorators.NoDecorators, vehicleState.Timestamp); } } // notify ArbiterOutput.Output("Turn behavior reached last level of using no turn boundaries"); // when ignoring small obstacles does not work, send the turn without boundaries and ignore small obstacles turnState.UseTurnBounds = false; turnState.LeftBound = null; turnState.RightBound = null; defaultTurnBehavior.RightBound = null; defaultTurnBehavior.LeftBound = null; // ignoring small obstacles ShutUpAndDoItDecorator saudiNoBoundsDecorator = new ShutUpAndDoItDecorator(SAUDILevel.L1); defaultTurnBehavior.Decorators.Add(saudiNoBoundsDecorator); turnState.Saudi = SAUDILevel.L1; // chill out BlockageHandler.SetDefaultBlockageCooldown(); // go to the turn state return new Maneuver(defaultTurnBehavior, turnState, TurnDecorators.NoDecorators, vehicleState.Timestamp); }
private void InitializeReverse() { // reverse one vehicle length reverseDist = TahoeParams.VL; reverseTimestamp = curTimestamp; prevCurvature = double.NaN; reverseGear = true; behaviorTimestamp = curTimestamp; approachSpeed = 1; speedCommand = new StopAtDistSpeedCommand(reverseDist, true); }
/// <summary> /// Secondary maneuver when current lane is the desired lane /// </summary> /// <param name="arbiterLane"></param> /// <param name="vehicleState"></param> /// <param name="roadPlan"></param> /// <param name="blockages"></param> /// <param name="ignorable"></param> /// <returns></returns> public Maneuver? AdvancedSecondaryNextCheck(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan, List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask) { // check if the forward vehicle exists if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.PassedLongDelayedBirth) { // distance to forward vehicle double distanceToForwardVehicle = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition); #region Distance From Forward Not Enough // check distance to forward vehicle if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed && distanceToForwardVehicle < 8.0 && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle) { // set name ArbiterLane al = lane; // distance to forward vehicle too small double distToForwards = al.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition); double distToReverse = Math.Max(1.0, 8.0 - distToForwards); if (distToForwards < 8.0) { // notify ArbiterOutput.Output("Secondary: NOT Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " distance: " + distToForwards.ToString("f2")); this.RearMonitor = new RearQuadrantMonitor(al, SideObstacleSide.Driver); this.RearMonitor.Update(vehicleState); if (this.RearMonitor.CurrentVehicle != null) { double distToRearVehicle = al.DistanceBetween(this.RearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Position) - TahoeParams.RL; double distNeedClear = distToReverse + 2.0; if (distToRearVehicle < distNeedClear) { // notify ArbiterOutput.Output("Secondary: Rear: Not enough room to clear in rear: " + distToRearVehicle.ToString("f2") + " < " + distNeedClear.ToString("f2")); return null; } } double distToLaneStart = al.DistanceBetween(al.LanePath().StartPoint.Location, vehicleState.Position) - TahoeParams.RL; if (distToReverse > distToLaneStart) { // notify ArbiterOutput.Output("Secondary: Rear: Not enough room in lane to reverse in rear: " + distToLaneStart.ToString("f2") + " < " + distToReverse.ToString("f2")); return null; } else { // notify ArbiterOutput.Output("Secondary: Reversing to pass Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " reversing distance: " + distToReverse.ToString("f2")); StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(distToReverse, true); StayInLaneBehavior silb = new StayInLaneBehavior(al.LaneId, sadsc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true)); return new Maneuver(silb, CoreCommon.CorePlanningState, TurnDecorators.HazardDecorator, vehicleState.Timestamp); } } } #endregion // get distance to next lane major List<WaypointType> wts = new List<WaypointType>(new WaypointType[] { WaypointType.Stop, WaypointType.End }); ArbiterWaypoint nextWaypoint = lane.GetNext(lane.GetClosestPartition(vehicleState.Position).Final, wts, new List<ArbiterWaypoint>()); // check if the vehicle occurs before the next major thing double distanceToNextMajor = lane.DistanceBetween(vehicleState.Front, nextWaypoint.Position); // check if vehicle occurs before the next lane major if (distanceToForwardVehicle < distanceToNextMajor) { // check if forward vehicle exists in this lane and is closer then the lane goal if (TacticalDirector.VehicleAreas.ContainsKey(lane) && TacticalDirector.VehicleAreas[lane].Contains(this.ForwardMonitor.ForwardVehicle.CurrentVehicle) && lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) < lane.DistanceBetween(vehicleState.Front, laneGoal.Position)) { // some constants double desiredDistance = 50.0; // get the lane goal distance double distanceToLaneGoal = lane.DistanceBetween(vehicleState.Front, laneGoal.Position); // check if necessary to be in current lane if (distanceToLaneGoal <= desiredDistance) { // default secondary maneuver ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal < 50: " + desiredDistance.ToString("f2")); return this.SecondaryManeuver(lane, vehicleState, roadPlan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), bestTask); } // otherwise would be good but not necessary else { // return the maneuver from the vehicle being outside the min cap ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal > 50: " + desiredDistance.ToString("f2")); return this.AdvancedSecondaryOutsideMinCap(lane, laneGoal, vehicleState, roadPlan, blockages, ignorable, bestTask); } } } } // no secondary return null; }
/// <summary> /// 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> /// 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; }