/// <summary>
/// Checks if it is clear to go into the other lane
/// </summary>
/// <param name="state"></param>
/// <returns></returns>
public bool IsClear(VehicleState state, double vUs)
{
// temp get
bool b = this.CheckClear(state, vUs);
if (!b)
{
this.ResetTiming();
return false;
}
// if none found, tiemr not running start timer
if (!this.RearClearStopwatch.IsRunning)
{
this.ResetTiming();
this.RearClearStopwatch.Start();
ArbiterOutput.Output("Forward Rear: " + this.VehicleSide.ToString() + " Clear, starting cooldown");
return false;
}
// enough time
else if (this.RearClearStopwatch.IsRunning && this.RearClearStopwatch.ElapsedMilliseconds / 1000.0 > 0.5)
{
ArbiterOutput.Output("Forward Rear: " + this.VehicleSide.ToString() + " Clear, cooldown complete");
return true;
}
// not enough time
else
{
double timer = this.RearClearStopwatch.ElapsedMilliseconds / 1000.0;
ArbiterOutput.Output("Forward Rear: " + this.VehicleSide.ToString() + " Clear, cooldown timer: " + timer.ToString("F2"));
return false;
}
}
/// <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;
}
public void MessageArrived(string channelName, object message)
{
if (channelName == "RndfNetworkChannel")
{
this.rndfNetwork = (Common.RndfNetwork.RndfNetwork)message;
Console.WriteLine(" > Rndf Received");
}
else if (channelName == "MdfChannel")
{
this.mdf = (Common.RndfNetwork.Mdf)message;
Console.WriteLine(" > Mdf Received");
}
else if (channelName == "PositionChannel")
{
this.vehicleState = (VehicleState) message;
Console.WriteLine(" > Position Received: " + this.vehicleState.ToString());
}
else if (channelName == "TestStringChannel")
{
Console.WriteLine("Received String On TestStringChannel: " + ((string)message));
}
else
{
throw new ArgumentException("Unknown Channel", channelName);
}
}
/// <summary>
/// Gets a default behavior for a path segment
/// </summary>
/// <param name="location"></param>
/// <param name="vehicleState"></param>
/// <param name="exit"></param>
/// <param name="relative"></param>
/// <param name="stopSpeed"></param>
/// <param name="aMax"></param>
/// <param name="dt">timestep in seconds</param>
/// <returns></returns>
public static PathFollowingBehavior DefaultStayInLaneBehavior(RndfLocation location, VehicleState vehicleState,
RndfWaypointID action, ActionType actionType, bool relative, double stopSpeed, double aMax, double dt,
double maxSpeed, IPath path)
{
// get lane path
//IPath path = RoadToolkit.LanePath(location.Partition.FinalWaypoint.Lane, vehicleState, relative);
// check if the action is just a goal (note that exit and stop take precedence)
if (actionType == ActionType.Goal)
{
// get maximum speed
//double maxSpeed = location.Partition.FinalWaypoint.Lane.Way.Segment.SpeedInformation.MaxSpeed;
//double maxSpeed = maxV;
// generate path following behavior
//return new PathFollowingBehavior(path, new ScalarSpeedCommand(maxSpeed));
return null;
}
else
{
// get maximum speed
//double maxSpeed = location.Partition.FinalWaypoint.Lane.Way.Segment.SpeedInformation.MaxSpeed;
// get operational required distance to hand over to operational stop
double distance = RoadToolkit.DistanceUntilOperationalStop(RoadToolkit.DistanceToWaypoint(location, action)-TahoeParams.FL);
// get desired velocity
double desiredSpeed = RoadToolkit.InferFinalSpeed(0, stopSpeed, distance, maxSpeed, aMax, dt);
// generate path following behavior
//return new PathFollowingBehavior(path, new ScalarSpeedCommand(desiredSpeed));
return null;
}
}
public UrbanChallenge.Behaviors.Behavior Resume(VehicleState currentState, double speed)
{
if (StateToResume != null)
return StateToResume.Resume(currentState, speed);
else
return null;
}
/// <summary>
/// Get partition of startup chute we might be upon
/// </summary>
/// <param name="vehicleState"></param>
/// <returns></returns>
public ArbiterLanePartition GetStartupChute(VehicleState vehicleState)
{
// current road network
ArbiterRoadNetwork arn = CoreCommon.RoadNetwork;
// get startup chutes
List<ArbiterLanePartition> startupChutes = new List<ArbiterLanePartition>();
foreach (ArbiterSegment asg in arn.ArbiterSegments.Values)
{
foreach (ArbiterLane al in asg.Lanes.Values)
{
foreach (ArbiterLanePartition alp in al.Partitions)
{
if (alp.Type == PartitionType.Startup)
startupChutes.Add(alp);
}
}
}
// figure out which startup chute we are in
foreach (ArbiterLanePartition alp in startupChutes)
{
// check if within 40m of it
if(vehicleState.Front.DistanceTo(alp.Initial.Position) < 40.0)
{
// check orientation relative to lane
Coordinates laneVector = alp.Vector().Normalize();
// get our heading
Coordinates ourHeading = vehicleState.Heading.Normalize();
// check dist to each other to make sure forwards
if (laneVector.DistanceTo(ourHeading) < Math.Sqrt(2.0))
{
// figure out extension
Coordinates backC = alp.Initial.Position - alp.Vector().Normalize(40.0);
// generate new line path
LinePath lp = new LinePath(new Coordinates[] { backC, alp.Final.Position });
LinePath lpL = lp.ShiftLateral(alp.Lane.Width / 2.0);
LinePath lpR = lp.ShiftLateral(-alp.Lane.Width / 2.0);
List<Coordinates> poly = new List<Coordinates>();
poly.AddRange(lpL);
poly.AddRange(lpR);
Polygon chutePoly = Polygon.GrahamScan(poly);
// check if we're inside the chute
if (chutePoly.IsInside(vehicleState.Front))
{
// return the chute
return alp;
}
}
}
}
// fallout
return null;
}
/// <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>
/// Constructor
/// </summary>
/// <param name="localRoute"></param>
/// <param name="bestFullRoute"></param>
/// <param name="vehicleState"></param>
/// <param name="maneuver"></param>
public AiRemoteListener(Routes localRoute, FullRoute bestFullRoute, VehicleState vehicleState, Behavior behavior,
IPath path, DynamicObstacles dynamicObstacles, StaticObstacles staticObstacles)
{
this.LocalRoute = localRoute;
this.BestFullRoute = bestFullRoute;
this.VehicleState = vehicleState;
this.behavior = behavior;
this.path = path;
this.DynamicObstacles = dynamicObstacles;
this.StaticObstacles = staticObstacles;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="currentArbiterState"></param>
/// <param name="internalCarMode"></param>
/// <param name="fullRoute"></param>
/// <param name="routeTime"></param>
/// <param name="actionPoint"></param>
/// <param name="currentGoal"></param>
/// <param name="goals"></param>
/// <param name="behavior"></param>
public ArbiterInformation(string currentArbiterState, CarMode internalCarMode, FullRoute fullRoute, double routeTime,
RndfWaypointID actionPoint, RndfWaypointID currentGoal, Queue<RndfWaypointID> goals, Behavior behavior, VehicleState planningState)
{
this.CurrentArbiterState = currentArbiterState;
this.InternalCarMode = internalCarMode;
this.FullRoute = fullRoute;
this.RouteTime = routeTime;
this.ActionPoint = actionPoint;
this.CurrentGoal = currentGoal;
this.Goals = goals;
this.Behavior = behavior;
this.PlanningState = planningState;
}
private void RenderRelativePath(VehicleState vs, Path transPath, Graphics g, WorldTransform t, bool isArbiterPath)
{
if((isArbiterPath && DrawingUtility.DrawArbiterLanePath) ||
(!isArbiterPath && DrawingUtility.DrawOperationalLanePath))
{
// compute the rotation matrix to add in our vehicles rotation
/*Matrix3 rotMatrix = new Matrix3(
Math.Cos(vs.heading.ArcTan), -Math.Sin(vs.heading.ArcTan), 0,
Math.Sin(vs.heading.ArcTan), Math.Cos(vs.heading.ArcTan), 0,
0, 0, 1);
// compute the translation matrix to move our vehicle's location
Matrix3 transMatrix = new Matrix3(
1, 0, vs.xyPosition.X,
0, 1, vs.xyPosition.Y,
0, 0, 1);
// compute the combined transformation matrix
Matrix3 m = rotMatrix * transMatrix;
// clone, transform and add each segment to our path
transPath.Transform(m);*/
float nomPixelWidth = 2.0f;
float penWidth = nomPixelWidth / t.Scale;
Pen arbiterPen = new Pen(Color.FromArgb(100, DrawingUtility.ArbiterLanePath), penWidth);
Pen operationalPen = new Pen(Color.FromArgb(100, DrawingUtility.OperationalLanePath), penWidth);
// display path
foreach (IPathSegment ps in transPath)
{
if (ps is BezierPathSegment)
{
BezierPathSegment seg = (BezierPathSegment)ps;
CubicBezier cb = seg.cb;
if (isArbiterPath)
{
g.DrawBezier(arbiterPen, DrawingUtility.ToPointF(cb.P0), DrawingUtility.ToPointF(cb.P1), DrawingUtility.ToPointF(cb.P2), DrawingUtility.ToPointF(cb.P3));
}
else
{
g.DrawBezier(operationalPen, DrawingUtility.ToPointF(cb.P0), DrawingUtility.ToPointF(cb.P1), DrawingUtility.ToPointF(cb.P2), DrawingUtility.ToPointF(cb.P3));
}
}
}
}
}
public Behavior Resume(VehicleState currentState, double speed)
{
// decorators
List<BehaviorDecorator> bds = new List<BehaviorDecorator>();
// zero decorators
bds.Add(new TurnSignalDecorator(TurnSignal.Off));
// get no op
Behavior b = new NullBehavior();
// set decorators
b.Decorators = bds;
// return
return b;
}
public OpposingLanesState(ArbiterLane opposingLane, bool resetLaneAgent, IState previous, VehicleState vs)
{
this.resetLaneAgent = resetLaneAgent;
this.OpposingLane = opposingLane;
this.OpposingWay = opposingLane.Way.WayId;
this.ClosestGoodLane = null;
this.SetClosestGood(vs);
if (previous is ChangeLanesState)
{
EntryParameters = ((ChangeLanesState)previous).Parameters;
}
else if (previous is OpposingLanesState)
{
EntryParameters = ((OpposingLanesState)previous).EntryParameters;
}
else
{
EntryParameters = null;
}
}
static void Main(string[] args)
{
// Initialize Communications
Console.WriteLine("1. Initializing Communications...");
Communicator communicator = new Communicator();
Console.WriteLine(" > Communications Initialized\n");
// Wait for Rndf,Mdf to continue
Console.WriteLine("2. Waiting for Rndf, Mdf to continue...");
communicator.InitializeTestServer();
Console.WriteLine(" > Rndf, Mdf Received\n");
// Wait for User continue
Console.WriteLine("3. Ready To Start Testing, Press ENTER to Continue...");
Console.ReadLine();
// Wait for 1 second
Thread.Sleep(1000);
/*
// Turn => Stopped at Stop
// Create a fake vehicle state
RndfWayPoint position = communicator.RndfNetwork.Goals[1].Waypoint;
Console.WriteLine(" > Position: " + position.WaypointID.ToString());
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = position.Position;
LaneEstimate laneEstimate = new LaneEstimate(position.Lane.LaneID, position.Lane.LaneID, 1);
List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
communicator.PublishVehicleState(vehicleState);
Console.WriteLine(" > Published Position");*/
/*
// Road
// Create a fake vehicle state
RndfWaypointID tmpID = new RndfWaypointID(new LaneID(new WayID(new SegmentID(1), 1), 1), 1);
RndfWayPoint position = communicator.RndfNetwork.Waypoints[tmpID];
Console.WriteLine(" > Position: " + position.WaypointID.ToString());
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = position.Position;
LaneEstimate laneEstimate = new LaneEstimate(position.Lane.LaneID, position.Lane.LaneID, 1);
List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
vehicleState.speed = 3;
communicator.PublishVehicleState(vehicleState);
Console.WriteLine(" > Published Position");*/
/*
// Intersection
// Create a fake vehicle state
RndfWaypointID tmpID = new RndfWaypointID(new LaneID(new WayID(new SegmentID(1), 1), 1), 2);
RndfWaypointID tmpEndID = new RndfWaypointID(new LaneID(new WayID(new SegmentID(26), 2), 3), 1);
InterconnectID testID = new InterconnectID(tmpID, tmpEndID);
Interconnect testInter = communicator.RndfNetwork.Interconnects[testID];
RndfWayPoint position = communicator.RndfNetwork.Waypoints[tmpID];
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = position.Position + new UrbanChallenge.Common.Coordinates(4, 4);
LaneEstimate laneEstimate = new LaneEstimate(tmpID.LaneID, tmpEndID.LaneID, 1);
List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
vehicleState.speed = 3;
communicator.PublishVehicleState(vehicleState);
Console.WriteLine(" > Published Position");
*/
// In Lane
// Create a fake vehicle state
RndfWayPoint position = communicator.RndfNetwork.Goals[2].Waypoint;
Console.WriteLine(" > Position: " + position.WaypointID.ToString());
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = position.Position;
LaneEstimate laneEstimate = new LaneEstimate(position.Lane.LaneID, position.Lane.LaneID, 1);
List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.speed = 0;
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
communicator.PublishVehicleState(vehicleState);
//Console.WriteLine(" > Published Position");
/*// Read the configuration file.
RemotingConfiguration.Configure("..\\..\\App.config", false);
WellKnownServiceTypeEntry[] wkst = RemotingConfiguration.GetRegisteredWellKnownServiceTypes();
// "Activate" the NameService singleton.
ObjectDirectory objectDirectory = (ObjectDirectory)Activator.GetObject(typeof(ObjectDirectory), wkst[0].ObjectUri);
// Receive the facades of components we want to use
RndfEditorFacade editorFacade = (RndfEditorFacade)objectDirectory.Resolve("RndfEditor");
// Get the RndfNetwork
RndfNetwork rndfNetwork = editorFacade.RndfNetwork;
// Create the Goals from the RndfNetwork
Queue<Goal> goals = generateGoals(rndfNetwork);
Console.WriteLine("Created Goals");
// Create Speed Limits from the RndfNetwork
List<SpeedInformation> speedLimits = generateSpeedLimits(rndfNetwork);
Console.WriteLine("Created speed limits");
// Create Mdf
Mdf mdf = new Mdf(goals, speedLimits);
Console.WriteLine("Created Mdf");
// Create a fake vehicle state
RndfWayPoint position = rndfNetwork.Goals[1].Waypoint;
Console.WriteLine("Position: " + position.WaypointID.ToString());
VehicleState vehicleState = new VehicleState();
vehicleState.xyPosition = position.Position;
LaneEstimate laneEstimate = new LaneEstimate(position.Lane.LaneID, position.Lane.LaneID, 1);
List<LaneEstimate> laneEstimates = new List<LaneEstimate>();
laneEstimates.Add(laneEstimate);
vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates);
// Test
Console.WriteLine("Number of RndfNetwork Segments: " + rndfNetwork.Segments.Values.Count);
// Bind the facades of components we implement.
objectDirectory.Rebind(TestDataServerFacadeImpl.Instance(rndfNetwork, mdf, vehicleState), "TestServer");*/
// Show that the navigation system is ready and waiting for input
Console.WriteLine("");
Console.WriteLine("4. Test Serve Complete, Waiting. Press ENTER to Shut Down");
Console.ReadLine();
Console.WriteLine("");
communicator.ShutDown();
}
public UrbanChallenge.Behaviors.Behavior Resume(VehicleState currentState, double speed)
{
throw new Exception("The method or operation is not implemented.");
}
/// <summary>
/// Updates this monitor
/// </summary>
public void Update(VehicleState ourState)
{
// make sure not our vehicle
if (!isOurs)
{
// get a list of vehicles possibly in the exit
List<VehicleAgent> stopVehicles = new List<VehicleAgent>();
// check all valid vehicles
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
// check if any of the point inside the polygon
for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
{
// get abs coord
Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);
// check if inside poly
if (this.stoppedExit.ExitPolygon.IsInside(c) && !stopVehicles.Contains(va))
{
// add vehicle
stopVehicles.Add(va);
}
}
}
// check occluded
foreach (VehicleAgent va in TacticalDirector.OccludedVehicles.Values)
{
// check if any of the point inside the polygon
for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
{
// get abs coord
Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);
// check if inside poly
if (this.stoppedExit.ExitPolygon.IsInside(c) && !stopVehicles.Contains(va))
{
// add vehicle
ArbiterOutput.Output("Added occluded vehicle: " + va.ToString() + " to SEM: " + this.stoppedExit.ToString());
stopVehicles.Add(va);
}
}
}
// check if we already have a vehicle we are referencing
if (this.currentVehicle != null && !stopVehicles.Contains(this.currentVehicle))
{
// get the polygon of the current vehicle and inflate a tiny bit?
Polygon p = this.currentVehicle.GetAbsolutePolygon(ourState);
p = p.Inflate(1.0);
// Vehicle agent to switch to if exists
VehicleAgent newer = null;
double distance = Double.MaxValue;
// check for closest vehicle to exist in that polygon
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
// check if vehicle is stopped
if (stopVehicles.Contains(va))
{
// get distance to stop
double stopLineDist;
va.ClosestPointTo(this.stoppedExit.Waypoint.Position, ourState, out stopLineDist);
// check all vehicle points
foreach (Coordinates rel in va.StateMonitor.Observed.relativePoints)
{
Coordinates abs = va.TransformCoordAbs(rel, ourState);
double tmpDist = stopLineDist;
// check for the clsoest with points inside vehicle
if (p.IsInside(abs) && (newer == null || tmpDist < distance))
{
newer = va;
distance = tmpDist;
}
}
}
}
// check if we can switch vehicles
if (newer != null)
{
ArbiterOutput.Output("For StopLine: " + this.stoppedExit.ToString() + " switched vehicleId: " + this.currentVehicle.VehicleId.ToString() + " for vehicleId: " + newer.VehicleId.ToString());
this.currentVehicle = newer;
this.NotFoundPrevious = notFoundDefault;
}
else if(NotFoundPrevious == 0)
{
// set this as not having a vehicle
this.currentVehicle = null;
// set not found before
this.NotFoundPrevious = this.notFoundDefault;
// stop timers
this.timer.Stop();
this.timer.Reset();
}
else
{
// set not found before
this.NotFoundPrevious--;
ArbiterOutput.Output("current not found, not found previous: " + this.NotFoundPrevious.ToString() + ", at stopped exit: " + this.stoppedExit.Waypoint.ToString());
}
}
// update the current vehicle
else if (this.currentVehicle != null && stopVehicles.Contains(this.currentVehicle))
{
// udpate current
this.currentVehicle = stopVehicles[stopVehicles.IndexOf(this.currentVehicle)];
// set as found previous
this.NotFoundPrevious = this.notFoundDefault;
// check if need to update timer
if (this.currentVehicle.IsStopped && !this.timer.IsRunning)
{
this.timer.Stop();
this.timer.Reset();
this.timer.Start();
}
// otherwise check if moving
else if (!this.currentVehicle.IsStopped && this.timer.IsRunning)
{
this.timer.Stop();
this.timer.Reset();
}
}
// otherwise if we have no vehicle and exist vehicles in stop area
else if (this.currentVehicle == null && stopVehicles.Count > 0)
{
// get closest vehicle to the stop
VehicleAgent toTrack = null;
double distance = Double.MaxValue;
// set as found previous
this.NotFoundPrevious = this.notFoundDefault;
// loop through
foreach (VehicleAgent va in stopVehicles)
{
// check if vehicle is stopped
if (va.IsStopped)
{
// distance of vehicle to stop line
double distanceToStop;
Coordinates closestToStop = va.ClosestPointTo(this.stoppedExit.Waypoint.Position, ourState, out distanceToStop).Value;
// check if we don't have one or tmp closer than tracked
if (toTrack == null || distanceToStop < distance)
{
toTrack = va;
distance = distanceToStop;
}
}
}
// check if we have one to track
if (toTrack != null)
this.currentVehicle = toTrack;
}
}
}
public UrbanChallenge.Behaviors.Behavior Resume(UrbanChallenge.Common.Vehicle.VehicleState currentState, double speed)
{
return(new StayInLaneBehavior(OpposingLane.LaneId, new ScalarSpeedCommand(0.0), new List <int>()));
}
public PathsDisplay(VehicleState updatedState, VehicleState arbiterState, IPath path)
{
this.updatedState = updatedState;
this.arbiterState = arbiterState;
this.path = path;
}
/// <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>
/// Gets parameterization
/// </summary>
/// <param name="lane"></param>
/// <param name="speed"></param>
/// <param name="distance"></param>
/// <param name="state"></param>
/// <returns></returns>
public TravelingParameters GetParameters(ArbiterLane lane, double speed, double distance, VehicleState state)
{
double distanceCutOff = CoreCommon.OperationslStopLineSearchDistance;
Maneuver m = new Maneuver();
SpeedCommand sc;
bool usingSpeed = true;
#region Distance Cutoff
// check if distance is less than cutoff
if (distance < distanceCutOff)
{
// default behavior
sc = new StopAtDistSpeedCommand(distance);
Behavior b = new StayInLaneBehavior(lane.LaneId, sc, new List<int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(state.Front, true), lane.NumberOfLanesRight(state.Front, true));
// stopping so not using speed param
usingSpeed = false;
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(lane, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, state.Timestamp);
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// get lane
ArbiterLane al = lane;
// default behavior
sc = new ScalarSpeedCommand(speed);
Behavior b = new StayInLaneBehavior(al.LaneId, sc, 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), TurnDecorators.NoDecorators, state.Timestamp);
}
#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>
/// Updates vehicles
/// </summary>
/// <param name="vehicles"></param>
/// <param name="state"></param>
public void Update(SceneEstimatorTrackedClusterCollection vehicles, VehicleState state)
{
// vehicle population
this.PopulateValidVehicles(vehicles);
this.PopulateAreaVehicles();
this.UpdateQueuingMonitors(state.Timestamp);
}
public bool IsCompletelyClear(UrbanChallenge.Common.Vehicle.VehicleState ourState)
{
this.Update(ourState);
return(this.Clear(ourState));
}
/// <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)
{
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " filled with vehicle: " + va.ToString());
this.CurrentVehicle = va;
this.Reset();
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("Forward Lateral: " + this.VehicleSide.ToString() + " SIDE OBSTACLE DETECTED");
return true;
}
// none found
this.CurrentVehicle = null;
// if none found, timer not running start timer
if (!this.LateralClearStopwatch.IsRunning)
{
this.CurrentVehicle = new VehicleAgent(true, true);
this.Reset();
this.LateralClearStopwatch.Start();
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, starting cooldown");
return true;
}
// check enough time
else if (this.LateralClearStopwatch.IsRunning && this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0 > 0.5)
{
this.CurrentVehicle = null;
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown complete");
return false;
}
// not enough time
else
{
this.CurrentVehicle = new VehicleAgent(true, true);
double timer = this.LateralClearStopwatch.ElapsedMilliseconds / 1000.0;
ArbiterOutput.Output("Forward Lateral: " + this.VehicleSide.ToString() + " Clear, cooldown timer: " + timer.ToString("F2"));
return true;
}
}
/// <summary>
/// Check if a side sick blocking obstacle is detected
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
private bool SideSickObstacleDetected(ArbiterLane lane, VehicleState state)
{
try
{
// get distance from vehicle to lane opp side
Coordinates vec = state.Front - state.Position;
vec = this.VehicleSide == SideObstacleSide.Driver ? vec.Rotate90() : vec.RotateM90();
SideObstacles sobs = CoreCommon.Communications.GetSideObstacles(this.VehicleSide);
if (sobs != null && sobs.obstacles != null)
{
foreach (SideObstacle so in sobs.obstacles)
{
Coordinates cVec = state.Position + vec.Normalize(so.distance);
if (so.height > 0.7 && lane.LanePolygon.IsInside(cVec))
return true;
}
}
}
catch (Exception ex)
{
ArbiterOutput.Output("side sick obstacle exception: " + ex.ToString());
}
return false;
}
public bool Clear(UrbanChallenge.Common.Vehicle.VehicleState ourState)
{
return(this.currentVehicle == null ||
(this.currentVehicle.StateMonitor.Observed.speedValid && !this.currentVehicle.IsStopped && this.currentVehicle.Speed > 2.0));
}
public void Update(UrbanChallenge.Common.Vehicle.VehicleState ourState)
{
// get lane
ArbiterLane lane = this.turnFinalWaypoint.Lane;
// get the possible vehicles that could be in this stop
if (TacticalDirector.VehicleAreas.ContainsKey(lane))
{
// get vehicles in the the lane of the exit
List <VehicleAgent> possibleVehicles = TacticalDirector.VehicleAreas[lane];
// get the point we are looking from
LinePath.PointOnPath referencePoint = lane.LanePath().GetClosestPoint(this.turnFinalWaypoint.Position);
// tmp
VehicleAgent tmp = null;
double tmpDist = Double.MaxValue;
// check over all possible vehicles
foreach (VehicleAgent possible in possibleVehicles)
{
// get vehicle point on lane
LinePath.PointOnPath vehiclePoint = lane.LanePath().GetClosestPoint(possible.ClosestPosition);
// check if the vehicle is in front of the reference point
double vehicleDist = lane.LanePath().DistanceBetween(referencePoint, vehiclePoint);
if (vehicleDist >= 0 && vehicleDist < TahoeParams.VL * 2.0)
{
tmp = possible;
tmpDist = vehicleDist;
}
}
if (tmp != null)
{
if (this.currentVehicle == null || !this.currentVehicle.Equals(tmp))
{
this.timer.Stop();
this.timer.Reset();
this.currentVehicle = tmp;
}
else
{
this.currentVehicle = tmp;
}
if (this.currentVehicle.IsStopped && !this.timer.IsRunning)
{
this.timer.Stop();
this.timer.Reset();
this.timer.Start();
}
else if (!this.currentVehicle.IsStopped && this.timer.IsRunning)
{
this.timer.Stop();
this.timer.Reset();
}
}
else
{
this.currentVehicle = null;
this.timer.Stop();
this.timer.Reset();
}
}
else
{
this.timer.Stop();
this.timer.Reset();
this.currentVehicle = 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>
/// Resume from pause
/// </summary>
/// <returns></returns>
public Behavior Resume(VehicleState currentState, double speed)
{
// turn behavior into chute
TurnBehavior b = new TurnBehavior(this.Final.Lane.LaneId, this.Final.PartitionPath,
this.Final.PartitionPath.ShiftLateral(this.Final.Lane.Width / 2.0),
this.Final.PartitionPath.ShiftLateral(-this.Final.Lane.Width / 2.0),
new ScalarSpeedCommand(1.4), null);
// return behavior
return b;
}
public bool VehicleExistsInsidePolygon(VehicleAgent va, Polygon p, VehicleState ourState)
{
for (int i = 0; i < va.StateMonitor.Observed.relativePoints.Length; i++)
{
Coordinates c = va.TransformCoordAbs(va.StateMonitor.Observed.relativePoints[i], ourState);
if (p.IsInside(c))
return true;
}
return false;
}
/// <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
}
public bool VehiclePassableInPolygon(ArbiterLane al, VehicleAgent va, Polygon p, VehicleState ourState, Polygon circ)
{
Polygon vehiclePoly = va.GetAbsolutePolygon(ourState);
vehiclePoly = Polygon.ConvexMinkowskiConvolution(circ, vehiclePoly);
List<Coordinates> pointsOutside = new List<Coordinates>();
ArbiterLanePartition alp = al.GetClosestPartition(va.ClosestPosition);
foreach(Coordinates c in vehiclePoly)
{
if (!p.IsInside(c))
pointsOutside.Add(c);
}
foreach (Coordinates m in pointsOutside)
{
foreach (Coordinates n in pointsOutside)
{
if(!m.Equals(n))
{
if (GeneralToolkit.TriangleArea(alp.Initial.Position, m, alp.Final.Position) *
GeneralToolkit.TriangleArea(alp.Initial.Position, n, alp.Final.Position) < 0)
{
return false;
}
}
}
}
return true;
}
/// <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>
/// If this is clear or not at this current timestep
/// </summary>
public bool Clear(VehicleState ourState)
{
if (!isOurs)
return this.currentVehicle == null;
else
return false;
}
/// <summary>
/// Generic plan
/// </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, INavigationalPlan navigationalPlan, VehicleState vehicleState,
SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles,
List<ITacticalBlockage> blockages, double vehicleSpeed)
{
// update stuff
this.Update(vehicles, vehicleState);
#region Plan Roads
if (planningState is TravelState)
{
Maneuver roadFinal = roadTactical.Plan(
planningState,
(RoadPlan)navigationalPlan,
vehicleState,
vehicles,
obstacles,
blockages,
vehicleSpeed);
// return
return roadFinal;
}
#endregion
#region Plan Intersection
else if (planningState is IntersectionState)
{
Maneuver intersectionFinal = intersectionTactical.Plan(
planningState,
navigationalPlan,
vehicleState,
vehicles,
obstacles,
blockages);
// return
return intersectionFinal;
}
#endregion
#region Plan Zone
else if (planningState is ZoneState)
{
Maneuver zoneFinal = zoneTactical.Plan(
planningState,
navigationalPlan,
vehicleState,
vehicles,
obstacles,
blockages);
// return
return zoneFinal;
}
#endregion
#region Plan Blockage
else if (planningState is BlockageState)
{
Maneuver blockageFinal = blockageTactical.Plan(
planningState,
vehicleState,
vehicleSpeed,
blockages,
navigationalPlan);
return blockageFinal;
}
#endregion
#region Unknown
else
{
throw new Exception("Unknown planning state type: " + planningState.GetType().ToString());
}
#endregion
}
/// <summary>
/// Checks if this area is completely clear of vehicles and no chance of one coming in soon
/// </summary>
/// <param name="ourState"></param>
/// <returns></returns>
public bool IsCompletelyClear(VehicleState ourState)
{
// amke sure not ours
if (!isOurs)
{
// get the possible vehicles that could be in this stop
if (TacticalDirector.VehicleAreas.ContainsKey(this.stoppedExit.Waypoint.Lane))
{
// get vehicles in the the lane of the exit
List<VehicleAgent> possibleVehicles = TacticalDirector.VehicleAreas[this.stoppedExit.Waypoint.Lane];
// inflate the polygon
Polygon exitPolygon = this.stoppedExit.ExitPolygon.Inflate(TahoeParams.VL);
// check vehicles in lane
foreach (VehicleAgent va in possibleVehicles)
{
// check if vehicle inside inflated exit polygon
if (exitPolygon.IsInside(va.ClosestPosition))
{
return false;
}
}
}
// clear if made it this far
return true;
}
else
{
// ours is always clear relative to us
return true;
}
}
