/// <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;
 }
Example #9
0
 /// <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>
 /// Constructor
 /// </summary>
 /// <param name="number"></param>
 /// <param name="zoneId"></param>
 public ArbiterParkingSpotId(int number, ArbiterZoneId zoneId)
 {
     this.spotNumber = number;
     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;
        }