/// <summary> /// Plan given that we are starting on a road /// </summary> /// <param name="currentLane"></param> /// <param name="currentPosition"></param> /// <param name="goal"></param> /// <returns></returns> public RoadPlan PlanNavigableArea(INavigableTravelArea currentArea, Coordinates currentPosition, INavigableNode goal, List <ArbiterWaypoint> ignorable) { // get all downstream points of interest as well as the goal point of interest List <DownstreamPointOfInterest> downstreamPoints = currentArea.Downstream(currentPosition, ignorable, goal); // so, for each exit downstream we need to plan from the end of each interconnect to the goal this.RouteTimes(downstreamPoints, goal); // get road plan RoadPlan rp = this.GetRoadPlan(downstreamPoints); #region Output // update arbiter information List <RouteInformation> routeInfo = rp.RouteInformation(currentPosition); // make sure we're in a road state if (CoreCommon.CorePlanningState == null || CoreCommon.CorePlanningState is UrbanChallenge.Arbiter.Core.Common.State.TravelState) { // check route 1 if (routeInfo.Count > 0) { RouteInformation ri = routeInfo[0]; CoreCommon.CurrentInformation.Route1 = ri; CoreCommon.CurrentInformation.Route1Time = ri.RouteTimeCost.ToString("F6"); CoreCommon.CurrentInformation.Route1Wp = ri.Waypoint; } } #endregion // return road plan return(rp); }
/// <summary> /// Plan given that we are starting on a road /// </summary> /// <param name="currentLane"></param> /// <param name="currentPosition"></param> /// <param name="goal"></param> /// <returns></returns> public RoadPlan PlanRoads(ArbiterLane currentLane, Coordinates currentPosition, INavigableNode goal, List <ArbiterWaypoint> ignorable) { // get all downstream points of interest List <DownstreamPointOfInterest> downstreamPoints = new List <DownstreamPointOfInterest>(); // get exits downstream from this current position in the way downstreamPoints.AddRange(this.Downstream(currentPosition, currentLane.Way, true, ignorable)); // determine if goal is downstream in a specific lane, add to possible route times to consider DownstreamPointOfInterest goalDownstream = this.IsGoalDownStream(currentLane.Way, currentPosition, goal); // add goal to points downstream if it exists if (goalDownstream != null) { downstreamPoints.Add(goalDownstream); } // so, for each exit downstream we need to plan from the end of each interconnect to the goal this.DetermineDownstreamPointRouteTimes(downstreamPoints, goal, currentLane.Way); // get road plan RoadPlan rp = this.GetRoadPlan(downstreamPoints, currentLane.Way); // update arbiter information List <RouteInformation> routeInfo = rp.RouteInformation(currentPosition); // make sure we're in a road state if (CoreCommon.CorePlanningState == null || CoreCommon.CorePlanningState is TravelState || CoreCommon.CorePlanningState is TurnState) { // check route 1 if (routeInfo.Count > 0) { RouteInformation ri = routeInfo[0]; CoreCommon.CurrentInformation.Route1 = ri; CoreCommon.CurrentInformation.Route1Time = ri.RouteTimeCost.ToString("F6"); CoreCommon.CurrentInformation.Route1Wp = ri.Waypoint; } // check route 2 if (routeInfo.Count > 1) { RouteInformation ri = routeInfo[1]; CoreCommon.CurrentInformation.Route2 = ri; CoreCommon.CurrentInformation.Route2Time = ri.RouteTimeCost.ToString("F6"); CoreCommon.CurrentInformation.Route2Wp = ri.Waypoint; } } // return road plan return(rp); }