/// <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>
/// Gets the road plan
/// </summary>
/// <param name="pointsOfInterest"></param>
/// <returns></returns>
private RoadPlan GetRoadPlan(List <DownstreamPointOfInterest> pointsOfInterest)
{
// road plan
RoadPlan rp = new RoadPlan(new Dictionary <ArbiterLaneId, LanePlan>());
// lane assigned points
Dictionary <ArbiterLaneId, List <DownstreamPointOfInterest> > lanePoints = new Dictionary <ArbiterLaneId, List <DownstreamPointOfInterest> >();
// assign dpoi to lanes
foreach (DownstreamPointOfInterest dpoi in pointsOfInterest)
{
if (lanePoints.ContainsKey(dpoi.PointOfInterest.Lane.LaneId))
{
lanePoints[dpoi.PointOfInterest.Lane.LaneId].Add(dpoi);
}
else
{
lanePoints.Add(dpoi.PointOfInterest.Lane.LaneId, new List <DownstreamPointOfInterest>());
lanePoints[dpoi.PointOfInterest.Lane.LaneId].Add(dpoi);
}
}
// find the best in each lane
foreach (KeyValuePair <ArbiterLaneId, List <DownstreamPointOfInterest> > pairs in lanePoints)
{
List <DownstreamPointOfInterest> dpois = pairs.Value;
dpois.Sort();
// choose best and add
rp.LanePlans.Add(pairs.Key, new LanePlan(dpois[0]));
}
// return
return(rp);
}
/// <summary>
/// Plans over an intersection
/// </summary>
/// <param name="exitWaypoint"></param>
/// <param name="goal"></param>
/// <returns></returns>
public IntersectionPlan PlanIntersection(ITraversableWaypoint exitWaypoint, INavigableNode goal)
{
// road plan if the itnersection has a road available to take from it
RoadPlan rp = null;
// check if road waypoint
if (exitWaypoint is ArbiterWaypoint)
{
// get exit
ArbiterWaypoint awp = (ArbiterWaypoint)exitWaypoint;
// check if it has lane partition moving outwards
if (awp.NextPartition != null)
{
// road plan ignoring exit
List <ArbiterWaypoint> iws = RoadToolkit.WaypointsClose(awp.Lane.Way, awp.Position, awp);
rp = this.PlanNavigableArea(awp.Lane, awp.Position, goal, iws);
}
}
// get exit plan
IntersectionPlan ip = this.GetIntersectionExitPlan(exitWaypoint, goal);
// add road plan if exists
ip.SegmentPlan = rp;
// return the plan
return(ip);
}
/// <summary>
/// Gets the road plan
/// </summary>
/// <param name="pointsOfInterest"></param>
/// <returns></returns>
private RoadPlan GetRoadPlan(List <DownstreamPointOfInterest> pointsOfInterest, ArbiterWay way)
{
// road plan
RoadPlan rp = new RoadPlan(new Dictionary <ArbiterLaneId, LanePlan>());
// find the best in each lane
foreach (ArbiterLane al in way.Lanes.Values)
{
// points in lane
List <DownstreamPointOfInterest> lanePoints = new List <DownstreamPointOfInterest>();
// loop over all points
foreach (DownstreamPointOfInterest dpoi in pointsOfInterest)
{
if (dpoi.PointOfInterest.Lane.Equals(al))
{
lanePoints.Add(dpoi);
}
}
// if exist any
if (lanePoints.Count > 0)
{
// sort
lanePoints.Sort();
// choose best and add
rp.LanePlans.Add(al.LaneId, new LanePlan(lanePoints[0]));
}
}
// return
return(rp);
}
/// <summary>
/// Plans the forward maneuver and secondary maneuver
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="p"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver ForwardManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages)
{
// get primary maneuver
Maneuver primary = this.OpposingForwardMonitor.PrimaryManeuver(arbiterLane, closestGood, vehicleState, blockages);
// return primary for now
return primary;
}
/// <summary>
/// Get a road plan while setting partition costs very high
/// </summary>
/// <param name="partition"></param>
/// <param name="goal"></param>
/// <param name="blockAdjacent"></param>
/// <param name="c"></param>
/// <returns></returns>
public RoadPlan PlanRoadOppositeWithoutPartition(ArbiterLanePartition partition, ArbiterLanePartition opposite, IArbiterWaypoint goal, bool blockAdjacent, Coordinates c, bool sameWay)
{
KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> > tmpCurrentTimes = currentTimes;
this.currentTimes = new KeyValuePair <int, Dictionary <ArbiterWaypointId, DownstreamPointOfInterest> >();
RoadPlan rp = null;
if (!blockAdjacent)
{
NavigationBlockage nb = partition.Blockage;
NavigationBlockage tmp = new NavigationBlockage(double.MaxValue);
partition.Blockage = tmp;
rp = this.PlanNavigableArea(partition.Lane, c, goal, new List <ArbiterWaypoint>());
partition.Blockage = nb;
}
else
{
// save
List <KeyValuePair <ArbiterLanePartition, NavigationBlockage> > savedBlockages = new List <KeyValuePair <ArbiterLanePartition, NavigationBlockage> >();
// set
savedBlockages.Add(new KeyValuePair <ArbiterLanePartition, NavigationBlockage>(partition, partition.Blockage));
// create new
NavigationBlockage anewerBlockage = new NavigationBlockage(Double.MaxValue);
anewerBlockage.BlockageExists = true;
partition.Blockage = anewerBlockage;
foreach (ArbiterLanePartition alp in partition.NonLaneAdjacentPartitions)
{
if (alp.IsInside(c) && (!sameWay || (sameWay && partition.Lane.Way.Equals(alp.Lane.Way))))
{
savedBlockages.Add(new KeyValuePair <ArbiterLanePartition, NavigationBlockage>(alp, alp.Blockage));
// create new
NavigationBlockage newerBlockage = new NavigationBlockage(Double.MaxValue);
newerBlockage.BlockageExists = true;
alp.Blockage = newerBlockage;
}
}
// plan
rp = this.PlanNavigableArea(opposite.Lane, c, goal, new List <ArbiterWaypoint>());
// restore
foreach (KeyValuePair <ArbiterLanePartition, NavigationBlockage> saved in savedBlockages)
{
saved.Key.Blockage = saved.Value;
}
}
// restore
this.currentTimes = tmpCurrentTimes;
// return
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);
}
/// <summary>
/// Secondary maneuver outside minimum cap
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? AdvancedSecondaryOutsideMinCap(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
{
// get proper reasoning component
List<LateralReasoning> adjacentReasonings = new List<LateralReasoning>();
if (bestTask != TypeOfTasks.Left)
{
if (this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists && this.rightLateralReasoning.ExistsExactlyHere(vehicleState))
adjacentReasonings.Add((LateralReasoning)this.rightLateralReasoning);
else if (this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists && this.leftLateralReasoning.ExistsExactlyHere(vehicleState))
adjacentReasonings.Add((LateralReasoning)this.leftLateralReasoning);
}
else
{
if (this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists && this.leftLateralReasoning.ExistsExactlyHere(vehicleState))
adjacentReasonings.Add((LateralReasoning)this.leftLateralReasoning);
else if (this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists && this.rightLateralReasoning.ExistsExactlyHere(vehicleState))
adjacentReasonings.Add((LateralReasoning)this.rightLateralReasoning);
}
// loop through possible
foreach(LateralReasoning adjacentReasoning in adjacentReasonings)
{
// check if adjacent reasoning exists
if (adjacentReasoning != null && adjacentReasoning.Exists)
{
// update adjacent reasoning
adjacentReasoning.ForwardMonitor.Primary(adjacentReasoning.LateralLane, vehicleState, roadPlan, blockages, ignorable, false);
// otherwise check if forward vehicle slow
LaneChangeInformation forwardVehicleSecondary;
// check if should pass the forward vehicle
if (this.ForwardMonitor.ForwardVehicle.ShouldPass(out forwardVehicleSecondary, lane))
{
// check if forward vehicle failed
if (forwardVehicleSecondary.Reason == LaneChangeReason.FailedForwardVehicle)
{
ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " failed");
// make sure the failed vehicle is not within 50m of the goal
double vehicleDistanceToGoal = lane.DistanceBetween(ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, laneGoal.Position);
ArbiterOutput.Output("Failed FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", DistGoal: " + vehicleDistanceToGoal.ToString("f2") + ", speed: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f1"));
if ((vehicleDistanceToGoal > 50))
{
// check if adjacent has no forward vehicle
if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker)
{
// plan the lane change
return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, forwardVehicleSecondary);
}
// otherwise a forward vehicle exists
else
{
// otherwise check if forward vehicle slow
LaneChangeInformation lateralVehicleInformation;
// check if lateral vehicle fine
if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldPass(out lateralVehicleInformation, adjacentReasoning.LateralLane))
{
// plan the lane change
return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
}
// check if lateral vehicle failed or slow
else
{
// check distance to lateral > distance to forward + 25
double distToAdjacent = lane.DistanceBetween(vehicleState.Front, adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
double distToForward = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
if (distToAdjacent > distToForward + 25.0)
{
// plan the lane change
return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
}
}
}
}
}
// otherwise check if they are slow
else if (forwardVehicleSecondary.Reason == LaneChangeReason.SlowForwardVehicle)
{
ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " slow");
// make sure the slow vehicle is not within 50m of the goal if velocity is > 5mph
double vehicleDistanceToGoal = lane.DistanceBetween(ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, laneGoal.Position);
ArbiterOutput.Output("Slow FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", DistGoal: " + vehicleDistanceToGoal.ToString("f2") + ", speed: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f1"));
if ((vehicleDistanceToGoal > 50 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 2.24) ||
(vehicleDistanceToGoal > 75 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 4.48) ||
(vehicleDistanceToGoal > 100 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 6.72) ||
(vehicleDistanceToGoal > 125 && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed < 8.96))
{
// check if adjacent has no forward vehicle
if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker)
{
// plan the lane change
ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: No vehicle in adjacent");
return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
}
// otherwise a forward vehicle exists
else
{
// otherwise check if forward vehicle slow
LaneChangeInformation lateralVehicleInformation;
// check if lateral vehicle fine
if (!adjacentReasoning.ForwardMonitor.ForwardVehicle.ShouldPass(out lateralVehicleInformation, adjacentReasoning.LateralLane))
{
// check distance to lateral > distance to forward + 25
double distToAdjacent = lane.DistanceBetween(vehicleState.Front, adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
double distToForward = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
ArbiterOutput.WriteToLog("AdvancedSecondaryOutsideMinCap: Normal vehicle in adjacent: " + adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + "FV Dist: " + distToForward.ToString("f2") + ", Adj Dist: " + adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString());
// check distance greater and adjacent speed greater than forward speed
if (distToAdjacent > distToForward + 25.0 &&
adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed > this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed)
{
// plan the lane change
return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
}
// plan the lane change
//return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
}
else if (
lane.DistanceBetween(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition,
adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) > 65.0 &&
(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped ||
(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed + 4.48 < adjacentReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed)))
{
// plan the lane change
return this.PlanLaneChange(lane, laneGoal, adjacentReasoning, vehicleState, this.ForwardMonitor.ForwardVehicle.CurrentVehicle, new LaneChangeInformation(LaneChangeReason.Navigation, adjacentReasoning.LateralMonitor.CurrentVehicle));
}
}
}
}
}
}
}
// normal secondary parameterization as a fall through on the naviagation lane changes
return this.SecondaryManeuver(lane, vehicleState, roadPlan, blockages, ignorable, bestTask);
}
/// <summary>
/// Resets values held over time
/// </summary>
public void Reset()
{
currentLane = null;
navigationPlan = null;
}
/// <summary>
/// Sets the supra road plan
/// </summary>
/// <param name="rp"></param>
/// <param name="current"></param>
public void SetSupraRoadPlan(RoadPlan rp, SupraLane current)
{
this.navigationPlan = rp;
// left
List<LanePlan> left = new List<LanePlan>();
// straight
List<LanePlan> straight = new List<LanePlan>();
// right
List<LanePlan> right = new List<LanePlan>();
Dictionary<ArbiterLaneId, LanePlan> plans = rp.LanePlans;
// straight
if (plans.ContainsKey(current.Initial.LaneId))
{
straight.Add(plans[current.Initial.LaneId]);
}
if (plans.ContainsKey(current.Final.LaneId))
{
straight.Add(plans[current.Final.LaneId]);
}
// create tasks
tasks = new Dictionary<TypeOfTasks, List<LanePlan>>();
tasks.Add(TypeOfTasks.Left, left);
tasks.Add(TypeOfTasks.Straight, straight);
tasks.Add(TypeOfTasks.Right, right);
//s
/*Dictionary<ArbiterLaneId, LanePlan> plans = rp.LanePlans;
// left
List<LanePlan> left = new List<LanePlan>();
// straight
List<LanePlan> straight = new List<LanePlan>();
// right
List<LanePlan> right = new List<LanePlan>();
// tmp
ArbiterLane temp = current.Initial.LaneOnLeft;
// left
while (temp != null && (temp.Way.Equals(current.Initial.Way) || temp.Way.Equals(current.Final.Way)))
{
if (plans.ContainsKey(temp.LaneId))
{
left.Add(plans[temp.LaneId]);
}
temp = temp.LaneOnLeft;
}
// right
temp = current.Initial.LaneOnRight;
while (temp != null && (temp.Way.Equals(current.Initial.Way) || temp.Way.Equals(current.Final.Way)))
{
if (plans.ContainsKey(temp.LaneId))
{
right.Add(plans[temp.LaneId]);
}
temp = temp.LaneOnRight;
}
// straight
if (plans.ContainsKey(current.Initial.LaneId))
{
straight.Add(plans[current.Initial.LaneId]);
}
if (plans.ContainsKey(current.Final.LaneId))
{
straight.Add(plans[current.Final.LaneId]);
}
// create tasks
tasks = new Dictionary<TypeOfTasks, List<LanePlan>>();
tasks.Add(TypeOfTasks.Left, left);
tasks.Add(TypeOfTasks.Straight, straight);
tasks.Add(TypeOfTasks.Right, right);*/
}
/// <summary>
/// Distinctly want to make lane change, parameters for doing so
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="left"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? AdvancedDesiredLaneChangeManeuver(ArbiterLane lane, bool left, ArbiterWaypoint goal, RoadPlan rp, VehicleState vehicleState,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, LaneChangeInformation laneChangeInformation, Maneuver? secondary,
out LaneChangeParameters parameters)
{
// set aprams
parameters = new LaneChangeParameters();
// get final maneuver
Maneuver? final = null;
// check partition is not a startup chute
if (lane.GetClosestPartition(vehicleState.Front).Type != PartitionType.Startup)
{
// get the lane goal distance
double distanceToLaneGoal = lane.DistanceBetween(vehicleState.Front, goal.Position);
// check if our distance is less than 50m to the goal
if (distanceToLaneGoal < 50.0)
{
// use old
final = this.LaneChangeManeuver(lane, left, goal, vehicleState, blockages, ignorable, laneChangeInformation, secondary, out parameters);
try
{
// check final null
if (final == null)
{
// check for checkpoint within 4VL of front of failed vehicle
ArbiterCheckpoint acCurrecnt = CoreCommon.Mission.MissionCheckpoints.Peek();
if (acCurrecnt.WaypointId is ArbiterWaypointId)
{
// get waypoint
ArbiterWaypoint awCheckpoint = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[acCurrecnt.WaypointId];
// check way
if (awCheckpoint.Lane.Way.Equals(lane.Way))
{
// distance to wp
double distToWp = lane.DistanceBetween(vehicleState.Front, awCheckpoint.Position);
// check close to waypoint and stopped
if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 && distToWp < TahoeParams.VL * 1.0)
{
ArbiterOutput.Output("Removing checkpoint: " + acCurrecnt.WaypointId.ToString() + " Stopped next to it");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
}
}
catch (Exception) { }
}
// no forward vehicle
else if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle == null)
{
// adjacent monitor
LateralReasoning adjacent = null;
if (left && this.leftLateralReasoning is LateralReasoning && this.leftLateralReasoning.Exists)
{
// update
adjacent = (LateralReasoning)this.leftLateralReasoning;
}
else if (!left && this.rightLateralReasoning is LateralReasoning && this.rightLateralReasoning.Exists)
{
// update
adjacent = (LateralReasoning)this.rightLateralReasoning;
}
// check adj
if (adjacent != null)
{
// update
adjacent.ForwardMonitor.Primary(adjacent.LateralLane, vehicleState, rp, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), false);
if (adjacent.ForwardMonitor.ForwardVehicle.CurrentVehicle == null && adjacent.AdjacentAndRearClear(vehicleState))
{
// use old
final = this.LaneChangeManeuver(lane, left, goal, vehicleState, blockages, ignorable, laneChangeInformation, secondary, out parameters);
}
}
}
}
if (!final.HasValue)
{
if (!secondary.HasValue)
{
List<TravelingParameters> falloutParams = new List<TravelingParameters>();
TravelingParameters t1 = this.ForwardMonitor.ParameterizationHelper(lane, lane, goal.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
falloutParams.Add(t1);
falloutParams.Add(this.ForwardMonitor.LaneParameters);
if (this.ForwardMonitor.FollowingParameters.HasValue)
falloutParams.Add(this.ForwardMonitor.FollowingParameters.Value);
falloutParams.Sort();
TravelingParameters tpCatch = falloutParams[0];
return new Maneuver(tpCatch.Behavior, tpCatch.NextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
return secondary;
}
}
else
{
return final;
}
}
/// <summary>
/// Set the road plan and populate the tasks
/// </summary>
/// <param name="rp"></param>
/// <param name="current"></param>
public void SetRoadPlan(RoadPlan rp, ArbiterLane current)
{
Dictionary<ArbiterLaneId, LanePlan> plans = rp.LanePlans;
// left
List<LanePlan> left = new List<LanePlan>();
// straight
List<LanePlan> straight = new List<LanePlan>();
// right
List<LanePlan> right = new List<LanePlan>();
// tmp
ArbiterLane temp = current.LaneOnLeft;
// left
while (temp != null && temp.Way.Equals(current.Way))
{
if (plans.ContainsKey(temp.LaneId))
{
left.Add(plans[temp.LaneId]);
}
temp = temp.LaneOnLeft;
}
// right
temp = current.LaneOnRight;
while (temp != null && temp.Way.Equals(current.Way))
{
if (plans.ContainsKey(temp.LaneId))
{
right.Add(plans[temp.LaneId]);
}
temp = temp.LaneOnRight;
}
// straight
if (plans.ContainsKey(current.LaneId))
{
straight.Add(plans[current.LaneId]);
}
// create tasks
tasks = new Dictionary<TypeOfTasks, List<LanePlan>>();
tasks.Add(TypeOfTasks.Left, left);
tasks.Add(TypeOfTasks.Straight, straight);
tasks.Add(TypeOfTasks.Right, right);
}
/// <summary>
/// Gets the road plan
/// </summary>
/// <param name="pointsOfInterest"></param>
/// <returns></returns>
private RoadPlan GetRoadPlan(List<DownstreamPointOfInterest> pointsOfInterest)
{
// road plan
RoadPlan rp = new RoadPlan(new Dictionary<ArbiterLaneId, LanePlan>());
// lane assigned points
Dictionary<ArbiterLaneId, List<DownstreamPointOfInterest>> lanePoints = new Dictionary<ArbiterLaneId, List<DownstreamPointOfInterest>>();
// assign dpoi to lanes
foreach (DownstreamPointOfInterest dpoi in pointsOfInterest)
{
if (lanePoints.ContainsKey(dpoi.PointOfInterest.Lane.LaneId))
{
lanePoints[dpoi.PointOfInterest.Lane.LaneId].Add(dpoi);
}
else
{
lanePoints.Add(dpoi.PointOfInterest.Lane.LaneId, new List<DownstreamPointOfInterest>());
lanePoints[dpoi.PointOfInterest.Lane.LaneId].Add(dpoi);
}
}
// find the best in each lane
foreach (KeyValuePair<ArbiterLaneId, List<DownstreamPointOfInterest>> pairs in lanePoints)
{
List<DownstreamPointOfInterest> dpois = pairs.Value;
dpois.Sort();
// choose best and add
rp.LanePlans.Add(pairs.Key, new LanePlan(dpois[0]));
}
// return
return rp;
}
/// <summary>
/// Gets the road plan
/// </summary>
/// <param name="pointsOfInterest"></param>
/// <returns></returns>
private RoadPlan GetRoadPlan(List<DownstreamPointOfInterest> pointsOfInterest, ArbiterWay way)
{
// road plan
RoadPlan rp = new RoadPlan(new Dictionary<ArbiterLaneId, LanePlan>());
// find the best in each lane
foreach (ArbiterLane al in way.Lanes.Values)
{
// points in lane
List<DownstreamPointOfInterest> lanePoints = new List<DownstreamPointOfInterest>();
// loop over all points
foreach (DownstreamPointOfInterest dpoi in pointsOfInterest)
{
if (dpoi.PointOfInterest.Lane.Equals(al))
lanePoints.Add(dpoi);
}
// if exist any
if (lanePoints.Count > 0)
{
// sort
lanePoints.Sort();
// choose best and add
rp.LanePlans.Add(al.LaneId, new LanePlan(lanePoints[0]));
}
}
// return
return rp;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="waypoint"></param>
/// <param name="entries"></param>
public IntersectionPlan(ITraversableWaypoint waypoint, List<PlanableInterconnect> entries, RoadPlan roadPlan)
{
this.ExitWaypoint = waypoint;
this.PossibleEntries = entries;
this.SegmentPlan = roadPlan;
}
/// <summary>
/// Plans what maneuer we should take next
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, RoadPlan navigationalPlan, VehicleState vehicleState,
SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles,
List<ITacticalBlockage> blockages, double vehicleSpeed)
{
// assign vehicles to their lanes
this.roadMonitor.Assign(vehicles);
// navigation tasks
this.taskReasoning.navigationPlan = navigationalPlan;
#region Stay in lane
// maneuver given we are in a lane
if (planningState is StayInLaneState)
{
// get state
StayInLaneState sils = (StayInLaneState)planningState;
// check reasoning if needs to be different
if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(sils.Lane))
{
if (sils.Lane.LaneOnLeft == null)
this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver);
else if (sils.Lane.LaneOnLeft.Way.Equals(sils.Lane.Way))
this.leftLateralReasoning = new LateralReasoning(sils.Lane.LaneOnLeft, SideObstacleSide.Driver);
else
this.leftLateralReasoning = new OpposingLateralReasoning(sils.Lane.LaneOnLeft, SideObstacleSide.Driver);
if (sils.Lane.LaneOnRight == null)
this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger);
else if (sils.Lane.LaneOnRight.Way.Equals(sils.Lane.Way))
this.rightLateralReasoning = new LateralReasoning(sils.Lane.LaneOnRight, SideObstacleSide.Passenger);
else
this.rightLateralReasoning = new OpposingLateralReasoning(sils.Lane.LaneOnRight, SideObstacleSide.Passenger);
this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, sils.Lane);
}
// populate navigation with road plan
taskReasoning.SetRoadPlan(navigationalPlan, sils.Lane);
// as penalties for lane changes already taken into account, can just look at
// best lane plan to figure out what to do
TypeOfTasks bestTask = taskReasoning.Best;
// get the forward lane plan
Maneuver forwardManeuver = forwardReasoning.ForwardManeuver(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore);
// get the secondary
Maneuver? secondaryManeuver = forwardReasoning.AdvancedSecondary(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore, bestTask); //forwardReasoning.SecondaryManeuver(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore, bestTask);
// check behavior type for uturn
if (secondaryManeuver.HasValue && secondaryManeuver.Value.PrimaryBehavior is UTurnBehavior)
return secondaryManeuver.Value;
// check if we wish to change lanes here
if (bestTask != TypeOfTasks.Straight)
{
// parameters
LaneChangeParameters parameters;
secondaryManeuver = this.forwardReasoning.AdvancedDesiredLaneChangeManeuver(sils.Lane, bestTask == TypeOfTasks.Left ? true : false, navigationalPlan.BestPlan.laneWaypointOfInterest.PointOfInterest,
navigationalPlan, vehicleState, blockages, sils.WaypointsToIgnore, new LaneChangeInformation(LaneChangeReason.Navigation, this.forwardReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle), secondaryManeuver, out parameters);
}
// final maneuver
Maneuver finalManeuver = secondaryManeuver.HasValue ? secondaryManeuver.Value : forwardManeuver;
// set opposing vehicle flag
if (false && this.leftLateralReasoning != null && this.leftLateralReasoning is OpposingLateralReasoning && finalManeuver.PrimaryBehavior is StayInLaneBehavior)
{
StayInLaneBehavior silb = (StayInLaneBehavior)finalManeuver.PrimaryBehavior;
OpposingLateralReasoning olr = (OpposingLateralReasoning)this.leftLateralReasoning;
olr.ForwardMonitor.ForwardVehicle.Update(olr.lane, vehicleState);
if (olr.ForwardMonitor.ForwardVehicle.CurrentVehicle != null)
{
ForwardVehicleTrackingControl fvtc = olr.ForwardMonitor.ForwardVehicle.GetControl(olr.lane, vehicleState);
BehaviorDecorator[] bds = new BehaviorDecorator[finalManeuver.PrimaryBehavior.Decorators.Count];
finalManeuver.PrimaryBehavior.Decorators.CopyTo(bds);
finalManeuver.PrimaryBehavior.Decorators = new List<BehaviorDecorator>(bds);
silb.Decorators.Add(new OpposingLaneDecorator(fvtc.xSeparation, olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed));
ArbiterOutput.Output("Added Opposing Lane Decorator: " + fvtc.xSeparation.ToString("F2") + "m, " + olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f2") + "m/s");
}
finalManeuver.PrimaryBehavior = silb;
}
// return the final
return finalManeuver;
}
#endregion
#region Stay in supra lane
else if (CoreCommon.CorePlanningState is StayInSupraLaneState)
{
// get state
StayInSupraLaneState sisls = (StayInSupraLaneState)planningState;
// check reasoning
if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(sisls.Lane))
{
if (sisls.Lane.Initial.LaneOnLeft == null)
this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver);
else if (sisls.Lane.Initial.LaneOnLeft.Way.Equals(sisls.Lane.Initial.Way))
this.leftLateralReasoning = new LateralReasoning(sisls.Lane.Initial.LaneOnLeft, SideObstacleSide.Driver);
else
this.leftLateralReasoning = new OpposingLateralReasoning(sisls.Lane.Initial.LaneOnLeft, SideObstacleSide.Driver);
if (sisls.Lane.Initial.LaneOnRight == null)
this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger);
else if (sisls.Lane.Initial.LaneOnRight.Way.Equals(sisls.Lane.Initial.Way))
this.rightLateralReasoning = new LateralReasoning(sisls.Lane.Initial.LaneOnRight, SideObstacleSide.Passenger);
else
this.rightLateralReasoning = new OpposingLateralReasoning(sisls.Lane.Initial.LaneOnRight, SideObstacleSide.Passenger);
this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, sisls.Lane);
}
// populate navigation with road plan
taskReasoning.SetSupraRoadPlan(navigationalPlan, sisls.Lane);
// as penalties for lane changes already taken into account, can just look at
// best lane plan to figure out what to do
// TODO: NOTE THAT THIS BEST TASK SHOULD BE IN THE SUPRA LANE!! (DO WE NEED THIS)
TypeOfTasks bestTask = taskReasoning.Best;
// get the forward lane plan
Maneuver forwardManeuver = forwardReasoning.ForwardManeuver(sisls.Lane, vehicleState, navigationalPlan, blockages, sisls.WaypointsToIgnore);
// get hte secondary
Maneuver? secondaryManeuver = forwardReasoning.AdvancedSecondary(sisls.Lane, vehicleState, navigationalPlan, blockages, new List<ArbiterWaypoint>(), bestTask);//forwardReasoning.SecondaryManeuver(sisls.Lane, vehicleState, navigationalPlan, blockages, sisls.WaypointsToIgnore, bestTask);
// final maneuver
Maneuver finalManeuver = secondaryManeuver.HasValue ? secondaryManeuver.Value : forwardManeuver;
// check if stay in lane
if (false && this.leftLateralReasoning != null && this.leftLateralReasoning is OpposingLateralReasoning && finalManeuver.PrimaryBehavior is SupraLaneBehavior)
{
SupraLaneBehavior silb = (SupraLaneBehavior)finalManeuver.PrimaryBehavior;
OpposingLateralReasoning olr = (OpposingLateralReasoning)this.leftLateralReasoning;
olr.ForwardMonitor.ForwardVehicle.Update(olr.lane, vehicleState);
if (olr.ForwardMonitor.ForwardVehicle.CurrentVehicle != null)
{
ForwardVehicleTrackingControl fvtc = olr.ForwardMonitor.ForwardVehicle.GetControl(olr.lane, vehicleState);
BehaviorDecorator[] bds = new BehaviorDecorator[finalManeuver.PrimaryBehavior.Decorators.Count];
finalManeuver.PrimaryBehavior.Decorators.CopyTo(bds);
finalManeuver.PrimaryBehavior.Decorators = new List<BehaviorDecorator>(bds);
silb.Decorators.Add(new OpposingLaneDecorator(fvtc.xSeparation, olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed));
ArbiterOutput.Output("Added Opposing Lane Decorator: " + fvtc.xSeparation.ToString("F2") + "m, " + olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f2") + "m/s");
}
finalManeuver.PrimaryBehavior = silb;
}
// return the final
return finalManeuver;
// notify
/*if (secondaryManeuver.HasValue)
ArbiterOutput.Output("Secondary Maneuver");
// check for forward's secondary maneuver for desired behavior other than going straight
if (secondaryManeuver.HasValue)
{
// return the secondary maneuver
return secondaryManeuver.Value;
}
// otherwise our default behavior and posibly desired is going straight
else
{
// return default forward maneuver
return forwardManeuver;
}*/
}
#endregion
#region Change Lanes State
// maneuver given we are changing lanes
else if (planningState is ChangeLanesState)
{
// get state
ChangeLanesState cls = (ChangeLanesState)planningState;
LaneChangeReasoning lcr = new LaneChangeReasoning();
Maneuver final = lcr.PlanLaneChange(cls, vehicleState, navigationalPlan, blockages, new List<ArbiterWaypoint>());
#warning need to filter through waypoints to ignore so don't get stuck by a stop line
//Maneuver final = new Maneuver(cls.Resume(vehicleState, vehicleSpeed), cls, cls.DefaultStateDecorators, vehicleState.Timestamp);
// return the final planned maneuver
return final;
/*if (!cls.parameters..TargetIsOnComing)
{
// check reasoning
if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(cls.TargetLane))
{
if (cls.TargetLane.LaneOnLeft.Way.Equals(cls.TargetLane.Way))
this.leftLateralReasoning = new LateralReasoning(cls.TargetLane.LaneOnLeft);
else
this.leftLateralReasoning = new OpposingLateralReasoning(cls.TargetLane.LaneOnLeft);
if (cls.TargetLane.LaneOnRight.Way.Equals(cls.TargetLane.Way))
this.rightLateralReasoning = new LateralReasoning(cls.TargetLane.LaneOnRight);
else
this.rightLateralReasoning = new OpposingLateralReasoning(cls.TargetLane.LaneOnRight);
this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, cls.TargetLane);
}
// get speed command
double speed;
double distance;
this.forwardReasoning.ForwardMonitor.StoppingParams(new ArbiterWaypoint(cls.TargetUpperBound.pt, null), cls.TargetLane, vehicleState.Front, vehicleState.ENCovariance, out speed, out distance);
SpeedCommand sc = new ScalarSpeedCommand(Math.Max(speed, 0.0));
cls.distanceLeft = distance;
// get behavior
ChangeLaneBehavior clb = new ChangeLaneBehavior(cls.InitialLane.LaneId, cls.TargetLane.LaneId, cls.InitialLane.LaneOnLeft != null && cls.InitialLane.LaneOnLeft.Equals(cls.TargetLane),
distance, sc, new List<int>(), cls.InitialLane.PartitionPath, cls.TargetLane.PartitionPath, cls.InitialLane.Width, cls.TargetLane.Width);
// plan over the target lane
//Maneuver targetManeuver = forwardReasoning.ForwardManeuver(cls.TargetLane, vehicleState, !cls.TargetIsOnComing, blockage, cls.InitialLaneState.IgnorableWaypoints);
// plan over the initial lane
//Maneuver initialManeuver = forwardReasoning.ForwardManeuver(cls.InitialLane, vehicleState, !cls.InitialIsOncoming, blockage, cls.InitialLaneState.IgnorableWaypoints);
// generate the change lanes command
//Maneuver final = laneChangeReasoning.PlanLaneChange(cls, initialManeuver, targetManeuver);
}
else
{
throw new Exception("Change lanes into oncoming not supported yet by road tactical");
}*/
}
#endregion
#region Opposing Lanes State
// maneuver given we are in an opposing lane
else if (planningState is OpposingLanesState)
{
// get state
OpposingLanesState ols = (OpposingLanesState)planningState;
ArbiterWayId opposingWay = ols.OpposingWay;
ols.SetClosestGood(vehicleState);
ols.ResetLaneAgent = false;
// check reasoning
if (this.opposingReasoning == null || !this.opposingReasoning.Lane.Equals(ols.OpposingLane))
{
if (ols.OpposingLane.LaneOnRight == null)
this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver);
else if (!ols.OpposingLane.LaneOnRight.Way.Equals(ols.OpposingLane.Way))
this.leftLateralReasoning = new LateralReasoning(ols.OpposingLane.LaneOnRight, SideObstacleSide.Driver);
else
this.leftLateralReasoning = new OpposingLateralReasoning(ols.OpposingLane.LaneOnRight, SideObstacleSide.Driver);
if (ols.OpposingLane.LaneOnLeft == null)
this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger);
else if (!ols.OpposingLane.LaneOnLeft.Way.Equals(ols.OpposingLane.Way))
this.rightLateralReasoning = new LateralReasoning(ols.OpposingLane.LaneOnLeft, SideObstacleSide.Passenger);
else
this.rightLateralReasoning = new OpposingLateralReasoning(ols.OpposingLane.LaneOnLeft, SideObstacleSide.Passenger);
this.opposingReasoning = new OpposingReasoning(this.leftLateralReasoning, this.rightLateralReasoning, ols.OpposingLane);
}
// get the forward lane plan
Maneuver forwardManeuver = this.opposingReasoning.ForwardManeuver(ols.OpposingLane, ols.ClosestGoodLane, vehicleState, navigationalPlan, blockages);
// get the secondary maneuver
Maneuver? secondaryManeuver = null;
if(ols.ClosestGoodLane != null)
secondaryManeuver = this.opposingReasoning.SecondaryManeuver(ols.OpposingLane, ols.ClosestGoodLane, vehicleState, blockages, ols.EntryParameters);
// check for reasonings secondary maneuver for desired behavior other than going straight
if (secondaryManeuver != null)
{
// return the secondary maneuver
return secondaryManeuver.Value;
}
// otherwise our default behavior and posibly desired is going straight
else
{
// return default forward maneuver
return forwardManeuver;
}
}
#endregion
#region not imp
/*
#region Uturn
// we are making a uturn
else if (planningState is uTurnState)
{
// get the uturn state
uTurnState uts = (uTurnState)planningState;
// get the final lane we wish to be in
ArbiterLane targetLane = uts.TargetLane;
// get operational state
Type operationalBehaviorType = CoreCommon.Communications.GetCurrentOperationalBehavior();
// check if we have completed the uturn
bool complete = operationalBehaviorType == typeof(StayInLaneBehavior);
// default next behavior
Behavior nextBehavior = new StayInLaneBehavior(targetLane.LaneId, new ScalarSpeedCommand(CoreCommon.OperationalStopSpeed), new List<int>());
nextBehavior.Decorators = TurnDecorators.NoDecorators;
// check if complete
if (complete)
{
// stay in lane
List<ArbiterLaneId> aprioriLanes = new List<ArbiterLaneId>();
aprioriLanes.Add(targetLane.LaneId);
return new Maneuver(nextBehavior, new StayInLaneState(targetLane), null, null, aprioriLanes, true);
}
// otherwise keep same
else
{
// set abort behavior
((StayInLaneBehavior)nextBehavior).SpeedCommand = new ScalarSpeedCommand(0.0);
// maneuver
return new Maneuver(uts.DefaultBehavior, uts, nextBehavior, new StayInLaneState(targetLane));
}
}
#endregion*/
#endregion
#region Unknown
// unknown state
else
{
throw new Exception("Unknown Travel State type: planningState: " + planningState.ToString() + "\n with type: " + planningState.GetType().ToString());
}
#endregion
}
/// <summary>
/// Makes new parameterization for nav
/// </summary>
/// <param name="lane"></param>
/// <param name="lanePlan"></param>
/// <param name="speed"></param>
/// <param name="distance"></param>
/// <param name="stopType"></param>
/// <returns></returns>
public TravelingParameters NavStopParameterization(IFQMPlanable lane, RoadPlan roadPlan, double speed, double distance,
ArbiterWaypoint stopWaypoint, StopType stopType, VehicleState state)
{
// get min dist
double distanceCutOff = stopType == StopType.StopLine ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance;
#region Get Decorators
// turn direction default
ArbiterTurnDirection atd = ArbiterTurnDirection.Straight;
List<BehaviorDecorator> decorators = TurnDecorators.NoDecorators;
// check if need decorators
if (lane is ArbiterLane &&
stopWaypoint.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest) &&
roadPlan.BestPlan.laneWaypointOfInterest.IsExit &&
distance < 40.0)
{
if (roadPlan.BestPlan.laneWaypointOfInterest.BestExit == null)
ArbiterOutput.Output("NAV BUG: lanePlan.laneWaypointOfInterest.BestExit: FQM NavStopParameterization");
else
{
switch (roadPlan.BestPlan.laneWaypointOfInterest.BestExit.TurnDirection)
{
case ArbiterTurnDirection.Left:
decorators = TurnDecorators.LeftTurnDecorator;
atd = ArbiterTurnDirection.Left;
break;
case ArbiterTurnDirection.Right:
atd = ArbiterTurnDirection.Right;
decorators = TurnDecorators.RightTurnDecorator;
break;
case ArbiterTurnDirection.Straight:
atd = ArbiterTurnDirection.Straight;
decorators = TurnDecorators.NoDecorators;
break;
case ArbiterTurnDirection.UTurn:
atd = ArbiterTurnDirection.UTurn;
decorators = TurnDecorators.LeftTurnDecorator;
break;
}
}
}
else if (lane is SupraLane)
{
SupraLane sl = (SupraLane)lane;
double distToInterconnect = sl.DistanceBetween(state.Front, sl.Interconnect.InitialGeneric.Position);
if ((distToInterconnect > 0 && distToInterconnect < 40.0) || sl.ClosestComponent(state.Front) == SLComponentType.Interconnect)
{
switch (sl.Interconnect.TurnDirection)
{
case ArbiterTurnDirection.Left:
decorators = TurnDecorators.LeftTurnDecorator;
atd = ArbiterTurnDirection.Left;
break;
case ArbiterTurnDirection.Right:
atd = ArbiterTurnDirection.Right;
decorators = TurnDecorators.RightTurnDecorator;
break;
case ArbiterTurnDirection.Straight:
atd = ArbiterTurnDirection.Straight;
decorators = TurnDecorators.NoDecorators;
break;
case ArbiterTurnDirection.UTurn:
atd = ArbiterTurnDirection.UTurn;
decorators = TurnDecorators.LeftTurnDecorator;
break;
}
}
}
#endregion
#region Get Maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
SpeedCommand sc = new StopAtDistSpeedCommand(distance);
#region Distance Cutoff
// check if distance is less than cutoff
if (distance < distanceCutOff && stopType != StopType.EndOfLane)
{
// default behavior
Behavior b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtDistSpeedCommand(distance), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true));
// stopping so not using speed param
usingSpeed = false;
// exit is next
if (stopType == StopType.Exit)
{
// exit means stopping at a good exit in our current lane
IState nextState = new StoppingAtExitState(stopWaypoint.Lane, stopWaypoint, atd, true, roadPlan.BestPlan.laneWaypointOfInterest.BestExit, state.Timestamp, state.Front);
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
// stop line is left
else if (stopType == StopType.StopLine)
{
// determine if hte stop line is the best exit
bool isNavExit = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Equals(stopWaypoint);
// get turn direction
atd = isNavExit ? atd : ArbiterTurnDirection.Straight;
// predetermine interconnect if best exit
ArbiterInterconnect desired = null;
if (isNavExit)
desired = roadPlan.BestPlan.laneWaypointOfInterest.BestExit;
else if (stopWaypoint.NextPartition != null && state.Front.DistanceTo(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > 25)
desired = stopWaypoint.NextPartition.ToInterconnect;
// set decorators
decorators = isNavExit ? decorators : TurnDecorators.NoDecorators;
// stop at the stop
IState nextState = new StoppingAtStopState(stopWaypoint.Lane, stopWaypoint, atd, isNavExit, desired);
b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtLineSpeedCommand(), new List<int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true));
m = new Maneuver(b, nextState, decorators, state.Timestamp);
sc = new StopAtLineSpeedCommand();
}
else if(stopType == StopType.LastGoal)
{
// stop at the last goal
IState nextState = new StayInLaneState(stopWaypoint.Lane, CoreCommon.CorePlanningState);
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// set speed
sc = new ScalarSpeedCommand(speed);
// check if lane
if (lane is ArbiterLane)
{
// get lane
ArbiterLane al = (ArbiterLane)lane;
// default behavior
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), decorators, state.Timestamp);
}
// check if supra lane
else if (lane is SupraLane)
{
// get lane
SupraLane sl = (SupraLane)lane;
// get sl state
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
// get default beheavior
Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List<int>());
// standard behavior is fine for maneuver
m = new Maneuver(b, sisls, decorators, state.Timestamp);
}
}
#endregion
#endregion
#region Parameterize
// create new params
TravelingParameters tp = new TravelingParameters();
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = distance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = speed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.SpeedCommand = sc;
tp.VehiclesToIgnore = new List<int>();
// return navigation params
return tp;
#endregion
}
/// <summary>
/// Makes use of parameterizations made from the initial forward maneuver plan
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? SecondaryManeuver(IFQMPlanable arbiterLane, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
{
// check if we might be able to pass here
bool validArea = arbiterLane is ArbiterLane || (((SupraLane)arbiterLane).ClosestComponent(vehicleState.Front) == SLComponentType.Initial);
ArbiterLane ourForwardLane = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial;
// check if the forward vehicle exists and we're in a valid area
if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && validArea)
{
// check if we should pass the vehicle ahead
LaneChangeInformation lci;
bool sp = this.ForwardMonitor.ForwardVehicle.ShouldPass(out lci);
// make sure we should do something before processing extras
if(sp)
{
// available parameterizations for the lane change
List<LaneChangeParameters> changeParams = new List<LaneChangeParameters>();
// get lane
ArbiterLane al = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial;
// get the location we need to return by
Coordinates absoluteUpperBound = arbiterLane is ArbiterLane ?
roadPlan.LanePlans[al.LaneId].laneWaypointOfInterest.PointOfInterest.Position :
((SupraLane)arbiterLane).Interconnect.InitialGeneric.Position;
#region Failed Forward
// if failed, parameterize ourselved if we're following them
if (lci.Reason == LaneChangeReason.FailedForwardVehicle && this.ForwardMonitor.CurrentParameters.Type == TravellingType.Vehicle)
{
// notify
ArbiterOutput.Output("Failed Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.VehicleId.ToString());
// get traveling params from FQM to make sure we stopped for vehicle, behind vehicle
double v = CoreCommon.Communications.GetVehicleSpeed().Value;
TravelingParameters fqmParams = this.ForwardMonitor.CurrentParameters;
double d = this.ForwardMonitor.ForwardVehicle.DistanceToVehicle(arbiterLane, vehicleState.Front);
Coordinates departUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d - 3.0).Location;
// check stopped behing failed forward
try
{
if (fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
{
// check for checkpoint within 4VL of front of failed vehicle
ArbiterCheckpoint acCurrecnt = CoreCommon.Mission.MissionCheckpoints.Peek();
if (acCurrecnt.WaypointId.AreaSubtypeId.Equals(al.LaneId))
{
// check distance
ArbiterWaypoint awCheckpoint = (ArbiterWaypoint)CoreCommon.RoadNetwork.ArbiterWaypoints[acCurrecnt.WaypointId];
double cpDistacne = Lane.DistanceBetween(vehicleState.Front, awCheckpoint.Position);
if (cpDistacne < d || cpDistacne - d < TahoeParams.VL * 4.5)
{
ArbiterOutput.Output("Removing checkpoint: " + acCurrecnt.WaypointId.ToString() + " as failed vehicle over it");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
}catch (Exception) { }
#region Right Lateral Reasoning Forwards
// check right lateral reasoning for existence, if so parametrize
if (rightLateralReasoning.Exists && fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
{
// get lane
ArbiterLane lane = al;
// determine failed vehicle lane change distance params
Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location;
Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.0)).Location;
Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location;
// get params for lane change
LaneChangeParameters? lcp = this.LaneChangeParameterization(
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
lane, lane.LaneOnRight, false, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound, blockages, ignorable,
vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);
// check if exists to generate full param
if (lcp.HasValue)
{
// get param
LaneChangeParameters tp = lcp.Value;
// notify
ArbiterOutput.WriteToLog("Failed Forward: Right Lateral Reasoning Forwards: Available: " + tp.Available.ToString() + ", Feasable: " + tp.Feasible.ToString());
// get behavior
ChangeLaneBehavior clb = new ChangeLaneBehavior(
al.LaneId, al.LaneOnRight.LaneId, false, al.DistanceBetween(vehicleState.Front, departUpperBound),
new ScalarSpeedCommand(tp.Parameters.RecommendedSpeed), tp.Parameters.VehiclesToIgnore,
al.LanePath(), al.LaneOnRight.LanePath(), al.Width, al.LaneOnRight.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
tp.Behavior = clb;
tp.Decorators = TurnDecorators.RightTurnDecorator;
// next state
ChangeLanesState cls = new ChangeLanesState(tp);
tp.NextState = cls;
// add parameterization to possible
changeParams.Add(tp);
}
}
#endregion
#region Left Lateral Reasoning
// check left lateral reasoning
if(leftLateralReasoning.Exists)
{
#region Left Lateral Opposing
// check opposing
ArbiterLane closestOpposingLane = this.GetClosestOpposing(ourForwardLane, vehicleState);
if(closestOpposingLane != null && (leftLateralReasoning.IsOpposing || !closestOpposingLane.Equals(leftLateralReasoning.LateralLane)))
{
// check room of initial
bool enoughRoom =
(arbiterLane is ArbiterLane && (!roadPlan.BestPlan.laneWaypointOfInterest.IsExit || ((ArbiterLane)arbiterLane).DistanceBetween(vehicleState.Front, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > TahoeParams.VL * 5.0)) ||
(arbiterLane is SupraLane && ((SupraLane)arbiterLane).DistanceBetween(vehicleState.Front, ((SupraLane)arbiterLane).Interconnect.InitialGeneric.Position) > TahoeParams.VL * 5.0);
// check opposing enough room
bool oppEnough = closestOpposingLane.DistanceBetween(closestOpposingLane.LanePath().StartPoint.Location, vehicleState.Front) > TahoeParams.VL * 5.0;
// check if enough room
if (enoughRoom && oppEnough)
{
// check if we're stopped and the current trav params were for a vehicle and we're close to the vehicle
bool stoppedBehindFV = fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle;
// check that we're stopped behind forward vehicle before attempting to change lanes
if (stoppedBehindFV)
{
#region Check Segment Blockage
// check need to make uturn (hack)
bool waitForUTurnCooldown;
BlockageTactical bt = CoreCommon.BlockageDirector;
StayInLaneBehavior tmpBlockBehavior = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(2.0), new List<int>(), al.LanePath(), al.Width, 0, 0);
ITacticalBlockage itbTmp = new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, TahoeParams.VL, BlockageType.Static, -1, false, tmpBlockBehavior.GetType()));
Maneuver tmpBlockManeuver = bt.LaneRecoveryManeuver(al, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value, roadPlan,
new BlockageRecoveryState(tmpBlockBehavior,
new StayInLaneState(al, CoreCommon.CorePlanningState), new StayInLaneState(al, CoreCommon.CorePlanningState), BlockageRecoveryDEFCON.REVERSE,
new EncounteredBlockageState(itbTmp, CoreCommon.CorePlanningState, BlockageRecoveryDEFCON.REVERSE, SAUDILevel.None), BlockageRecoverySTATUS.ENCOUNTERED), true, out waitForUTurnCooldown);
if (!waitForUTurnCooldown && tmpBlockManeuver.PrimaryBehavior is UTurnBehavior)
return tmpBlockManeuver;
else if (waitForUTurnCooldown)
return null;
#endregion
// distance to forward vehicle too small
double distToForwards = al.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
double distToReverse = Math.Max(1.0, 8.0 - distToForwards);
if (distToForwards < 8.0)
{
// notify
ArbiterOutput.WriteToLog("Secondary: NOT Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " distance: " + distToForwards.ToString("f2"));
this.RearMonitor = new RearQuadrantMonitor(al, SideObstacleSide.Driver);
this.RearMonitor.Update(vehicleState);
if (this.RearMonitor.CurrentVehicle != null)
{
double distToRearVehicle = al.DistanceBetween(this.RearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Position) - TahoeParams.RL;
double distNeedClear = distToReverse + 2.0;
if (distToRearVehicle < distNeedClear)
{
// notify
ArbiterOutput.Output("Secondary: Rear: Not enough room to clear in rear: " + distToRearVehicle.ToString("f2") + " < " + distNeedClear.ToString("f2"));
return null;
}
}
double distToLaneStart = al.DistanceBetween(al.LanePath().StartPoint.Location, vehicleState.Position) - TahoeParams.RL;
if (distToReverse > distToLaneStart)
{
// notify
ArbiterOutput.Output("Secondary: Rear: Not enough room in lane to reverse in rear: " + distToLaneStart.ToString("f2") + " < " + distToReverse.ToString("f2"));
return null;
}
else
{
// notify
ArbiterOutput.Output("Secondary: Reversing to pass Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " reversing distance: " + distToReverse.ToString("f2"));
StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(al.LaneId, sadsc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
return new Maneuver(silb, CoreCommon.CorePlanningState, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
}
else
{
// notify
ArbiterOutput.WriteToLog("Secondary: Left Lateral Opposing: Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString());
// determine failed vehicle lane change distance params
Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location;
Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.5)).Location;
Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location;
// check if enough room
if (al.DistanceBetween(vehicleState.Front, defaultReturnUpperBound) >= d + TahoeParams.VL * 4.5)
{
// get hte closest oppoing
ArbiterLane closestOpposing = this.GetClosestOpposing(al, vehicleState);
// check exists
if (closestOpposing != null)
{
// set/check secondary
if (this.secondaryLeftLateralReasoning == null || !this.secondaryLeftLateralReasoning.LateralLane.Equals(closestOpposing))
this.secondaryLeftLateralReasoning = new OpposingLateralReasoning(closestOpposing, SideObstacleSide.Driver);
// check the state of hte lanes next to us
if (this.leftLateralReasoning.LateralLane.Equals(closestOpposing) && this.leftLateralReasoning.ExistsExactlyHere(vehicleState))
{
#region Plan
// need to make sure that we wait for 3 seconds with the blinker on (resetting with pause)
if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds > 3000)
{
// notify
ArbiterOutput.Output("Scondary: Left Lateral Opposing: Wait Timer DONE");
// get parameterization
LaneChangeParameters? tp = this.LaneChangeParameterization(
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.ForwardMonitor.ForwardVehicle.CurrentVehicle),
al,
leftLateralReasoning.LateralLane,
true,
roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
departUpperBound,
defaultReturnLowerBound,
minimumReturnComplete,
defaultReturnUpperBound,
blockages,
ignorable,
vehicleState,
CoreCommon.Communications.GetVehicleSpeed().Value);
// check if available and feasible
if (tp.HasValue && tp.Value.Available && tp.Value.Feasible)
{
// notify
ArbiterOutput.Output("Scondary: Left Lateral Opposing: AVAILABLE & FEASIBLE");
LaneChangeParameters lcp = tp.Value;
lcp.Behavior = this.ForwardMonitor.CurrentParameters.Behavior;
lcp.Decorators = TurnDecorators.LeftTurnDecorator;
lcp.Behavior.Decorators = lcp.Decorators;
// next state
ChangeLanesState cls = new ChangeLanesState(tp.Value);
lcp.NextState = cls;
// add parameterization to possible
changeParams.Add(lcp);
}
// check if not available now but still feasible
else if (tp.HasValue && !tp.Value.Available && tp.Value.Feasible)
{
// notify
ArbiterOutput.Output("Scondary: Left Lateral Opposing: NOT Available, Still FEASIBLE, WAITING");
// wait and blink maneuver
TravelingParameters tp2 = this.ForwardMonitor.CurrentParameters;
tp2.Decorators = TurnDecorators.LeftTurnDecorator;
tp2.Behavior.Decorators = tp2.Decorators;
// create parameterization
LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, al.LaneOnLeft,
true, true, tp2.Behavior, 0.0, CoreCommon.CorePlanningState, tp2.Decorators, tp2, new Coordinates(),
new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle);
// add parameterization to possible
changeParams.Add(lcp);
}
}
// otherwise timer not running or not been long enough
else
{
// check if timer running
if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.IsRunning)
this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Start();
double waited = (double)(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds / 1000.0);
ArbiterOutput.Output("Waited for failed forwards: " + waited.ToString("F2") + " seconds");
// wait and blink maneuver
TravelingParameters tp = this.ForwardMonitor.CurrentParameters;
tp.Decorators = TurnDecorators.LeftTurnDecorator;
tp.Behavior.Decorators = tp.Decorators;
// create parameterization
LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, al.LaneOnLeft,
true, true, tp.Behavior, 0.0, CoreCommon.CorePlanningState, tp.Decorators, tp, new Coordinates(),
new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle);
// add parameterization to possible
changeParams.Add(lcp);
}
#endregion
}
else if (!this.leftLateralReasoning.LateralLane.Equals(closestOpposing) && !this.leftLateralReasoning.ExistsRelativelyHere(vehicleState))
{
// set and notify
ArbiterOutput.Output("superceeded left lateral reasoning with override for non adjacent left lateral reasoning");
ILateralReasoning tmpReasoning = this.leftLateralReasoning;
this.leftLateralReasoning = this.secondaryLeftLateralReasoning;
try
{
#region Plan
// need to make sure that we wait for 3 seconds with the blinker on (resetting with pause)
if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds > 3000)
{
// notify
ArbiterOutput.Output("Scondary: Left Lateral Opposing: Wait Timer DONE");
// get parameterization
LaneChangeParameters? tp = this.LaneChangeParameterization(
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, this.ForwardMonitor.ForwardVehicle.CurrentVehicle),
al,
leftLateralReasoning.LateralLane,
true,
roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
departUpperBound,
defaultReturnLowerBound,
minimumReturnComplete,
defaultReturnUpperBound,
blockages,
ignorable,
vehicleState,
CoreCommon.Communications.GetVehicleSpeed().Value);
// check if available and feasible
if (tp.HasValue && tp.Value.Available && tp.Value.Feasible)
{
// notify
ArbiterOutput.Output("Scondary: Left Lateral Opposing: AVAILABLE & FEASIBLE");
LaneChangeParameters lcp = tp.Value;
lcp.Behavior = this.ForwardMonitor.CurrentParameters.Behavior;
lcp.Decorators = TurnDecorators.LeftTurnDecorator;
lcp.Behavior.Decorators = TurnDecorators.LeftTurnDecorator;
// next state
ChangeLanesState cls = new ChangeLanesState(tp.Value);
lcp.NextState = cls;
// add parameterization to possible
changeParams.Add(lcp);
}
// check if not available now but still feasible
else if (tp.HasValue && !tp.Value.Available && tp.Value.Feasible)
{
// notify
ArbiterOutput.Output("Scondary: Left Lateral Opposing: NOT Available, Still FEASIBLE, WAITING");
// wait and blink maneuver
TravelingParameters tp2 = this.ForwardMonitor.CurrentParameters;
tp2.Decorators = TurnDecorators.LeftTurnDecorator;
tp2.Behavior.Decorators = tp2.Decorators;
// create parameterization
LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, this.leftLateralReasoning.LateralLane,
true, true, tp2.Behavior, 0.0, CoreCommon.CorePlanningState, tp2.Decorators, tp2, new Coordinates(),
new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle);
// add parameterization to possible
changeParams.Add(lcp);
}
}
// otherwise timer not running or not been long enough
else
{
// check if timer running
if (!this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.IsRunning)
this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Start();
double waited = (double)(this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.ElapsedMilliseconds / 1000.0);
ArbiterOutput.Output("Waited for failed forwards: " + waited.ToString("F2") + " seconds");
// wait and blink maneuver
TravelingParameters tp = this.ForwardMonitor.CurrentParameters;
tp.Decorators = TurnDecorators.LeftTurnDecorator;
tp.Behavior.Decorators = tp.Decorators;
// create parameterization
LaneChangeParameters lcp = new LaneChangeParameters(false, true, al, false, this.leftLateralReasoning.LateralLane,
true, true, tp.Behavior, 0.0, CoreCommon.CorePlanningState, tp.Decorators, tp, new Coordinates(),
new Coordinates(), new Coordinates(), new Coordinates(), LaneChangeReason.FailedForwardVehicle);
// add parameterization to possible
changeParams.Add(lcp);
}
#endregion
}
catch (Exception ex)
{
ArbiterOutput.Output("Core intelligence thread caught exception in forward reasoning secondary maneuver when non-standard adjacent left: " + ex.ToString());
}
// restore
this.leftLateralReasoning = tmpReasoning;
}
}
else
{
// do nuttin
ArbiterOutput.Output("no opposing adjacent");
}
}
else
{
// notify
ArbiterOutput.Output("Secondary: LeftLatOpp: Stopped Behind FV: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + ", but not enough room to pass");
}
}
}
else
{
this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop();
this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset();
// notify
ArbiterOutput.Output("Secondary: Left Lateral Opposing: NOT Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString());
}
}
else
{
ArbiterOutput.Output("Secondary Opposing: enough room to pass opposing: initial: " + enoughRoom.ToString() + ", opposing: " + oppEnough.ToString());
}
}
#endregion
#region Left Lateral Forwards
// otherwise parameterize
else if(fqmParams.Type == TravellingType.Vehicle && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
{
// get lane
ArbiterLane lane = al;
// determine failed vehicle lane change distance params
Coordinates defaultReturnLowerBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 2.0)).Location;
Coordinates minimumReturnComplete = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 3.0)).Location;
Coordinates defaultReturnUpperBound = al.LanePath().AdvancePoint(al.LanePath().GetClosestPoint(vehicleState.Front), d + (TahoeParams.VL * 5.0)).Location;
// get params for lane change
LaneChangeParameters? lcp = this.LaneChangeParameterization(
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
lane, lane.LaneOnLeft, false, roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
departUpperBound, defaultReturnLowerBound, minimumReturnComplete, defaultReturnUpperBound,
blockages, ignorable, vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);
// check if exists to generate full param
if (lcp.HasValue)
{
// set param
LaneChangeParameters tp = lcp.Value;
// notify
ArbiterOutput.Output("Secondary Failed Forward Reasoning Forwards: Available: " + tp.Available.ToString() + ", Feasible: " + tp.Feasible.ToString());
// get behavior
ChangeLaneBehavior clb = new ChangeLaneBehavior(
al.LaneId, al.LaneOnLeft.LaneId, true, al.DistanceBetween(vehicleState.Front, departUpperBound),
new ScalarSpeedCommand(tp.Parameters.RecommendedSpeed), tp.Parameters.VehiclesToIgnore,
al.LanePath(), al.LaneOnLeft.LanePath(), al.Width, al.LaneOnLeft.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
tp.Behavior = clb;
tp.Decorators = TurnDecorators.LeftTurnDecorator;
// next state
ChangeLanesState cls = new ChangeLanesState(tp);
tp.NextState = cls;
// add parameterization to possible
changeParams.Add(tp);
}
}
#endregion
}
#endregion
}
#endregion
#region Slow Forward
// if pass, determine if should pass in terms or vehicles adjacent and in front then call lane change function for maneuver
else if (lci.Reason == LaneChangeReason.SlowForwardVehicle)
{
// if left exists and is not opposing, parameterize
if (leftLateralReasoning.Exists && !leftLateralReasoning.IsOpposing)
{
throw new Exception("slow forward vehicle pass not implemented yet");
}
// if right exists and is not opposing, parameterize
if (rightLateralReasoning.Exists && !rightLateralReasoning.IsOpposing)
{
throw new Exception("slow forward vehicle pass not implemented yet");
}
}
#endregion
#region Parameterize
// check params to see if any are good and available
if(changeParams != null && changeParams.Count > 0)
{
// sort the parameterizations
changeParams.Sort();
// get first
LaneChangeParameters final = changeParams[0];
// notify
ArbiterOutput.Output("Secondary Reasoning Final: Available: " + final.Available.ToString() + ", Feasible: " + final.Feasible.ToString());
// make sure ok
if (final.Available && final.Feasible)
{
// return best param
return new Maneuver(changeParams[0].Behavior, changeParams[0].NextState, changeParams[0].Decorators, vehicleState.Timestamp);
}
}
#endregion
}
}
// fallout is null
return null;
}
/// <summary>
/// Secondary maneuver when current lane is the desired lane
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? AdvancedSecondaryNextCheck(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
{
// check if the forward vehicle exists
if (this.ForwardMonitor.ForwardVehicle.ShouldUseForwardTracker && this.ForwardMonitor.ForwardVehicle.CurrentVehicle.PassedLongDelayedBirth)
{
// distance to forward vehicle
double distanceToForwardVehicle = lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
#region Distance From Forward Not Enough
// check distance to forward vehicle
if (this.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed &&
distanceToForwardVehicle < 8.0 && this.ForwardMonitor.ForwardVehicle.StoppedBehindForwardVehicle)
{
// set name
ArbiterLane al = lane;
// distance to forward vehicle too small
double distToForwards = al.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition);
double distToReverse = Math.Max(1.0, 8.0 - distToForwards);
if (distToForwards < 8.0)
{
// notify
ArbiterOutput.Output("Secondary: NOT Properly Stopped Behind Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " distance: " + distToForwards.ToString("f2"));
this.RearMonitor = new RearQuadrantMonitor(al, SideObstacleSide.Driver);
this.RearMonitor.Update(vehicleState);
if (this.RearMonitor.CurrentVehicle != null)
{
double distToRearVehicle = al.DistanceBetween(this.RearMonitor.CurrentVehicle.ClosestPosition, vehicleState.Position) - TahoeParams.RL;
double distNeedClear = distToReverse + 2.0;
if (distToRearVehicle < distNeedClear)
{
// notify
ArbiterOutput.Output("Secondary: Rear: Not enough room to clear in rear: " + distToRearVehicle.ToString("f2") + " < " + distNeedClear.ToString("f2"));
return null;
}
}
double distToLaneStart = al.DistanceBetween(al.LanePath().StartPoint.Location, vehicleState.Position) - TahoeParams.RL;
if (distToReverse > distToLaneStart)
{
// notify
ArbiterOutput.Output("Secondary: Rear: Not enough room in lane to reverse in rear: " + distToLaneStart.ToString("f2") + " < " + distToReverse.ToString("f2"));
return null;
}
else
{
// notify
ArbiterOutput.Output("Secondary: Reversing to pass Forward Vehicle: " + this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ToString() + " reversing distance: " + distToReverse.ToString("f2"));
StopAtDistSpeedCommand sadsc = new StopAtDistSpeedCommand(distToReverse, true);
StayInLaneBehavior silb = new StayInLaneBehavior(al.LaneId, sadsc, new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(vehicleState.Front, true), al.NumberOfLanesRight(vehicleState.Front, true));
return new Maneuver(silb, CoreCommon.CorePlanningState, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
}
}
#endregion
// get distance to next lane major
List<WaypointType> wts = new List<WaypointType>(new WaypointType[] { WaypointType.Stop, WaypointType.End });
ArbiterWaypoint nextWaypoint = lane.GetNext(lane.GetClosestPartition(vehicleState.Position).Final, wts, new List<ArbiterWaypoint>());
// check if the vehicle occurs before the next major thing
double distanceToNextMajor = lane.DistanceBetween(vehicleState.Front, nextWaypoint.Position);
// check if vehicle occurs before the next lane major
if (distanceToForwardVehicle < distanceToNextMajor)
{
// check if forward vehicle exists in this lane and is closer then the lane goal
if (TacticalDirector.VehicleAreas.ContainsKey(lane) &&
TacticalDirector.VehicleAreas[lane].Contains(this.ForwardMonitor.ForwardVehicle.CurrentVehicle) &&
lane.DistanceBetween(vehicleState.Front, this.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) < lane.DistanceBetween(vehicleState.Front, laneGoal.Position))
{
// some constants
double desiredDistance = 50.0;
// get the lane goal distance
double distanceToLaneGoal = lane.DistanceBetween(vehicleState.Front, laneGoal.Position);
// check if necessary to be in current lane
if (distanceToLaneGoal <= desiredDistance)
{
// default secondary maneuver
ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal < 50: " + desiredDistance.ToString("f2"));
return this.SecondaryManeuver(lane, vehicleState, roadPlan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>(), bestTask);
}
// otherwise would be good but not necessary
else
{
// return the maneuver from the vehicle being outside the min cap
ArbiterOutput.WriteToLog("AdvancedSecondaryNextCheck: DistToLaneGoal > 50: " + desiredDistance.ToString("f2"));
return this.AdvancedSecondaryOutsideMinCap(lane, laneGoal, vehicleState, roadPlan, blockages, ignorable, bestTask);
}
}
}
}
// no secondary
return null;
}
/// <summary>
/// Secondary maneuver inside our minimum cap
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? AdvancedSecondaryInsideMinCap(ArbiterLane lane, ArbiterWaypoint laneGoal, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
{
return null;
}
/// <summary>
/// Secondary maneuver when current lane is the desired lane
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="roadPlan"></param>
/// <param name="blockages"></param>
/// <param name="ignorable"></param>
/// <returns></returns>
public Maneuver? AdvancedSecondary(IFQMPlanable arbiterLane, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, TypeOfTasks bestTask)
{
// check if we might be able to pass here
bool validArea = arbiterLane is ArbiterLane || (((SupraLane)arbiterLane).ClosestComponent(vehicleState.Front) == SLComponentType.Initial);
// check normal valid area
if (validArea)
{
// get lane we are in
ArbiterLane ourForwardLane = arbiterLane is ArbiterLane ? (ArbiterLane)arbiterLane : ((SupraLane)arbiterLane).Initial;
// check sl
if (arbiterLane is SupraLane)
{
Dictionary<ArbiterLaneId, LanePlan> tmpPlans = new Dictionary<ArbiterLaneId,LanePlan>();
tmpPlans.Add(ourForwardLane.LaneId, roadPlan.LanePlans[ourForwardLane.LaneId]);
RoadPlan tmp = new RoadPlan(tmpPlans);
roadPlan = tmp;
}
// return the advanced plan
ArbiterWaypoint goalWp = arbiterLane is SupraLane ? (ArbiterWaypoint)((SupraLane)arbiterLane).Interconnect.InitialGeneric : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest;
return this.AdvancedSecondaryNextCheck(ourForwardLane, goalWp, vehicleState, roadPlan, blockages, ignorable, bestTask);
}
// fall through
return null;
}
/// <summary>
/// Plan a lane change
/// </summary>
/// <param name="cls"></param>
/// <param name="initialManeuver"></param>
/// <param name="targetManeuver"></param>
/// <returns></returns>
public Maneuver PlanLaneChange(ChangeLanesState cls, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable)
{
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is LaneChangeBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// go to a blockage handling tactical
return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// lanes of the lane change
ArbiterLane initial = cls.Parameters.Initial;
ArbiterLane target = cls.Parameters.Target;
#region Initial Forwards
if (!cls.Parameters.InitialOncoming)
{
ForwardReasoning initialReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), initial);
#region Target Forwards
if (!cls.Parameters.TargetOncoming)
{
// target reasoning
ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target);
#region Navigation
if (cls.Parameters.Reason == LaneChangeReason.Navigation)
{
// parameters to follow
List<TravelingParameters> tps = new List<TravelingParameters>();
// vehicles to ignore
List<int> ignorableVehicles = new List<int>();
// params for forward lane
initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle);
ArbiterOutput.Output("initial dist to go: " + initialParams.DistanceToGo.ToString("f3"));
if (initialParams.Type == TravellingType.Vehicle && !initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
{
tps.Add(initialParams);
}
else
tps.Add(initialReasoning.ForwardMonitor.NavigationParameters);
ignorableVehicles.AddRange(initialParams.VehiclesToIgnore);
// get params for the final lane
targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List<ArbiterWaypoint>());
TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters;
tps.Add(targetParams);
ignorableVehicles.AddRange(targetParams.VehiclesToIgnore);
try
{
if (CoreCommon.Communications.GetVehicleSpeed().Value < 0.1 &&
targetParams.Type == TravellingType.Vehicle &&
targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle != null &&
targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.QueuingState.Queuing == QueuingState.Failed)
{
return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
catch (Exception) { }
ArbiterOutput.Output("target dist to go: " + targetParams.DistanceToGo.ToString("f3"));
// decorators
List<BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;
// distance
double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
cls.Parameters.DistanceToDepartUpperBound = distanceToGo;
// check if need to modify distance to go
if (initialParams.Type == TravellingType.Vehicle && initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
{
double curDistToUpper = cls.Parameters.DistanceToDepartUpperBound;
double newVhcDistToUpper = initial.DistanceBetween(vehicleState.Front, initialReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition) - 2.0;
if (curDistToUpper > newVhcDistToUpper)
{
distanceToGo = newVhcDistToUpper;
}
}
// get final
tps.Sort();
// get the proper speed command
ScalarSpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed);
if (sc.Speed < 8.84)
sc = new ScalarSpeedCommand(Math.Min(targetParams.RecommendedSpeed, 8.84));
// continue the lane change with the proper speed command
ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo,
sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
initial.NumberOfLanesRight(vehicleState.Front, true));
// standard maneuver
return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp);
}
#endregion
#region Failed Forwards
else if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
{
// parameters to follow
List<TravelingParameters> tps = new List<TravelingParameters>();
// vehicles to ignore
List<int> ignorableVehicles = new List<int>();
// params for forward lane
initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
initial.WaypointList[initial.WaypointList.Count - 1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
tps.Add(initialParams);
ignorableVehicles.AddRange(initialParams.VehiclesToIgnore);
// get params for the final lane
targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, new List<ArbiterWaypoint>());
TravelingParameters targetParams = targetReasoning.ForwardMonitor.CurrentParameters;
tps.Add(targetParams);
ignorableVehicles.AddRange(targetParams.VehiclesToIgnore);
// decorators
List<BehaviorDecorator> decorators = initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target) ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;
// distance
double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
cls.Parameters.DistanceToDepartUpperBound = distanceToGo;
// get final
tps.Sort();
// get the proper speed command
SpeedCommand sc = new ScalarSpeedCommand(tps[0].RecommendedSpeed);
// continue the lane change with the proper speed command
ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, initial.LaneOnLeft != null && initial.LaneOnLeft.Equals(target), distanceToGo,
sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
initial.NumberOfLanesRight(vehicleState.Front, true));
// standard maneuver
return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp);
}
#endregion
#region Slow
else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
#endregion
else
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
}
#endregion
#region Target Oncoming
else
{
OpposingReasoning targetReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), target);
#region Failed Forward
if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
{
// parameters to follow
List<TravelingParameters> tps = new List<TravelingParameters>();
// ignore the forward vehicle but keep params for forward lane
initialReasoning.ForwardManeuver(initial, vehicleState, roadPlan, blockages, ignorable);
TravelingParameters initialParams = initialReasoning.ForwardMonitor.ParameterizationHelper(initial, initial,
CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
initial.WaypointList[initial.WaypointList.Count-1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position, vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, null);
tps.Add(initialParams);
// get params for the final lane
targetReasoning.ForwardManeuver(target, initial, vehicleState, roadPlan, blockages);
TravelingParameters targetParams = targetReasoning.OpposingForwardMonitor.CurrentParamters.Value;
tps.Add(targetParams);
// decorators
List<BehaviorDecorator> decorators = cls.Parameters.ToLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator;
// distance
double distanceToGo = initial.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
cls.Parameters.DistanceToDepartUpperBound = distanceToGo;
// get final
tps.Sort();
// get the proper speed command
SpeedCommand sc = new ScalarSpeedCommand(Math.Min(tps[0].RecommendedSpeed, 2.24));
// check final for stopped failed opposing
VehicleAgent forwardVa = targetReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle;
if (forwardVa != null)
{
// dist between
double distToFV = -targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition);
// check stopped
bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5;
// check distance
bool distOk = distToFV < 2.5 * TahoeParams.VL;
// check failed
bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed;
// notify
ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString());
// check all for failed
if (stopped && distOk && failed)
{
// check inside target
if (target.LanePolygon.IsInside(vehicleState.Front))
{
// blockage recovery
StayInLaneState sils = new StayInLaneState(initial, CoreCommon.CorePlanningState);
StayInLaneBehavior silb = new StayInLaneBehavior(initial.LaneId, new StopAtDistSpeedCommand(TahoeParams.VL * 2.0, true), new List<int>(), initial.LanePath(), initial.Width, initial.NumberOfLanesLeft(vehicleState.Front, false), initial.NumberOfLanesRight(vehicleState.Front, false));
BlockageRecoveryState brs = new BlockageRecoveryState(silb, sils, sils, BlockageRecoveryDEFCON.REVERSE,
new EncounteredBlockageState(new LaneBlockage(new TrajectoryBlockedReport(CompletionResult.Stopped, 4.0, BlockageType.Static, -1, true, silb.GetType())), sils, BlockageRecoveryDEFCON.INITIAL, SAUDILevel.None),
BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brs, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
// check which lane we are in
else
{
// return to forward lane
return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(initial, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
// continue the lane change with the proper speed command
ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo,
sc, targetParams.VehiclesToIgnore, initial.LanePath(), target.ReversePath, initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, true),
initial.NumberOfLanesRight(vehicleState.Front, true));
// standard maneuver
return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp);
}
#endregion
#region Other
else if (cls.Parameters.Reason == LaneChangeReason.Navigation)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
else
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
#endregion
}
#endregion
}
#endregion
#region Initial Oncoming
else
{
OpposingReasoning initialReasoning = new OpposingReasoning(new OpposingLateralReasoning(null, SideObstacleSide.Driver), new OpposingLateralReasoning(null, SideObstacleSide.Driver), initial);
#region Target Forwards
if (!cls.Parameters.TargetOncoming)
{
ForwardReasoning targetReasoning = new ForwardReasoning(new LateralReasoning(null, SideObstacleSide.Driver), new LateralReasoning(null, SideObstacleSide.Driver), target);
if (cls.Parameters.Reason == LaneChangeReason.FailedForwardVehicle)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
#region Navigation
else if (cls.Parameters.Reason == LaneChangeReason.Navigation)
{
// parameters to follow
List<TravelingParameters> tps = new List<TravelingParameters>();
// distance to the upper bound of the change
double distanceToGo = target.DistanceBetween(vehicleState.Front, cls.Parameters.DepartUpperBound);
cls.Parameters.DistanceToDepartUpperBound = distanceToGo;
// get params for the initial lane
initialReasoning.ForwardManeuver(initial, target, vehicleState, roadPlan, blockages);
// current params of the fqm
TravelingParameters initialParams = initialReasoning.OpposingForwardMonitor.CurrentParamters.Value;
if (initialParams.Type == TravellingType.Vehicle)
{
if(!initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.IsStopped)
tps.Add(initialParams);
else
{
tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters);
distanceToGo = initial.DistanceBetween(initialReasoning.OpposingForwardMonitor.ForwardVehicle.CurrentVehicle.ClosestPosition, vehicleState.Front) - TahoeParams.VL;
}
}
else
tps.Add(initialReasoning.OpposingForwardMonitor.NaviationParameters);
// get params for forward lane
targetReasoning.ForwardManeuver(target, vehicleState, roadPlan, blockages, ignorable);
TravelingParameters targetParams = targetReasoning.ForwardMonitor.ParameterizationHelper(target, target,
CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.WaypointId) ?
target.WaypointList[target.WaypointList.Count-1].Position : roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position,
vehicleState.Front, CoreCommon.CorePlanningState, vehicleState, targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle);
tps.Add(targetParams);
// ignoring vehicles add
List<int> ignoreVehicles = initialParams.VehiclesToIgnore;
ignoreVehicles.AddRange(targetParams.VehiclesToIgnore);
// decorators
List<BehaviorDecorator> decorators = !cls.Parameters.ToLeft ? TurnDecorators.RightTurnDecorator : TurnDecorators.LeftTurnDecorator;
// get final
tps.Sort();
// get the proper speed command
SpeedCommand sc = tps[0].SpeedCommand;
if (sc is StopAtDistSpeedCommand)
{
sc = new ScalarSpeedCommand(0.0);
}
// check final for stopped failed opposing
VehicleAgent forwardVa = targetReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle;
if (forwardVa != null)
{
// dist between
double distToFV = targetReasoning.Lane.DistanceBetween(vehicleState.Front, forwardVa.ClosestPosition);
// check stopped
bool stopped = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.5;
// check distance
bool distOk = distToFV < 2.5 * TahoeParams.VL;
// check failed
bool failed = forwardVa.QueuingState.Queuing == QueuingState.Failed;
// notify
ArbiterOutput.Output("Forward Vehicle: Stopped: " + stopped.ToString() + ", DistOk: " + distOk.ToString() + ", Failed: " + failed.ToString());
// check all for failed
if (stopped && distOk && failed)
{
// check which lane we are in
if (initial.LanePolygon.IsInside(vehicleState.Front))
{
// return to opposing lane
return new Maneuver(new HoldBrakeBehavior(), new OpposingLanesState(initial, true, CoreCommon.CorePlanningState, vehicleState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// lane state
return new Maneuver(new HoldBrakeBehavior(), new StayInLaneState(target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
// continue the lane change with the proper speed command
ChangeLaneBehavior clb = new ChangeLaneBehavior(initial.LaneId, target.LaneId, cls.Parameters.ToLeft, distanceToGo,
sc, ignoreVehicles, initial.ReversePath, target.LanePath(), initial.Width, target.Width, initial.NumberOfLanesLeft(vehicleState.Front, false),
initial.NumberOfLanesRight(vehicleState.Front, false));
// standard maneuver
return new Maneuver(clb, CoreCommon.CorePlanningState, decorators, vehicleState.Timestamp);
}
#endregion
else if (cls.Parameters.Reason == LaneChangeReason.SlowForwardVehicle)
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
else
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
}
#endregion
else
{
// fallout exception
throw new Exception("currently unsupported lane change type");
}
}
#endregion
}
/// <summary>
/// Plans the forward maneuver and secondary maneuver
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="p"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver ForwardManeuver(IFQMPlanable arbiterLane, VehicleState vehicleState, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable)
{
// get primary parameterization
TravelingParameters primary = this.ForwardMonitor.Primary(arbiterLane, vehicleState, roadPlan, blockages, ignorable, true);
// return primary for now
return new Maneuver(primary.Behavior, primary.NextState, primary.Decorators, vehicleState.Timestamp);
}
/// <summary>
/// Behavior given we stay in the current lane
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <param name="downstreamPoint"></param>
/// <returns></returns>
public TravelingParameters Primary(IFQMPlanable lane, VehicleState state, RoadPlan roadPlan,
List<ITacticalBlockage> blockages, List<ArbiterWaypoint> ignorable, bool log)
{
// possible parameterizations
List<TravelingParameters> tps = new List<TravelingParameters>();
#region Lane Major Parameterizations with Current Lane Goal Params, If Best Goal Exists in Current Lane
// check if the best goal is in the current lane
ArbiterWaypoint lanePoint = null;
if (lane.AreaComponents.Contains(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Lane))
lanePoint = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest;
// get the next thing we need to stop at no matter what and parameters for stopping at it
ArbiterWaypoint laneNavStop;
double laneNavStopSpeed;
double laneNavStopDistance;
StopType laneNavStopType;
this.NextNavigationalStop(lane, lanePoint, state.Front, state.ENCovariance, ignorable,
out laneNavStopSpeed, out laneNavStopDistance, out laneNavStopType, out laneNavStop);
// create parameterization of the stop
TravelingParameters laneNavParams = this.NavStopParameterization(lane, roadPlan, laneNavStopSpeed, laneNavStopDistance, laneNavStop, laneNavStopType, state);
this.navigationParameters = laneNavParams;
this.laneParameters = laneNavParams;
tps.Add(laneNavParams);
#region Log
if (log)
{
// add to current parames to arbiter information
CoreCommon.CurrentInformation.FQMBehavior = laneNavParams.Behavior.ToShortString();
CoreCommon.CurrentInformation.FQMBehaviorInfo = laneNavParams.Behavior.ShortBehaviorInformation();
CoreCommon.CurrentInformation.FQMSpeedCommand = laneNavParams.Behavior.SpeedCommandString();
CoreCommon.CurrentInformation.FQMDistance = laneNavParams.DistanceToGo.ToString("F6");
CoreCommon.CurrentInformation.FQMSpeed = laneNavParams.RecommendedSpeed.ToString("F6");
CoreCommon.CurrentInformation.FQMState = laneNavParams.NextState.ShortDescription();
CoreCommon.CurrentInformation.FQMStateInfo = laneNavParams.NextState.StateInformation();
CoreCommon.CurrentInformation.FQMStopType = laneNavStopType.ToString();
CoreCommon.CurrentInformation.FQMWaypoint = laneNavStop.ToString();
CoreCommon.CurrentInformation.FQMSegmentSpeedLimit = lane.CurrentMaximumSpeed(state.Position).ToString("F1");
}
#endregion
#endregion
#region Forward Vehicle Parameterization
// forward vehicle update
this.ForwardVehicle.Update(lane, state);
// clear current params
this.followingParameters = null;
// check not in a sparse area
bool sparseArea = lane is ArbiterLane ?
((ArbiterLane)lane).GetClosestPartition(state.Front).Type == PartitionType.Sparse :
((SupraLane)lane).ClosestComponent(state.Front) == SLComponentType.Initial && ((SupraLane)lane).Initial.GetClosestPartition(state.Front).Type == PartitionType.Sparse;
// exists forward vehicle
if (!sparseArea && this.ForwardVehicle.ShouldUseForwardTracker)
{
// get forward vehicle params, set lane decorators
TravelingParameters vehicleParams = this.ForwardVehicle.Follow(lane, state, ignorable);
vehicleParams.Behavior.Decorators.AddRange(this.laneParameters.Decorators);
this.FollowingParameters = vehicleParams;
#region Log
if (log)
{
// add to current parames to arbiter information
CoreCommon.CurrentInformation.FVTBehavior = vehicleParams.Behavior.ToShortString();
CoreCommon.CurrentInformation.FVTSpeed = this.ForwardVehicle.FollowingParameters.RecommendedSpeed.ToString("F3");
CoreCommon.CurrentInformation.FVTSpeedCommand = vehicleParams.Behavior.SpeedCommandString();
CoreCommon.CurrentInformation.FVTDistance = vehicleParams.DistanceToGo.ToString("F2");
CoreCommon.CurrentInformation.FVTState = vehicleParams.NextState.ShortDescription();
CoreCommon.CurrentInformation.FVTStateInfo = vehicleParams.NextState.StateInformation();
// set xSeparation
CoreCommon.CurrentInformation.FVTXSeparation = this.ForwardVehicle.ForwardControl.xSeparation.ToString("F0");
}
#endregion
// check if we're stopped behind fv, allow wait timer if true, stop wait timer if not behind fv
bool forwardVehicleStopped = this.ForwardVehicle.CurrentVehicle.IsStopped;
bool forwardSeparationGood = this.ForwardVehicle.ForwardControl.xSeparation < TahoeParams.VL * 2.5;
bool wereStopped = CoreCommon.Communications.GetVehicleSpeed().Value < 0.1;
bool forwardDistanceToGo = vehicleParams.DistanceToGo < 3.5;
if (forwardVehicleStopped && forwardSeparationGood && wereStopped && forwardDistanceToGo)
this.ForwardVehicle.StoppedBehindForwardVehicle = true;
else
{
this.ForwardVehicle.StoppedBehindForwardVehicle = false;
this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Stop();
this.ForwardVehicle.CurrentVehicle.QueuingState.WaitTimer.Reset();
}
// add vehicle param
tps.Add(vehicleParams);
}
else
{
// no forward vehicle
this.followingParameters = null;
this.ForwardVehicle.StoppedBehindForwardVehicle = false;
}
#endregion
#region Sparse Waypoint Parameterization
// check for sparse waypoints downstream
bool sparseDownstream;
bool sparseNow;
double sparseDistance;
lane.SparseDetermination(state.Front, out sparseDownstream, out sparseNow, out sparseDistance);
// check if sparse areas downstream
if (sparseDownstream)
{
// set the distance to the sparse area
if (sparseNow)
sparseDistance = 0.0;
// get speed
double speed = SpeedTools.GenerateSpeed(sparseDistance, 2.24, lane.CurrentMaximumSpeed(state.Front));
// maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
SpeedCommand sc = new ScalarSpeedCommand(speed);
#region Parameterize Given Speed Command
// check if lane
if (lane is ArbiterLane)
{
// get lane
ArbiterLane al = (ArbiterLane)lane;
// default behavior
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List<int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
b.Decorators = this.laneParameters.Decorators;
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), this.laneParameters.Decorators, state.Timestamp);
}
// check if supra lane
else if (lane is SupraLane)
{
// get lane
SupraLane sl = (SupraLane)lane;
// get sl state
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
// get default beheavior
Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List<int>());
b.Decorators = this.laneParameters.Decorators;
// standard behavior is fine for maneuver
m = new Maneuver(b, sisls, this.laneParameters.Decorators, state.Timestamp);
}
#endregion
#region Parameterize
// create new params
TravelingParameters tp = new TravelingParameters();
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = this.laneParameters.Decorators;
tp.DistanceToGo = Double.MaxValue;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = speed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.SpeedCommand = sc;
tp.VehiclesToIgnore = new List<int>();
// return navigation params
tps.Add(tp);
#endregion
}
#endregion
// sort params by most urgent
tps.Sort();
// set current params
this.currentParameters = tps[0];
// get behavior to check add vehicles to ignore
if (this.currentParameters.Behavior is StayInLaneBehavior)
((StayInLaneBehavior)this.currentParameters.Behavior).IgnorableObstacles = this.ForwardVehicle.VehiclesToIgnore;
// out of navigation, blockages, and vehicle following determine the actual primary parameters for this lane
return tps[0];
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="waypoint"></param>
/// <param name="entries"></param>
public IntersectionPlan(ITraversableWaypoint waypoint, List <PlanableInterconnect> entries, RoadPlan roadPlan)
{
this.ExitWaypoint = waypoint;
this.PossibleEntries = entries;
this.SegmentPlan = roadPlan;
}
