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