/// <summary> /// Constructor /// </summary> /// <param name="zoneId"></param> /// <param name="perimter"></param> /// <param name="parkingSpots"></param> public ArbiterZone(ArbiterZoneId zoneId, ArbiterPerimeter perimter, List <ArbiterParkingSpot> parkingSpots, ArbiterRoadNetwork roadNetwork) { this.ZoneId = zoneId; this.Perimeter = perimter; this.ParkingSpots = parkingSpots; this.ParkingSpotWaypoints = new Dictionary <ArbiterParkingSpotWaypointId, ArbiterParkingSpotWaypoint>(); this.ZoneExits = new List <ArbiterPerimeterWaypoint>(); this.StayOutAreas = new List <Polygon>(); this.NavigationNodes = new List <INavigableNode>(); this.NavigableEdges = new List <NavigableEdge>(); this.RoadNetwork = roadNetwork; // set zone this.Perimeter.Zone = this; // create waypoint lookup foreach (ArbiterParkingSpot aps in parkingSpots) { // set zone aps.Zone = this; foreach (ArbiterParkingSpotWaypoint apsw in aps.Waypoints.Values) { ParkingSpotWaypoints.Add(apsw.WaypointId, apsw); } } // create exits foreach (ArbiterPerimeterWaypoint apw in perimter.PerimeterPoints.Values) { ZoneExits.Add(apw); } }
/// <summary> /// Constructor /// </summary> /// <param name="zonePerimeter">Perimeter of hte zone</param> /// <param name="stayOutside">Polygons to stay outside of</param> /// <param name="speedCommand">Recommended Speed command</param> public ZoneBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand recommendedSpeedCommand) { this.zoneId = zoneId; this.zonePerimeter = zonePerimeter; this.stayOutside = stayOutside; this.speedCommand = recommendedSpeedCommand; }
/// <summary> /// Constructor /// </summary> /// <param name="zoneId"></param> /// <param name="perimter"></param> /// <param name="parkingSpots"></param> public ArbiterZone(ArbiterZoneId zoneId, ArbiterPerimeter perimter, List<ArbiterParkingSpot> parkingSpots, ArbiterRoadNetwork roadNetwork) { this.ZoneId = zoneId; this.Perimeter = perimter; this.ParkingSpots = parkingSpots; this.ParkingSpotWaypoints = new Dictionary<ArbiterParkingSpotWaypointId, ArbiterParkingSpotWaypoint>(); this.ZoneExits = new List<ArbiterPerimeterWaypoint>(); this.StayOutAreas = new List<Polygon>(); this.NavigationNodes = new List<INavigableNode>(); this.NavigableEdges = new List<NavigableEdge>(); this.RoadNetwork = roadNetwork; // set zone this.Perimeter.Zone = this; // create waypoint lookup foreach (ArbiterParkingSpot aps in parkingSpots) { // set zone aps.Zone = this; foreach (ArbiterParkingSpotWaypoint apsw in aps.Waypoints.Values) { ParkingSpotWaypoints.Add(apsw.WaypointId, apsw); } } // create exits foreach (ArbiterPerimeterWaypoint apw in perimter.PerimeterPoints.Values) { ZoneExits.Add(apw); } }
/// <summary> /// Constructor /// </summary> /// <param name="zoneId"></param> /// <param name="zonePerimeter"></param> /// <param name="stayOutside"></param> /// <param name="speedCommand"></param> /// <param name="recommendedPath"></param> /// <param name="endingLeftBound"></param> /// <param name="endingRightBound"></param> public ZoneTravelingBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand, LinePath recommendedPath, LinePath endingLeftBound, LinePath endingRightBound) : base(zoneId, zonePerimeter, stayOutside, speedCommand) { this.RecommendedPath = recommendedPath; this.EndingLeftBound = endingLeftBound; this.EndingRightBound = endingRightBound; }
/// <summary> /// Constructor /// </summary> /// <param name="zoneId"></param> /// <param name="zonePerimeter"></param> /// <param name="stayOutside"></param> /// <param name="speedCommand"></param> /// <param name="parkingSpotPath"></param> /// <param name="spotLeftBound"></param> /// <param name="spotRightBound"></param> /// <param name="spotId"></param> /// <param name="extraDistance"></param> /// <param name="endingPolygon"></param> /// <param name="orientations"></param> public ZoneParkingPullOutBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand, LinePath parkingSpotPath, LinePath spotLeftBound, LinePath spotRightBound, ArbiterParkingSpotId spotId, LinePath reversePath, LinePath finalLeftBound, LinePath finalRightBound) : base(zoneId, zonePerimeter, stayOutside, speedCommand, parkingSpotPath, spotLeftBound, spotRightBound, spotId, 0.0) { this.RecommendedPullOutPath = reversePath; this.EndingLeftBound = finalLeftBound; this.EndingRightBound = finalRightBound; }
/// <summary> /// Constructor /// </summary> /// <param name="zoneId"></param> /// <param name="zonePerimeter"></param> /// <param name="stayOutside"></param> /// <param name="speedCommand"></param> /// <param name="parkingSpotPath"></param> /// <param name="spotLeftBound"></param> /// <param name="spotRightBound"></param> /// <param name="spotId"></param> /// <param name="extraDistance"></param> public ZoneParkingBehavior(ArbiterZoneId zoneId, Polygon zonePerimeter, Polygon[] stayOutside, ScalarSpeedCommand speedCommand, LinePath parkingSpotPath, LinePath spotLeftBound, LinePath spotRightBound, ArbiterParkingSpotId spotId, double extraDistance) : base(zoneId, zonePerimeter, stayOutside, speedCommand) { this.ParkingSpotPath = parkingSpotPath; this.SpotLeftBound = spotLeftBound; this.SpotRightBound = spotRightBound; this.ParkingSpotId = spotId; this.ExtraDistance = extraDistance; }
/// <summary> /// Generates mission /// </summary> /// <param name="mdf"></param> /// <returns></returns> public ArbiterMissionDescription GenerateMission(IMdf mdf, ArbiterRoadNetwork arn) { Queue<ArbiterCheckpoint> checks = new Queue<ArbiterCheckpoint>(); List<ArbiterSpeedLimit> speeds = new List<ArbiterSpeedLimit>(); // checkpoints foreach (string s in mdf.CheckpointOrder) { int num = int.Parse(s); checks.Enqueue(new ArbiterCheckpoint(num, arn.Checkpoints[num].AreaSubtypeWaypointId)); } // speeds foreach (SpeedLimit sl in mdf.SpeedLimits) { ArbiterSpeedLimit asl = new ArbiterSpeedLimit(); asl.MaximumSpeed = sl.MaximumVelocity * 0.44704; asl.MinimumSpeed = sl.MinimumVelocity * 0.44704; asl.Traveled = false; ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(sl.SegmentID)); ArbiterZoneId azi = new ArbiterZoneId(int.Parse(sl.SegmentID)); if (arn.ArbiterZones.ContainsKey(azi)) asl.Area = azi; else if (arn.ArbiterSegments.ContainsKey(asi)) asl.Area = asi; else throw new Exception("Unknown area id: " + sl.SegmentID); speeds.Add(asl); } // return return new ArbiterMissionDescription(checks, speeds); }
/// <summary> /// Constructor /// </summary> /// <param name="number"></param> /// <param name="zoneId"></param> public ArbiterParkingSpotId(int number, ArbiterZoneId zoneId) { this.spotNumber = number; this.ZoneId = zoneId; }
/// <summary> /// Constructor /// </summary> /// <param name="perimeterNumber"></param> /// <param name="zoneId"></param> public ArbiterPerimeterId(int perimeterNumber, ArbiterZoneId zoneId) { this.perimeterNumber = perimeterNumber; this.ZoneId = zoneId; }
/// <summary> /// Generates the arbiter zones form the internal xy zones for the input road network /// </summary> /// <param name="arn"></param> /// <returns></returns> /// <remarks>TODO: add zone cost map, adjacency of parking spots, figure out width</remarks> public ArbiterRoadNetwork GenerateZones(ArbiterRoadNetwork arn) { Dictionary <ArbiterZoneId, ArbiterZone> zones = new Dictionary <ArbiterZoneId, ArbiterZone>(); List <IArbiterWaypoint> waypoints = new List <IArbiterWaypoint>(); foreach (SimpleZone sz in xyZones) { ArbiterZoneId azi = new ArbiterZoneId(int.Parse(sz.ZoneID)); #region Generate Perimeter // old perim ZonePerimeter zp = sz.Perimeter; // perim id ArbiterPerimeterId api = new ArbiterPerimeterId(GenerationTools.GetId(zp.PerimeterID)[1], azi); #region Perimeter Waypoints List <ArbiterPerimeterWaypoint> perimeterWaypoints = new List <ArbiterPerimeterWaypoint>(); foreach (PerimeterPoint pp in zp.PerimeterPoints) { // id ArbiterPerimeterWaypointId apwi = new ArbiterPerimeterWaypointId( GenerationTools.GetId(pp.ID)[2], api); // point ArbiterPerimeterWaypoint apw = new ArbiterPerimeterWaypoint(apwi, pp.position); // add perimeterWaypoints.Add(apw); waypoints.Add(apw); arn.DisplayObjects.Add(apw); arn.LegacyWaypointLookup.Add(pp.ID, apw); } #endregion // generate perimeter ArbiterPerimeter ap = new ArbiterPerimeter(api, perimeterWaypoints); arn.DisplayObjects.Add(ap); // set per in points foreach (ArbiterPerimeterWaypoint apw in perimeterWaypoints) { apw.Perimeter = ap; } #region Set Defined Links // set links among perimeter nodes for (int i = 1; i <= ap.PerimeterPoints.Count; i++) { ArbiterPerimeterWaypointId apwi = new ArbiterPerimeterWaypointId(i, ap.PerimeterId); ArbiterPerimeterWaypoint apw = ap.PerimeterPoints[apwi]; if (i < ap.PerimeterPoints.Count) { ArbiterPerimeterWaypointId apwiNext = new ArbiterPerimeterWaypointId(i + 1, ap.PerimeterId); apw.NextPerimeterPoint = ap.PerimeterPoints[apwiNext]; } else { ArbiterPerimeterWaypointId apwiNext = new ArbiterPerimeterWaypointId(1, ap.PerimeterId); apw.NextPerimeterPoint = ap.PerimeterPoints[apwiNext]; } } #endregion #endregion #region Generate Parking Spots List <ArbiterParkingSpot> parkingSpots = new List <ArbiterParkingSpot>(); #region Parking Spots foreach (ParkingSpot ps in sz.ParkingSpots) { // spot id int apsiNum = GenerationTools.GetId(ps.SpotID)[1]; ArbiterParkingSpotId apsi = new ArbiterParkingSpotId(apsiNum, azi); // spot width double spotWidth; // check if spot width not set if (ps.SpotWidth == null || ps.SpotWidth == "" || ps.SpotWidth == "0") { spotWidth = 3.0; } else { // convert feet to meters spotWidth = double.Parse(ps.SpotWidth) * 0.3048; } // spot ArbiterParkingSpot aps = new ArbiterParkingSpot(spotWidth, apsi); arn.DisplayObjects.Add(aps); // waypoints List <ArbiterParkingSpotWaypoint> parkingSpotWaypoints = new List <ArbiterParkingSpotWaypoint>(); #region Parking Spot Waypoints #region Waypoint 1 // id int apwi1Number = GenerationTools.GetId(ps.Waypoint1.ID)[2]; ArbiterParkingSpotWaypointId apwi1 = new ArbiterParkingSpotWaypointId(apwi1Number, apsi); // generate waypoint 1 ArbiterParkingSpotWaypoint apw1 = new ArbiterParkingSpotWaypoint( ps.Waypoint1.Position, apwi1, aps); // set parkingSpotWaypoints.Add(apw1); waypoints.Add(apw1); arn.DisplayObjects.Add(apw1); arn.LegacyWaypointLookup.Add(ps.Waypoint1.ID, apw1); apw1.ParkingSpot = aps; // checkpoint or not? if (ps.CheckpointWaypointID == ps.Waypoint1.ID) { apw1.IsCheckpoint = true; apw1.CheckpointId = int.Parse(ps.CheckpointID); aps.Checkpoint = apw1; arn.Checkpoints.Add(apw1.CheckpointId, apw1); } else { aps.NormalWaypoint = apw1; } #endregion #region Waypoint 2 // id int apwi2Number = GenerationTools.GetId(ps.Waypoint2.ID)[2]; ArbiterParkingSpotWaypointId apwi2 = new ArbiterParkingSpotWaypointId(apwi2Number, apsi); // generate waypoint 2 ArbiterParkingSpotWaypoint apw2 = new ArbiterParkingSpotWaypoint( ps.Waypoint2.Position, apwi2, aps); // set parkingSpotWaypoints.Add(apw2); waypoints.Add(apw2); arn.DisplayObjects.Add(apw2); arn.LegacyWaypointLookup.Add(ps.Waypoint2.ID, apw2); apw2.ParkingSpot = aps; // checkpoint or not? if (ps.CheckpointWaypointID == ps.Waypoint2.ID) { apw2.IsCheckpoint = true; apw2.CheckpointId = int.Parse(ps.CheckpointID); aps.Checkpoint = apw2; arn.Checkpoints.Add(apw2.CheckpointId, apw2); } else { aps.NormalWaypoint = apw2; } #endregion #endregion // set waypoints aps.SetWaypoints(parkingSpotWaypoints); // add parkingSpots.Add(aps); } #endregion #endregion #region Create Zone // create zone ArbiterZone az = new ArbiterZone(azi, ap, parkingSpots, arn); // zone az.SpeedLimits = new ArbiterSpeedLimit(); az.SpeedLimits.MaximumSpeed = 2.24; az.SpeedLimits.MinimumSpeed = 2.24; // add to final dictionary zones.Add(az.ZoneId, az); arn.DisplayObjects.Add(az); #endregion } // set zones arn.ArbiterZones = zones; // add waypoints foreach (IArbiterWaypoint iaw in waypoints) { // set waypoint arn.ArbiterWaypoints.Add(iaw.AreaSubtypeWaypointId, iaw); } // return return(arn); }
/// <summary> /// Generates the arbiter zones form the internal xy zones for the input road network /// </summary> /// <param name="arn"></param> /// <returns></returns> /// <remarks>TODO: add zone cost map, adjacency of parking spots, figure out width</remarks> public ArbiterRoadNetwork GenerateZones(ArbiterRoadNetwork arn) { Dictionary<ArbiterZoneId, ArbiterZone> zones = new Dictionary<ArbiterZoneId,ArbiterZone>(); List<IArbiterWaypoint> waypoints = new List<IArbiterWaypoint>(); foreach (SimpleZone sz in xyZones) { ArbiterZoneId azi = new ArbiterZoneId(int.Parse(sz.ZoneID)); #region Generate Perimeter // old perim ZonePerimeter zp = sz.Perimeter; // perim id ArbiterPerimeterId api = new ArbiterPerimeterId(GenerationTools.GetId(zp.PerimeterID)[1], azi); #region Perimeter Waypoints List<ArbiterPerimeterWaypoint> perimeterWaypoints = new List<ArbiterPerimeterWaypoint>(); foreach (PerimeterPoint pp in zp.PerimeterPoints) { // id ArbiterPerimeterWaypointId apwi = new ArbiterPerimeterWaypointId( GenerationTools.GetId(pp.ID)[2], api); // point ArbiterPerimeterWaypoint apw = new ArbiterPerimeterWaypoint(apwi, pp.position); // add perimeterWaypoints.Add(apw); waypoints.Add(apw); arn.DisplayObjects.Add(apw); arn.LegacyWaypointLookup.Add(pp.ID, apw); } #endregion // generate perimeter ArbiterPerimeter ap = new ArbiterPerimeter(api, perimeterWaypoints); arn.DisplayObjects.Add(ap); // set per in points foreach (ArbiterPerimeterWaypoint apw in perimeterWaypoints) { apw.Perimeter = ap; } #region Set Defined Links // set links among perimeter nodes for (int i = 1; i <= ap.PerimeterPoints.Count; i++) { ArbiterPerimeterWaypointId apwi = new ArbiterPerimeterWaypointId(i, ap.PerimeterId); ArbiterPerimeterWaypoint apw = ap.PerimeterPoints[apwi]; if (i < ap.PerimeterPoints.Count) { ArbiterPerimeterWaypointId apwiNext = new ArbiterPerimeterWaypointId(i+1, ap.PerimeterId); apw.NextPerimeterPoint = ap.PerimeterPoints[apwiNext]; } else { ArbiterPerimeterWaypointId apwiNext = new ArbiterPerimeterWaypointId(1, ap.PerimeterId); apw.NextPerimeterPoint = ap.PerimeterPoints[apwiNext]; } } #endregion #endregion #region Generate Parking Spots List<ArbiterParkingSpot> parkingSpots = new List<ArbiterParkingSpot>(); #region Parking Spots foreach (ParkingSpot ps in sz.ParkingSpots) { // spot id int apsiNum = GenerationTools.GetId(ps.SpotID)[1]; ArbiterParkingSpotId apsi = new ArbiterParkingSpotId(apsiNum, azi); // spot width double spotWidth; // check if spot width not set if(ps.SpotWidth == null || ps.SpotWidth == "" || ps.SpotWidth == "0") { spotWidth = 3.0; } else { // convert feet to meters spotWidth = double.Parse(ps.SpotWidth) * 0.3048; } // spot ArbiterParkingSpot aps = new ArbiterParkingSpot(spotWidth, apsi); arn.DisplayObjects.Add(aps); // waypoints List<ArbiterParkingSpotWaypoint> parkingSpotWaypoints = new List<ArbiterParkingSpotWaypoint>(); #region Parking Spot Waypoints #region Waypoint 1 // id int apwi1Number = GenerationTools.GetId(ps.Waypoint1.ID)[2]; ArbiterParkingSpotWaypointId apwi1 = new ArbiterParkingSpotWaypointId(apwi1Number, apsi); // generate waypoint 1 ArbiterParkingSpotWaypoint apw1 = new ArbiterParkingSpotWaypoint( ps.Waypoint1.Position, apwi1, aps); // set parkingSpotWaypoints.Add(apw1); waypoints.Add(apw1); arn.DisplayObjects.Add(apw1); arn.LegacyWaypointLookup.Add(ps.Waypoint1.ID, apw1); apw1.ParkingSpot = aps; // checkpoint or not? if (ps.CheckpointWaypointID == ps.Waypoint1.ID) { apw1.IsCheckpoint = true; apw1.CheckpointId = int.Parse(ps.CheckpointID); aps.Checkpoint = apw1; arn.Checkpoints.Add(apw1.CheckpointId, apw1); } else { aps.NormalWaypoint = apw1; } #endregion #region Waypoint 2 // id int apwi2Number = GenerationTools.GetId(ps.Waypoint2.ID)[2]; ArbiterParkingSpotWaypointId apwi2 = new ArbiterParkingSpotWaypointId(apwi2Number, apsi); // generate waypoint 2 ArbiterParkingSpotWaypoint apw2 = new ArbiterParkingSpotWaypoint( ps.Waypoint2.Position, apwi2, aps); // set parkingSpotWaypoints.Add(apw2); waypoints.Add(apw2); arn.DisplayObjects.Add(apw2); arn.LegacyWaypointLookup.Add(ps.Waypoint2.ID, apw2); apw2.ParkingSpot = aps; // checkpoint or not? if (ps.CheckpointWaypointID == ps.Waypoint2.ID) { apw2.IsCheckpoint = true; apw2.CheckpointId = int.Parse(ps.CheckpointID); aps.Checkpoint = apw2; arn.Checkpoints.Add(apw2.CheckpointId, apw2); } else { aps.NormalWaypoint = apw2; } #endregion #endregion // set waypoints aps.SetWaypoints(parkingSpotWaypoints); // add parkingSpots.Add(aps); } #endregion #endregion #region Create Zone // create zone ArbiterZone az = new ArbiterZone(azi, ap, parkingSpots, arn); // zone az.SpeedLimits = new ArbiterSpeedLimit(); az.SpeedLimits.MaximumSpeed = 2.24; az.SpeedLimits.MinimumSpeed = 2.24; // add to final dictionary zones.Add(az.ZoneId, az); arn.DisplayObjects.Add(az); #endregion } // set zones arn.ArbiterZones = zones; // add waypoints foreach (IArbiterWaypoint iaw in waypoints) { // set waypoint arn.ArbiterWaypoints.Add(iaw.AreaSubtypeWaypointId, iaw); } // return return arn; }