/// <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);
}
