/// <summary> /// Gets time cost between two waypoints in the same lane /// </summary> /// <param name="initial"></param> /// <param name="final"></param> /// <returns></returns> private double TimeCostInLane(ArbiterWaypoint initial, ArbiterWaypoint final) { ArbiterWaypoint current = initial; double cost = 0.0; while (current != null) { if (current.Equals(final)) { return(cost); } else { if (current.NextPartition == null) { return(cost); } cost += current.NextPartition.TimeCost(); if (current.NextPartition != null) { current = current.NextPartition.Final; } else { current = null; } } } return(Double.MaxValue); }
/// <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 }