public WaitingAtIntersectionExitState(ITraversableWaypoint exit, ArbiterTurnDirection turnDirection, IntersectionDescription description, ArbiterInterconnect desired) { this.exitWaypoint = exit; this.turnDirection = turnDirection; this.IntersectionDescription = description; this.desired = desired; this.turnTestState = TurnTestState.Unknown; }
public WaitingAtIntersectionExitState(ITraversableWaypoint exit, ArbiterTurnDirection turnDirection, IntersectionDescription description, ArbiterInterconnect desired) { this.exitWaypoint = exit; this.turnDirection = turnDirection; this.IntersectionDescription = description; this.desired = desired; this.turnTestState = TurnTestState.Unknown; }
/// <summary> /// Constructor /// </summary> /// <param name="lane"></param> /// <param name="waypoint"></param> /// <param name="turnDirection"></param> /// <param name="isNavigationExit"></param> /// <param name="desiredExit"></param> public StoppingAtStopState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection, bool isNavigationExit, ArbiterInterconnect desiredExit) { this.lane = lane; this.waypoint = waypoint; this.turnDirection = turnDirection; this.isNavigationExit = isNavigationExit; this.desiredExit = desiredExit; this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId); }
/// <summary> /// Constructor /// </summary> /// <param name="lane"></param> /// <param name="waypoint"></param> /// <param name="turnDirection"></param> /// <param name="isNavigationExit"></param> /// <param name="desiredExit"></param> public StoppingAtStopState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection, bool isNavigationExit, ArbiterInterconnect desiredExit) { this.lane = lane; this.waypoint = waypoint; this.turnDirection = turnDirection; this.isNavigationExit = isNavigationExit; this.desiredExit = desiredExit; this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId); }
/// <summary> /// Constructor /// </summary> /// <param name="lane"></param> /// <param name="waypoint"></param> /// <param name="turnDirection"></param> /// <param name="isNavigationExit"></param> /// <param name="desiredExit"></param> public StoppingAtExitState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection, bool isNavigationExit, ArbiterInterconnect desiredExit, double timeStamp, Coordinates currentPosition) { this.lane = lane; this.waypoint = waypoint; this.turnDirection = turnDirection; this.isNavigationExit = isNavigationExit; this.desiredExit = desiredExit; this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId); this.timeStamp = timeStamp; this.currentPosition = currentPosition; this.internalLaneState = new InternalState(this.lane.LaneId, this.lane.LaneId); }
/// <summary> /// Constructor /// </summary> /// <param name="interconnect"></param> /// <param name="turn"></param> public TurnState(ArbiterInterconnect interconnect, ArbiterTurnDirection turn, ArbiterLane target, LinePath endingPath, LineList left, LineList right, SpeedCommand speed, SAUDILevel saudi, bool useTurnBounds) { this.Interconnect = interconnect; this.turnDirection = turn; this.TargetLane = target; this.EndingPath = endingPath; this.LeftBound = left; this.RightBound = right; this.SpeedCommand = speed; this.Saudi = saudi; this.UseTurnBounds = useTurnBounds; }
/// <summary> /// Constructor /// </summary> /// <param name="interconnect"></param> /// <param name="turn"></param> public TurnState(ArbiterInterconnect interconnect, ArbiterTurnDirection turn, ArbiterLane target, LinePath endingPath, LineList left, LineList right, SpeedCommand speed, SAUDILevel saudi, bool useTurnBounds) { this.Interconnect = interconnect; this.turnDirection = turn; this.TargetLane = target; this.EndingPath = endingPath; this.LeftBound = left; this.RightBound = right; this.SpeedCommand = speed; this.Saudi = saudi; this.UseTurnBounds = useTurnBounds; }
/// <summary> /// Constructor /// </summary> /// <param name="lane"></param> /// <param name="waypoint"></param> /// <param name="turnDirection"></param> /// <param name="isNavigationExit"></param> /// <param name="desiredExit"></param> public StoppingAtExitState(ArbiterLane lane, ArbiterWaypoint waypoint, ArbiterTurnDirection turnDirection, bool isNavigationExit, ArbiterInterconnect desiredExit, double timeStamp, Coordinates currentPosition) { this.lane = lane; this.waypoint = waypoint; this.turnDirection = turnDirection; this.isNavigationExit = isNavigationExit; this.desiredExit = desiredExit; this.internalLaneState = new InternalState(lane.LaneId, lane.LaneId); this.timeStamp = timeStamp; this.currentPosition = currentPosition; this.internalLaneState = new InternalState(this.lane.LaneId, this.lane.LaneId); }
private static Polygon GenerateMiddlePathPolygon(LinePath initial, LinePath middle, LinePath final, ArbiterLane lane) { // wp's ArbiterWaypoint w1 = new ArbiterWaypoint(initial[0], new ArbiterWaypointId(1, lane.LaneId)); ArbiterWaypoint w2 = new ArbiterWaypoint(initial[1], new ArbiterWaypointId(2, lane.LaneId)); ArbiterWaypoint w3 = new ArbiterWaypoint(final[0], new ArbiterWaypointId(3, lane.LaneId)); ArbiterWaypoint w4 = new ArbiterWaypoint(final[1], new ArbiterWaypointId(4, lane.LaneId)); // set lane w1.Lane = lane; w2.Lane = lane; w3.Lane = lane; w4.Lane = lane; // alps ArbiterLanePartition alp1 = new ArbiterLanePartition(new ArbiterLanePartitionId(w1.WaypointId, w2.WaypointId, lane.LaneId), w1, w2, lane.Way.Segment); ArbiterLanePartition alp2 = new ArbiterLanePartition(new ArbiterLanePartitionId(w2.WaypointId, w3.WaypointId, lane.LaneId), w2, w3, lane.Way.Segment); ArbiterLanePartition alp3 = new ArbiterLanePartition(new ArbiterLanePartitionId(w3.WaypointId, w4.WaypointId, lane.LaneId), w3, w4, lane.Way.Segment); // set links w1.NextPartition = alp1; w2.NextPartition = alp2; w3.NextPartition = alp3; w4.PreviousPartition = alp3; w3.PreviousPartition = alp2; w2.PreviousPartition = alp1; // get poly ArbiterTurnDirection atd = PartitionTurnDirection(alp2); if (atd != ArbiterTurnDirection.Straight) { ArbiterInterconnect ai = alp2.ToInterconnect; ai.TurnDirection = atd; GenerateInterconnectPolygon(ai); return(ai.TurnPolygon); } return(null); }
/// <summary> /// Constructor /// </summary> /// <param name="initial"></param> /// <param name="final"></param> public ArbiterInterconnect(IArbiterWaypoint initial, IArbiterWaypoint final, ArbiterTurnDirection turnDirection) : base(false, null, false, null, new List <IConnectAreaWaypoints>(), initial, final) { this.initialWaypoint = initial; this.finalWaypoint = final; this.InterconnectId = new ArbiterInterconnectId(initialWaypoint.AreaSubtypeWaypointId, finalWaypoint.AreaSubtypeWaypointId); // create a path of the partition and get the closest List <Coordinates> ips = new List <Coordinates>(); ips.Add(initial.Position); ips.Add(final.Position); this.InterconnectPath = new LinePath(ips); // nav edge stuff this.Contained.Add(this); this.blockage = new NavigationBlockage(0.0); this.TurnDirection = turnDirection; this.TurnPolygon = this.DefaultPoly(); this.InnerCoordinates = new List <Coordinates>(); }
/// <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 static Polygon PartitionPolygon(ArbiterLanePartition alp) { if (alp.Initial.PreviousPartition != null && alp.Final.NextPartition != null && alp.Length < 30.0 && alp.Length > 4.0) { // get partition turn direction ArbiterTurnDirection pTD = PartitionTurnDirection(alp); // check if angles of previous and next are such that not straight through if (pTD != ArbiterTurnDirection.Straight) { // get partition poly ArbiterInterconnect tmpAi = alp.ToInterconnect; tmpAi.TurnDirection = pTD; GenerateInterconnectPolygon(tmpAi); Polygon pPoly = tmpAi.TurnPolygon; // here is default partition polygon LinePath alplb = alp.PartitionPath.ShiftLateral(-alp.Lane.Width / 2.0); LinePath alprb = alp.PartitionPath.ShiftLateral(alp.Lane.Width / 2.0); alprb.Reverse(); List <Coordinates> alpdefaultPoly = alplb; alpdefaultPoly.AddRange(alprb); // get full poly pPoly.AddRange(alpdefaultPoly); pPoly = Polygon.GrahamScan(pPoly); return(pPoly); } } else if (alp.Length >= 30) { Polygon pBase = GenerateSimplePartitionPolygon(alp, alp.PartitionPath, alp.Lane.Width); if (alp.Initial.PreviousPartition != null && Math.Abs(FinalIntersectionAngle(alp.Initial.PreviousPartition)) > 15) { // initial portion Coordinates i1 = alp.Initial.Position - alp.Initial.PreviousPartition.Vector().Normalize(15.0); Coordinates i2 = alp.Initial.Position; Coordinates i3 = i2 + alp.Vector().Normalize(15.0); LinePath il12 = new LinePath(new Coordinates[] { i1, i2 }); LinePath il23 = new LinePath(new Coordinates[] { i2, i3 }); LinePath il13 = new LinePath(new Coordinates[] { i1, i3 }); Coordinates iCC = il13.GetClosestPoint(i2).Location; if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0) { il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0); } else { il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0); } LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC); iCCP = il13.AdvancePoint(iCCP, -10.0); il13 = il13.SubPath(iCCP, 20.0); Polygon iBase = GenerateSimplePolygon(il23, alp.Lane.Width); iBase.Add(il13[1]); Polygon iP = Polygon.GrahamScan(iBase); pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP })); } if (alp.Final.NextPartition != null && Math.Abs(FinalIntersectionAngle(alp)) > 15) { // initial portion Coordinates i1 = alp.Final.Position - alp.Vector().Normalize(15.0); Coordinates i2 = alp.Final.Position; Coordinates i3 = i2 + alp.Final.NextPartition.Vector().Normalize(15.0); LinePath il12 = new LinePath(new Coordinates[] { i1, i2 }); LinePath il23 = new LinePath(new Coordinates[] { i2, i3 }); LinePath il13 = new LinePath(new Coordinates[] { i1, i3 }); Coordinates iCC = il13.GetClosestPoint(i2).Location; if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0) { il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0); } else { il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0); } LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC); iCCP = il13.AdvancePoint(iCCP, -10.0); il13 = il13.SubPath(iCCP, 20.0); Polygon iBase = GenerateSimplePolygon(il12, alp.Lane.Width); iBase.Add(il13[0]); Polygon iP = Polygon.GrahamScan(iBase); pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP })); } return(pBase); } // fall out return(null); }
public IntersectionInvolved(IVehicleArea area) { this.Exit = null; this.Area = area; this.TurnDirection = ArbiterTurnDirection.Unknown; }
/// <summary> /// Constructor /// </summary> /// <param name="exit"></param> /// <param name="area"></param> /// <param name="turnDirection"></param> public IntersectionInvolved(ITraversableWaypoint exit, IVehicleArea area, ArbiterTurnDirection turnDirection) { this.Exit = exit; this.Area = area; this.TurnDirection = turnDirection; }
public IntersectionInvolved(IVehicleArea area) { this.Exit = null; this.Area = area; this.TurnDirection = ArbiterTurnDirection.Unknown; }
/// <summary> /// Constructor /// </summary> /// <param name="exit"></param> /// <param name="area"></param> /// <param name="turnDirection"></param> public IntersectionInvolved(ITraversableWaypoint exit, IVehicleArea area, ArbiterTurnDirection turnDirection) { this.Exit = exit; this.Area = area; this.TurnDirection = turnDirection; }