public int CompareTo(object obj) { if (obj is RouteInformation) { RouteInformation other = (RouteInformation)obj; if (this.RouteTimeCost < other.RouteTimeCost) { return(-1); } else if (this.RouteTimeCost == other.RouteTimeCost) { return(0); } else { return(1); } } else { throw new Exception("obj is not route information"); } }
/// <summary> /// Plan in a zone /// </summary> /// <param name="az"></param> /// <param name="goal"></param> /// <returns></returns> public ZonePlan PlanZone(ArbiterZone az, INavigableNode start, INavigableNode goal) { // zone plan ZonePlan zp = new ZonePlan(); zp.Zone = az; zp.Start = start; // given start and goal, get plan double time; List<INavigableNode> nodes; this.Plan(start, goal, out nodes, out time); zp.Time = time; // final path LinePath recommendedPath = new LinePath(); List<INavigableNode> pathNodes = new List<INavigableNode>(); // start and end counts int startCount = 0; int endCount = 0; // check start type if(start is ArbiterParkingSpotWaypoint) startCount = 2; // check end type if(goal is ArbiterParkingSpotWaypoint && ((ArbiterParkingSpotWaypoint)goal).ParkingSpot.Zone.Equals(az)) endCount = -2; // loop through nodes for(int i = startCount; i < nodes.Count + endCount; i++) { // go to parking spot or endpoint if(nodes[i] is ArbiterParkingSpotWaypoint) { // set zone goal zp.ZoneGoal = ((ArbiterParkingSpotWaypoint)nodes[i]).ParkingSpot.Checkpoint; // set path, return zp.RecommendedPath = recommendedPath; zp.PathNodes = pathNodes; // set route info RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString()); CoreCommon.CurrentInformation.Route1 = ri; // return the plan return zp; } // go to perimeter waypoint if this is one else if(nodes[i] is ArbiterPerimeterWaypoint && ((ArbiterPerimeterWaypoint)nodes[i]).IsExit) { // add recommendedPath.Add(nodes[i].Position); // set zone goal zp.ZoneGoal = nodes[i]; // set path, return zp.RecommendedPath = recommendedPath; zp.PathNodes = pathNodes; // set route info RouteInformation ri = new RouteInformation(recommendedPath, time, goal.ToString()); CoreCommon.CurrentInformation.Route1 = ri; // return the plan return zp; } // otherwise just add else { // add recommendedPath.Add(nodes[i].Position); pathNodes.Add(nodes[i]); } } // set zone goal zp.ZoneGoal = goal; // set path, return zp.RecommendedPath = recommendedPath; // set route info CoreCommon.CurrentInformation.Route1 = new RouteInformation(recommendedPath, time, goal.ToString()); // return the plan return zp; }
/// <summary> /// Gets route information for sending to remote listeners /// </summary> public List<RouteInformation> RouteInformation(Coordinates currentPosition) { List<RouteInformation> routes = new List<RouteInformation>(); foreach (KeyValuePair<ArbiterLaneId, LanePlan> lp in this.LanePlans) { List<Coordinates> route = new List<Coordinates>(); double time = lp.Value.laneWaypointOfInterest.TotalTime; // get lane coords ArbiterLanePartition current = lp.Value.laneWaypointOfInterest.PointOfInterest.Lane.GetClosestPartition(currentPosition); if (CoreCommon.CorePlanningState is StayInSupraLaneState) { StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState; if(sisls.Lane.ClosestComponent(currentPosition) == SLComponentType.Initial) { LinePath p = sisls.Lane.LanePath(currentPosition, sisls.Lane.Interconnect.InitialGeneric.Position); route.AddRange(p); route.Add(sisls.Lane.Interconnect.FinalGeneric.Position); current = ((ArbiterWaypoint)sisls.Lane.Interconnect.FinalGeneric).NextPartition; } } while (current != null && current.Initial != lp.Value.laneWaypointOfInterest.PointOfInterest) { route.Add(current.Final.Position); current = current.Final.NextPartition; } // get route coords if (lp.Value.laneWaypointOfInterest.BestRoute != null) { foreach (INavigableNode inn in lp.Value.laneWaypointOfInterest.BestRoute) { route.Add(inn.Position); } } RouteInformation ri = new RouteInformation(route, time, lp.Value.laneWaypointOfInterest.PointOfInterest.WaypointId.ToString()); routes.Add(ri); } routes.Sort(); return routes; }