public PointAnalysisTool(PlanarProjection projection, bool snap, ArbiterRoadNetwork arn, WorldTransform wt) { this.projection = projection; this.snapToWaypoints = snap; this.roadNetwork = arn; this.wt = wt; }
/// <summary> /// World service /// </summary> /// <param name="arn"></param> public WorldService(ArbiterRoadNetwork arn) { this.RoadNetwork = arn; this.worldState = new WorldState(); this.lidar = new SimSensor(Math.PI/3, -Math.PI/3, 0.5*Math.PI/180.0, SensorType.Scan, 50); this.lidar2 = new SimSensor(4 * Math.PI / 3, 2 * Math.PI / 3, 0.5 * Math.PI / 180.0, SensorType.Scan, 30); }
public ShiftNetwork(ArbiterRoadNetwork roads, RoadDisplay roadDisplay) { InitializeComponent(); this.roads = roads; this.display = roadDisplay; }
public IntersectionToolbox(ArbiterRoadNetwork arn, RoadDisplay rd, Editor ed) { this.arn = arn; this.rd = rd; this.ed = ed; InitializeComponent(); }
/// <summary> /// Generates lane partition adjacency /// </summary> /// <param name="arn"></param> private void GenerateLanePartitionAdjacency(ArbiterRoadNetwork arn) { // loop over segments foreach (ArbiterSegment asg in arn.ArbiterSegments.Values) { // loop over lanes foreach (ArbiterLane al in asg.Lanes.Values) { // loop over lane partitions foreach (ArbiterLanePartition alp in al.Partitions) { // left lane if (al.LaneOnLeft != null) { this.GenerateSinglePartitionAdjacency(alp, al.LaneOnLeft); } // right lane if (al.LaneOnRight != null) { this.GenerateSinglePartitionAdjacency(alp, al.LaneOnRight); } } } } }
/// <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="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> /// Generates all adjacency mappings for the road network /// </summary> /// <param name="arn"></param> /// <returns></returns> public ArbiterRoadNetwork GenerateAdjacencies(ArbiterRoadNetwork arn) { // generate lane adjacency this.GenerateLaneAdjacency(arn); // generate partition adjacency this.GenerateLanePartitionAdjacency(arn); // generate entry adjacency this.GenerateNavigationalAdjacency(arn); // return return arn; }
/// <summary> /// Constructor /// </summary> /// <param name="p"></param> /// <param name="exits"></param> /// <param name="involved"></param> /// <param name="incoming"></param> public ArbiterIntersection(Polygon p, List<ArbiterStoppedExit> exits, Dictionary<ArbiterInterconnect, List<IntersectionInvolved>> priority, Dictionary<ArbiterLane, LinePath.PointOnPath> incoming, Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint> allExits, Coordinates center, ArbiterIntersectionId id, ArbiterRoadNetwork network, Dictionary<IAreaSubtypeWaypointId, ITraversableWaypoint> entries) { this.IntersectionPolygon = p; this.StoppedExits = exits; this.PriorityLanes = priority; this.IncomingLanePoints = incoming; this.AllExits = allExits; this.Center = center; this.IntersectionId = id; this.RoadNetwork = network; this.AllEntries = entries; }
/// <summary> /// Constructor /// </summary> /// <param name="p"></param> /// <param name="exits"></param> /// <param name="involved"></param> /// <param name="incoming"></param> public ArbiterIntersection(Polygon p, List <ArbiterStoppedExit> exits, Dictionary <ArbiterInterconnect, List <IntersectionInvolved> > priority, Dictionary <ArbiterLane, LinePath.PointOnPath> incoming, Dictionary <IAreaSubtypeWaypointId, ITraversableWaypoint> allExits, Coordinates center, ArbiterIntersectionId id, ArbiterRoadNetwork network, Dictionary <IAreaSubtypeWaypointId, ITraversableWaypoint> entries) { this.IntersectionPolygon = p; this.StoppedExits = exits; this.PriorityLanes = priority; this.IncomingLanePoints = incoming; this.AllExits = allExits; this.Center = center; this.IntersectionId = id; this.RoadNetwork = network; this.AllEntries = entries; }
/// <summary> /// Generates all adjacency mappings for the road network /// </summary> /// <param name="arn"></param> /// <returns></returns> public ArbiterRoadNetwork GenerateAdjacencies(ArbiterRoadNetwork arn) { // generate lane adjacency this.GenerateLaneAdjacency(arn); // generate partition adjacency this.GenerateLanePartitionAdjacency(arn); // generate entry adjacency this.GenerateNavigationalAdjacency(arn); // return return(arn); }
/// <summary> /// Constructor /// </summary> /// <param name="arn"></param> /// <param name="rd"></param> /// <param name="ed"></param> public ZoneTool(ArbiterRoadNetwork arn, RoadDisplay rd, Editor ed) { // set helpers we can access this.arn = arn; this.rd = rd; this.ed = ed; // helpers to wrap intersections for polygons this.WrappingHelpers = new List<Coordinates>(); // create toolbox zt = new ZoneToolbox(this); zt.Show(); }
/// <summary> /// Constructor /// </summary> /// <param name="arn"></param> /// <param name="rd"></param> /// <param name="ed"></param> public IntersectionPulloutTool(ArbiterRoadNetwork arn, RoadDisplay rd, Editor ed, bool show) { // set helpers we can access this.arn = arn; this.rd = rd; this.ed = ed; // helpers to wrap intersections for polygons this.WrappingHelpers = new List<Coordinates>(); // create toolbox if (show) { it = new IntersectionToolbox(arn, rd, ed); it.Show(); } }
/// <summary> /// Generate the arbiter road network from the internal xyRndf /// </summary> /// <returns></returns> public ArbiterRoadNetwork GenerateRoadNetwork() { // the road network we're generating ArbiterRoadNetwork arn = new ArbiterRoadNetwork(); arn.Name = xyRndf.Name; arn.CreationDate = xyRndf.CreationDate; // if zoens exist if (xyRndf.Zones != null) { // construct zone generator ZoneGeneration zg = new ZoneGeneration(xyRndf.Zones); // generate zones arn = zg.GenerateZones(arn); } // if segments exist if (xyRndf.Segments != null) { // generate segments SegmentGeneration sg = new SegmentGeneration(xyRndf.Segments); // generate segments arn = sg.GenerateSegments(arn); } // interconnects InterconnectGeneration ig = new InterconnectGeneration(xyRndf); arn = ig.GenerateInterconnects(arn); // adjacency (lane, partition, entry) AdjacencyGeneration ag = new AdjacencyGeneration(); arn = ag.GenerateAdjacencies(arn); // other stuff (vehicle areas) arn.GenerateVehicleAreas(); // return return(arn); }
/// <summary> /// Generate the arbiter road network from the internal xyRndf /// </summary> /// <returns></returns> public ArbiterRoadNetwork GenerateRoadNetwork() { // the road network we're generating ArbiterRoadNetwork arn = new ArbiterRoadNetwork(); arn.Name = xyRndf.Name; arn.CreationDate = xyRndf.CreationDate; // if zoens exist if (xyRndf.Zones != null) { // construct zone generator ZoneGeneration zg = new ZoneGeneration(xyRndf.Zones); // generate zones arn = zg.GenerateZones(arn); } // if segments exist if (xyRndf.Segments != null) { // generate segments SegmentGeneration sg = new SegmentGeneration(xyRndf.Segments); // generate segments arn = sg.GenerateSegments(arn); } // interconnects InterconnectGeneration ig = new InterconnectGeneration(xyRndf); arn = ig.GenerateInterconnects(arn); // adjacency (lane, partition, entry) AdjacencyGeneration ag = new AdjacencyGeneration(); arn = ag.GenerateAdjacencies(arn); // other stuff (vehicle areas) arn.GenerateVehicleAreas(); // return return arn; }
/// <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> /// Generates interconnects into the road network /// </summary> /// <param name="arn"></param> /// <returns></returns> public ArbiterRoadNetwork GenerateInterconnects(ArbiterRoadNetwork arn) { // list of all exit entries in the xy rndf List <SimpleExitEntry> sees = new List <SimpleExitEntry>(); // zones if (xyRndf.Zones != null) { // loop over zones foreach (SimpleZone sz in xyRndf.Zones) { // add all ee's sees.AddRange(sz.Perimeter.ExitEntries); } } // segments if (xyRndf.Segments != null) { // loop over segments foreach (SimpleSegment ss in xyRndf.Segments) { // lanes foreach (SimpleLane sl in ss.Lanes) { // add all ee's sees.AddRange(sl.ExitEntries); } } } // loop over ee's and create interconnects foreach (SimpleExitEntry see in sees) { IArbiterWaypoint initial = arn.LegacyWaypointLookup[see.ExitId]; IArbiterWaypoint final = arn.LegacyWaypointLookup[see.EntryId]; ArbiterInterconnect ai = new ArbiterInterconnect(initial, final); arn.ArbiterInterconnects.Add(ai.InterconnectId, ai); arn.DisplayObjects.Add(ai); if (initial is ITraversableWaypoint) { ITraversableWaypoint initialWaypoint = (ITraversableWaypoint)initial; initialWaypoint.IsExit = true; if (initialWaypoint.Exits == null) { initialWaypoint.Exits = new List <ArbiterInterconnect>(); } initialWaypoint.Exits.Add(ai); } else { throw new Exception("initial wp of ee: " + see.ExitId + " is not ITraversableWaypoint"); } if (final is ITraversableWaypoint) { ITraversableWaypoint finalWaypoint = (ITraversableWaypoint)final; finalWaypoint.IsEntry = true; if (finalWaypoint.Entries == null) { finalWaypoint.Entries = new List <ArbiterInterconnect>(); } finalWaypoint.Entries.Add(ai); } else { throw new Exception("final wp of ee: " + see.EntryId + " is not ITraversableWaypoint"); } // set the turn direction this.SetTurnDirection(ai); // interconnectp olygon stuff this.GenerateInterconnectPolygon(ai); if (ai.TurnPolygon.IsComplex) { Console.WriteLine("Found complex polygon for interconnect: " + ai.ToString()); ai.TurnPolygon = ai.DefaultPoly(); } } return(arn); }
/// <summary> /// Sets the road network and mission for the car /// </summary> /// <param name="roadNetwork"></param> /// <param name="mission"></param> public abstract void SetRoadNetworkAndMission(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission);
/// <summary> /// Starts the vehicle /// </summary> /// <param name="arbiterRoadNetwork"></param> /// <param name="arbiterMissionDescription"></param> /// <returns></returns> public bool StartupVehicle(ArbiterRoadNetwork arbiterRoadNetwork, ArbiterMissionDescription arbiterMissionDescription) { try { // get arbiter this.client.ClientArbiter = (ArbiterAdvancedRemote)this.objectDirectory.Resolve("ArbiterAdvancedRemote_" + SimulatorClient.MachineName); // set road and mission and spool up ai to wait for initial data stream this.client.ClientArbiter.JumpstartArbiter(arbiterRoadNetwork, arbiterMissionDescription); // success! return true; } catch (Exception e) { Console.WriteLine("Error spooling up ai: \n" + e.ToString()); return false; } }
public abstract void JumpstartArbiter(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission);
/// <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); }
public abstract void SetRoadNetwork(ArbiterRoadNetwork roadNetwork);
/// <summary> /// Generates lane adjacency in the network /// </summary> /// <param name="arn"></param> private void GenerateLaneAdjacency(ArbiterRoadNetwork arn) { // loop over segments foreach (ArbiterSegment asg in arn.ArbiterSegments.Values) { // check if both ways exist for first algorithm if (asg.Way1.IsValid && asg.Way2.IsValid) { // lanes of the segment Dictionary <ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes; // get a sample lane from way 1 Dictionary <ArbiterLaneId, ArbiterLane> .Enumerator way1Enum = asg.Way1.Lanes.GetEnumerator(); way1Enum.MoveNext(); ArbiterLane way1Sample = way1Enum.Current.Value; // get a sample lane from way 2 Dictionary <ArbiterLaneId, ArbiterLane> .Enumerator way2Enum = asg.Way2.Lanes.GetEnumerator(); way2Enum.MoveNext(); ArbiterLane way2Sample = way2Enum.Current.Value; // direction, 1 means way1 has lower # lanes int modifier = 1; // check if way 2 has lower lane numbers if (way1Sample.LaneId.Number > way2Sample.LaneId.Number) { // set modifier to -1 so count other way modifier = -1; } // loop over lanes foreach (ArbiterLane al in segLanes.Values) { // if not lane 1 if (al.LaneId.Number != 1) { // get lower # lane in way 1 ArbiterLaneId lowerNumWay1Id = new ArbiterLaneId(al.LaneId.Number - 1, asg.Way1.WayId); // check if the segment contains this lane if (segLanes.ContainsKey(lowerNumWay1Id)) { // get lane ArbiterLane lowerNumWay1 = segLanes[lowerNumWay1Id]; // check if current lane is also in way 1 if (lowerNumWay1.Way.WayId.Equals(al.Way.WayId)) { // check modifier for 1 => lower is to right if (modifier == 1) { al.LaneOnRight = lowerNumWay1; lowerNumWay1.LaneOnLeft = al; } // otherwise -1 => lane is to left else { al.LaneOnLeft = lowerNumWay1; lowerNumWay1.LaneOnRight = al; } } // otherwise the current lane is in a different way else { // the lane is to the left by default al.LaneOnLeft = lowerNumWay1; lowerNumWay1.LaneOnLeft = al; } } // otherwise the lowe lane is in way 2 else { // set lane ArbiterLane lowerNumWay2 = segLanes[new ArbiterLaneId(al.LaneId.Number - 1, asg.Way2.WayId)]; // check if current lane is also in way 2 if (lowerNumWay2.Way.WayId.Equals(al.Way.WayId)) { // check modifier for 1 => lower is to left if (modifier == 1) { al.LaneOnLeft = lowerNumWay2; lowerNumWay2.LaneOnRight = al; } // otherwise -1 => lane is to right else { al.LaneOnRight = lowerNumWay2; lowerNumWay2.LaneOnLeft = al; } } // otherwise the current lane is in a different way else { // the lane is to the left by default al.LaneOnLeft = lowerNumWay2; lowerNumWay2.LaneOnLeft = al; } } } } // loop over lanes } // both ways valid // otherwise only a single way is valid else { // lanes of the segment Dictionary <ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes; // make sure more than one lane exists if (segLanes.Count > 1) { // loop over lanes foreach (ArbiterLane al in segLanes.Values) { // get theoretical id of lane one number up ArbiterLaneId ali = new ArbiterLaneId(al.LaneId.Number + 1, al.LaneId.WayId); // check if lane one number up exists if (segLanes.ContainsKey(ali)) { // get lane one number up ArbiterLane alu = segLanes[ali]; // check # waypoints if (al.Waypoints.Count > 1 && alu.Waypoints.Count > 1) { // get closest points on this lane and other lane PointOnPath p1; PointOnPath p2; double distance; CreationTools.GetClosestPoints(al.PartitionPath, alu.PartitionPath, out p1, out p2, out distance); // get partition points of closest point on this lane Coordinates partitionStart = p1.segment.Start; Coordinates partitionEnd = p1.segment.End; // get area of partition triangle double triangeArea = CreationTools.TriangleArea(partitionStart, p2.pt, partitionEnd); // determine if closest point on other lane is to the left or right of partition bool onLeft = true; if (triangeArea >= 0) { onLeft = false; } // set adjacency accordingly for both lanes if (onLeft) { al.LaneOnLeft = alu; alu.LaneOnRight = al; } // otherwise on right else { al.LaneOnRight = alu; alu.LaneOnLeft = al; } } } } } } // end single way only valid // loop over lanes to print info on adjacency /*foreach (ArbiterLane al in asg.Lanes.Values) * { * Console.Write(al.LaneId.ToString() + ": "); * * if (al.LaneOnLeft != null) * { * Console.Write("Left-" + al.LaneOnLeft.LaneId.ToString()); * } * * if (al.LaneOnRight != null) * { * Console.Write("Right-" + al.LaneOnRight.LaneId.ToString()); * } * * Console.Write("\n"); * } */ } // segment loop }
/// <summary> /// Generates entry adjacency /// </summary> /// <param name="arn"></param> /// <remarks>Determines for every entry in a segment, the closest, reachable /// waypoints on lanes in the same way if it exists</remarks> private void GenerateNavigationalAdjacency(ArbiterRoadNetwork arn) { // loop over segments foreach(ArbiterSegment asg in arn.ArbiterSegments.Values) { // loop over segment waypoints foreach(ArbiterWaypoint aw in asg.Waypoints.Values) { #region Next Waypoint // check if has next if (aw.NextPartition != null) { // add next waypoint aw.OutgoingConnections.Add(aw.NextPartition); } #endregion #region Exits // check if exit if (aw.IsExit) { // loop over interconnect foreach (ArbiterInterconnect ai in aw.Exits) { // add entries aw.OutgoingConnections.Add(ai); } } #endregion #region Adjacent Lanes // check if entry if (aw.IsEntry) { // foreach lane test in same way as aw foreach (ArbiterLane al in aw.Lane.Way.Lanes.Values) { // check if not same lane if (!aw.Lane.Equals(al) && al.RelativelyInside(aw.Position)) { // check ok if ((aw.Lane.LaneOnLeft != null && aw.Lane.LaneOnLeft.Equals(al) && aw.Lane.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) || (aw.Lane.LaneOnRight != null && aw.Lane.LaneOnRight.Equals(al) && aw.Lane.BoundaryRight != ArbiterLaneBoundary.SolidWhite)) { // get closest partition to aw's point ArbiterLanePartition alp = al.GetClosestPartition(aw.Position); // check downstream from this lane? if (aw.Lane.DistanceBetween(aw.Position, alp.Final.Position) >= -0.01 || (alp.Final.NextPartition != null && aw.Lane.DistanceBetween(aw.Position, alp.Final.NextPartition.Final.Position) >= -0.01)) { // new list of contained partitions List<IConnectAreaWaypoints> containedPartitions = new List<IConnectAreaWaypoints>(); containedPartitions.Add(alp); containedPartitions.Add(aw.NextPartition); // set aw as linking to that partition's final waypoint aw.OutgoingConnections.Add(new NavigableEdge(false, null, true, al.Way.Segment, containedPartitions, aw, alp.Final)); } } } } } #endregion } #region Adjacent starting in middle // loop over segment lanes foreach (ArbiterLane al in asg.Lanes.Values) { // get init wp ArbiterWaypoint initial = al.WaypointList[0]; // get adjacent lanes foreach (ArbiterLane adj in al.Way.Lanes.Values) { if (!adj.Equals(al)) { // check if initial is inside other if (adj.RelativelyInside(initial.Position)) { if ((adj.LaneOnLeft != null && adj.LaneOnLeft.Equals(al) && adj.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) || (adj.LaneOnRight != null && adj.LaneOnRight.Equals(al) && adj.BoundaryRight != ArbiterLaneBoundary.SolidWhite)) { // closest partition ArbiterLanePartition alp = adj.GetClosestPartition(initial.Position); // new list of contained partitions List<IConnectAreaWaypoints> containedPartitions = new List<IConnectAreaWaypoints>(); containedPartitions.Add(alp); containedPartitions.Add(initial.NextPartition); // set aw as linking to that partition's final waypoint alp.Initial.OutgoingConnections.Add(new NavigableEdge(false, null, true, al.Way.Segment, containedPartitions, alp.Initial, initial)); } } } } } #endregion } // loop over zones foreach (ArbiterZone az in arn.ArbiterZones.Values) { #region Perimeter Point adjacency // loop over zone perimeter points foreach (ArbiterPerimeterWaypoint apw in az.Perimeter.PerimeterPoints.Values) { #region Old Connectivity // check if this is an entry /*if (apw.IsEntry) { #region Link Perimeter Points // loop over perimeter points foreach(ArbiterPerimeterWaypoint apwTest in az.Perimeter.PerimeterPoints.Values) { // check not this and is exit if (!apw.Equals(apwTest) && apwTest.IsExit) { // add to connections apw.OutgoingConnections.Add(new NavigableEdge(true, apw.Perimeter.Zone, false, null, new List<IConnectAreaWaypoints>(), apw, apwTest)); } } #endregion #region Link Checkpoints // loop over parking spot waypoints foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values) { // check if checkpoint if (apsw.IsCheckpoint) { // add to connections apw.OutgoingConnections.Add(new NavigableEdge(true, apw.Perimeter.Zone, false, null, new List<IConnectAreaWaypoints>(), apw, apsw)); } } #endregion }*/ #endregion // check if the point is an exit if (apw.IsExit) { foreach (ArbiterInterconnect ai in apw.Exits) { // add to connections apw.OutgoingConnections.Add(ai); } } } #endregion #region Checkpoint adjacency // loop over parking spot waypoints foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values) { if (apsw.ParkingSpot.NormalWaypoint.Equals(apsw)) { apsw.OutgoingConnections.Add( new NavigableEdge(true, az, false, null, new List<IConnectAreaWaypoints>(), apsw, apsw.ParkingSpot.Checkpoint)); } else { apsw.OutgoingConnections.Add( new NavigableEdge(true, az, false, null, new List<IConnectAreaWaypoints>(), apsw, apsw.ParkingSpot.NormalWaypoint)); } } #region Old /* // loop over parking spot waypoints foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values) { // check if checkpoint if (apsw.IsCheckpoint) { #region Link Perimeter Points // loop over perimeter points foreach (ArbiterPerimeterWaypoint apwTest in az.Perimeter.PerimeterPoints.Values) { // check not this and is exit if (apwTest.IsExit) { // add to connections apsw.OutgoingConnections.Add(new NavigableEdge(true, apsw.ParkingSpot.Zone, false, null, new List<IConnectAreaWaypoints>(), apsw, apwTest)); } } #endregion #region Link Checkpoints // loop over parking spot waypoints foreach (ArbiterParkingSpotWaypoint apswTest in az.ParkingSpotWaypoints.Values) { // check if checkpoint if (!apsw.Equals(apswTest) && apswTest.IsCheckpoint) { // add to connections apsw.OutgoingConnections.Add(new NavigableEdge(true, apsw.ParkingSpot.Zone, false, null, new List<IConnectAreaWaypoints>(), apsw, apswTest)); } } #endregion } } */ #endregion #endregion } }
/// <summary> /// Jumpstarts the ai with a new road network and mission if we can /// </summary> /// <param name="roadNetwork"></param> /// <param name="mission"></param> public override void JumpstartArbiter(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission) { try { ArbiterOutput.Output("Jumpstarting Arbiter"); // warn if carmode not correct CarMode carMode = CoreCommon.Communications.GetCarMode(); if (carMode != CarMode.Pause && carMode != CarMode.Human) ArbiterOutput.Output("Warning: Vehicle is in CarMode: " + carMode.ToString()); if (roadNetwork != null && mission != null) { // destroy intelligence if exists this.intelligenceCore.DestroyIntelligence(); // create roads and mission roadNetwork.SetSpeedLimits(mission.SpeedLimits); roadNetwork.GenerateVehicleAreas(); this.arbiterRoadNetwork = roadNetwork; this.arbiterMissionDescription = mission; CoreCommon.RoadNetwork = roadNetwork; CoreCommon.Mission = mission; // startup ai this.intelligenceCore.Jumpstart(); } else { ArbiterOutput.Output("RoadNetwork and Mission must both have value"); } } catch (Exception ex) { ArbiterOutput.Output("JumpstartArbiter(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission) Failed", ex); } }
/// <summary> /// Generates the xySegments into segments and inputs them into the input road network /// </summary> /// <param name="arn"></param> /// <returns></returns> public ArbiterRoadNetwork GenerateSegments(ArbiterRoadNetwork arn) { foreach (SimpleSegment ss in segments) { // seg ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(ss.Id)); ArbiterSegment asg = new ArbiterSegment(asi); arn.ArbiterSegments.Add(asi, asg); asg.RoadNetwork = arn; asg.SpeedLimits = new ArbiterSpeedLimit(); asg.SpeedLimits.MaximumSpeed = 13.4112; // 30mph max speed // way1 ArbiterWayId awi1 = new ArbiterWayId(1, asi); ArbiterWay aw1 = new ArbiterWay(awi1); aw1.Segment = asg; asg.Ways.Add(awi1, aw1); asg.Way1 = aw1; // way2 ArbiterWayId awi2 = new ArbiterWayId(2, asi); ArbiterWay aw2 = new ArbiterWay(awi2); aw2.Segment = asg; asg.Ways.Add(awi2, aw2); asg.Way2 = aw2; // make lanes foreach (SimpleLane sl in ss.Lanes) { // lane ArbiterLaneId ali; ArbiterLane al; // get way of lane id if (ss.Way1Lanes.Contains(sl)) { ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi1); al = new ArbiterLane(ali); aw1.Lanes.Add(ali, al); al.Way = aw1; } else { ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi2); al = new ArbiterLane(ali); aw2.Lanes.Add(ali, al); al.Way = aw2; } // add to display arn.DisplayObjects.Add(al); // width al.Width = sl.LaneWidth == 0 ? TahoeParams.T * 2.0 : sl.LaneWidth * 0.3048; if (sl.LaneWidth == 0) { Console.WriteLine("lane: " + ali.ToString() + " contains no lane width, setting to 4m"); } // lane boundaries al.BoundaryLeft = this.GenerateLaneBoundary(sl.LeftBound); al.BoundaryRight = this.GenerateLaneBoundary(sl.RightBound); // add lane to seg asg.Lanes.Add(ali, al); // waypoints List <ArbiterWaypoint> waypointList = new List <ArbiterWaypoint>(); // generate waypoints foreach (SimpleWaypoint sw in sl.Waypoints) { // waypoint ArbiterWaypointId awi = new ArbiterWaypointId(GenerationTools.GetId(sw.ID)[2], ali); ArbiterWaypoint aw = new ArbiterWaypoint(sw.Position, awi); aw.Lane = al; // stop if (sl.Stops.Contains(sw.ID)) { aw.IsStop = true; } // checkpoint foreach (SimpleCheckpoint sc in sl.Checkpoints) { if (sw.ID == sc.WaypointId) { aw.IsCheckpoint = true; aw.CheckpointId = int.Parse(sc.CheckpointId); arn.Checkpoints.Add(aw.CheckpointId, aw); } } // add asg.Waypoints.Add(awi, aw); arn.ArbiterWaypoints.Add(awi, aw); al.Waypoints.Add(awi, aw); waypointList.Add(aw); arn.DisplayObjects.Add(aw); arn.LegacyWaypointLookup.Add(sw.ID, aw); } al.WaypointList = waypointList; // lane partitions List <ArbiterLanePartition> alps = new List <ArbiterLanePartition>(); al.Partitions = alps; // generate lane partitions for (int i = 0; i < waypointList.Count - 1; i++) { // create lane partition ArbiterLanePartitionId alpi = new ArbiterLanePartitionId(waypointList[i].WaypointId, waypointList[i + 1].WaypointId, ali); ArbiterLanePartition alp = new ArbiterLanePartition(alpi, waypointList[i], waypointList[i + 1], asg); alp.Lane = al; waypointList[i].NextPartition = alp; waypointList[i + 1].PreviousPartition = alp; alps.Add(alp); arn.DisplayObjects.Add(alp); // crete initial user partition ArbiterUserPartitionId aupi = new ArbiterUserPartitionId(alp.PartitionId, waypointList[i].WaypointId, waypointList[i + 1].WaypointId); ArbiterUserPartition aup = new ArbiterUserPartition(aupi, alp, waypointList[i], waypointList[i + 1]); List <ArbiterUserPartition> aups = new List <ArbiterUserPartition>(); aups.Add(aup); alp.UserPartitions = aups; alp.SetDefaultSparsePolygon(); arn.DisplayObjects.Add(aup); } // path segments of lane List <IPathSegment> ips = new List <IPathSegment>(); List <Coordinates> pathSegments = new List <Coordinates>(); pathSegments.Add(alps[0].Initial.Position); // loop foreach (ArbiterLanePartition alPar in alps) { ips.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position)); // make new segment pathSegments.Add(alPar.Final.Position); } // generate lane partition path LinePath partitionPath = new LinePath(pathSegments); al.SetLanePath(partitionPath); al.PartitionPath = new Path(ips, CoordinateMode.AbsoluteProjected); // safeto zones foreach (ArbiterWaypoint aw in al.Waypoints.Values) { if (aw.IsStop) { LinePath.PointOnPath end = al.GetClosestPoint(aw.Position); double dist = -30; LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist); if (dist != 0) { begin = al.LanePath().StartPoint; } ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin); asz.isExit = true; asz.Exit = aw; al.SafetyZones.Add(asz); arn.DisplayObjects.Add(asz); arn.ArbiterSafetyZones.Add(asz); } } } } return(arn); }
/// <summary> /// Restore from a saved editor save /// </summary> /// <param name="es"></param> private void Restore(SimulatorSave es) { // restore the editor this.gridSizeToolStripComboBox.SelectedIndex = es.GridSizeIndex; this.arbiterRoads = es.ArbiterRoads; this.simEngine = es.SimEngine; this.simEngine.simulationMain = this; this.simEngine.SetPropertyGrid(this.SimulatorPropertyGrid); this.defaultMissionDescription = es.Mission; // restore the display this.roadDisplay1.LoadSave(es.displaySave); // attempt to rehash the clients this.clientHandler.ReBindAll(this.simEngine.Vehicles); }
/// <summary> /// Sets the road network /// </summary> /// <param name="roads"></param> private void SetRoadNetwork(ArbiterRoadNetwork roads) { // set network RemoraCommon.RoadNetwork = roads; // clean out display this.roadDisplay1.RemoveDisplayObjectType(this.roadDisplay1.RoadNetworkFilter); // add to display this.roadDisplay1.AddDisplayObjectRange(roads.DisplayObjects); // notify RemoraOutput.WriteLine("Set Road Network", OutputType.Remora); // test mission if (RemoraCommon.Mission != null) { RemoraOutput.WriteLine("Testing Previously Loaded Mission aginst new Road Network, Removing and Reloading", OutputType.Remora); ArbiterMissionDescription tmp = RemoraCommon.Mission; RemoraCommon.Mission = null; this.SetMission(tmp); } // redraw this.roadDisplay1.Invalidate(); }
/// <summary> /// Open a road network from a file /// </summary> /// <param name="p"></param> private void OpenRoadNetworkFromFile(string p) { // create file FileStream fs = new FileStream(p, FileMode.Open); // serializer BinaryFormatter bf = new BinaryFormatter(); // serialize ArbiterRoadNetwork es = (ArbiterRoadNetwork)bf.Deserialize(fs); // remove previous road network this.roadDisplay1.RemoveDisplayObjectType(this.roadDisplay1.RoadNetworkFilter); // set netwrok this.arbiterRoads = es; // display this.roadDisplay1.AddDisplayObjectRange(es.DisplayObjects); // release holds fs.Dispose(); }
/// <summary> /// Generates lane adjacency in the network /// </summary> /// <param name="arn"></param> private void GenerateLaneAdjacency(ArbiterRoadNetwork arn) { // loop over segments foreach (ArbiterSegment asg in arn.ArbiterSegments.Values) { // check if both ways exist for first algorithm if (asg.Way1.IsValid && asg.Way2.IsValid) { // lanes of the segment Dictionary<ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes; // get a sample lane from way 1 Dictionary<ArbiterLaneId, ArbiterLane>.Enumerator way1Enum = asg.Way1.Lanes.GetEnumerator(); way1Enum.MoveNext(); ArbiterLane way1Sample = way1Enum.Current.Value; // get a sample lane from way 2 Dictionary<ArbiterLaneId, ArbiterLane>.Enumerator way2Enum = asg.Way2.Lanes.GetEnumerator(); way2Enum.MoveNext(); ArbiterLane way2Sample = way2Enum.Current.Value; // direction, 1 means way1 has lower # lanes int modifier = 1; // check if way 2 has lower lane numbers if (way1Sample.LaneId.Number > way2Sample.LaneId.Number) { // set modifier to -1 so count other way modifier = -1; } // loop over lanes foreach (ArbiterLane al in segLanes.Values) { // if not lane 1 if (al.LaneId.Number != 1) { // get lower # lane in way 1 ArbiterLaneId lowerNumWay1Id = new ArbiterLaneId(al.LaneId.Number - 1, asg.Way1.WayId); // check if the segment contains this lane if (segLanes.ContainsKey(lowerNumWay1Id)) { // get lane ArbiterLane lowerNumWay1 = segLanes[lowerNumWay1Id]; // check if current lane is also in way 1 if (lowerNumWay1.Way.WayId.Equals(al.Way.WayId)) { // check modifier for 1 => lower is to right if (modifier == 1) { al.LaneOnRight = lowerNumWay1; lowerNumWay1.LaneOnLeft = al; } // otherwise -1 => lane is to left else { al.LaneOnLeft = lowerNumWay1; lowerNumWay1.LaneOnRight = al; } } // otherwise the current lane is in a different way else { // the lane is to the left by default al.LaneOnLeft = lowerNumWay1; lowerNumWay1.LaneOnLeft = al; } } // otherwise the lowe lane is in way 2 else { // set lane ArbiterLane lowerNumWay2 = segLanes[new ArbiterLaneId(al.LaneId.Number - 1, asg.Way2.WayId)]; // check if current lane is also in way 2 if (lowerNumWay2.Way.WayId.Equals(al.Way.WayId)) { // check modifier for 1 => lower is to left if (modifier == 1) { al.LaneOnLeft = lowerNumWay2; lowerNumWay2.LaneOnRight = al; } // otherwise -1 => lane is to right else { al.LaneOnRight = lowerNumWay2; lowerNumWay2.LaneOnLeft = al; } } // otherwise the current lane is in a different way else { // the lane is to the left by default al.LaneOnLeft = lowerNumWay2; lowerNumWay2.LaneOnLeft = al; } } } } // loop over lanes } // both ways valid // otherwise only a single way is valid else { // lanes of the segment Dictionary<ArbiterLaneId, ArbiterLane> segLanes = asg.Lanes; // make sure more than one lane exists if(segLanes.Count > 1) { // loop over lanes foreach (ArbiterLane al in segLanes.Values) { // get theoretical id of lane one number up ArbiterLaneId ali = new ArbiterLaneId(al.LaneId.Number + 1, al.LaneId.WayId); // check if lane one number up exists if(segLanes.ContainsKey(ali)) { // get lane one number up ArbiterLane alu = segLanes[ali]; // check # waypoints if (al.Waypoints.Count > 1 && alu.Waypoints.Count > 1) { // get closest points on this lane and other lane PointOnPath p1; PointOnPath p2; double distance; CreationTools.GetClosestPoints(al.PartitionPath, alu.PartitionPath, out p1, out p2, out distance); // get partition points of closest point on this lane Coordinates partitionStart = p1.segment.Start; Coordinates partitionEnd = p1.segment.End; // get area of partition triangle double triangeArea = CreationTools.TriangleArea(partitionStart, p2.pt, partitionEnd); // determine if closest point on other lane is to the left or right of partition bool onLeft = true; if (triangeArea >= 0) { onLeft = false; } // set adjacency accordingly for both lanes if (onLeft) { al.LaneOnLeft = alu; alu.LaneOnRight = al; } // otherwise on right else { al.LaneOnRight = alu; alu.LaneOnLeft = al; } } } } } } // end single way only valid // loop over lanes to print info on adjacency /*foreach (ArbiterLane al in asg.Lanes.Values) { Console.Write(al.LaneId.ToString() + ": "); if (al.LaneOnLeft != null) { Console.Write("Left-" + al.LaneOnLeft.LaneId.ToString()); } if (al.LaneOnRight != null) { Console.Write("Right-" + al.LaneOnRight.LaneId.ToString()); } Console.Write("\n"); } */ } // segment loop }
/// <summary> /// Generates the xySegments into segments and inputs them into the input road network /// </summary> /// <param name="arn"></param> /// <returns></returns> public ArbiterRoadNetwork GenerateSegments(ArbiterRoadNetwork arn) { foreach (SimpleSegment ss in segments) { // seg ArbiterSegmentId asi = new ArbiterSegmentId(int.Parse(ss.Id)); ArbiterSegment asg = new ArbiterSegment(asi); arn.ArbiterSegments.Add(asi, asg); asg.RoadNetwork = arn; asg.SpeedLimits = new ArbiterSpeedLimit(); asg.SpeedLimits.MaximumSpeed = 13.4112; // 30mph max speed // way1 ArbiterWayId awi1 = new ArbiterWayId(1, asi); ArbiterWay aw1 = new ArbiterWay(awi1); aw1.Segment = asg; asg.Ways.Add(awi1, aw1); asg.Way1 = aw1; // way2 ArbiterWayId awi2 = new ArbiterWayId(2, asi); ArbiterWay aw2 = new ArbiterWay(awi2); aw2.Segment = asg; asg.Ways.Add(awi2, aw2); asg.Way2 = aw2; // make lanes foreach (SimpleLane sl in ss.Lanes) { // lane ArbiterLaneId ali; ArbiterLane al; // get way of lane id if (ss.Way1Lanes.Contains(sl)) { ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi1); al = new ArbiterLane(ali); aw1.Lanes.Add(ali, al); al.Way = aw1; } else { ali = new ArbiterLaneId(GenerationTools.GetId(sl.Id)[1], awi2); al = new ArbiterLane(ali); aw2.Lanes.Add(ali, al); al.Way = aw2; } // add to display arn.DisplayObjects.Add(al); // width al.Width = sl.LaneWidth == 0 ? TahoeParams.T * 2.0 : sl.LaneWidth * 0.3048; if(sl.LaneWidth == 0) Console.WriteLine("lane: " + ali.ToString() + " contains no lane width, setting to 4m"); // lane boundaries al.BoundaryLeft = this.GenerateLaneBoundary(sl.LeftBound); al.BoundaryRight = this.GenerateLaneBoundary(sl.RightBound); // add lane to seg asg.Lanes.Add(ali, al); // waypoints List<ArbiterWaypoint> waypointList = new List<ArbiterWaypoint>(); // generate waypoints foreach (SimpleWaypoint sw in sl.Waypoints) { // waypoint ArbiterWaypointId awi = new ArbiterWaypointId(GenerationTools.GetId(sw.ID)[2], ali); ArbiterWaypoint aw = new ArbiterWaypoint(sw.Position, awi); aw.Lane = al; // stop if (sl.Stops.Contains(sw.ID)) { aw.IsStop = true; } // checkpoint foreach (SimpleCheckpoint sc in sl.Checkpoints) { if (sw.ID == sc.WaypointId) { aw.IsCheckpoint = true; aw.CheckpointId = int.Parse(sc.CheckpointId); arn.Checkpoints.Add(aw.CheckpointId, aw); } } // add asg.Waypoints.Add(awi, aw); arn.ArbiterWaypoints.Add(awi, aw); al.Waypoints.Add(awi, aw); waypointList.Add(aw); arn.DisplayObjects.Add(aw); arn.LegacyWaypointLookup.Add(sw.ID, aw); } al.WaypointList = waypointList; // lane partitions List<ArbiterLanePartition> alps = new List<ArbiterLanePartition>(); al.Partitions = alps; // generate lane partitions for (int i = 0; i < waypointList.Count-1; i++) { // create lane partition ArbiterLanePartitionId alpi = new ArbiterLanePartitionId(waypointList[i].WaypointId, waypointList[i + 1].WaypointId, ali); ArbiterLanePartition alp = new ArbiterLanePartition(alpi, waypointList[i], waypointList[i+1], asg); alp.Lane = al; waypointList[i].NextPartition = alp; waypointList[i + 1].PreviousPartition = alp; alps.Add(alp); arn.DisplayObjects.Add(alp); // crete initial user partition ArbiterUserPartitionId aupi = new ArbiterUserPartitionId(alp.PartitionId, waypointList[i].WaypointId, waypointList[i + 1].WaypointId); ArbiterUserPartition aup = new ArbiterUserPartition(aupi, alp, waypointList[i], waypointList[i + 1]); List<ArbiterUserPartition> aups = new List<ArbiterUserPartition>(); aups.Add(aup); alp.UserPartitions = aups; alp.SetDefaultSparsePolygon(); arn.DisplayObjects.Add(aup); } // path segments of lane List<IPathSegment> ips = new List<IPathSegment>(); List<Coordinates> pathSegments = new List<Coordinates>(); pathSegments.Add(alps[0].Initial.Position); // loop foreach(ArbiterLanePartition alPar in alps) { ips.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position)); // make new segment pathSegments.Add(alPar.Final.Position); } // generate lane partition path LinePath partitionPath = new LinePath(pathSegments); al.SetLanePath(partitionPath); al.PartitionPath = new Path(ips, CoordinateMode.AbsoluteProjected); // safeto zones foreach (ArbiterWaypoint aw in al.Waypoints.Values) { if(aw.IsStop) { LinePath.PointOnPath end = al.GetClosestPoint(aw.Position); double dist = -30; LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist); if (dist != 0) { begin = al.LanePath().StartPoint; } ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin); asz.isExit = true; asz.Exit = aw; al.SafetyZones.Add(asz); arn.DisplayObjects.Add(asz); arn.ArbiterSafetyZones.Add(asz); } } } } return arn; }
/// <summary> /// Begin simulation /// </summary> public void BeginSimulation(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission) { try { // startup clients foreach (KeyValuePair<SimVehicleId, SimVehicle> vhcs in this.Vehicles) { // set road and mission SimulatorClientFacade scf = this.simulationMain.clientHandler.AvailableClients[this.simulationMain.clientHandler.VehicleToClientMap[vhcs.Key]]; // check if we need to randomize mission if (vhcs.Value.RandomMission) { // create random mission Queue<ArbiterCheckpoint> checks = new Queue<ArbiterCheckpoint>(60); int num = mission.MissionCheckpoints.Count - 1; ArbiterCheckpoint[] checkPointArray = mission.MissionCheckpoints.ToArray(); Random r = new Random(); for (int i = 0; i < 60; i++) { checks.Enqueue(checkPointArray[r.Next(num)]); } ArbiterMissionDescription amd = new ArbiterMissionDescription(checks, mission.SpeedLimits); // set road , mission scf.SetRoadNetworkAndMission(roadNetwork, amd); } // otherwise no random mission else { // set road , mission scf.SetRoadNetworkAndMission(roadNetwork, mission); } // startup ai bool b = scf.StartupVehicle(); // check for false if (!b) { Console.WriteLine("Error starting simulation for vehicle id: " + vhcs.Key.ToString()); return; } } } catch (Exception e) { Console.WriteLine("Error starting simulation: \n" + e.ToString()); return; } // run sim this.RunSimulation(); }
/// <summary> /// Sets the road network and mission for the car /// </summary> /// <param name="roadNetwork"></param> /// <param name="mission"></param> public override void SetRoadNetworkAndMission(ArbiterRoadNetwork roadNetwork, ArbiterMissionDescription mission) { this.roadNetwork = roadNetwork; this.mission = mission; this.roadNetwork.SetSpeedLimits(mission.SpeedLimits); this.world = new WorldService(this.roadNetwork); }
/// <summary> /// Constructor /// </summary> /// <param name="roadNetwork"></param> public SceneRoadGraphGeneration(ArbiterRoadNetwork roadNetwork) { this.roadNetwork = roadNetwork; }
/// <summary> /// Generates entry adjacency /// </summary> /// <param name="arn"></param> /// <remarks>Determines for every entry in a segment, the closest, reachable /// waypoints on lanes in the same way if it exists</remarks> private void GenerateNavigationalAdjacency(ArbiterRoadNetwork arn) { // loop over segments foreach (ArbiterSegment asg in arn.ArbiterSegments.Values) { // loop over segment waypoints foreach (ArbiterWaypoint aw in asg.Waypoints.Values) { #region Next Waypoint // check if has next if (aw.NextPartition != null) { // add next waypoint aw.OutgoingConnections.Add(aw.NextPartition); } #endregion #region Exits // check if exit if (aw.IsExit) { // loop over interconnect foreach (ArbiterInterconnect ai in aw.Exits) { // add entries aw.OutgoingConnections.Add(ai); } } #endregion #region Adjacent Lanes // check if entry if (aw.IsEntry) { // foreach lane test in same way as aw foreach (ArbiterLane al in aw.Lane.Way.Lanes.Values) { // check if not same lane if (!aw.Lane.Equals(al) && al.RelativelyInside(aw.Position)) { // check ok if ((aw.Lane.LaneOnLeft != null && aw.Lane.LaneOnLeft.Equals(al) && aw.Lane.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) || (aw.Lane.LaneOnRight != null && aw.Lane.LaneOnRight.Equals(al) && aw.Lane.BoundaryRight != ArbiterLaneBoundary.SolidWhite)) { // get closest partition to aw's point ArbiterLanePartition alp = al.GetClosestPartition(aw.Position); // check downstream from this lane? if (aw.Lane.DistanceBetween(aw.Position, alp.Final.Position) >= -0.01 || (alp.Final.NextPartition != null && aw.Lane.DistanceBetween(aw.Position, alp.Final.NextPartition.Final.Position) >= -0.01)) { // new list of contained partitions List <IConnectAreaWaypoints> containedPartitions = new List <IConnectAreaWaypoints>(); containedPartitions.Add(alp); containedPartitions.Add(aw.NextPartition); // set aw as linking to that partition's final waypoint aw.OutgoingConnections.Add(new NavigableEdge(false, null, true, al.Way.Segment, containedPartitions, aw, alp.Final)); } } } } } #endregion } #region Adjacent starting in middle // loop over segment lanes foreach (ArbiterLane al in asg.Lanes.Values) { // get init wp ArbiterWaypoint initial = al.WaypointList[0]; // get adjacent lanes foreach (ArbiterLane adj in al.Way.Lanes.Values) { if (!adj.Equals(al)) { // check if initial is inside other if (adj.RelativelyInside(initial.Position)) { if ((adj.LaneOnLeft != null && adj.LaneOnLeft.Equals(al) && adj.BoundaryLeft != ArbiterLaneBoundary.SolidWhite) || (adj.LaneOnRight != null && adj.LaneOnRight.Equals(al) && adj.BoundaryRight != ArbiterLaneBoundary.SolidWhite)) { // closest partition ArbiterLanePartition alp = adj.GetClosestPartition(initial.Position); // new list of contained partitions List <IConnectAreaWaypoints> containedPartitions = new List <IConnectAreaWaypoints>(); containedPartitions.Add(alp); containedPartitions.Add(initial.NextPartition); // set aw as linking to that partition's final waypoint alp.Initial.OutgoingConnections.Add(new NavigableEdge(false, null, true, al.Way.Segment, containedPartitions, alp.Initial, initial)); } } } } } #endregion } // loop over zones foreach (ArbiterZone az in arn.ArbiterZones.Values) { #region Perimeter Point adjacency // loop over zone perimeter points foreach (ArbiterPerimeterWaypoint apw in az.Perimeter.PerimeterPoints.Values) { #region Old Connectivity // check if this is an entry /*if (apw.IsEntry) * { #region Link Perimeter Points * * // loop over perimeter points * foreach(ArbiterPerimeterWaypoint apwTest in az.Perimeter.PerimeterPoints.Values) * { * // check not this and is exit * if (!apw.Equals(apwTest) && apwTest.IsExit) * { * // add to connections * apw.OutgoingConnections.Add(new NavigableEdge(true, apw.Perimeter.Zone, false, null, new List<IConnectAreaWaypoints>(), apw, apwTest)); * } * } * #endregion * #region Link Checkpoints * * // loop over parking spot waypoints * foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values) * { * // check if checkpoint * if (apsw.IsCheckpoint) * { * // add to connections * apw.OutgoingConnections.Add(new NavigableEdge(true, apw.Perimeter.Zone, false, null, new List<IConnectAreaWaypoints>(), apw, apsw)); * } * } * #endregion * }*/ #endregion // check if the point is an exit if (apw.IsExit) { foreach (ArbiterInterconnect ai in apw.Exits) { // add to connections apw.OutgoingConnections.Add(ai); } } } #endregion #region Checkpoint adjacency // loop over parking spot waypoints foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values) { if (apsw.ParkingSpot.NormalWaypoint.Equals(apsw)) { apsw.OutgoingConnections.Add( new NavigableEdge(true, az, false, null, new List <IConnectAreaWaypoints>(), apsw, apsw.ParkingSpot.Checkpoint)); } else { apsw.OutgoingConnections.Add( new NavigableEdge(true, az, false, null, new List <IConnectAreaWaypoints>(), apsw, apsw.ParkingSpot.NormalWaypoint)); } } #region Old /* * // loop over parking spot waypoints * foreach (ArbiterParkingSpotWaypoint apsw in az.ParkingSpotWaypoints.Values) * { * // check if checkpoint * if (apsw.IsCheckpoint) * { #region Link Perimeter Points * * // loop over perimeter points * foreach (ArbiterPerimeterWaypoint apwTest in az.Perimeter.PerimeterPoints.Values) * { * // check not this and is exit * if (apwTest.IsExit) * { * // add to connections * apsw.OutgoingConnections.Add(new NavigableEdge(true, apsw.ParkingSpot.Zone, false, null, new List<IConnectAreaWaypoints>(), apsw, apwTest)); * } * } * #endregion * #region Link Checkpoints * * // loop over parking spot waypoints * foreach (ArbiterParkingSpotWaypoint apswTest in az.ParkingSpotWaypoints.Values) * { * // check if checkpoint * if (!apsw.Equals(apswTest) && apswTest.IsCheckpoint) * { * // add to connections * apsw.OutgoingConnections.Add(new NavigableEdge(true, apsw.ParkingSpot.Zone, false, null, new List<IConnectAreaWaypoints>(), apsw, apswTest)); * } * } * #endregion * } * } */ #endregion #endregion } }
public IntersectionPulloutTool(ArbiterRoadNetwork arn) { this.arn = arn; }
public RulerTool(bool snap, ArbiterRoadNetwork arn, WorldTransform wt) { this.snapToWaypoints = snap; this.roadNetwork = arn; this.wt = wt; }
/// <summary> /// Generates interconnects into the road network /// </summary> /// <param name="arn"></param> /// <returns></returns> public ArbiterRoadNetwork GenerateInterconnects(ArbiterRoadNetwork arn) { // list of all exit entries in the xy rndf List<SimpleExitEntry> sees = new List<SimpleExitEntry>(); // zones if(xyRndf.Zones != null) { // loop over zones foreach (SimpleZone sz in xyRndf.Zones) { // add all ee's sees.AddRange(sz.Perimeter.ExitEntries); } } // segments if (xyRndf.Segments != null) { // loop over segments foreach (SimpleSegment ss in xyRndf.Segments) { // lanes foreach (SimpleLane sl in ss.Lanes) { // add all ee's sees.AddRange(sl.ExitEntries); } } } // loop over ee's and create interconnects foreach (SimpleExitEntry see in sees) { IArbiterWaypoint initial = arn.LegacyWaypointLookup[see.ExitId]; IArbiterWaypoint final = arn.LegacyWaypointLookup[see.EntryId]; ArbiterInterconnect ai = new ArbiterInterconnect(initial, final); arn.ArbiterInterconnects.Add(ai.InterconnectId, ai); arn.DisplayObjects.Add(ai); if (initial is ITraversableWaypoint) { ITraversableWaypoint initialWaypoint = (ITraversableWaypoint)initial; initialWaypoint.IsExit = true; if (initialWaypoint.Exits == null) initialWaypoint.Exits = new List<ArbiterInterconnect>(); initialWaypoint.Exits.Add(ai); } else { throw new Exception("initial wp of ee: " + see.ExitId + " is not ITraversableWaypoint"); } if (final is ITraversableWaypoint) { ITraversableWaypoint finalWaypoint = (ITraversableWaypoint)final; finalWaypoint.IsEntry = true; if (finalWaypoint.Entries == null) finalWaypoint.Entries = new List<ArbiterInterconnect>(); finalWaypoint.Entries.Add(ai); } else { throw new Exception("final wp of ee: " + see.EntryId + " is not ITraversableWaypoint"); } // set the turn direction this.SetTurnDirection(ai); // interconnectp olygon stuff this.GenerateInterconnectPolygon(ai); if (ai.TurnPolygon.IsComplex) { Console.WriteLine("Found complex polygon for interconnect: " + ai.ToString()); ai.TurnPolygon = ai.DefaultPoly(); } } return arn; }