/// <summary>
/// Checks if hte opposing lane is clear to pass an opposing vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
public bool ClearForDisabledVehiclePass(ArbiterLane lane, VehicleState state, double vUs, Coordinates minReturn)
{
// update the forward vehicle
this.ForwardVehicle.Update(lane, state);
// check if the rear vehicle exists and is moving along with us
if (this.ForwardVehicle.ShouldUseForwardTracker && this.ForwardVehicle.CurrentVehicle != null)
{
// distance from other to us
double currentDistance = lane.DistanceBetween(this.ForwardVehicle.CurrentVehicle.ClosestPosition, state.Front) - (2 * TahoeParams.VL);
double minChangeDist = lane.DistanceBetween(minReturn, state.Front);
// check if he's within min return dist
if (currentDistance > minChangeDist)
{
// params
double vOther = this.ForwardVehicle.CurrentVehicle.StateMonitor.Observed.speedValid ? this.ForwardVehicle.CurrentVehicle.Speed : lane.Way.Segment.SpeedLimits.MaximumSpeed;
// get distance of envelope for him to slow to our speed
double xEnvelope = (Math.Pow(vUs, 2.0) - Math.Pow(vOther, 2.0)) / (2.0 * -0.5);
// check to see if vehicle is outside of the envelope to slow down for us after 3 seconds
double xSafe = currentDistance - minChangeDist - (xEnvelope + (vOther * 15.0));
return(xSafe > 0 ? true : false);
}
else
{
return(false);
}
}
else
{
return(true);
}
}
/// <summary>
/// Determines proper speed commands given we want to stop at a certain waypoint
/// </summary>
/// <param name="waypoint"></param>
/// <param name="lane"></param>
/// <param name="position"></param>
/// <param name="enCovariance"></param>
/// <param name="stopSpeed"></param>
/// <param name="stopDistance"></param>
public void StoppingParams(ArbiterWaypoint waypoint, ArbiterLane lane, Coordinates position, double extraDistance,
out double stopSpeed, out double stopDistance)
{
// get dist to waypoint
stopDistance = lane.DistanceBetween(position, waypoint.Position) - extraDistance;
// speed tools
stopSpeed = SpeedTools.GenerateSpeed(stopDistance, CoreCommon.OperationalStopSpeed, 2.24);
}
/// <summary>
/// Gets the next navigational stop relavant to us (stop or end) in the closest good lane or our current opposing lane
/// </summary>
/// <param name="closestGood"></param>
/// <param name="coordinates"></param>
/// <param name="ignorable"></param>
/// <param name="navStopSpeed"></param>
/// <param name="navStopDistance"></param>
/// <param name="navStopType"></param>
/// <param name="navStop"></param>
private void NextOpposingNavigationalStop(ArbiterLane opposing, ArbiterLane closestGood, Coordinates coordinates, double extraDistance,
out double navStopSpeed, out double navStopDistance, out StopType navStopType, out ArbiterWaypoint navStop)
{
ArbiterWaypoint current = null;
double minDist = Double.MaxValue;
StopType st = StopType.EndOfLane;
#region Closest Good Parameterization
foreach (ArbiterWaypoint aw in closestGood.WaypointList)
{
if (aw.IsStop || aw.NextPartition == null)
{
double dist = closestGood.DistanceBetween(coordinates, aw.Position);
if (dist < minDist && dist >= 0)
{
current = aw;
minDist = dist;
st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane;
}
}
}
#endregion
#region Opposing Parameterization
ArbiterWaypoint opStart = opposing.GetClosestPartition(coordinates).Initial;
int startIndex = opposing.WaypointList.IndexOf(opStart);
for (int i = startIndex; i >= 0; i--)
{
ArbiterWaypoint aw = opposing.WaypointList[i];
if (aw.IsStop || aw.PreviousPartition == null)
{
double dist = opposing.DistanceBetween(aw.Position, coordinates);
if (dist < minDist && dist >= 0)
{
current = aw;
minDist = dist;
st = aw.IsStop ? StopType.StopLine : StopType.EndOfLane;
}
}
}
#endregion
double tmpDistanceIgnore;
this.StoppingParams(current, closestGood, coordinates, extraDistance, out navStopSpeed, out tmpDistanceIgnore);
navStop = current;
navStopDistance = minDist;
navStopType = st;
}
/// <summary>
/// Updates the current vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
public void Update(ArbiterLane lane, VehicleState state)
{
// get the forward path
LinePath p = lane.LanePath().Clone();
p.Reverse();
// get our position
Coordinates f = state.Front;
// get all vehicles associated with those components
List <VehicleAgent> vas = new List <VehicleAgent>();
foreach (IVehicleArea iva in lane.AreaComponents)
{
if (TacticalDirector.VehicleAreas.ContainsKey(iva))
{
vas.AddRange(TacticalDirector.VehicleAreas[iva]);
}
}
// get the closest forward of us
double minDistance = Double.MaxValue;
VehicleAgent closest = null;
// get clsoest
foreach (VehicleAgent va in vas)
{
// get position of front
Coordinates frontPos = va.ClosestPosition;
// gets distance from other vehicle to us along the lane
double frontDist = lane.DistanceBetween(frontPos, f);
if (frontDist >= 0 && frontDist < minDistance)
{
minDistance = frontDist;
closest = va;
}
}
this.CurrentVehicle = closest;
this.currentDistance = minDistance;
}
/// <summary>
/// Helps with parameterizations for lateral reasoning
/// </summary>
/// <param name="referenceLane"></param>
/// <param name="fqmLane"></param>
/// <param name="goal"></param>
/// <param name="vehicleFront"></param>
/// <returns></returns>
public TravelingParameters ParameterizationHelper(ArbiterLane referenceLane, ArbiterLane fqmLane,
Coordinates goal, Coordinates vehicleFront, IState nextState, VehicleState state, VehicleAgent va)
{
// get traveling parameterized list
List <TravelingParameters> tps = new List <TravelingParameters>();
// get distance to the goal
double goalDistance;
double goalSpeed;
this.StoppingParams(goal, referenceLane, vehicleFront, new double[] { }, out goalSpeed, out goalDistance);
tps.Add(this.GetParameters(referenceLane, goalSpeed, goalDistance, state));
// get next stop
ArbiterWaypoint stopPoint;
double stopSpeed;
double stopDistance;
StopType stopType;
this.NextLaneStop(fqmLane, vehicleFront, new double[] { }, new List <ArbiterWaypoint>(),
out stopPoint, out stopSpeed, out stopDistance, out stopType);
this.StoppingParams(stopPoint.Position, referenceLane, vehicleFront, new double[] { },
out stopSpeed, out stopDistance);
tps.Add(this.GetParameters(referenceLane, stopSpeed, stopDistance, state));
// get vehicle
if (va != null)
{
VehicleAgent tmp = this.ForwardVehicle.CurrentVehicle;
double tmpDist = this.ForwardVehicle.currentDistance;
this.ForwardVehicle.CurrentVehicle = va;
this.ForwardVehicle.currentDistance = referenceLane.DistanceBetween(state.Front, va.ClosestPosition);
TravelingParameters tp = this.ForwardVehicle.Follow(referenceLane, state, new List <ArbiterWaypoint>());
tps.Add(tp);
this.ForwardVehicle.CurrentVehicle = tmp;
this.ForwardVehicle.currentDistance = tmpDist;
}
// parameterize
tps.Sort();
return(tps[0]);
}
/// <summary>
/// Update the rear monitor with the closest vehicle in the rear
/// </summary>
/// <param name="state"></param>
public void Update(VehicleState state)
{
// get the forward path
LinePath p = lane.LanePath();
// get our position
Coordinates f = state.Front - state.Heading.Normalize(TahoeParams.VL);
// get all vehicles associated with those components
List <VehicleAgent> vas = new List <VehicleAgent>();
foreach (IVehicleArea iva in lane.AreaComponents)
{
if (TacticalDirector.VehicleAreas.ContainsKey(iva))
{
vas.AddRange(TacticalDirector.VehicleAreas[iva]);
}
}
// get the closest forward of us
double minDistance = Double.MaxValue;
VehicleAgent closest = null;
// get clsoest
foreach (VehicleAgent va in vas)
{
// get position of front
Coordinates frontPos = va.ClosestPosition;
double frontDist = lane.DistanceBetween(f, frontPos);
if (frontDist >= 0 && frontDist < minDistance)
{
minDistance = frontDist;
closest = va;
}
}
this.CurrentVehicle = closest;
this.currentDistance = minDistance;
}
/// <summary>
/// Given vehicles and there locations determines if the lane adjacent to us is occupied adjacent to the vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
public bool Occupied(ArbiterLane lane, VehicleState state)
{
// check stopwatch time for proper elapsed
if (this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 10)
{
this.Reset();
}
if (TacticalDirector.VehicleAreas.ContainsKey(lane))
{
// vehicles in the lane
List <VehicleAgent> laneVehicles = TacticalDirector.VehicleAreas[lane];
// position of the center of our vehicle
Coordinates center = state.Front - state.Heading.Normalize(TahoeParams.VL / 2.0);
// cutoff for allowing vehicles outside this range
double dCutOff = TahoeParams.VL * 1.5;
// loop through vehicles
foreach (VehicleAgent va in laneVehicles)
{
// vehicle high level distance
double d = Math.Abs(lane.DistanceBetween(center, va.ClosestPosition));
// check less than distance cutoff
if (d < dCutOff)
{
this.Reset();
this.CurrentVehicle = va;
ArbiterOutput.Output("Opposing Lateral: " + this.VehicleSide.ToString() + " filled with vehicle: " + va.ToString());
return(true);
}
}
}
// now check the lateral sensor for being occupied
if (this.SideSickObstacleDetected(lane, state))
{
this.CurrentVehicle = new VehicleAgent(true, true);
this.Reset();
ArbiterOutput.Output("Opposing Lateral: " + this.VehicleSide.ToString() + " SIDE OBSTACLE DETECTED");
return(true);
}
// none found
this.CurrentVehicle = null;
// if none found, tiemr not running start timer
if (!this.LateralClearStopwatch.IsRunning)
{
this.Reset();
this.LateralClearStopwatch.Start();
ArbiterOutput.Output("Opposing Lateral: " + this.VehicleSide.ToString() + " Clear, starting cooldown");
return(true);
}
// enough time
else if (this.LateralClearStopwatch.IsRunning && this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 1.0)
{
ArbiterOutput.Output("Opposing Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown complete");
return(false);
}
// not enough time
else
{
double timer = this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0;
ArbiterOutput.Output("Opposing Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown timer: " + timer.ToString("F2"));
return(true);
}
}
/// <summary>
/// Behavior we would like to do other than going straight
/// </summary>
/// <param name="arbiterLane"></param>
/// <param name="vehicleState"></param>
/// <param name="p"></param>
/// <param name="blockages"></param>
/// <returns></returns>
/// <remarks>tries to go right, if not goest left if needs
/// to if forward vehicle ahead and we're stopped because of them</remarks>
public Maneuver?SecondaryManeuver(ArbiterLane arbiterLane, ArbiterLane closestGood, VehicleState vehicleState, List <ITacticalBlockage> blockages,
LaneChangeParameters?entryParameters)
{
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is OpposingLaneBlockage)
{
// 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));
}
// check dist needed to complete
double neededDistance = (Math.Abs(arbiterLane.LaneId.Number - closestGood.LaneId.Number) * 1.5 * TahoeParams.VL) +
(-Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2) / (4 * CoreCommon.MaximumNegativeAcceleration));
// get upper bound
LinePath.PointOnPath xFront = arbiterLane.LanePath().GetClosestPoint(vehicleState.Front);
Coordinates xUpper = arbiterLane.LanePath().AdvancePoint(xFront, -neededDistance).Location;
if (entryParameters.HasValue)
{
// check if we should get back, keep speed nice n' lowi fpassing failed
if (entryParameters.Value.Reason == LaneChangeReason.FailedForwardVehicle)
{
double xToReturn = arbiterLane.DistanceBetween(entryParameters.Value.DefaultReturnLowerBound, vehicleState.Front);
if (xToReturn >= 0.0)
{
ArbiterOutput.Output("Distance until must return to lane: " + xToReturn);
}
else
{
ArbiterOutput.Output("Can return to lane from arbitrary upper bound: " + xToReturn);
}
// check can return
if (xToReturn < 0)
{
// check if right lateral exists exactly here
if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning good and exists exactly here");
return(this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true));
}
else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here");
if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood))
{
this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger);
}
if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState))
{
ILateralReasoning tmpReasoning = this.rightLateralReasoning;
this.rightLateralReasoning = this.secondaryLateralReasoning;
Maneuver?tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, true);
this.rightLateralReasoning = tmpReasoning;
return(tmp);
}
else
{
ArbiterOutput.Output("Cosest good lane does not exist here??");
return(null);
}
}
else
{
ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() +
", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString());
return(null);
}
}
else
{
return(null);
}
}
}
// lane change info
LaneChangeInformation lci = new LaneChangeInformation(LaneChangeReason.Navigation, null);
// notify
ArbiterOutput.Output("In Opposing with no Previous state knowledge, attempting to return");
// check if right lateral exists exactly here
if (this.rightLateralReasoning.ExistsExactlyHere(vehicleState) && this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning good and exists exactly here");
return(this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false));
}
else if (!this.rightLateralReasoning.ExistsRelativelyHere(vehicleState) && !this.rightLateralReasoning.LateralLane.Equals(closestGood))
{
ArbiterOutput.Output("Right lateral reasoning not good closest and does not exist here");
if (this.secondaryLateralReasoning == null || !this.secondaryLateralReasoning.LateralLane.Equals(closestGood))
{
this.secondaryLateralReasoning = new LateralReasoning(closestGood, UrbanChallenge.Common.Sensors.SideObstacleSide.Passenger);
}
if (this.secondaryLateralReasoning.ExistsExactlyHere(vehicleState))
{
ILateralReasoning tmpReasoning = this.rightLateralReasoning;
this.rightLateralReasoning = this.secondaryLateralReasoning;
Maneuver?tmp = this.DefaultRightToGoodChange(arbiterLane, closestGood, vehicleState, blockages, xUpper, false);
this.rightLateralReasoning = tmpReasoning;
return(tmp);
}
else
{
ArbiterOutput.Output("Cosest good lane does not exist here??");
return(null);
}
}
else
{
ArbiterOutput.Output("Can't change lanes!!, RL exists exactly: " + this.rightLateralReasoning.ExistsExactlyHere(vehicleState).ToString() +
", RL exists rel: " + this.rightLateralReasoning.ExistsRelativelyHere(vehicleState).ToString() + ", RL closest good: " + this.rightLateralReasoning.LateralLane.Equals(closestGood).ToString());
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>
/// Check if we can go
/// </summary>
/// <param name="vs"></param>
private bool CanGo(VehicleState vs)
{
#region Moving Vehicles Inside Turn
// check if we can still go through this turn
if (TacticalDirector.VehicleAreas.ContainsKey(this.turn))
{
// get the subpath of the interconnect we care about
LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front);
LinePath aiSubpath = this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint);
if (aiSubpath.PathLength > 4.0)
{
aiSubpath = aiSubpath.SubPath(aiSubpath.StartPoint, aiSubpath.PathLength - 2.0);
// get vehicles
List <VehicleAgent> turnVehicles = TacticalDirector.VehicleAreas[this.turn];
// loop vehicles
foreach (VehicleAgent va in turnVehicles)
{
// check if inside turn
LinePath.PointOnPath vaPop = aiSubpath.GetClosestPoint(va.ClosestPosition);
if (!va.IsStopped && this.turn.TurnPolygon.IsInside(va.ClosestPosition) && !vaPop.Equals(aiSubpath.StartPoint) && !vaPop.Equals(aiSubpath.EndPoint))
{
ArbiterOutput.Output("Vehicle seen inside our turn: " + va.ToString() + ", stopping");
return(false);
}
}
}
}
#endregion
// test if this turn is part of an intersection
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(this.turn.InitialGeneric.AreaSubtypeWaypointId))
{
// intersection
ArbiterIntersection inter = CoreCommon.RoadNetwork.IntersectionLookup[this.turn.InitialGeneric.AreaSubtypeWaypointId];
// check if priority lanes exist for this interconnect
if (inter.PriorityLanes.ContainsKey(this.turn))
{
// get all the default priority lanes
List <IntersectionInvolved> priorities = inter.PriorityLanes[this.turn];
// get the subpath of the interconnect we care about
//LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front);
LinePath aiSubpath = new LinePath(new List <Coordinates>(new Coordinates[] { vs.Front, this.turn.FinalGeneric.Position })); //this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint);
// check if path ended
if (aiSubpath.Count < 2)
{
return(true);
}
// determine all of the new priority lanes
List <IntersectionInvolved> updatedPriorities = new List <IntersectionInvolved>();
#region Determine new priority areas given position
// loop through old priorities
foreach (IntersectionInvolved ii in priorities)
{
// check ii lane
if (ii.Area is ArbiterLane)
{
#region Lane Intersects Turn Path w/ Point of No Return
// check if the waypoint is not the last waypoint in the lane
if (ii.Exit == null || ((ArbiterWaypoint)ii.Exit).NextPartition != null)
{
// check where line intersects path
Coordinates?intersect = this.LaneIntersectsPath(ii, aiSubpath, this.turn.FinalGeneric);
// check for an intersection
if (intersect.HasValue)
{
// distance to intersection
double distanceToIntersection = (intersect.Value.DistanceTo(vs.Front) + ((ArbiterLane)ii.Area).LanePath().GetClosestPoint(vs.Front).Location.DistanceTo(vs.Front)) / 2.0;
// determine if we can stop before or after the intersection
double distanceToStoppage = RoadToolkit.DistanceToStop(CoreCommon.Communications.GetVehicleSpeed().Value);
// check dist to intersection > distance to stoppage
if (distanceToIntersection > distanceToStoppage)
{
// add updated priority
updatedPriorities.Add(new IntersectionInvolved(new ArbiterWaypoint(intersect.Value, new ArbiterWaypointId(0, ((ArbiterLane)ii.Area).LaneId)),
ii.Area, ArbiterTurnDirection.Straight));
}
else
{
ArbiterOutput.Output("Passed point of No Return for Lane: " + ii.Area.ToString());
}
}
}
#endregion
// we know there is an exit and it is the last waypoint of the segment, fuxk!
else
{
#region Turns Intersect
// get point to look at if exists
ArbiterInterconnect interconnect;
Coordinates? intersect = this.TurnIntersects(aiSubpath, ii.Exit, out interconnect);
// check for the intersect
if (intersect.HasValue)
{
ArbiterLane al = (ArbiterLane)ii.Area;
LinePath lp = al.LanePath().SubPath(al.LanePath().StartPoint, al.LanePath().GetClosestPoint(ii.Exit.Position));
lp.Add(interconnect.InterconnectPath.EndPoint.Location);
// get our time to the intersection point
//double ourTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value));
// get our time to the intersection point
double ourSpeed = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value);
double stoppedTime = ourSpeed < 1.0 ? 1.5 : 0.0;
double extraTime = 1.5;
double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed;
double ourTime = Math.Min(6.5, stoppedTime + extraTime + interconnectTime);
// get closest vehicle in that lane to the intersection
List <VehicleAgent> toLook = new List <VehicleAgent>();
if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area))
{
foreach (VehicleAgent tmpVa in TacticalDirector.VehicleAreas[ii.Area])
{
double upstreamDist = al.DistanceBetween(tmpVa.ClosestPosition, ii.Exit.Position);
if (upstreamDist > 0 && tmpVa.PassedDelayedBirth)
{
toLook.Add(tmpVa);
}
}
}
if (TacticalDirector.VehicleAreas.ContainsKey(interconnect))
{
toLook.AddRange(TacticalDirector.VehicleAreas[interconnect]);
}
// check length of stuff to look at
if (toLook.Count > 0)
{
foreach (VehicleAgent va in toLook)
{
// distance along path to location of intersect
double distToIntersect = lp.DistanceBetween(lp.GetClosestPoint(va.ClosestPosition), lp.GetClosestPoint(aiSubpath.GetClosestPoint(va.ClosestPosition).Location));
double speed = va.Speed == 0.0 ? 0.01 : va.Speed;
double vaTime = distToIntersect / Math.Abs(speed);
if (vaTime > 0 && vaTime < ourTime)
{
ArbiterOutput.Output("va: " + va.ToString() + " CollisionTimer: " + vaTime.ToString("f2") + " < TimeUs: " + ourTime.ToString("F2") + ", NOGO");
return(false);
}
}
}
}
#endregion
}
}
}
#endregion
#region Updated Priority Intersection Code
// loop through updated priorities
bool updatedPrioritiesClear = true;
foreach (IntersectionInvolved ii in updatedPriorities)
{
// lane
ArbiterLane al = (ArbiterLane)ii.Area;
// get our time to the intersection point
double ourSpeed = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value);
double stoppedTime = ourSpeed < 1.0 ? 1.5 : 0.0;
double extraTime = 1.5;
double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed;
double ourTime = Math.Min(6.5, stoppedTime + extraTime + interconnectTime);
// double outTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value));
// get closest vehicle in that lane to the intersection
if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area))
{
// get lane vehicles
List <VehicleAgent> vas = TacticalDirector.VehicleAreas[ii.Area];
// determine for all
VehicleAgent closestLaneVa = null;
double closestDistanceVa = Double.MaxValue;
double closestTime = Double.MaxValue;
foreach (VehicleAgent testVa in vas)
{
// check upstream
double distance = al.DistanceBetween(testVa.ClosestPosition, ii.Exit.Position);
// get speed
double speed = testVa.Speed;
double time = testVa.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed;
// check distance > 0
if (distance > 0)
{
// check if closer or none other exists
if (closestLaneVa == null || time < closestTime)
{
closestLaneVa = testVa;
closestDistanceVa = distance;
closestTime = time;
}
}
}
// check if closest exists
if (closestLaneVa != null)
{
// set va
VehicleAgent va = closestLaneVa;
double distance = closestDistanceVa;
// check dist and birth time
if (distance > 0 && va.PassedDelayedBirth)
{
// check time
double speed = va.Speed == 0.0 ? 0.01 : va.Speed;
double time = va.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed;
// too close
if (!al.LanePolygon.IsInside(CoreCommon.Communications.GetVehicleState().Front) &&
distance < 25 && (!va.StateMonitor.Observed.speedValid || !va.StateMonitor.Observed.isStopped) &&
CoreCommon.Communications.GetVehicleState().Front.DistanceTo(va.ClosestPosition) < 20)
{
ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + " vehicle: " + va.ToString() + " possibly moving to close, stopping");
//return false;
updatedPrioritiesClear = false;
return(false);
}
else if (time > 0 && time < ourTime)
{
ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2"));
//return false;
updatedPrioritiesClear = false;
return(false);
}
else
{
ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2"));
//return true;
}
}
}
else
{
ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + " has no traffic vehicles");
}
}
}
return(updatedPrioritiesClear);
#endregion
}
}
// fall through fine to go
ArbiterOutput.Output("In Turn, CAN GO, Clear of vehicles upstream");
return(true);
}