public static Polygon DefaultLanePolygon(ArbiterLane al)
{
// fist get the right boundary of the lane
LinePath lb = al.LanePath().ShiftLateral(-al.Width / 2.0);
LinePath rb = al.LanePath().ShiftLateral(al.Width / 2.0);
rb.Reverse();
List <Coordinates> defaultPoly = lb;
defaultPoly.AddRange(rb);
// start the polygon
Polygon poly = new Polygon(defaultPoly);
return(poly);
}
public static Polygon LanePolygon(ArbiterLane al)
{
// fist get the right boundary of the lane
LinePath lb = al.LanePath().ShiftLateral(-al.Width / 2.0);
LinePath rb = al.LanePath().ShiftLateral(al.Width / 2.0);
rb.Reverse();
List <Coordinates> defaultPoly = lb;
defaultPoly.AddRange(rb);
// start the polygon
Polygon poly = new Polygon(defaultPoly);
// loop through partitions
foreach (ArbiterLanePartition alp in al.Partitions)
{
//if (alp.Initial.PreviousPartition != null && alp.Final.NextPartition != null)
//{
// get the good polygon
Polygon pPoly = PartitionPolygon(alp);
// check not null
if (pPoly != null)
{
poly = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { poly, pPoly }));
}
//}
}
// circles for intersections of partitions
foreach (ArbiterLanePartition alp in al.Partitions)
{
if (alp.Final.NextPartition != null)
{
double interAngle = Math.Abs(FinalIntersectionAngle(alp));
if (interAngle > 15)
{
Circle connect = new Circle((alp.Lane.Width / 2.0) + (interAngle / 15.0 * 0.5), alp.Final.Position);
poly = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { poly, connect.ToPolygon(16) }));
}
}
}
// return the polygon
return(poly);
}
/// <summary>
/// Gets priotiy lane determination
/// </summary>
/// <param name="ii"></param>
/// <param name="path"></param>
/// <param name="end"></param>
/// <returns></returns>
private Coordinates?LaneIntersectsPath(IntersectionInvolved ii, LinePath path, IArbiterWaypoint end)
{
ArbiterLane al = (ArbiterLane)ii.Area;
LinePath.PointOnPath current = path.StartPoint;
bool go = true;
while (go) // && !(current.Location.DistanceTo(path.EndPoint.Location) < 0.1))
{
Coordinates alClose = al.LanePath().GetClosestPoint(current.Location).Location;
double alDist = alClose.DistanceTo(current.Location);
if (alDist <= 0.05)
{
return(al.LanePath().GetClosestPoint(current.Location).Location);
}
if (current.Location.Equals(path.EndPoint.Location))
{
go = false;
}
current = path.AdvancePoint(current, 0.1);
}
/*if (ii.Exit != null)
* {
* ITraversableWaypoint laneExit = ii.Exit;
* foreach (ArbiterInterconnect tmpAi in laneExit.OutgoingConnections)
* {
* if (tmpAi.FinalGeneric.Equals(end))
* {
* return tmpAi.FinalGeneric.Position;
* }
* }
* }*/
return(null);
}
/// <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>
/// 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>
/// Parameters to follow the forward vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
public TravelingParameters Follow(ArbiterLane lane, VehicleState state)
{
// travelling parameters
TravelingParameters tp = new TravelingParameters();
// ignorable obstacles
List <int> ignoreVehicle = new List <int>();
ignoreVehicle.Add(CurrentVehicle.VehicleId);
// get control parameters
ForwardVehicleTrackingControl fvtc = GetControl(lane, state);
// init params
tp.DistanceToGo = fvtc.xDistanceToGood;
tp.NextState = CoreCommon.CorePlanningState;
tp.RecommendedSpeed = fvtc.vFollowing;
tp.Type = TravellingType.Vehicle;
tp.Decorators = TurnDecorators.NoDecorators;
// flag to ignore forward vehicle
bool ignoreForward = false;
// reversed lane path
LinePath lp = lane.LanePath().Clone();
lp.Reverse();
#region Following Control
#region Immediate Stop
// need to stop immediately
if (fvtc.vFollowing == 0.0)
{
// don't ignore forward
ignoreForward = false;
// speed command
SpeedCommand sc = new ScalarSpeedCommand(0.0);
tp.UsingSpeed = true;
// standard path following behavior
Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
tp.Behavior = final;
tp.SpeedCommand = sc;
}
#endregion
#region Distance Stop
// stop at distance
else if (fvtc.vFollowing < 0.7 &&
CoreCommon.Communications.GetVehicleSpeed().Value <= 2.24 &&
fvtc.xSeparation > fvtc.xAbsMin)
{
// ignore forward vehicle
ignoreForward = true;
// speed command
SpeedCommand sc = new StopAtDistSpeedCommand(fvtc.xDistanceToGood);
tp.UsingSpeed = false;
// standard path following behavior
Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
tp.Behavior = final;
tp.SpeedCommand = sc;
}
#endregion
#region Normal Following
// else normal
else
{
// ignore the forward vehicle as we are tracking properly
ignoreForward = true;
// speed command
SpeedCommand sc = new ScalarSpeedCommand(fvtc.vFollowing);
tp.DistanceToGo = fvtc.xDistanceToGood;
tp.NextState = CoreCommon.CorePlanningState;
tp.RecommendedSpeed = fvtc.vFollowing;
tp.Type = TravellingType.Vehicle;
tp.UsingSpeed = true;
// standard path following behavior
Behavior final = new StayInLaneBehavior(lane.LaneId, sc, ignoreVehicle, lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
tp.Behavior = final;
tp.SpeedCommand = sc;
}
#endregion
#endregion
// check ignore
if (ignoreForward)
{
List <int> ignorable = new List <int>();
ignorable.Add(this.CurrentVehicle.VehicleId);
tp.VehiclesToIgnore = ignorable;
}
else
{
tp.VehiclesToIgnore = new List <int>();
}
// return parameterization
return(tp);
}
/// <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>
/// 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>
/// Populates mapping of areas to vehicles
/// </summary>
public void PopulateAreaVehicles()
{
TacticalDirector.VehicleAreas = new Dictionary <IVehicleArea, List <VehicleAgent> >();
VehicleState vs = CoreCommon.Communications.GetVehicleState();
Circle circ = new Circle(TahoeParams.T + 0.3, new Coordinates());
Polygon conv = circ.ToPolygon(32);
Circle circ1 = new Circle(TahoeParams.T + 0.6, new Coordinates());
Polygon conv1 = circ1.ToPolygon(32);
Circle circ2 = new Circle(TahoeParams.T + 1.4, new Coordinates());
Polygon conv2 = circ2.ToPolygon(32);
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
#region Standard Areas
List <AreaProbability> AreaProbabilities = new List <AreaProbability>();
#region Add up probablities
for (int i = 0; i < va.StateMonitor.Observed.closestPartitions.Length; i++)
{
SceneEstimatorClusterPartition secp = va.StateMonitor.Observed.closestPartitions[i];
if (CoreCommon.RoadNetwork.VehicleAreaMap.ContainsKey(secp.partitionID))
{
IVehicleArea iva = CoreCommon.RoadNetwork.VehicleAreaMap[secp.partitionID];
bool found = false;
for (int j = 0; j < AreaProbabilities.Count; j++)
{
AreaProbability ap = AreaProbabilities[j];
if (ap.Key.Equals(iva))
{
ap.Value = ap.Value + secp.probability;
found = true;
}
}
if (!found)
{
AreaProbabilities.Add(new AreaProbability(iva, secp.probability));
}
}
else
{
ArbiterOutput.Output("Core Intelligence thread caught exception, partition: " + secp.partitionID + " not found in Vehicle Area Map");
}
}
#endregion
#region Assign
if (AreaProbabilities.Count > 0)
{
double rP = 0.0;
foreach (AreaProbability ap in AreaProbabilities)
{
if (ap.Key is ArbiterLane)
{
rP += ap.Value;
}
}
if (rP > 0.1)
{
foreach (AreaProbability ap in AreaProbabilities)
{
if (ap.Key is ArbiterLane)
{
// get lane
ArbiterLane al = (ArbiterLane)ap.Key;
// probability says in area
double vP = ap.Value / rP;
if (vP > 0.3)
{
#region Check if obstacle enough in area
bool ok = false;
if (ap.Key is ArbiterLane)
{
Coordinates closest = va.ClosestPointToLine(al.LanePath(), vs).Value;
// dist to closest
double distanceToClosest = vs.Front.DistanceTo(closest);
// get our dist to closest
if (30.0 < distanceToClosest && distanceToClosest < (30.0 + ((5.0 / 2.24) * Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value))))
{
if (al.LanePolygon != null)
{
ok = this.VehicleExistsInsidePolygon(va, al.LanePolygon, vs);
}
else
{
ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
}
}
else if (distanceToClosest <= 30.0)
{
if (al.LanePolygon != null)
{
#warning Darpa is an asshole
if (!va.IsStopped && this.VehicleAllInsidePolygon(va, al.LanePolygon, vs))
{
ok = true;
}
else
{
if (va.IsStopped)
{
// check vehicle is in a safety zone
bool isInSafety = false;
try
{
foreach (ArbiterIntersection ai in CoreCommon.RoadNetwork.ArbiterIntersections.Values)
{
if (ai.IntersectionPolygon.IsInside(va.ClosestPosition))
{
isInSafety = true;
}
}
foreach (ArbiterSafetyZone asz in al.SafetyZones)
{
if (asz.IsInSafety(va.ClosestPosition))
{
isInSafety = true;
}
}
}
catch (Exception) { }
if (isInSafety)
{
if (!this.VehiclePassableInPolygon(al, va, al.LanePolygon, vs, conv1))
{
ok = true;
}
}
else
{
if (!this.VehiclePassableInPolygon(al, va, al.LanePolygon, vs, conv))
{
ok = true;
}
}
}
else
{
if (!this.VehiclePassableInPolygon(al, va, al.LanePolygon, vs, conv2))
{
ok = true;
}
}
}
}
else
{
ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
}
}
else
{
ok = true;
}
}
#endregion
if (ok)
{
if (TacticalDirector.VehicleAreas.ContainsKey(ap.Key))
{
TacticalDirector.VehicleAreas[ap.Key].Add(va);
}
else
{
List <VehicleAgent> vas = new List <VehicleAgent>();
vas.Add(va);
TacticalDirector.VehicleAreas.Add(ap.Key, vas);
}
}
}
}
}
}
}
#endregion
#endregion
#region Interconnect Area Mappings
foreach (ArbiterInterconnect ai in CoreCommon.RoadNetwork.ArbiterInterconnects.Values)
{
if (ai.TurnPolygon.IsInside(va.StateMonitor.Observed.closestPoint))
{
if (TacticalDirector.VehicleAreas.ContainsKey(ai) && !TacticalDirector.VehicleAreas[ai].Contains(va))
{
TacticalDirector.VehicleAreas[ai].Add(va);
}
else
{
List <VehicleAgent> vas = new List <VehicleAgent>();
vas.Add(va);
TacticalDirector.VehicleAreas.Add(ai, vas);
}
// check if uturn
if (ai.TurnDirection == ArbiterTurnDirection.UTurn &&
ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint)
{
// get the lanes the uturn is a part of
ArbiterLane initialLane = ((ArbiterWaypoint)ai.InitialGeneric).Lane;
ArbiterLane targetLane = ((ArbiterWaypoint)ai.FinalGeneric).Lane;
if (TacticalDirector.VehicleAreas.ContainsKey(initialLane))
{
if (!TacticalDirector.VehicleAreas[initialLane].Contains(va))
{
TacticalDirector.VehicleAreas[initialLane].Add(va);
}
}
else
{
List <VehicleAgent> vas = new List <VehicleAgent>();
vas.Add(va);
TacticalDirector.VehicleAreas.Add(initialLane, vas);
}
if (TacticalDirector.VehicleAreas.ContainsKey(targetLane))
{
if (!TacticalDirector.VehicleAreas[targetLane].Contains(va))
{
TacticalDirector.VehicleAreas[targetLane].Add(va);
}
}
else
{
List <VehicleAgent> vas = new List <VehicleAgent>();
vas.Add(va);
TacticalDirector.VehicleAreas.Add(targetLane, vas);
}
}
}
}
#endregion
}
}
/// <summary>
/// Makes new parameterization for nav
/// </summary>
/// <param name="lane"></param>
/// <param name="lanePlan"></param>
/// <param name="speed"></param>
/// <param name="distance"></param>
/// <param name="stopType"></param>
/// <returns></returns>
public TravelingParameters NavStopParameterization(IFQMPlanable lane, RoadPlan roadPlan, double speed, double distance,
ArbiterWaypoint stopWaypoint, StopType stopType, VehicleState state)
{
// get min dist
double distanceCutOff = stopType == StopType.StopLine ? CoreCommon.OperationslStopLineSearchDistance : CoreCommon.OperationalStopDistance;
#region Get Decorators
// turn direction default
ArbiterTurnDirection atd = ArbiterTurnDirection.Straight;
List <BehaviorDecorator> decorators = TurnDecorators.NoDecorators;
// check if need decorators
if (lane is ArbiterLane &&
stopWaypoint.Equals(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest) &&
roadPlan.BestPlan.laneWaypointOfInterest.IsExit &&
distance < 40.0)
{
if (roadPlan.BestPlan.laneWaypointOfInterest.BestExit == null)
{
ArbiterOutput.Output("NAV BUG: lanePlan.laneWaypointOfInterest.BestExit: FQM NavStopParameterization");
}
else
{
switch (roadPlan.BestPlan.laneWaypointOfInterest.BestExit.TurnDirection)
{
case ArbiterTurnDirection.Left:
decorators = TurnDecorators.LeftTurnDecorator;
atd = ArbiterTurnDirection.Left;
break;
case ArbiterTurnDirection.Right:
atd = ArbiterTurnDirection.Right;
decorators = TurnDecorators.RightTurnDecorator;
break;
case ArbiterTurnDirection.Straight:
atd = ArbiterTurnDirection.Straight;
decorators = TurnDecorators.NoDecorators;
break;
case ArbiterTurnDirection.UTurn:
atd = ArbiterTurnDirection.UTurn;
decorators = TurnDecorators.LeftTurnDecorator;
break;
}
}
}
else if (lane is SupraLane)
{
SupraLane sl = (SupraLane)lane;
double distToInterconnect = sl.DistanceBetween(state.Front, sl.Interconnect.InitialGeneric.Position);
if ((distToInterconnect > 0 && distToInterconnect < 40.0) || sl.ClosestComponent(state.Front) == SLComponentType.Interconnect)
{
switch (sl.Interconnect.TurnDirection)
{
case ArbiterTurnDirection.Left:
decorators = TurnDecorators.LeftTurnDecorator;
atd = ArbiterTurnDirection.Left;
break;
case ArbiterTurnDirection.Right:
atd = ArbiterTurnDirection.Right;
decorators = TurnDecorators.RightTurnDecorator;
break;
case ArbiterTurnDirection.Straight:
atd = ArbiterTurnDirection.Straight;
decorators = TurnDecorators.NoDecorators;
break;
case ArbiterTurnDirection.UTurn:
atd = ArbiterTurnDirection.UTurn;
decorators = TurnDecorators.LeftTurnDecorator;
break;
}
}
}
#endregion
#region Get Maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
SpeedCommand sc = new StopAtDistSpeedCommand(distance);
#region Distance Cutoff
// check if distance is less than cutoff
if (distance < distanceCutOff && stopType != StopType.EndOfLane)
{
// default behavior
Behavior b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtDistSpeedCommand(distance), new List <int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true));
// stopping so not using speed param
usingSpeed = false;
// exit is next
if (stopType == StopType.Exit)
{
// exit means stopping at a good exit in our current lane
IState nextState = new StoppingAtExitState(stopWaypoint.Lane, stopWaypoint, atd, true, roadPlan.BestPlan.laneWaypointOfInterest.BestExit, state.Timestamp, state.Front);
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
// stop line is left
else if (stopType == StopType.StopLine)
{
// determine if hte stop line is the best exit
bool isNavExit = roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Equals(stopWaypoint);
// get turn direction
atd = isNavExit ? atd : ArbiterTurnDirection.Straight;
// predetermine interconnect if best exit
ArbiterInterconnect desired = null;
if (isNavExit)
{
desired = roadPlan.BestPlan.laneWaypointOfInterest.BestExit;
}
else if (stopWaypoint.NextPartition != null && state.Front.DistanceTo(roadPlan.BestPlan.laneWaypointOfInterest.PointOfInterest.Position) > 25)
{
desired = stopWaypoint.NextPartition.ToInterconnect;
}
// set decorators
decorators = isNavExit ? decorators : TurnDecorators.NoDecorators;
// stop at the stop
IState nextState = new StoppingAtStopState(stopWaypoint.Lane, stopWaypoint, atd, isNavExit, desired);
b = new StayInLaneBehavior(stopWaypoint.Lane.LaneId, new StopAtLineSpeedCommand(), new List <int>(), lane.LanePath(), stopWaypoint.Lane.Width, stopWaypoint.Lane.NumberOfLanesLeft(state.Front, true), stopWaypoint.Lane.NumberOfLanesRight(state.Front, true));
m = new Maneuver(b, nextState, decorators, state.Timestamp);
sc = new StopAtLineSpeedCommand();
}
else if (stopType == StopType.LastGoal)
{
// stop at the last goal
IState nextState = new StayInLaneState(stopWaypoint.Lane, CoreCommon.CorePlanningState);
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// set speed
sc = new ScalarSpeedCommand(speed);
// check if lane
if (lane is ArbiterLane)
{
// get lane
ArbiterLane al = (ArbiterLane)lane;
// default behavior
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(speed), new List <int>(), al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
// standard behavior is fine for maneuver
m = new Maneuver(b, new StayInLaneState(al, CoreCommon.CorePlanningState), decorators, state.Timestamp);
}
// check if supra lane
else if (lane is SupraLane)
{
// get lane
SupraLane sl = (SupraLane)lane;
// get sl state
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
// get default beheavior
Behavior b = sisls.GetBehavior(new ScalarSpeedCommand(speed), state.Front, new List <int>());
// standard behavior is fine for maneuver
m = new Maneuver(b, sisls, decorators, state.Timestamp);
}
}
#endregion
#endregion
#region Parameterize
// create new params
TravelingParameters tp = new TravelingParameters();
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = distance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = speed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.SpeedCommand = sc;
tp.VehiclesToIgnore = new List <int>();
// return navigation params
return(tp);
#endregion
}
/// <summary>
/// Parameters to follow the forward vehicle
/// </summary>
/// <param name="lane"></param>
/// <param name="state"></param>
/// <returns></returns>
public TravelingParameters Follow(IFQMPlanable lane, VehicleState state, List <ArbiterWaypoint> ignorable)
{
// travelling parameters
TravelingParameters tp = new TravelingParameters();
// get control parameters
ForwardVehicleTrackingControl fvtc = GetControl(lane, state, ignorable);
this.ForwardControl = fvtc;
// initialize the parameters
tp.DistanceToGo = fvtc.xDistanceToGood;
tp.NextState = CoreCommon.CorePlanningState;
tp.RecommendedSpeed = fvtc.vFollowing;
tp.Type = TravellingType.Vehicle;
tp.Decorators = new List <BehaviorDecorator>();
// ignore the forward vehicles
tp.VehiclesToIgnore = this.VehiclesToIgnore;
#region Following Control
#region Immediate Stop
// need to stop immediately
if (fvtc.vFollowing == 0.0)
{
// speed command
SpeedCommand sc = new ScalarSpeedCommand(0.0);
tp.SpeedCommand = sc;
tp.UsingSpeed = true;
if (lane is ArbiterLane)
{
// standard path following behavior
ArbiterLane al = ((ArbiterLane)lane);
Behavior final = new StayInLaneBehavior(al.LaneId, sc, this.VehiclesToIgnore, al.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
final.Decorators = tp.Decorators;
tp.Behavior = final;
}
else
{
SupraLane sl = (SupraLane)lane;
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
Behavior final = sisls.GetBehavior(sc, state.Front, this.VehiclesToIgnore);
final.Decorators = tp.Decorators;
tp.Behavior = final;
}
}
#endregion
#region Stopping at Distance
// stop at distance
else if (fvtc.vFollowing < 0.7 &&
CoreCommon.Communications.GetVehicleSpeed().Value <= 2.24 &&
fvtc.xSeparation > fvtc.xAbsMin)
{
// speed command
SpeedCommand sc = new StopAtDistSpeedCommand(fvtc.xDistanceToGood);
tp.SpeedCommand = sc;
tp.UsingSpeed = false;
if (lane is ArbiterLane)
{
ArbiterLane al = (ArbiterLane)lane;
// standard path following behavior
Behavior final = new StayInLaneBehavior(al.LaneId, sc, this.VehiclesToIgnore, lane.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
final.Decorators = tp.Decorators;
tp.Behavior = final;
}
else
{
SupraLane sl = (SupraLane)lane;
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
Behavior final = sisls.GetBehavior(sc, state.Front, this.VehiclesToIgnore);
final.Decorators = tp.Decorators;
tp.Behavior = final;
}
}
#endregion
#region Normal Following
// else normal
else
{
// speed command
SpeedCommand sc = new ScalarSpeedCommand(fvtc.vFollowing);
tp.DistanceToGo = fvtc.xDistanceToGood;
tp.NextState = CoreCommon.CorePlanningState;
tp.RecommendedSpeed = fvtc.vFollowing;
tp.Type = TravellingType.Vehicle;
tp.UsingSpeed = true;
tp.SpeedCommand = sc;
if (lane is ArbiterLane)
{
ArbiterLane al = ((ArbiterLane)lane);
// standard path following behavior
Behavior final = new StayInLaneBehavior(al.LaneId, sc, this.VehiclesToIgnore, lane.LanePath(), al.Width, al.NumberOfLanesLeft(state.Front, true), al.NumberOfLanesRight(state.Front, true));
final.Decorators = tp.Decorators;
tp.Behavior = final;
}
else
{
SupraLane sl = (SupraLane)lane;
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
Behavior final = sisls.GetBehavior(sc, state.Front, this.VehiclesToIgnore);
final.Decorators = tp.Decorators;
tp.Behavior = final;
}
}
#endregion
#endregion
#region Check for Oncoming Vehicles
// check if need to add current lane oncoming vehicle decorator
if (false && this.CurrentVehicle.PassedDelayedBirth && fvtc.forwardOncoming && fvtc.xSeparation > TahoeParams.VL && fvtc.xSeparation < 30)
{
// check valid lane area
if (lane is ArbiterLane || ((SupraLane)lane).ClosestComponent(this.CurrentVehicle.ClosestPosition) == SLComponentType.Initial)
{
// get distance to and speed of the forward vehicle
double fvDistance = fvtc.xSeparation;
double fvSpeed = fvtc.vTarget;
// create the 5mph behavior
ScalarSpeedCommand updated = new ScalarSpeedCommand(2.24);
// set that we are using speed
tp.UsingSpeed = true;
tp.RecommendedSpeed = updated.Speed;
tp.DistanceToGo = fvtc.xSeparation;
// create the decorator
OncomingVehicleDecorator ovd = new OncomingVehicleDecorator(updated, fvDistance, fvSpeed);
// add the decorator
tp.Behavior.Decorators.Add(ovd);
tp.Decorators.Add(ovd);
}
}
#endregion
// set current
this.followingParameters = tp;
// return parameterization
return(tp);
}
public string PartitionIdString()
{
string final = "";
#region Standard Areas
List <AreaProbability> AreaProbabilities = new List <AreaProbability>();
Circle circ = new Circle(TahoeParams.T + 0.3, new Coordinates());
Polygon conv = circ.ToPolygon(32);
Circle circ1 = new Circle(TahoeParams.T + 0.6, new Coordinates());
Polygon conv1 = circ1.ToPolygon(32);
Circle circ2 = new Circle(TahoeParams.T + 1.4, new Coordinates());
Polygon conv2 = circ2.ToPolygon(32);
for (int i = 0; i < this.trackedCluster.closestPartitions.Length; i++)
{
SceneEstimatorClusterPartition secp = this.trackedCluster.closestPartitions[i];
if (RemoraCommon.RoadNetwork.VehicleAreaMap.ContainsKey(secp.partitionID))
{
IVehicleArea iva = RemoraCommon.RoadNetwork.VehicleAreaMap[secp.partitionID];
bool found = false;
for (int j = 0; j < AreaProbabilities.Count; j++)
{
AreaProbability ap = AreaProbabilities[j];
if (ap.Key.Equals(iva))
{
ap.Value = ap.Value + secp.probability;
found = true;
}
}
if (!found)
{
AreaProbabilities.Add(new AreaProbability(iva, secp.probability));
}
}
else
{
RemoraOutput.WriteLine("Remora caught exception, partition: " + secp.partitionID + " not found in Vehicle Area Map", OutputType.Remora);
}
}
if (AreaProbabilities.Count > 0)
{
double rP = 0.0;
foreach (AreaProbability ap in AreaProbabilities)
{
if (ap.Key is ArbiterLane)
{
rP += ap.Value;
}
}
if (rP > 0.1)
{
foreach (AreaProbability ap in AreaProbabilities)
{
if (ap.Key is ArbiterLane)
{
// proabbility says in area
double vP = ap.Value / rP;
if (vP > 0.3)
{
#region Check if obstacle enough in area
bool ok = false;
if (ap.Key is ArbiterLane)
{
VehicleState vs = RemoraCommon.Communicator.GetVehicleState();
ArbiterLane al = (ArbiterLane)ap.Key;
Coordinates closest = this.ClosestPointToLine(al.LanePath(), vs).Value;
// dist to closest
double distanceToClosest = vs.Front.DistanceTo(closest);
// get our dist to closest
if (30.0 < distanceToClosest && distanceToClosest < (30.0 + ((5.0 / 2.24) * Math.Abs(RemoraCommon.Communicator.GetVehicleSpeed().Value))))
{
if (al.LanePolygon != null)
{
ok = this.VehicleExistsInsidePolygon(al.LanePolygon, vs);
}
else
{
ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
}
}
else if (distanceToClosest <= 30.0)
{
if (al.LanePolygon != null)
{
if (!this.trackedCluster.isStopped && this.VehicleAllInsidePolygon(al.LanePolygon, vs))
{
ok = true;
}
else
{
if (this.trackedCluster.isStopped)
{
bool isInSafety = false;
foreach (ArbiterIntersection ai in RemoraCommon.RoadNetwork.ArbiterIntersections.Values)
{
if (ai.IntersectionPolygon.IsInside(this.trackedCluster.closestPoint))
{
isInSafety = true;
}
}
foreach (ArbiterSafetyZone asz in al.SafetyZones)
{
if (asz.IsInSafety(this.trackedCluster.closestPoint))
{
isInSafety = true;
}
}
if (isInSafety)
{
if (!this.VehiclePassableInPolygon(al, al.LanePolygon, vs, conv1))
{
ok = true;
}
}
else
{
if (!this.VehiclePassableInPolygon(al, al.LanePolygon, vs, conv))
{
ok = true;
}
}
}
else
{
if (!this.VehiclePassableInPolygon(al, al.LanePolygon, vs, conv2))
{
ok = true;
}
}
}
}
else
{
ok = al.LanePath().GetClosestPoint(closest).Location.DistanceTo(closest) < al.Width / 2.0;
}
}
else
{
ok = true;
}
if (ok)
{
final = final + ap.Key.ToString() + ": " + vP.ToString("F4") + "\n";
}
}
#endregion
}
}
}
}
}
#endregion
#region Interconnect Area Mappings
foreach (ArbiterInterconnect ai in RemoraCommon.RoadNetwork.ArbiterInterconnects.Values)
{
if (ai.TurnPolygon.IsInside(this.trackedCluster.closestPoint))
{
final = final + ai.ToString() + "\n";
if (ai.TurnDirection == ArbiterTurnDirection.UTurn && ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint)
{
// initial
ArbiterLane initialLane = ((ArbiterWaypoint)ai.InitialGeneric).Lane;
ArbiterLane targetLane = ((ArbiterWaypoint)ai.FinalGeneric).Lane;
final = final + "UTurn2Lane: " + initialLane.ToString() + " & " + targetLane.ToString() + "\n";
}
}
}
#endregion
#region Intersections
foreach (ArbiterIntersection aInt in RemoraCommon.RoadNetwork.ArbiterIntersections.Values)
{
if (aInt.IntersectionPolygon.IsInside(this.trackedCluster.closestPoint))
{
final = final + "Inter: " + aInt.ToString() + "\n";
}
}
#endregion
return(final);
}
/// <summary>
/// Generate the traveling parameterization for the desired behaivor
/// </summary>
/// <param name="lane"></param>
/// <param name="navStopSpeed"></param>
/// <param name="navStopDistance"></param>
/// <param name="navStop"></param>
/// <param name="navStopType"></param>
/// <param name="state"></param>
/// <returns></returns>
private TravelingParameters NavStopParameterization(ArbiterLane lane, double navStopSpeed, double navStopDistance, ArbiterWaypoint navStop, StopType navStopType, VehicleState state)
{
// get min dist
double distanceCutOff = CoreCommon.OperationalStopDistance;
// turn direction default
List <BehaviorDecorator> decorators = TurnDecorators.NoDecorators;
// create new params
TravelingParameters tp = new TravelingParameters();
#region Get Maneuver
Maneuver m = new Maneuver();
bool usingSpeed = true;
// get lane path
LinePath lp = lane.LanePath().Clone();
lp.Reverse();
#region Distance Cutoff
// check if distance is less than cutoff
if (navStopDistance < distanceCutOff)
{
// default behavior
tp.SpeedCommand = new StopAtDistSpeedCommand(navStopDistance);
Behavior b = new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(navStopDistance), new List <int>(), lp, lane.Width, lane.NumberOfLanesLeft(state.Front, false), lane.NumberOfLanesRight(state.Front, false));
// stopping so not using speed param
usingSpeed = false;
IState nextState = CoreCommon.CorePlanningState;
m = new Maneuver(b, nextState, decorators, state.Timestamp);
}
#endregion
#region Outisde Distance Envelope
// not inside distance envalope
else
{
// get lane
ArbiterLane al = lane;
// default behavior
tp.SpeedCommand = new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24));
Behavior b = new StayInLaneBehavior(al.LaneId, new ScalarSpeedCommand(Math.Min(navStopSpeed, 2.24)), new List <int>(), lp, al.Width, al.NumberOfLanesRight(state.Front, false), al.NumberOfLanesLeft(state.Front, false));
// standard behavior is fine for maneuver
m = new Maneuver(b, CoreCommon.CorePlanningState, decorators, state.Timestamp);
}
#endregion
#endregion
#region Parameterize
tp.Behavior = m.PrimaryBehavior;
tp.Decorators = m.PrimaryBehavior.Decorators;
tp.DistanceToGo = navStopDistance;
tp.NextState = m.PrimaryState;
tp.RecommendedSpeed = navStopSpeed;
tp.Type = TravellingType.Navigation;
tp.UsingSpeed = usingSpeed;
tp.VehiclesToIgnore = new List <int>();
// return navigation params
return(tp);
#endregion
}
public Behavior Resume(VehicleState currentState, double speed)
{
// get dist
double dist = lane.PartitionPath.DistanceBetween(lane.PartitionPath.GetClosest(currentPosition), lane.PartitionPath.GetClosest(waypoint.Position));
return(new StayInLaneBehavior(lane.LaneId, new StopAtDistSpeedCommand(dist), new List <int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(currentState.Position, true), lane.NumberOfLanesRight(currentState.Position, true)));
}
/// <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);
}
/// <summary>
/// Checks if vehicle is far enough away to leave
/// </summary>
public bool Clear(VehicleState ourState)
{
// if no current vehicle, the stuff is fine
if (currentVehicle == null)
{
return(true);
}
else
{
// check exit exists
if (this.exit != null)
{
// check not inside polygon
double distToExit;
if (this.exitPolygon.IsInside(this.currentVehicle.ClosestPointTo(this.exit.Position, ourState, out distToExit).Value))
{
if (!this.LargerExcessiveWaiting)
{
ArbiterOutput.Output("DLEM: " + this.lane.ToString() + " not clear, vehicle " + this.currentVehicle.ToString() + " in exit polygon" +
", speed valid: " + this.currentVehicle.StateMonitor.Observed.speedValid.ToString() +
", is stopped: " + this.currentVehicle.IsStopped);
return(false);
}
else
{
ArbiterOutput.Output("DLEM: " + this.lane.ToString() + " not clear, vehicle " + this.currentVehicle.ToString() + " in exit polygon" +
", speed valid: " + this.currentVehicle.StateMonitor.Observed.speedValid.ToString() +
", is stopped: " + this.currentVehicle.IsStopped + ", passed large excessive wait test so going");
return(true);
}
}
}
// get the point we are looking from
LinePath.PointOnPath referencePoint;
// reference from exit if exists
if (exit != null)
{
LinePath.PointOnPath pop = lane.LanePath().GetClosestPoint(exit.Position);
referencePoint = lane.LanePath().AdvancePoint(pop, 3.0);
}
// otherwise look from where we are closest
else
{
referencePoint = lane.LanePath().GetClosestPoint(ourState.Front);
}
// vehicle point
LinePath.PointOnPath vehiclePoint = lane.LanePath().GetClosestPoint(currentVehicle.ClosestPosition);
// distance
double distance = lane.LanePath().DistanceBetween(vehiclePoint, referencePoint);
if (distance < 30 && (!this.currentVehicle.StateMonitor.Observed.speedValid || !this.currentVehicle.IsStopped))
{
ArbiterOutput.Output("DLEM: " + this.lane.ToString() + " not clear, moving vehicle " + this.currentVehicle.ToString() + " within 30m of intersection" +
", speed valid: " + this.currentVehicle.StateMonitor.Observed.speedValid.ToString() +
", is stopped: " + this.currentVehicle.IsStopped);
return(false);
}
else if (distance > 0)
{
// vehicles speed
double vehicleSpeed = this.currentVehicle.StateMonitor.Observed.speedValid ? Math.Abs(currentVehicle.Speed) : lane.Way.Segment.SpeedLimits.MaximumSpeed;
if (!this.currentVehicle.StateMonitor.Observed.speedValid)
{
ArbiterOutput.Output("DLEM: " + this.lane.ToString() + ", vehicle: " + this.currentVehicle.ToString() + " speed not valid, set to: " + lane.Way.Segment.SpeedLimits.MaximumSpeed.ToString("F2"));
}
// check time
double time = vehicleSpeed != 0 ? distance / vehicleSpeed : 777;
// clear if time greater than 7.0 seconds
if (time > 8.0)
{
ArbiterOutput.Output("DLEM: " + this.lane.ToString() + " clear, vehicle " + this.currentVehicle.ToString() +
", speed valid: " + this.currentVehicle.StateMonitor.Observed.speedValid.ToString() +
", speed: " + this.currentVehicle.Speed.ToString("F2") +
", time to collision: " + time.ToString("F2") + " > 8");
return(true);
}
else
{
ArbiterOutput.Output("DLEM: " + this.lane.ToString() + " NOT clear, vehicle " + this.currentVehicle.ToString() +
", speed valid: " + this.currentVehicle.StateMonitor.Observed.speedValid.ToString() +
", speed: " + this.currentVehicle.Speed.ToString("F2") +
", time to collision: " + time.ToString("F2") + " < 8");
return(false);
}
}
else
{
ArbiterOutput.Output("Exception: distance to vehicle negative in dlem is clear: " + distance.ToString("F3"));
return(false);
}
}
}
/// <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
}
public Behavior Resume(VehicleState currentState, double speed)
{
return(new UTurnBehavior(Polygon, TargetLane.LanePath(), TargetLane.LaneId, new ScalarSpeedCommand(2.0)));
}
public UrbanChallenge.Behaviors.Behavior Resume(VehicleState currentState, double speed)
{
return(new StayInLaneBehavior(lane.LaneId, new StopAtLineSpeedCommand(), new List <int>(), lane.LanePath(), lane.Width, lane.NumberOfLanesLeft(currentState.Position, true), lane.NumberOfLanesRight(currentState.Position, true)));
}
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>
/// Resume from pause
/// </summary>
/// <returns></returns>
public Behavior Resume(VehicleState currentState, double speed)
{
// new default stay in lane behavior
Behavior b = new StayInLaneBehavior(Lane.LaneId, new ScalarSpeedCommand(0.0), new int[] { }, Lane.LanePath(), Lane.Width, Lane.NumberOfLanesLeft(currentState.Position, true), Lane.NumberOfLanesRight(currentState.Position, true));
// set default blinkers
b.Decorators = this.DefaultStateDecorators;
// return behavior
return(b);
}
private void ShiftNetworkOkButton_Click(object sender, EventArgs e)
{
try
{
double east = double.Parse(this.ShiftNetworkEastTextBox.Text);
double north = double.Parse(this.ShiftNetworkNorthTextBox.Text);
Coordinates shift = new Coordinates(east, north);
foreach (IArbiterWaypoint iaw in roads.ArbiterWaypoints.Values)
{
iaw.Position = iaw.Position + shift;
}
// safety zone filter
DisplayObjectFilter f = delegate(IDisplayObject target)
{
// check if target is network object
if (target is ArbiterSafetyZone)
{
return(true);
}
else
{
return(false);
}
};
// remove safety zones
display.RemoveDisplayObjectType(f);
// new safety
roads.ArbiterSafetyZones = new List <ArbiterSafetyZone>();
// remove from network
List <IDisplayObject> displayObjects = new List <IDisplayObject>();
foreach (IDisplayObject ido in roads.DisplayObjects)
{
if (!f(ido))
{
displayObjects.Add(ido);
}
}
// remove lane safety zones, create new partition paths
foreach (ArbiterSegment asg in roads.ArbiterSegments.Values)
{
foreach (ArbiterLane al in asg.Lanes.Values)
{
al.SafetyZones = new List <ArbiterSafetyZone>();
// path segments of lane
List <IPathSegment> pathSegments = new List <IPathSegment>();
// loop
foreach (ArbiterLanePartition alPar in al.Partitions)
{
// make new segment
pathSegments.Add(new LinePathSegment(alPar.Initial.Position, alPar.Final.Position));
}
// generate lane partition path
Path partitionPath = new Path(pathSegments);
al.PartitionPath = partitionPath;
}
}
// recreate safety zones
foreach (IArbiterWaypoint iaw in roads.ArbiterWaypoints.Values)
{
if (iaw is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)iaw;
if (aw.IsStop)
{
ArbiterLane al = aw.Lane;
LinePath.PointOnPath end = al.GetClosestPoint(aw.Position);
double dist = -30;
LinePath.PointOnPath begin = al.LanePath().AdvancePoint(end, ref dist);
if (dist != 0)
{
EditorOutput.WriteLine(aw.ToString() + " safety zone too close to start of lane, setting start to start of lane");
begin = al.LanePath().StartPoint;
}
ArbiterSafetyZone asz = new ArbiterSafetyZone(al, end, begin);
asz.isExit = true;
asz.Exit = aw;
al.SafetyZones.Add(asz);
roads.DisplayObjects.Add(asz);
roads.ArbiterSafetyZones.Add(asz);
display.AddDisplayObject(asz);
}
}
}
// redraw
this.display.Invalidate();
// notify
EditorOutput.WriteLine("Shifted road network: east: " + east.ToString("F6") + ", north: " + north.ToString("F6"));
EditorOutput.WriteLine("Recomputed position-dependent types");
// close form
this.Close();
}
catch (Exception ex)
{
EditorOutput.WriteLine("Shift of road network failed: \n" + ex.ToString());
}
}
/// <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);
}
}