private void ProcessReverse()
{
double planningDistance = GetPlanningDistance();
// update the rndf path
RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curRndfPath = rndfPath.Transform(relTransfrom);
Console.WriteLine("cur rndf path count: " + curRndfPath.Count + ", " + curRndfPath.PathLength);
Console.WriteLine("cur rndf path zero point valid: " + curRndfPath.ZeroPoint.Valid + ", loc: " + curRndfPath.ZeroPoint.Location + ", index: " + curRndfPath.ZeroPoint.Index);
Console.WriteLine("planning dist: " + planningDistance + ", stop dist: " + GetSpeedCommandStopDistance());
// get the path in reverse
double dist = -(planningDistance + TahoeParams.RL + 2);
LinePath targetPath = curRndfPath.SubPath(curRndfPath.ZeroPoint, ref dist);
if (dist < 0)
{
targetPath.Add(curRndfPath[0] - curRndfPath.GetSegment(0).Vector.Normalize(-dist));
}
Console.WriteLine("target path count: " + targetPath.Count + ", " + targetPath.PathLength);
Console.WriteLine("target path zero point valud: " + targetPath.ZeroPoint.Valid);
// generate a box by shifting the path
LinePath leftBound = targetPath.ShiftLateral(rndfPathWidth / 2.0);
LinePath rightBound = targetPath.ShiftLateral(-rndfPathWidth / 2.0);
double leftStartDist, rightStartDist;
GetLaneBoundStartDists(targetPath, rndfPathWidth, out leftStartDist, out rightStartDist);
leftBound = leftBound.RemoveBefore(leftBound.AdvancePoint(leftBound.StartPoint, leftStartDist));
rightBound = rightBound.RemoveBefore(rightBound.AdvancePoint(rightBound.StartPoint, rightStartDist));
AddTargetPath(targetPath, 0.0025);
AddLeftBound(leftBound, false);
AddRightBound(rightBound, false);
avoidanceBasePath = targetPath;
double targetDist = Math.Max(targetPath.PathLength - (TahoeParams.RL + 2), planningDistance);
smootherBasePath = targetPath.SubPath(targetPath.StartPoint, targetDist);
settings.maxSpeed = GetMaxSpeed(null, LinePath.PointOnPath.Invalid);
settings.endingPositionFixed = true;
settings.endingPositionMax = rndfPathWidth / 2.0;
settings.endingPositionMin = -rndfPathWidth / 2.0;
settings.Options.reverse = true;
Services.UIService.PushLineList(smootherBasePath, curTimestamp, "subpath", true);
Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true);
Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true);
SmoothAndTrack(commandLabel, true);
}
/// <summary>
/// Turn information
/// </summary>
/// <param name="entry"></param>
/// <param name="finalPath"></param>
/// <param name="leftBound"></param>
/// <param name="rightBound"></param>
public static void ZoneTurnInfo(ArbiterInterconnect ai, ArbiterPerimeterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound)
{
//Coordinates centerVec = entry.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center - entry.Position;
Coordinates centerVec = ai.InterconnectPath[1] - ai.InterconnectPath[0];
centerVec = centerVec.Normalize(TahoeParams.VL);
finalPath = new LinePath(new Coordinates[] { entry.Position, entry.Position + centerVec });
leftBound = finalPath.ShiftLateral(TahoeParams.T * 2.0);
rightBound = finalPath.ShiftLateral(-TahoeParams.T * 2.0);
}
private static Polygon GenerateSimplePolygon(LinePath path, double width)
{
// here is default partition polygon
LinePath alplb = path.ShiftLateral(-width / 2.0);
LinePath alprb = path.ShiftLateral(width / 2.0);
alprb.Reverse();
List <Coordinates> alpdefaultPoly = alplb;
alpdefaultPoly.AddRange(alprb);
return(new Polygon(alpdefaultPoly));
}
public Polygon GetForwardPolygon(double distanceForward, double halfWidth)
{
LinePath lp1 = new LinePath(new Coordinates[] { this.Front, this.Front + this.Heading.Normalize(distanceForward) });
LinePath lpL = lp1.ShiftLateral(halfWidth);
LinePath lpR = lp1.ShiftLateral(-halfWidth);
List <Coordinates> polyCoors = new List <Coordinates>();
polyCoors.AddRange(lpL);
polyCoors.AddRange(lpR);
Polygon p = Polygon.GrahamScan(polyCoors);
return(p);
}
private void GenerateSafetyZone()
{
if (!safetyZoneBegin.Location.Equals(safetyZoneEnd.Location))
{
LinePath lp = new LinePath(new Coordinates[] { safetyZoneBegin.Location, safetyZoneEnd.Location });
LinePath lp1 = lp.ShiftLateral(-lane.Width / 2.0);
LinePath lp2 = lp.ShiftLateral(lane.Width / 2.0);
List <Coordinates> aszCoords = lp2;
aszCoords.AddRange(lp1);
this.SafetyPolygon = Polygon.GrahamScan(aszCoords);
}
else
{
}
}
private static Polygon GenerateSimplePartitionPolygon(ArbiterLanePartition alp, LinePath path, double width)
{
// here is default partition polygon
LinePath alplb = path.ShiftLateral(-width / 2.0);
LinePath alprb = path.ShiftLateral(width / 2.0);
alprb.Reverse();
List <Coordinates> alpdefaultPoly = alplb;
alpdefaultPoly.AddRange(alprb);
foreach (ArbiterUserWaypoint auw in alp.UserWaypoints)
{
alpdefaultPoly.Add(auw.Position);
}
return(Polygon.GrahamScan(alpdefaultPoly));
}
/// <summary>
/// Turn information
/// </summary>
/// <param name="entry"></param>
/// <param name="finalPath"></param>
/// <param name="leftBound"></param>
/// <param name="rightBound"></param>
public static void TurnInfo(ArbiterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound)
{
if (entry.NextPartition != null)
{
double distance = entry.NextPartition.Length;
// get left bound
rightBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position,
entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).RotateM90());
// get right bound
leftBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position,
entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).Rotate90());
ArbiterWaypoint current = entry.NextPartition.Final;
while (current.NextPartition != null && distance < 50)
{
distance += current.NextPartition.Length;
LineList rtTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position,
current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).RotateM90());
rightBound.Add(rtTemp[rtTemp.Count - 1]);
LineList ltTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position,
current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).Rotate90());
leftBound.Add(ltTemp[ltTemp.Count - 1]);
current = current.NextPartition.Final;
}
finalPath = entry.Lane.LanePath(entry, 50.0);
}
else
{
Coordinates final = entry.Position + entry.PreviousPartition.Vector().Normalize(TahoeParams.VL);
finalPath = new LinePath(new Coordinates[] { entry.Position, final });
LinePath lB = finalPath.ShiftLateral(entry.Lane.Width / 2.0);
LinePath rB = finalPath.ShiftLateral(-entry.Lane.Width / 2.0);
leftBound = new LineList(lB);
rightBound = new LineList(rB);
}
}
/// <summary>
/// Turn information
/// </summary>
/// <param name="entry"></param>
/// <param name="finalPath"></param>
/// <param name="leftBound"></param>
/// <param name="rightBound"></param>
public static void TurnInfo(ArbiterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound)
{
if (entry.NextPartition != null)
{
double distance = entry.NextPartition.Length;
// get left bound
rightBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position,
entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).RotateM90());
// get right bound
leftBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position,
entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).Rotate90());
ArbiterWaypoint current = entry.NextPartition.Final;
while (current.NextPartition != null && distance < 50)
{
distance += current.NextPartition.Length;
LineList rtTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position,
current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).RotateM90());
rightBound.Add(rtTemp[rtTemp.Count - 1]);
LineList ltTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position,
current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).Rotate90());
leftBound.Add(ltTemp[ltTemp.Count - 1]);
current = current.NextPartition.Final;
}
finalPath = entry.Lane.LanePath(entry, 50.0);
}
else
{
Coordinates final = entry.Position + entry.PreviousPartition.Vector().Normalize(TahoeParams.VL);
finalPath = new LinePath(new Coordinates[] { entry.Position, final });
LinePath lB = finalPath.ShiftLateral(entry.Lane.Width / 2.0);
LinePath rB = finalPath.ShiftLateral(-entry.Lane.Width / 2.0);
leftBound = new LineList(lB);
rightBound = new LineList(rB);
}
}
public void GenerateInterconnectPolygon(ArbiterInterconnect ai)
{
List <Coordinates> polyPoints = new List <Coordinates>();
try
{
// width
double width = 3.0;
if (ai.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
ai.TurnDirection == ArbiterTurnDirection.Straight ||
!(ai.InitialGeneric is ArbiterWaypoint) ||
!(ai.FinalGeneric is ArbiterWaypoint))
{
LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
polyPoints.AddRange(lp);
polyPoints.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(polyPoints);
if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
{
List <Coordinates> updatedPts = new List <Coordinates>();
LinePath interTmp = ai.InterconnectPath.Clone();
Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
lp = interTmp.ShiftLateral(TahoeParams.VL);
rp = interTmp.ShiftLateral(-TahoeParams.VL);
updatedPts.AddRange(lp);
updatedPts.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
}
}
else
{
// polygon points
List <Coordinates> interPoints = new List <Coordinates>();
// waypoint
ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;
// left and right path
LinePath leftPath = new LinePath();
LinePath rightPath = new LinePath();
// some initial points
LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
LinePath il = initialPath.ShiftLateral(width / 2.0);
LinePath ir = initialPath.ShiftLateral(-width / 2.0);
leftPath.Add(il[1]);
rightPath.Add(ir[1]);
// some final points
LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
LinePath fl = finalPath.ShiftLateral(width / 2.0);
LinePath fr = finalPath.ShiftLateral(-width / 2.0);
leftPath.Add(fl[0]);
rightPath.Add(fr[0]);
// initial and final paths
Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);
// get where the paths intersect and vector to normal path
Coordinates c;
iPath.Intersect(fPath, out c);
Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
Coordinates center = c + vector.Normalize((vector.Length / 2.0));
// get width expansion
Coordinates iVec = awI.PreviousPartition != null?awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);
double iRot = -iVec.ArcTan;
Coordinates fVec = awF.NextPartition != null?awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
double fDeg = fVec.ToDegrees();
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0;
// get inner point (small scale)
Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);
// get outer
Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);
if (ai.TurnDirection == ArbiterTurnDirection.Right)
{
rightPath.Insert(1, innerPoint);
ai.InnerCoordinates = rightPath;
leftPath.Reverse();
leftPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
else
{
leftPath.Insert(1, innerPoint);
ai.InnerCoordinates = leftPath;
rightPath.Reverse();
rightPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
}
}
catch (Exception e)
{
Console.WriteLine("error generating turn polygon: " + ai.ToString());
ai.TurnPolygon = ai.DefaultPoly();
}
}
/// <summary>
/// Plans over the zone
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockages"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan,
VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles,
SceneEstimatorUntrackedClusterCollection obstacles, List <ITacticalBlockage> blockages)
{
#region Zone Travelling State
if (planningState is ZoneTravelingState)
{
// check blockages
/*if (blockages != null && blockages.Count > 0 && blockages[0] is ZoneBlockage)
* {
* // 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);
* }*/
// cast state
ZoneState zs = (ZoneState)planningState;
// plan over state and zone
ZonePlan zp = (ZonePlan)navigationalPlan;
// check zone path does not exist
if (zp.RecommendedPath.Count < 2)
{
// zone startup again
ZoneStartupState zss = new ZoneStartupState(zs.Zone, true);
return(new Maneuver(new HoldBrakeBehavior(), zss, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
// final path seg
LinePath.PointOnPath endBack = zp.RecommendedPath.AdvancePoint(zp.RecommendedPath.EndPoint, -TahoeParams.VL);
LinePath lp = zp.RecommendedPath.SubPath(endBack, zp.RecommendedPath.EndPoint);
LinePath lB = lp.ShiftLateral(TahoeParams.T);
LinePath rB = lp.ShiftLateral(-TahoeParams.T);
// add to info
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound));
// get speed command
ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24);
// Behavior
Behavior b = new ZoneTravelingBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, zp.Zone.StayOutAreas.ToArray(),
sc, zp.RecommendedPath, lB, rB);
// maneuver
return(new Maneuver(b, CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
#region Parking State
else if (planningState is ParkingState)
{
// get state
ParkingState ps = (ParkingState)planningState;
// determine stay out areas to use
List <Polygon> stayOuts = new List <Polygon>();
foreach (Polygon p in ps.Zone.StayOutAreas)
{
if (!p.IsInside(ps.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(ps.ParkingSpot.Checkpoint.Position))
{
stayOuts.Add(p);
}
}
LinePath rB = ps.ParkingSpot.GetRightBound();
LinePath lB = ps.ParkingSpot.GetLeftBound();
// add to info
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound));
// create behavior
ZoneParkingBehavior zpb = new ZoneParkingBehavior(ps.Zone.ZoneId, ps.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(), new ScalarSpeedCommand(2.24),
ps.ParkingSpot.GetSpotPath(), lB, rB, ps.ParkingSpot.SpotId, 1.0);
// maneuver
return(new Maneuver(zpb, ps, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
#region Pulling Out State
else if (planningState is PullingOutState)
{
// get state
PullingOutState pos = (PullingOutState)planningState;
// plan over state and zone
ZonePlan zp = (ZonePlan)navigationalPlan;
// final path seg
Coordinates endVec = zp.RecommendedPath[0] - zp.RecommendedPath[1];
Coordinates endBack = zp.RecommendedPath[0] + endVec.Normalize(TahoeParams.VL * 2.0);
LinePath rp = new LinePath(new Coordinates[] { pos.ParkingSpot.Checkpoint.Position, pos.ParkingSpot.NormalWaypoint.Position,
zp.RecommendedPath[0], endBack });
LinePath lp = new LinePath(new Coordinates[] { zp.RecommendedPath[0], endBack });
LinePath lB = lp.ShiftLateral(TahoeParams.T * 2.0);
LinePath rB = lp.ShiftLateral(-TahoeParams.T * 2.0);
// add to info
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rp, ArbiterInformationDisplayObjectType.leftBound));
// determine stay out areas to use
List <Polygon> stayOuts = new List <Polygon>();
foreach (Polygon p in pos.Zone.StayOutAreas)
{
if (!p.IsInside(pos.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(pos.ParkingSpot.Checkpoint.Position))
{
stayOuts.Add(p);
}
}
// get speed command
ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24);
// Behavior
Behavior b = new ZoneParkingPullOutBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(),
sc, pos.ParkingSpot.GetSpotPath(), pos.ParkingSpot.GetLeftBound(), pos.ParkingSpot.GetRightBound(), pos.ParkingSpot.SpotId,
rp, lB, rB);
// maneuver
return(new Maneuver(b, pos, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
#region Zone Startup State
else if (planningState is ZoneStartupState)
{
// state
ZoneStartupState zss = (ZoneStartupState)planningState;
// get the zone
ArbiterZone az = zss.Zone;
// navigational edge
INavigableNode inn = CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId];
// check over all the parking spaces
foreach (ArbiterParkingSpot aps in az.ParkingSpots)
{
// inside both parts of spot
if ((vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position) && vehicleState.VehiclePolygon.IsInside(aps.Checkpoint.Position)) ||
(vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position)))
{
// want to just park in it again
return(new Maneuver(new HoldBrakeBehavior(), new ParkingState(az, aps), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
Polygon forwardPolygon = vehicleState.ForwardPolygon;
Polygon rearPolygon = vehicleState.RearPolygon;
Navigator nav = CoreCommon.Navigation;
List <ZoneNavigationEdgeSort> forwardForward = new List <ZoneNavigationEdgeSort>();
List <ZoneNavigationEdgeSort> reverseReverse = new List <ZoneNavigationEdgeSort>();
List <ZoneNavigationEdgeSort> perpendicularPerpendicular = new List <ZoneNavigationEdgeSort>();
List <ZoneNavigationEdgeSort> allEdges = new List <ZoneNavigationEdgeSort>();
List <ZoneNavigationEdgeSort> allEdgesNoLimits = new List <ZoneNavigationEdgeSort>();
foreach (NavigableEdge ne in az.NavigableEdges)
{
if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint))
{
// get distance to edge
LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position });
double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
// get direction along segment
DirectionAlong da = vehicleState.DirectionAlongSegment(lp);
// check dist reasonable
if (distTmp > TahoeParams.VL)
{
// zone navigation edge sort item
ZoneNavigationEdgeSort znes = new ZoneNavigationEdgeSort(distTmp, ne, lp);
// add to lists
if (da == DirectionAlong.Forwards &&
(forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position)))
{
forwardForward.Add(znes);
}
/*else if (da == DirectionAlong.Perpendicular &&
* !(forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position)) &&
* !(rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position)))
* perpendicularPerpendicular.Add(znes);
* else if (rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position))
* reverseReverse.Add(znes);*/
// add to all edges
allEdges.Add(znes);
}
}
}
// sort by distance
forwardForward.Sort();
reverseReverse.Sort();
perpendicularPerpendicular.Sort();
allEdges.Sort();
ZoneNavigationEdgeSort bestZnes = null;
for (int i = 0; i < allEdges.Count; i++)
{
// get line to initial
Line toInitial = new Line(vehicleState.Front, allEdges[i].edge.Start.Position);
Line toFinal = new Line(vehicleState.Front, allEdges[i].edge.End.Position);
bool intersects = false;
Coordinates[] interPts;
foreach (Polygon sop in az.StayOutAreas)
{
if (!intersects &&
(sop.Intersect(toInitial, out interPts) && sop.Intersect(toFinal, out interPts)))
{
intersects = true;
}
}
if (!intersects)
{
allEdges[i].zp = nav.PlanZone(az, allEdges[i].edge.End, inn);
allEdges[i].zp.Time += vehicleState.Front.DistanceTo(allEdges[i].lp.GetClosestPoint(vehicleState.Front).Location) / 2.24;
ZoneNavigationEdgeSort tmpZnes = allEdges[i];
if ((bestZnes == null && tmpZnes.zp.RecommendedPath.Count > 1) ||
(bestZnes != null && tmpZnes.zp.RecommendedPath.Count > 1 && tmpZnes.zp.Time < bestZnes.zp.Time))
{
bestZnes = tmpZnes;
}
}
if (i > allEdges.Count / 2 && bestZnes != null)
{
break;
}
}
if (bestZnes != null)
{
ArbiterOutput.Output("Found good edge to start in zone");
return(new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, bestZnes.edge), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
else
{
ArbiterOutput.Output("Could not find good edge to start, choosing random not blocked");
List <ZoneNavigationEdgeSort> okZnes = new List <ZoneNavigationEdgeSort>();
foreach (NavigableEdge tmpOk in az.NavigableEdges)
{
// get line to initial
LinePath edgePath = new LinePath(new Coordinates[] { tmpOk.Start.Position, tmpOk.End.Position });
double dist = edgePath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
ZoneNavigationEdgeSort tmpZnes = new ZoneNavigationEdgeSort(dist, tmpOk, edgePath);
tmpZnes.zp = nav.PlanZone(az, tmpZnes.edge.End, inn);
tmpZnes.zp.Time += vehicleState.Front.DistanceTo(tmpZnes.lp.GetClosestPoint(vehicleState.Front).Location) / 2.24;
if (tmpZnes.zp.RecommendedPath.Count >= 2)
{
okZnes.Add(tmpZnes);
}
}
if (okZnes.Count == 0)
{
okZnes = allEdges;
}
else
{
okZnes.Sort();
}
// get random close edge
Random rand = new Random();
int chosen = rand.Next(Math.Min(5, okZnes.Count));
// get closest edge not part of a parking spot, get on it
NavigableEdge closest = okZnes[chosen].edge; //null;
//double distance = Double.MaxValue;
/*foreach (NavigableEdge ne in az.NavigableEdges)
* {
* if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint))
* {
* // get distance to edge
* LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position });
* double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
* if (closest == null || distTmp < distance)
* {
* closest = ne;
* distance = distTmp;
* }
* }
* }*/
return(new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, closest), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
}
#endregion
}
public Polygon GetForwardPolygon(double distanceForward, double halfWidth)
{
LinePath lp1 = new LinePath(new Coordinates[] { this.Front, this.Front + this.Heading.Normalize(distanceForward) });
LinePath lpL = lp1.ShiftLateral(halfWidth);
LinePath lpR = lp1.ShiftLateral(-halfWidth);
List<Coordinates> polyCoors = new List<Coordinates>();
polyCoors.AddRange(lpL);
polyCoors.AddRange(lpR);
Polygon p = Polygon.GrahamScan(polyCoors);
return p;
}
/// <summary>
/// Plans over the zone
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockages"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan,
VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles,
SceneEstimatorUntrackedClusterCollection obstacles, List<ITacticalBlockage> blockages)
{
#region Zone Travelling State
if (planningState is ZoneTravelingState)
{
// check blockages
/*if (blockages != null && blockages.Count > 0 && blockages[0] is ZoneBlockage)
{
// 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);
}*/
// cast state
ZoneState zs = (ZoneState)planningState;
// plan over state and zone
ZonePlan zp = (ZonePlan)navigationalPlan;
// check zone path does not exist
if (zp.RecommendedPath.Count < 2)
{
// zone startup again
ZoneStartupState zss = new ZoneStartupState(zs.Zone, true);
return new Maneuver(new HoldBrakeBehavior(), zss, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// final path seg
LinePath.PointOnPath endBack = zp.RecommendedPath.AdvancePoint(zp.RecommendedPath.EndPoint, -TahoeParams.VL);
LinePath lp = zp.RecommendedPath.SubPath(endBack, zp.RecommendedPath.EndPoint);
LinePath lB = lp.ShiftLateral(TahoeParams.T);
LinePath rB = lp.ShiftLateral(-TahoeParams.T);
// add to info
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound));
// get speed command
ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24);
// Behavior
Behavior b = new ZoneTravelingBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, zp.Zone.StayOutAreas.ToArray(),
sc, zp.RecommendedPath, lB, rB);
// maneuver
return new Maneuver(b, CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#endregion
#region Parking State
else if (planningState is ParkingState)
{
// get state
ParkingState ps = (ParkingState)planningState;
// determine stay out areas to use
List<Polygon> stayOuts = new List<Polygon>();
foreach (Polygon p in ps.Zone.StayOutAreas)
{
if (!p.IsInside(ps.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(ps.ParkingSpot.Checkpoint.Position))
stayOuts.Add(p);
}
LinePath rB = ps.ParkingSpot.GetRightBound();
LinePath lB = ps.ParkingSpot.GetLeftBound();
// add to info
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound));
// create behavior
ZoneParkingBehavior zpb = new ZoneParkingBehavior(ps.Zone.ZoneId, ps.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(), new ScalarSpeedCommand(2.24),
ps.ParkingSpot.GetSpotPath(), lB, rB, ps.ParkingSpot.SpotId, 1.0);
// maneuver
return new Maneuver(zpb, ps, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#endregion
#region Pulling Out State
else if (planningState is PullingOutState)
{
// get state
PullingOutState pos = (PullingOutState)planningState;
// plan over state and zone
ZonePlan zp = (ZonePlan)navigationalPlan;
// final path seg
Coordinates endVec = zp.RecommendedPath[0] - zp.RecommendedPath[1];
Coordinates endBack = zp.RecommendedPath[0] + endVec.Normalize(TahoeParams.VL * 2.0);
LinePath rp = new LinePath(new Coordinates[]{pos.ParkingSpot.Checkpoint.Position, pos.ParkingSpot.NormalWaypoint.Position,
zp.RecommendedPath[0], endBack});
LinePath lp = new LinePath(new Coordinates[]{zp.RecommendedPath[0], endBack});
LinePath lB = lp.ShiftLateral(TahoeParams.T * 2.0);
LinePath rB = lp.ShiftLateral(-TahoeParams.T * 2.0);
// add to info
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lB, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rB, ArbiterInformationDisplayObjectType.rightBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(rp, ArbiterInformationDisplayObjectType.leftBound));
// determine stay out areas to use
List<Polygon> stayOuts = new List<Polygon>();
foreach (Polygon p in pos.Zone.StayOutAreas)
{
if (!p.IsInside(pos.ParkingSpot.NormalWaypoint.Position) && !p.IsInside(pos.ParkingSpot.Checkpoint.Position))
stayOuts.Add(p);
}
// get speed command
ScalarSpeedCommand sc = new ScalarSpeedCommand(2.24);
// Behavior
Behavior b = new ZoneParkingPullOutBehavior(zp.Zone.ZoneId, zp.Zone.Perimeter.PerimeterPolygon, stayOuts.ToArray(),
sc, pos.ParkingSpot.GetSpotPath(), pos.ParkingSpot.GetLeftBound(), pos.ParkingSpot.GetRightBound(), pos.ParkingSpot.SpotId,
rp, lB, rB);
// maneuver
return new Maneuver(b, pos, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#endregion
#region Zone Startup State
else if (planningState is ZoneStartupState)
{
// state
ZoneStartupState zss = (ZoneStartupState)planningState;
// get the zone
ArbiterZone az = zss.Zone;
// navigational edge
INavigableNode inn = CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId];
// check over all the parking spaces
foreach (ArbiterParkingSpot aps in az.ParkingSpots)
{
// inside both parts of spot
if ((vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position) && vehicleState.VehiclePolygon.IsInside(aps.Checkpoint.Position)) ||
(vehicleState.VehiclePolygon.IsInside(aps.NormalWaypoint.Position)))
{
// want to just park in it again
return new Maneuver(new HoldBrakeBehavior(), new ParkingState(az, aps), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
Polygon forwardPolygon = vehicleState.ForwardPolygon;
Polygon rearPolygon = vehicleState.RearPolygon;
Navigator nav = CoreCommon.Navigation;
List<ZoneNavigationEdgeSort> forwardForward = new List<ZoneNavigationEdgeSort>();
List<ZoneNavigationEdgeSort> reverseReverse = new List<ZoneNavigationEdgeSort>();
List<ZoneNavigationEdgeSort> perpendicularPerpendicular = new List<ZoneNavigationEdgeSort>();
List<ZoneNavigationEdgeSort> allEdges = new List<ZoneNavigationEdgeSort>();
List<ZoneNavigationEdgeSort> allEdgesNoLimits = new List<ZoneNavigationEdgeSort>();
foreach (NavigableEdge ne in az.NavigableEdges)
{
if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint))
{
// get distance to edge
LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position });
double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
// get direction along segment
DirectionAlong da = vehicleState.DirectionAlongSegment(lp);
// check dist reasonable
if (distTmp > TahoeParams.VL)
{
// zone navigation edge sort item
ZoneNavigationEdgeSort znes = new ZoneNavigationEdgeSort(distTmp, ne, lp);
// add to lists
if (da == DirectionAlong.Forwards &&
(forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position)))
forwardForward.Add(znes);
/*else if (da == DirectionAlong.Perpendicular &&
!(forwardPolygon.IsInside(ne.Start.Position) || forwardPolygon.IsInside(ne.End.Position)) &&
!(rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position)))
perpendicularPerpendicular.Add(znes);
else if (rearPolygon.IsInside(ne.Start.Position) || rearPolygon.IsInside(ne.End.Position))
reverseReverse.Add(znes);*/
// add to all edges
allEdges.Add(znes);
}
}
}
// sort by distance
forwardForward.Sort();
reverseReverse.Sort();
perpendicularPerpendicular.Sort();
allEdges.Sort();
ZoneNavigationEdgeSort bestZnes = null;
for (int i = 0; i < allEdges.Count; i++)
{
// get line to initial
Line toInitial = new Line(vehicleState.Front, allEdges[i].edge.Start.Position);
Line toFinal = new Line(vehicleState.Front, allEdges[i].edge.End.Position);
bool intersects = false;
Coordinates[] interPts;
foreach (Polygon sop in az.StayOutAreas)
{
if (!intersects &&
(sop.Intersect(toInitial, out interPts) && sop.Intersect(toFinal, out interPts)))
intersects = true;
}
if (!intersects)
{
allEdges[i].zp = nav.PlanZone(az, allEdges[i].edge.End, inn);
allEdges[i].zp.Time += vehicleState.Front.DistanceTo(allEdges[i].lp.GetClosestPoint(vehicleState.Front).Location) / 2.24;
ZoneNavigationEdgeSort tmpZnes = allEdges[i];
if ((bestZnes == null && tmpZnes.zp.RecommendedPath.Count > 1) ||
(bestZnes != null && tmpZnes.zp.RecommendedPath.Count > 1 && tmpZnes.zp.Time < bestZnes.zp.Time))
bestZnes = tmpZnes;
}
if (i > allEdges.Count / 2 && bestZnes != null)
break;
}
if (bestZnes != null)
{
ArbiterOutput.Output("Found good edge to start in zone");
return new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, bestZnes.edge), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
ArbiterOutput.Output("Could not find good edge to start, choosing random not blocked");
List<ZoneNavigationEdgeSort> okZnes = new List<ZoneNavigationEdgeSort>();
foreach (NavigableEdge tmpOk in az.NavigableEdges)
{
// get line to initial
LinePath edgePath = new LinePath(new Coordinates[] { tmpOk.Start.Position, tmpOk.End.Position });
double dist = edgePath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
ZoneNavigationEdgeSort tmpZnes = new ZoneNavigationEdgeSort(dist, tmpOk, edgePath);
tmpZnes.zp = nav.PlanZone(az, tmpZnes.edge.End, inn);
tmpZnes.zp.Time += vehicleState.Front.DistanceTo(tmpZnes.lp.GetClosestPoint(vehicleState.Front).Location) / 2.24;
if (tmpZnes.zp.RecommendedPath.Count >= 2)
okZnes.Add(tmpZnes);
}
if (okZnes.Count == 0)
okZnes = allEdges;
else
okZnes.Sort();
// get random close edge
Random rand = new Random();
int chosen = rand.Next(Math.Min(5, okZnes.Count));
// get closest edge not part of a parking spot, get on it
NavigableEdge closest = okZnes[chosen].edge;//null;
//double distance = Double.MaxValue;
/*foreach (NavigableEdge ne in az.NavigableEdges)
{
if (!(ne.End is ArbiterParkingSpotWaypoint) && !(ne.Start is ArbiterParkingSpotWaypoint))
{
// get distance to edge
LinePath lp = new LinePath(new Coordinates[] { ne.Start.Position, ne.End.Position });
double distTmp = lp.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
if (closest == null || distTmp < distance)
{
closest = ne;
distance = distTmp;
}
}
}*/
return new Maneuver(new HoldBrakeBehavior(), new ZoneOrientationState(az, closest), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
}
#endregion
}
public static Polygon PartitionPolygon(ArbiterLanePartition alp)
{
if (alp.Initial.PreviousPartition != null &&
alp.Final.NextPartition != null &&
alp.Length < 30.0 && alp.Length > 4.0)
{
// get partition turn direction
ArbiterTurnDirection pTD = PartitionTurnDirection(alp);
// check if angles of previous and next are such that not straight through
if (pTD != ArbiterTurnDirection.Straight)
{
// get partition poly
ArbiterInterconnect tmpAi = alp.ToInterconnect;
tmpAi.TurnDirection = pTD;
GenerateInterconnectPolygon(tmpAi);
Polygon pPoly = tmpAi.TurnPolygon;
// here is default partition polygon
LinePath alplb = alp.PartitionPath.ShiftLateral(-alp.Lane.Width / 2.0);
LinePath alprb = alp.PartitionPath.ShiftLateral(alp.Lane.Width / 2.0);
alprb.Reverse();
List <Coordinates> alpdefaultPoly = alplb;
alpdefaultPoly.AddRange(alprb);
// get full poly
pPoly.AddRange(alpdefaultPoly);
pPoly = Polygon.GrahamScan(pPoly);
return(pPoly);
}
}
else if (alp.Length >= 30)
{
Polygon pBase = GenerateSimplePartitionPolygon(alp, alp.PartitionPath, alp.Lane.Width);
if (alp.Initial.PreviousPartition != null && Math.Abs(FinalIntersectionAngle(alp.Initial.PreviousPartition)) > 15)
{
// initial portion
Coordinates i1 = alp.Initial.Position - alp.Initial.PreviousPartition.Vector().Normalize(15.0);
Coordinates i2 = alp.Initial.Position;
Coordinates i3 = i2 + alp.Vector().Normalize(15.0);
LinePath il12 = new LinePath(new Coordinates[] { i1, i2 });
LinePath il23 = new LinePath(new Coordinates[] { i2, i3 });
LinePath il13 = new LinePath(new Coordinates[] { i1, i3 });
Coordinates iCC = il13.GetClosestPoint(i2).Location;
if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0)
{
il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
else
{
il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC);
iCCP = il13.AdvancePoint(iCCP, -10.0);
il13 = il13.SubPath(iCCP, 20.0);
Polygon iBase = GenerateSimplePolygon(il23, alp.Lane.Width);
iBase.Add(il13[1]);
Polygon iP = Polygon.GrahamScan(iBase);
pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP }));
}
if (alp.Final.NextPartition != null && Math.Abs(FinalIntersectionAngle(alp)) > 15)
{
// initial portion
Coordinates i1 = alp.Final.Position - alp.Vector().Normalize(15.0);
Coordinates i2 = alp.Final.Position;
Coordinates i3 = i2 + alp.Final.NextPartition.Vector().Normalize(15.0);
LinePath il12 = new LinePath(new Coordinates[] { i1, i2 });
LinePath il23 = new LinePath(new Coordinates[] { i2, i3 });
LinePath il13 = new LinePath(new Coordinates[] { i1, i3 });
Coordinates iCC = il13.GetClosestPoint(i2).Location;
if (GeneralToolkit.TriangleArea(i1, i2, i3) < 0)
{
il13 = il13.ShiftLateral(iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
else
{
il13 = il13.ShiftLateral(-iCC.DistanceTo(i2) + alp.Lane.Width / 2.0);
}
LinePath.PointOnPath iCCP = il13.GetClosestPoint(iCC);
iCCP = il13.AdvancePoint(iCCP, -10.0);
il13 = il13.SubPath(iCCP, 20.0);
Polygon iBase = GenerateSimplePolygon(il12, alp.Lane.Width);
iBase.Add(il13[0]);
Polygon iP = Polygon.GrahamScan(iBase);
pBase = PolygonToolkit.PolygonUnion(new List <Polygon>(new Polygon[] { pBase, iP }));
}
return(pBase);
}
// fall out
return(null);
}
public static bool TestBlockage(IList <Polygon> obstaclePolygons, LinePath leftBound, LinePath rightBound)
{
try
{
double expandDist = TahoeParams.T / 2.0 + 0.3;
Circle tahoeCircle = new Circle(expandDist, Coordinates.Zero);
Polygon tahoePoly = tahoeCircle.ToPolygon(24);
List <BlockageData> obstacles = new List <BlockageData>(obstaclePolygons.Count);
foreach (Polygon poly in obstaclePolygons)
{
Polygon convolvedPoly = null;
try
{
convolvedPoly = Polygon.ConvexMinkowskiConvolution(tahoePoly, poly);
}
catch (Exception)
{
// minkowski convolution failed, just expand that stuff with the gaheezy inflate method
convolvedPoly = poly.Inflate(expandDist);
}
// add the entry to the obstacle collection
BlockageData data = new BlockageData(convolvedPoly);
obstacles.Add(data);
}
// shrink in the lanes by a half tahoe width
leftBound = leftBound.ShiftLateral(-TahoeParams.T / 2.0);
rightBound = rightBound.ShiftLateral(-TahoeParams.T / 2.0);
Queue <BlockageData> testQueue = new Queue <BlockageData>();
foreach (BlockageData obs in obstacles)
{
if (obs.convolvedPolygon.DoesIntersect(leftBound))
{
// check if this hits the right bound
if (obs.convolvedPolygon.DoesIntersect(rightBound))
{
// this extends across the entire lane, the segment is blocked
return(true);
}
else
{
testQueue.Enqueue(obs);
obs.searchMark = true;
}
}
}
while (testQueue.Count > 0)
{
BlockageData obs = testQueue.Dequeue();
foreach (BlockageData neighbor in obstacles)
{
if (!neighbor.searchMark && Polygon.TestConvexIntersection(obs.convolvedPolygon, neighbor.convolvedPolygon))
{
if (neighbor.convolvedPolygon.DoesIntersect(rightBound))
{
return(true);
}
testQueue.Enqueue(neighbor);
neighbor.searchMark = true;
}
}
}
return(false);
}
catch (Exception) { }
return(false);
}
private ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp)
{
// check the lane model probability
if (laneModel.Probability < lane_probability_reject_threshold)
{
// we're rejecting this, just return a path lane model
return(new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth));
}
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);
// iterate through the waypoints in the RNDF path and project onto the lane model
// the first one that is over the threshold, we consider the waypoint before as a potential ending point
LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath();
// flag indicating if any of the waypoint tests failed because of the devation was too high
bool anyDeviationTooHigh = false;
// number of waypoints accepted
int numWaypointsAccepted = 0;
// get the vehicle's position on the rndf path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the vehicle's position on the lane model
LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint;
// get the last point we want to consider on the lane model
LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist);
// start walking forward through the waypoints on the rndf path
// this loop will implicitly exit when we're past the end of the lane model as the waypoints
// will stop being close to the lane model (GetClosestPoint returns the end point if we're past the
// end of the path)
for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++)
{
// get the waypoint
Coordinates rndfWaypoint = rndfPath[i];
// get the closest point on the lane model
LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint);
// compute the distance between the two
double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location);
// if this is above the deviation threshold, leave the loop
if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint)
{
// if we're at the end of the lane model path, we don't want to consider this a rejection
if (laneModelClosestPoint < laneModelFarthestPoint)
{
// mark that at least on deviation was too high
anyDeviationTooHigh = true;
}
break;
}
// increment the number of waypoint accepted
numWaypointsAccepted++;
// update the end point of where we're valid as the local road model was OK up to this point
laneModelDeviationEndPoint = laneModelClosestPoint;
}
// go through and figure out how far out the variance is within tolerance
LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath();
// walk forward from this point until the end of the lane mode path
for (int i = laneModelZeroPoint.Index + 1; i < laneModelPath.Count; i++)
{
// check if we're within the variance toleration
if (laneModel.LaneYVariance[i] <= y_var_reject_threshold)
{
// we are, update the point on path
laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i);
}
else
{
// we are out of tolerance, break out of the loop
break;
}
}
// now figure out everything out
// determine waypoint rejection status
WaypointRejectionResult waypointRejectionResult;
if (laneModelDeviationEndPoint.Valid)
{
// if the point is valid, that we had at least one waypoint that was ok
// check if any waypoints were rejected
if (anyDeviationTooHigh)
{
// some waypoint was ok, so we know that at least one waypoint was accepted
waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted;
}
else
{
// no waypoint triggered a rejection, but at least one was good
waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted;
}
}
else
{
// the point is not valid, so we either had no waypoints or we had all rejections
if (anyDeviationTooHigh)
{
// the first waypoint was rejected, so all are rejected
waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected;
}
else
{
// the first waypoint (if any) was past the end of the lane model
waypointRejectionResult = WaypointRejectionResult.NoWaypoints;
}
}
// criteria for determining if this path is valid:
// - if some or all waypoints were accepted, than this is probably a good path
// - if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted
// - if there were no waypoints, this is a potentially dangerous situation since we can't reject
// or confirm the local road model. for now, we'll assume that it is correct but this may need to change
// if we start handling intersections in this framework
// - if all waypoints were rejected, than we don't use the local road model
// - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of
// the lane model was in tolerance
// now build out the lane model
ILaneModel finalLaneModel;
// check if we rejected all waypoints or no lane model points satisified the variance threshold
if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid)
{
// want to just use the path lane model
finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
}
else
{
// we'll use the lane model
// need to build up the center line as well as left and right bounds
// to build up the center line, use the lane model as far as we feel comfortable (limited by either variance
// or by rejections) and then use the rndf lane after that.
LinePath centerLine = new LinePath();
// figure out the max distance
// if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model
if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints)
{
laneModelDeviationEndPoint = laneModelFarthestPoint;
}
// figure out the closer of the end points
LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint;
bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint;
// add the lane model to the center line
centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint));
// create a list to hold the width expansion values
List <double> widthValue = new List <double>();
// make the width expansion values the width of the path plus the 1-sigma values
for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index + centerLine.Count; i++)
{
widthValue.Add(laneModel.Width / 2.0 + Math.Sqrt(laneModel.LaneYVariance[i]));
}
// now figure out how to add the rndf path
// get the projection of the lane model end point on the rndf path
LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location);
// if the closest point is past the end of rndf path, then we don't want to tack anything on
if (rndfPathStartPoint != rndfPath.EndPoint)
{
// get the last segment of the new center line
Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector;
// get the last point of the new center line
Coordinates laneModelEndLoc = laneModelEndPoint.Location;
// now figure out the distance to the next waypoint
LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath();
// figure out if we're ending at a waypoint or not
if (endAtWaypoint)
{
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 1);
// if the distance from the start point to the next point is less than rndf_dist_min, then
// use the waypont after
double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint);
if (dist < rndf_dist_min)
{
if (rndfPathStartPoint.Index < rndfPath.Count - 2)
{
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2);
}
else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min)
{
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
else
{
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min * 2);
}
}
}
else
{
// track the last angle we had
double lastAngle = double.NaN;
// walk down the rndf path until we find a valid point
for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step)
{
// advance from the start point by dist
double distTemp = dist;
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp);
// if the distTemp is > 0, then we're past the end of the path
if (distTemp > 0)
{
// if we're immediately past the end, we don't want to tack anything on
if (dist == rndf_dist_min)
{
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
break;
}
// check the angle made by the last segment of center line and the segment
// formed between the end point of the center line and this new point
double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location - laneModelEndLoc).Normalize()));
// check if the angle satisfies the threshold or we're increasing the angle
if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle))
{
// break out of the loop, we're done searching
break;
}
lastAngle = angle;
}
}
// tack on the rndf starting at next point going to the end
if (rndfNextPoint.Valid)
{
LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint);
centerLine.AddRange(subPath);
// insert the lane model end point into the sub path
subPath.Insert(0, laneModelEndLoc);
// get the angles
List <Pair <int, double> > angles = subPath.GetIntersectionAngles(0, subPath.Count - 1);
// add the width of the path inflated by the angles
for (int i = 0; i < angles.Count; i++)
{
// calculate the width expansion factor
// 90 deg, 3x width
// 45 deg, 1.5x width
// 0 deg, 1x width
double widthFactor = Math.Pow(angles[i].Right / (Math.PI / 2.0), 2) * 2 + 1;
// add the width value
widthValue.Add(widthFactor * laneModel.Width / 2);
}
// add the final width
widthValue.Add(laneModel.Width / 2);
}
// set the rndf path start point to be the point we used
rndfPathStartPoint = rndfNextPoint;
}
// for now, calculate the left and right bounds the same way we do for the path lane model
// TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty
LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray());
// get the next shifts
for (int i = 0; i < widthValue.Count; i++)
{
widthValue[i] = -widthValue[i];
}
LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray());
// build the final lane model
finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp);
}
SendLaneModelToUI(finalLaneModel, rndfPathTimestamp);
// output the fit result
return(finalLaneModel);
}
/// <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);
}
public void GenerateInterconnectPolygon(ArbiterInterconnect ai)
{
List<Coordinates> polyPoints = new List<Coordinates>();
try
{
// width
double width = 3.0;
if (ai.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.InitialGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint aw = (ArbiterWaypoint)ai.FinalGeneric;
width = width < aw.Lane.Width ? aw.Lane.Width : width;
}
if (ai.TurnDirection == ArbiterTurnDirection.UTurn ||
ai.TurnDirection == ArbiterTurnDirection.Straight ||
!(ai.InitialGeneric is ArbiterWaypoint) ||
!(ai.FinalGeneric is ArbiterWaypoint))
{
LinePath lp = ai.InterconnectPath.ShiftLateral(width / 2.0);
LinePath rp = ai.InterconnectPath.ShiftLateral(-width / 2.0);
polyPoints.AddRange(lp);
polyPoints.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(polyPoints);
if (ai.TurnDirection == ArbiterTurnDirection.UTurn)
{
List<Coordinates> updatedPts = new List<Coordinates>();
LinePath interTmp = ai.InterconnectPath.Clone();
Coordinates pathVec = ai.FinalGeneric.Position - ai.InitialGeneric.Position;
interTmp[1] = interTmp[1] + pathVec.Normalize(width / 2.0);
interTmp[0] = interTmp[0] - pathVec.Normalize(width / 2.0);
lp = interTmp.ShiftLateral(TahoeParams.VL);
rp = interTmp.ShiftLateral(-TahoeParams.VL);
updatedPts.AddRange(lp);
updatedPts.AddRange(rp);
ai.TurnPolygon = Polygon.GrahamScan(updatedPts);
}
}
else
{
// polygon points
List<Coordinates> interPoints = new List<Coordinates>();
// waypoint
ArbiterWaypoint awI = (ArbiterWaypoint)ai.InitialGeneric;
ArbiterWaypoint awF = (ArbiterWaypoint)ai.FinalGeneric;
// left and right path
LinePath leftPath = new LinePath();
LinePath rightPath = new LinePath();
// some initial points
LinePath initialPath = new LinePath(new Coordinates[] { awI.PreviousPartition.Initial.Position, awI.Position });
LinePath il = initialPath.ShiftLateral(width / 2.0);
LinePath ir = initialPath.ShiftLateral(-width / 2.0);
leftPath.Add(il[1]);
rightPath.Add(ir[1]);
// some final points
LinePath finalPath = new LinePath(new Coordinates[] { awF.Position, awF.NextPartition.Final.Position });
LinePath fl = finalPath.ShiftLateral(width / 2.0);
LinePath fr = finalPath.ShiftLateral(-width / 2.0);
leftPath.Add(fl[0]);
rightPath.Add(fr[0]);
// initial and final paths
Line iPath = new Line(awI.PreviousPartition.Initial.Position, awI.Position);
Line fPath = new Line(awF.Position, awF.NextPartition.Final.Position);
// get where the paths intersect and vector to normal path
Coordinates c;
iPath.Intersect(fPath, out c);
Coordinates vector = ai.InterconnectPath.GetClosestPoint(c).Location - c;
Coordinates center = c + vector.Normalize((vector.Length / 2.0));
// get width expansion
Coordinates iVec = awI.PreviousPartition != null ? awI.PreviousPartition.Vector().Normalize(1.0) : awI.NextPartition.Vector().Normalize(1.0);
double iRot = -iVec.ArcTan;
Coordinates fVec = awF.NextPartition != null ? awF.NextPartition.Vector().Normalize(1.0) : awF.PreviousPartition.Vector().Normalize(1.0);
fVec = fVec.Rotate(iRot);
double fDeg = fVec.ToDegrees();
double arcTan = Math.Atan2(fVec.Y, fVec.X) * 180.0 / Math.PI;
double centerWidth = width + width * 2.0 * Math.Abs(arcTan) / 90.0;
// get inner point (small scale)
Coordinates innerPoint = center + vector.Normalize(centerWidth / 4.0);
// get outer
Coordinates outerPoint = center - vector.Normalize(centerWidth / 2.0);
if (ai.TurnDirection == ArbiterTurnDirection.Right)
{
rightPath.Insert(1, innerPoint);
ai.InnerCoordinates = rightPath;
leftPath.Reverse();
leftPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
else
{
leftPath.Insert(1, innerPoint);
ai.InnerCoordinates = leftPath;
rightPath.Reverse();
rightPath.Insert(1, outerPoint);
Polygon p = new Polygon(leftPath.ToArray());
p.AddRange(rightPath.ToArray());
ai.TurnPolygon = p;
}
}
}
catch (Exception e)
{
Console.WriteLine("error generating turn polygon: " + ai.ToString());
ai.TurnPolygon = ai.DefaultPoly();
}
}
/// <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 next maneuver
/// </summary>
/// <param name="roads"></param>
/// <param name="mission"></param>
/// <param name="vehicleState"></param>
/// <param name="CoreCommon.CorePlanningState"></param>
/// <param name="observedVehicles"></param>
/// <param name="observedObstacles"></param>
/// <param name="coreState"></param>
/// <param name="carMode"></param>
/// <returns></returns>
public Maneuver Plan(VehicleState vehicleState, double vehicleSpeed,
SceneEstimatorTrackedClusterCollection observedVehicles, SceneEstimatorUntrackedClusterCollection observedObstacles,
CarMode carMode, INavigableNode goal)
{
// set blockages
List<ITacticalBlockage> blockages = this.blockageHandler.DetermineBlockages(CoreCommon.CorePlanningState);
#region Travel State
if (CoreCommon.CorePlanningState is TravelState)
{
#region Stay in Lane State
if (CoreCommon.CorePlanningState is StayInLaneState)
{
// get lane state
StayInLaneState sils = (StayInLaneState)CoreCommon.CorePlanningState;
#region Blockages
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is LaneBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// check not from a dynamicly moving vehicle
if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
{
// go to a blockage handling tactical
return new Maneuver(new HoldBrakeBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
ArbiterOutput.Output("Lane blockage reported for moving vehicle, ignoring");
}
#endregion
// update the total time ignorable have been seen
sils.UpdateIgnoreList();
// nav plan to find poi
RoadPlan rp = navigation.PlanNavigableArea(sils.Lane, vehicleState.Position, goal, sils.WaypointsToIgnore);
// check for unreachable route
if (rp.BestPlan.laneWaypointOfInterest.BestRoute != null &&
rp.BestPlan.laneWaypointOfInterest.BestRoute.Count == 0 &&
rp.BestPlan.laneWaypointOfInterest.RouteTime >= Double.MaxValue - 1.0)
{
ArbiterOutput.Output("Removed Unreachable Checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else if (rp.BestPlan.laneWaypointOfInterest.TimeCostToPoint >= Double.MaxValue - 1.0)
{
ArbiterOutput.Output("Best Lane Waypoint of Interest is END OF LANE WITH NO INTERCONNECTS, LEADING NOWHERE");
ArbiterOutput.Output("Removed Unreachable Checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#region Check Supra Lane Availability
// if the poi is at the end of this lane, is not stop, leads to another lane, and has no overlapping lanes
// or if the poi's best exit is an exit in this lane, is not a stop, has no overlapping lanes and leads to another lane
// create supralane
// check if navigation is corrent in saying we want to continue on the current lane and we're far enough along the lane, 30m for now
if(rp.BestPlan.Lane.Equals(sils.Lane.LaneId))
{
// get navigation poi
DownstreamPointOfInterest dpoi = rp.BestPlan.laneWaypointOfInterest;
// check that the poi is not stop and is not the current checkpoint
if(!dpoi.PointOfInterest.IsStop && !(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(dpoi.PointOfInterest.WaypointId)))
{
// get the best exit or the poi
ArbiterInterconnect ai = dpoi.BestExit;
// check if exit goes into a lane and not a uturn
if(ai != null && ai.FinalGeneric is ArbiterWaypoint && ai.TurnDirection != ArbiterTurnDirection.UTurn)
{
// final lane or navigation poi interconnect
ArbiterLane al = ((ArbiterWaypoint)ai.FinalGeneric).Lane;
// check not same lane
if (!al.Equals(sils.Lane))
{
// check if enough room to start
bool enoughRoom = !sils.Lane.Equals(al) || sils.Lane.LanePath(sils.Lane.WaypointList[0].Position, vehicleState.Front).PathLength > 30;
if (enoughRoom)
{
// try to get intersection associated with the exit
ArbiterIntersection aInter = CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(dpoi.PointOfInterest.WaypointId) ?
CoreCommon.RoadNetwork.IntersectionLookup[dpoi.PointOfInterest.WaypointId] : null;
// check no intersection or no overlapping lanes
if (aInter == null || !aInter.PriorityLanes.ContainsKey(ai) || aInter.PriorityLanes[ai].Count == 0)
{
// create the supra lane
SupraLane sl = new SupraLane(sils.Lane, ai, al);
// switch to the supra lane state
StayInSupraLaneState sisls = new StayInSupraLaneState(sl, CoreCommon.CorePlanningState);
sisls.UpdateState(vehicleState.Front);
// set
return new Maneuver(new NullBehavior(), sisls, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
}
}
}
}
#endregion
// plan final tactical maneuver
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
#endregion
#region Stay in Supra Lane State
else if (CoreCommon.CorePlanningState is StayInSupraLaneState)
{
// state
StayInSupraLaneState sisls = (StayInSupraLaneState)CoreCommon.CorePlanningState;
#region Blockages
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is LaneBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// check not from a dynamicly moving vehicle
if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
{
// go to a blockage handling tactical
return new Maneuver(new HoldBrakeBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
ArbiterOutput.Output("Lane blockage reported for moving vehicle, ignoring");
}
#endregion
// check if we are in the final lane
if (sisls.Lane.ClosestComponent(vehicleState.Position) == SLComponentType.Final)
{
// go to stay in lane
return new Maneuver(new NullBehavior(), new StayInLaneState(sisls.Lane.Final, sisls), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// update ignorable
sisls.UpdateIgnoreList();
// nav plan to find points
RoadPlan rp = navigation.PlanNavigableArea(sisls.Lane, vehicleState.Position, goal, sisls.WaypointsToIgnore);
// check for unreachable route
if (rp.BestPlan.laneWaypointOfInterest.BestRoute != null &&
rp.BestPlan.laneWaypointOfInterest.BestRoute.Count == 0 &&
rp.BestPlan.laneWaypointOfInterest.RouteTime >= Double.MaxValue - 1.0)
{
ArbiterOutput.Output("Removed Unreachable Checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// update current state
sisls.UpdateState(vehicleState.Front);
// return final maneuver
return final;
}
#endregion
#region Stopping At Stop State
else if (CoreCommon.CorePlanningState is StoppingAtStopState)
{
// get state
StoppingAtStopState sass = (StoppingAtStopState)CoreCommon.CorePlanningState;
// check to see if we're stopped
// check if in other lane
if (CoreCommon.Communications.HasCompleted((new StayInLaneBehavior(null, null, null)).GetType()))
{
// update intersection monitor
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(sass.waypoint.AreaSubtypeWaypointId))
{
// nav plan
IntersectionPlan ip = navigation.PlanIntersection(sass.waypoint, goal);
// update intersection monitor
this.tactical.Update(observedVehicles, vehicleState);
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
sass.waypoint,
CoreCommon.RoadNetwork.IntersectionLookup[sass.waypoint.AreaSubtypeWaypointId],
vehicleState, ip.BestOption);
}
else
{
IntersectionTactical.IntersectionMonitor = null;
}
// check if we've hit goal if stop is cp
if (sass.waypoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
{
ArbiterOutput.Output("Stopped at current goal: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + ", Removing");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
if (CoreCommon.Mission.MissionCheckpoints.Count == 0)
{
return new Maneuver(new HoldBrakeBehavior(), new NoGoalsLeftState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
// move to the intersection
IState next = new WaitingAtIntersectionExitState(sass.waypoint, sass.turnDirection, new IntersectionDescription(), sass.desiredExit);
Behavior b = new HoldBrakeBehavior();
return new Maneuver(b, next, sass.DefaultStateDecorators, vehicleState.Timestamp);
}
else
{
// otherwise update the stop parameters
Behavior b = sass.Resume(vehicleState, vehicleSpeed);
return new Maneuver(b, CoreCommon.CorePlanningState, sass.DefaultStateDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Change Lanes State
else if (CoreCommon.CorePlanningState is ChangeLanesState)
{
// get state
ChangeLanesState cls = (ChangeLanesState)CoreCommon.CorePlanningState;
#region Blockages
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is LaneChangeBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// check not from a dynamicly moving vehicle
if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
{
// go to a blockage handling tactical
return new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
ArbiterOutput.Output("Lane Change blockage reported for moving vehicle, ignoring");
}
#endregion
// get a good lane
ArbiterLane goodLane = null;
if(!cls.Parameters.InitialOncoming)
goodLane = cls.Parameters.Initial;
else if(!cls.Parameters.TargetOncoming)
goodLane = cls.Parameters.Target;
else
throw new Exception("not going from or to good lane");
// nav plan to find poi
#warning make goal better if there is none come to stop
RoadPlan rp = navigation.PlanNavigableArea(goodLane, vehicleState.Front,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], new List<ArbiterWaypoint>());
// check current behavior type
bool done = CoreCommon.Communications.HasCompleted((new ChangeLaneBehavior(null, null, false, 0, null, null)).GetType());
if (done)
{
if (cls.Parameters.TargetOncoming)
return new Maneuver(
new StayInLaneBehavior(cls.Parameters.Target.LaneId,
new ScalarSpeedCommand(cls.Parameters.Parameters.RecommendedSpeed),
cls.Parameters.Parameters.VehiclesToIgnore,
cls.Parameters.Target.ReversePath,
cls.Parameters.Target.Width,
cls.Parameters.Target.NumberOfLanesRight(vehicleState.Front, !cls.Parameters.InitialOncoming),
cls.Parameters.Target.NumberOfLanesLeft(vehicleState.Front, !cls.Parameters.InitialOncoming)),
new OpposingLanesState(cls.Parameters.Target, true, cls, vehicleState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
else
return new Maneuver(
new StayInLaneBehavior(cls.Parameters.Target.LaneId,
new ScalarSpeedCommand(cls.Parameters.Parameters.RecommendedSpeed),
cls.Parameters.Parameters.VehiclesToIgnore,
cls.Parameters.Target.LanePath(),
cls.Parameters.Target.Width,
cls.Parameters.Target.NumberOfLanesLeft(vehicleState.Front, !cls.Parameters.InitialOncoming),
cls.Parameters.Target.NumberOfLanesRight(vehicleState.Front, !cls.Parameters.InitialOncoming)),
new StayInLaneState(cls.Parameters.Target, CoreCommon.CorePlanningState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
return tactical.Plan(cls, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
}
}
#endregion
#region Opposing Lanes State
else if (CoreCommon.CorePlanningState is OpposingLanesState)
{
// get state
OpposingLanesState ols = (OpposingLanesState)CoreCommon.CorePlanningState;
ols.SetClosestGood(vehicleState);
#region Blockages
// check blockages
if (blockages != null && blockages.Count > 0 && blockages[0] is OpposingLaneBlockage)
{
// create the blockage state
EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState);
// check not from a dynamicly moving vehicle
if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic)
{
// go to a blockage handling tactical
return new Maneuver(new HoldBrakeBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
ArbiterOutput.Output("Opposing Lane blockage reported for moving vehicle, ignoring");
}
#endregion
// check closest good null
if (ols.ClosestGoodLane != null)
{
// nav plan to find poi
RoadPlan rp = navigation.PlanNavigableArea(ols.ClosestGoodLane, vehicleState.Position, goal, new List<ArbiterWaypoint>());
// plan final tactical maneuver
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
// otherwise need to make a uturn
else
{
ArbiterOutput.Output("in opposing lane with no closest good, making a uturn");
ArbiterLanePartition alp = ols.OpposingLane.GetClosestPartition(vehicleState.Front);
Coordinates c1 = vehicleState.Front + alp.Vector().Normalize(8.0);
Coordinates c2 = vehicleState.Front - alp.Vector().Normalize(8.0);
LinePath lpTmp = new LinePath(new Coordinates[] { c1, c2 });
List<Coordinates> pCoords = new List<Coordinates>();
pCoords.AddRange(lpTmp.ShiftLateral(ols.OpposingLane.Width)); //* 1.5));
pCoords.AddRange(lpTmp.ShiftLateral(-ols.OpposingLane.Width));// / 2.0));
Polygon uturnPoly = Polygon.GrahamScan(pCoords);
uTurnState uts = new uTurnState(ols.OpposingLane, uturnPoly, true);
uts.Interconnect = alp.ToInterconnect;
// plan final tactical maneuver
Maneuver final = new Maneuver(new NullBehavior(), uts, TurnDecorators.LeftTurnDecorator, vehicleState.Timestamp);
// return final maneuver
return final;
}
}
#endregion
#region Starting up off of chute state
else if (CoreCommon.CorePlanningState is StartupOffChuteState)
{
// cast the type
StartupOffChuteState socs = (StartupOffChuteState)CoreCommon.CorePlanningState;
// check if in lane part of chute
if (CoreCommon.Communications.HasCompleted((new TurnBehavior(null, null, null, null, null, null)).GetType()))
{
// go to lane state
return new Maneuver(new NullBehavior(), new StayInLaneState(socs.Final.Lane, new Probability(0.8, 0.2), true, socs), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// otherwise continue
else
{
// simple maneuver generation
TurnBehavior tb = (TurnBehavior)socs.Resume(vehicleState, 1.4);
// add bounds to observable
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(tb.LeftBound, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(tb.RightBound, ArbiterInformationDisplayObjectType.rightBound));
// final maneuver
return new Maneuver(tb, socs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
}
#endregion
}
#endregion
#region Intersection State
else if (CoreCommon.CorePlanningState is IntersectionState)
{
#region Waiting at Intersection Exit State
if (CoreCommon.CorePlanningState is WaitingAtIntersectionExitState)
{
// get state
WaitingAtIntersectionExitState waies = (WaitingAtIntersectionExitState)CoreCommon.CorePlanningState;
// nav plan
IntersectionPlan ip = navigation.PlanIntersection(waies.exitWaypoint, goal);
// plan
Maneuver final = tactical.Plan(waies, ip, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
#endregion
#region Stopping At Exit State
else if (CoreCommon.CorePlanningState is StoppingAtExitState)
{
// get state
StoppingAtExitState saes = (StoppingAtExitState)CoreCommon.CorePlanningState;
// check to see if we're stopped
if (CoreCommon.Communications.HasCompleted((new StayInLaneBehavior(null, null, null)).GetType()))
{
// check if we've hit goal if stop is cp
if (saes.waypoint.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
{
ArbiterOutput.Output("Stopped at current goal: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + ", Removing");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
if (CoreCommon.Mission.MissionCheckpoints.Count == 0)
{
return new Maneuver(new HoldBrakeBehavior(), new NoGoalsLeftState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
// move to the intersection
IState next = new WaitingAtIntersectionExitState(saes.waypoint, saes.turnDirection, new IntersectionDescription(), saes.desiredExit);
Behavior b = new HoldBrakeBehavior();
return new Maneuver(b, next, saes.DefaultStateDecorators, vehicleState.Timestamp);
}
else
{
// nav plan
IntersectionPlan ip = navigation.PlanIntersection(saes.waypoint, goal);
// update the intersection monitor
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(saes.waypoint.AreaSubtypeWaypointId))
{
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
saes.waypoint,
CoreCommon.RoadNetwork.IntersectionLookup[saes.waypoint.AreaSubtypeWaypointId],
vehicleState, ip.BestOption);
}
else
IntersectionTactical.IntersectionMonitor = null;
// plan final tactical maneuver
Maneuver final = tactical.Plan(saes, ip, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
}
#endregion
#region Turn State
else if (CoreCommon.CorePlanningState is TurnState)
{
// get state
TurnState ts = (TurnState)CoreCommon.CorePlanningState;
// check if in other lane
if (CoreCommon.Communications.HasCompleted((new TurnBehavior(null, null, null, null, null, null)).GetType()))
{
if (ts.Interconnect.FinalGeneric is ArbiterWaypoint)
{
// get final wp, and if next cp, remove
ArbiterWaypoint final = (ArbiterWaypoint)ts.Interconnect.FinalGeneric;
if (CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.Equals(final.AreaSubtypeWaypointId))
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// stay in target lane
IState nextState = new StayInLaneState(ts.TargetLane, new Probability(0.8, 0.2), true, CoreCommon.CorePlanningState);
Behavior b = new NullBehavior();
return new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else if (ts.Interconnect.FinalGeneric is ArbiterPerimeterWaypoint)
{
// stay in target lane
IState nextState = new ZoneTravelingState(((ArbiterPerimeterWaypoint)ts.Interconnect.FinalGeneric).Perimeter.Zone, (INavigableNode)ts.Interconnect.FinalGeneric);
Behavior b = new NullBehavior();
return new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
throw new Exception("unhandled unterconnect final wp type");
}
// get interconnect
if (ts.Interconnect.FinalGeneric is ArbiterWaypoint)
{
// nav plan
IntersectionPlan ip = navigation.PlanIntersection((ITraversableWaypoint)ts.Interconnect.InitialGeneric, goal);
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, ip, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
// else to zone
else if (ts.Interconnect.FinalGeneric is ArbiterPerimeterWaypoint)
{
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
else
{
throw new Exception("method not imp");
}
}
#endregion
#region uTurn State
else if (CoreCommon.CorePlanningState is uTurnState)
{
// get state
uTurnState uts = (uTurnState)CoreCommon.CorePlanningState;
// plan over the target segment, ignoring the initial waypoint of the target lane
ArbiterWaypoint initial = uts.TargetLane.GetClosestPartition(vehicleState.Position).Initial;
List<ArbiterWaypoint> iws = RoadToolkit.WaypointsClose(initial.Lane.Way, vehicleState.Front, initial);
RoadPlan rp = navigation.PlanRoads(uts.TargetLane, vehicleState.Front, goal, iws);
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, rp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
#endregion
#region Intersection Startup State
else if (CoreCommon.CorePlanningState is IntersectionStartupState)
{
// get startup state
IntersectionStartupState iss = (IntersectionStartupState)CoreCommon.CorePlanningState;
// get intersection
ArbiterIntersection ai = iss.Intersection;
// get plan
IEnumerable<ITraversableWaypoint> entries = ai.AllEntries.Values;
IntersectionStartupPlan isp = navigation.PlanIntersectionStartup(entries, goal);
// plan tac
Maneuver final = tactical.Plan(iss, isp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return
return final;
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
}
#endregion
}
#endregion
#region Zone State
else if (CoreCommon.CorePlanningState is ZoneState)
{
#region Zone Travelling State
if (CoreCommon.CorePlanningState is ZoneTravelingState)
{
// set state
ZoneTravelingState zts = (ZoneTravelingState)CoreCommon.CorePlanningState;
// check to see if we're stopped
if (CoreCommon.Communications.HasCompleted((new ZoneTravelingBehavior(null, null, new Polygon[0], null, null, null, null)).GetType()))
{
// plan over state and zone
ZonePlan zp = this.navigation.PlanZone(zts.Zone, zts.Start, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]);
if (zp.ZoneGoal is ArbiterParkingSpotWaypoint)
{
// move to parking state
ParkingState ps = new ParkingState(zp.Zone, ((ArbiterParkingSpotWaypoint)zp.ZoneGoal).ParkingSpot);
return new Maneuver(new HoldBrakeBehavior(), ps, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else if(zp.ZoneGoal is ArbiterPerimeterWaypoint)
{
// get plan
IntersectionPlan ip = navigation.GetIntersectionExitPlan((ITraversableWaypoint)zp.ZoneGoal, goal);
// move to exit
WaitingAtIntersectionExitState waies = new WaitingAtIntersectionExitState((ITraversableWaypoint)zp.ZoneGoal, ip.BestOption.ToInterconnect.TurnDirection, new IntersectionDescription(), ip.BestOption.ToInterconnect);
return new Maneuver(new HoldBrakeBehavior(), waies, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
else
{
// plan over state and zone
ZonePlan zp = this.navigation.PlanZone(zts.Zone, zts.Start, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]);
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, zp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
}
#endregion
#region Parking State
else if (CoreCommon.CorePlanningState is ParkingState)
{
// set state
ParkingState ps = (ParkingState)CoreCommon.CorePlanningState;
// check to see if we're stopped
if (CoreCommon.Communications.HasCompleted((new ZoneParkingBehavior(null, null, new Polygon[0], null, null, null, null, null, 0.0)).GetType()))
{
if (ps.ParkingSpot.Checkpoint.CheckpointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber))
{
ArbiterOutput.Output("Reached Goal, cp: " + ps.ParkingSpot.Checkpoint.CheckpointId.ToString());
CoreCommon.Mission.MissionCheckpoints.Dequeue();
}
// pull out of the space
PullingOutState pos = new PullingOutState(ps.Zone, ps.ParkingSpot);
return new Maneuver(new HoldBrakeBehavior(), pos, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
}
#endregion
#region Pulling Out State
else if (CoreCommon.CorePlanningState is PullingOutState)
{
// set state
PullingOutState pos = (PullingOutState)CoreCommon.CorePlanningState;
// plan over state and zone
ZonePlan zp = this.navigation.PlanZone(pos.Zone, pos.ParkingSpot.Checkpoint, goal);
// check to see if we're stopped
if (CoreCommon.Communications.HasCompleted((new ZoneParkingPullOutBehavior(null, null, new Polygon[0], null, null, null, null, null, null, null, null)).GetType()))
{
// maneuver to next place to go
return new Maneuver(new HoldBrakeBehavior(), new ZoneTravelingState(pos.Zone, pos.ParkingSpot.Checkpoint), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// plan
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, zp, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
}
#endregion
#region Zone Startup State
else if (CoreCommon.CorePlanningState is ZoneStartupState)
{
// feed through the plan from the zone tactical
Maneuver final = tactical.Plan(CoreCommon.CorePlanningState, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return final maneuver
return final;
}
#endregion
#region Zone Orientation
else if (CoreCommon.CorePlanningState is ZoneOrientationState)
{
ZoneOrientationState zos = (ZoneOrientationState)CoreCommon.CorePlanningState;
// add bounds to observable
LinePath lp = new LinePath(new Coordinates[] { zos.final.Start.Position, zos.final.End.Position });
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lp.ShiftLateral(TahoeParams.T), ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(lp.ShiftLateral(-TahoeParams.T), ArbiterInformationDisplayObjectType.rightBound));
// check to see if we're stopped
//if (CoreCommon.Communications.HasCompleted((new UTurnBehavior(null, null, null, null)).GetType()))
//{
// maneuver to next place to go
return new Maneuver(new HoldBrakeBehavior(), new ZoneTravelingState(zos.Zone, zos.final.End), TurnDecorators.NoDecorators, vehicleState.Timestamp);
//}
// not stopped doing hte maneuver
//else
// return new Maneuver(zos.Resume(vehicleState, 1.4), zos, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
}
#endregion
}
#endregion
#region Other State
else if (CoreCommon.CorePlanningState is OtherState)
{
#region Start Up State
if (CoreCommon.CorePlanningState is StartUpState)
{
// get state
StartUpState sus = (StartUpState)CoreCommon.CorePlanningState;
// make a new startup agent
StartupReasoning sr = new StartupReasoning(this.laneAgent);
// get final state
IState nextState = sr.Startup(vehicleState, carMode);
// return no op ad zero all decorators
Behavior nextBehavior = sus.Resume(vehicleState, vehicleSpeed);
// return maneuver
return new Maneuver(nextBehavior, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
#endregion
#region Paused State
else if (CoreCommon.CorePlanningState is PausedState)
{
// if switch back to run
if (carMode == CarMode.Run)
{
// get state
PausedState ps = (PausedState)CoreCommon.CorePlanningState;
// get what we were previously doing
IState previousState = ps.PreviousState();
// check if can resume
if (previousState != null && previousState.CanResume())
{
// resume state is next
return new Maneuver(new HoldBrakeBehavior(), new ResumeState(previousState), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// otherwise go to startup
else
{
// next state is startup
IState nextState = new StartUpState();
// return no op
Behavior nextBehavior = new HoldBrakeBehavior();
// return maneuver
return new Maneuver(nextBehavior, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
// otherwise stay stopped
else
{
// stay stopped and paused
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Human State
else if (CoreCommon.CorePlanningState is HumanState)
{
// change to startup
if (carMode == CarMode.Run)
{
// next is startup
IState next = new StartUpState();
// next behavior just stay iin place for now
Behavior behavior = new HoldBrakeBehavior();
// return startup maneuver
return new Maneuver(behavior, next, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// in human mode still
else
{
// want to remove old behavior stuff
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Resume State
else if (CoreCommon.CorePlanningState is ResumeState)
{
// get state
ResumeState rs = (ResumeState)CoreCommon.CorePlanningState;
// make sure can resume (this is simple action)
if (rs.StateToResume != null && rs.StateToResume.CanResume())
{
// return old behavior
Behavior nextBehavior = rs.Resume(vehicleState, vehicleSpeed);
// return maneuver
return new Maneuver(nextBehavior, rs.StateToResume, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// otherwise just startup
else
{
// startup
return new Maneuver(new HoldBrakeBehavior(), new StartUpState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region No Goals Left State
else if (CoreCommon.CorePlanningState is NoGoalsLeftState)
{
// check if goals available
if (CoreCommon.Mission.MissionCheckpoints.Count > 0)
{
// startup
return new Maneuver(new HoldBrakeBehavior(), new StartUpState(), TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// stay paused
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region eStopped State
else if (CoreCommon.CorePlanningState is eStoppedState)
{
// change to startup
if (carMode == CarMode.Run)
{
// next is startup
IState next = new StartUpState();
// next behavior just stay iin place for now
Behavior behavior = new HoldBrakeBehavior();
// return startup maneuver
return new Maneuver(behavior, next, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// in human mode still
else
{
// want to remove old behavior stuff
return new Maneuver(new NullBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown OtherState type", "CoreCommon.CorePlanningState");
}
#endregion
}
#endregion
#region Blockage State
else if (CoreCommon.CorePlanningState is BlockageState)
{
#region Blockage State
// something is blocked, in the encountered state we want to filter to base components of state
if (CoreCommon.CorePlanningState is EncounteredBlockageState)
{
// cast blockage state
EncounteredBlockageState bs = (EncounteredBlockageState)CoreCommon.CorePlanningState;
// plan through the blockage state with no road plan as just a quick filter
Maneuver final = tactical.Plan(bs, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return the final maneuver
return final;
}
#endregion
#region Blockage Recovery State
// recover from blockages
else if (CoreCommon.CorePlanningState is BlockageRecoveryState)
{
// get the blockage recovery state
BlockageRecoveryState brs = (BlockageRecoveryState)CoreCommon.CorePlanningState;
#region Check Various Statuses of Completion
// check successful completion report of behavior
if (brs.RecoveryBehavior != null && CoreCommon.Communications.HasCompleted(brs.RecoveryBehavior.GetType()))
{
// set updated status
ArbiterOutput.Output("Successfully received completion of behavior: " + brs.RecoveryBehavior.ToShortString() + ", " + brs.RecoveryBehavior.ShortBehaviorInformation());
brs.RecoveryStatus = BlockageRecoverySTATUS.COMPLETED;
// move to the tactical plan
return this.tactical.Plan(brs, null, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
}
// check operational startup
else if (CoreCommon.Communications.HasCompleted((new OperationalStartupBehavior()).GetType()))
{
// check defcon types
if (brs.Defcon == BlockageRecoveryDEFCON.REVERSE)
{
// abort maneuver as operational has no state information
return new Maneuver(new NullBehavior(), brs.AbortState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Information
// set recovery information
CoreCommon.CurrentInformation.FQMState = brs.EncounteredState.ShortDescription();
CoreCommon.CurrentInformation.FQMStateInfo = brs.EncounteredState.StateInformation();
CoreCommon.CurrentInformation.FQMBehavior = brs.RecoveryBehavior != null ? brs.RecoveryBehavior.ToShortString() : "NONE";
CoreCommon.CurrentInformation.FQMBehaviorInfo = brs.RecoveryBehavior != null ? brs.RecoveryBehavior.ShortBehaviorInformation() : "NONE";
CoreCommon.CurrentInformation.FQMSpeed = brs.RecoveryBehavior != null ? brs.RecoveryBehavior.SpeedCommandString() : "NONE";
#endregion
#region Blocked
if (brs.RecoveryStatus == BlockageRecoverySTATUS.BLOCKED)
{
if (brs.RecoveryBehavior is ChangeLaneBehavior)
{
brs.RecoveryStatus = BlockageRecoverySTATUS.ENCOUNTERED;
brs.Defcon = BlockageRecoveryDEFCON.CHANGELANES_FORWARD;
return new Maneuver(new HoldBrakeBehavior(), brs, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
ArbiterOutput.Output("Recovery behavior blocked, reverting to abort state: " + brs.AbortState.ToString());
return new Maneuver(new HoldBrakeBehavior(), brs.AbortState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
#region Navigational Plan
// set navigational plan
INavigationalPlan navPlan = null;
#region Encountered
// blockage
if (brs.RecoveryStatus == BlockageRecoverySTATUS.ENCOUNTERED)
{
// get state
if (brs.AbortState is StayInLaneState)
{
// lane state
StayInLaneState sils = (StayInLaneState)brs.AbortState;
navPlan = navigation.PlanNavigableArea(sils.Lane, vehicleState.Position, goal, sils.WaypointsToIgnore);
}
}
#endregion
#region Completion
// blockage
if (brs.RecoveryStatus == BlockageRecoverySTATUS.COMPLETED)
{
// get state
if (brs.CompletionState is StayInLaneState)
{
// lane state
StayInLaneState sils = (StayInLaneState)brs.CompletionState;
navPlan = navigation.PlanNavigableArea(sils.Lane, vehicleState.Position, goal, sils.WaypointsToIgnore);
}
}
#endregion
#endregion
// move to the tactical plan
Maneuver final = this.tactical.Plan(brs, navPlan, vehicleState, observedVehicles, observedObstacles, blockages, vehicleSpeed);
// return the final maneuver
return final;
}
#endregion
}
#endregion
#region Unknown
else
{
// non-handled state
throw new ArgumentException("Unknown state", "CoreCommon.CorePlanningState");
}
// for now, return null
return new Maneuver();
#endregion
}
/// <summary>
/// Turn information
/// </summary>
/// <param name="entry"></param>
/// <param name="finalPath"></param>
/// <param name="leftBound"></param>
/// <param name="rightBound"></param>
public static void ZoneTurnInfo(ArbiterInterconnect ai, ArbiterPerimeterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound)
{
//Coordinates centerVec = entry.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center - entry.Position;
Coordinates centerVec = ai.InterconnectPath[1] - ai.InterconnectPath[0];
centerVec = centerVec.Normalize(TahoeParams.VL);
finalPath = new LinePath(new Coordinates[] { entry.Position, entry.Position + centerVec });
leftBound = finalPath.ShiftLateral(TahoeParams.T * 2.0);
rightBound = finalPath.ShiftLateral(-TahoeParams.T * 2.0);
}
public void Render(IGraphics g, WorldTransform wt)
{
LocalLaneModel laneModel = this.laneModel;
if (laneModel == null || laneModel.LanePath == null || laneModel.LanePath.Count <= 1)
{
return;
}
// generate the left and right bounds
LinePath leftBound = laneModel.LanePath.ShiftLateral(laneModel.Width / 2.0);
LinePath leftBound2 = null;
if (laneModel.WidthVariance > 0.01)
{
leftBound2 = laneModel.LanePath.ShiftLateral(laneModel.Width / 2.0 + Math.Sqrt(laneModel.WidthVariance) * 1.96 / 2.0);
}
LinePath rightBound = laneModel.LanePath.ShiftLateral(-laneModel.Width / 2.0);
LinePath rightBound2 = null;
if (laneModel.WidthVariance > 0.01)
{
rightBound2 = laneModel.LanePath.ShiftLateral(-laneModel.Width / 2.0 - Math.Sqrt(laneModel.WidthVariance) * 1.96 / 2.0);
}
// generate a polygon of the confidence region of the center line
Polygon centerConfLeft = null;
Polygon centerConfRight = null;
Polygon centerConfFull = null;
if (sigma > 0 && laneModel.LaneYVariance != null && laneModel.LaneYVariance.Length == laneModel.LanePath.Count)
{
float varianceThreshold = 9f * 9f;
if (laneModel.LaneYVariance[0] > varianceThreshold)
{
varianceThreshold = 600;
}
int numPoints;
for (numPoints = 0; numPoints < laneModel.LaneYVariance.Length; numPoints++)
{
if (laneModel.LaneYVariance[numPoints] > varianceThreshold)
{
break;
}
}
double[] leftDist = new double[numPoints];
double[] rightDist = new double[numPoints];
double prevDist = 0;
for (int i = 0; i < numPoints; i++)
{
double dist = laneModel.LaneYVariance[i];
if (dist > 0.01)
{
dist = Math.Sqrt(dist) * sigma;
leftDist[i] = dist;
rightDist[i] = -dist;
prevDist = dist;
}
else
{
leftDist[i] = prevDist;
rightDist[i] = -prevDist;
}
}
// get the subset of the lane model we're interested in
LinePath subset = laneModel.LanePath.SubPath(0, numPoints - 1);
LinePath left = subset.ShiftLateral(leftDist);
LinePath right = subset.ShiftLateral(rightDist);
Coordinates midTop = (left[left.Count - 1] + right[right.Count - 1]) / 2.0;
Coordinates midBottom = (left[0] + right[0]) / 2.0;
centerConfLeft = new Polygon(numPoints + 2);
centerConfLeft.Add(midTop);
centerConfLeft.Add(midBottom);
centerConfLeft.AddRange(left);
centerConfRight = new Polygon(numPoints + 2);
centerConfRight.Add(midTop);
centerConfRight.Add(midBottom);
centerConfRight.AddRange(right);
centerConfFull = new Polygon(numPoints * 2);
right.Reverse();
centerConfFull.AddRange(right);
centerConfFull.AddRange(left);
}
// draw the shits
IPen pen = g.CreatePen();
pen.Width = 1.0f / wt.Scale;
pen.Color = color;
// first draw the confidence polygon
if (centerConfFull != null)
{
g.FillPolygon(Color.FromArgb(20, color), Utility.ToPointF(centerConfLeft));
g.FillPolygon(Color.FromArgb(20, color), Utility.ToPointF(centerConfRight));
pen.Color = Color.FromArgb(30, color);
g.DrawPolygon(pen, Utility.ToPointF(centerConfFull));
}
// next draw the center line
pen.Color = color;
//g.DrawLines(pen, Utility.ToPointF(laneModel.LanePath));
//// next draw the left lane confidence bound
//if (leftBound2 != null) {
// pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dot;
// g.DrawLines(pen, Utility.ToPointF(leftBound2));
//}
//// draw the right lane confidence bound
//if (rightBound2 != null) {
// pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dot;
// g.DrawLines(pen, Utility.ToPointF(rightBound2));
//}
// draw the left bound
pen.DashStyle = System.Drawing.Drawing2D.DashStyle.Solid;
g.DrawLines(pen, Utility.ToPointF(leftBound));
// draw the right bound
g.DrawLines(pen, Utility.ToPointF(rightBound));
// draw the model confidence
string labelString = laneModel.Probability.ToString("F3");
SizeF stringSize = g.MeasureString(labelString, labelFont);
stringSize.Width /= wt.Scale;
stringSize.Height /= wt.Scale;
RectangleF rect = new RectangleF(Utility.ToPointF(laneModel.LanePath[0]), stringSize);
float inflateValue = 4 / wt.Scale;
rect.X -= inflateValue;
rect.Y -= inflateValue;
g.FillRectangle(Color.FromArgb(127, Color.White), rect);
g.DrawString(labelString, labelFont, Color.Black, Utility.ToPointF(laneModel.LanePath[0]));
prevLeftBound = leftBound;
prevRightBound = rightBound;
}
private void GenerateSafetyZone()
{
if (!safetyZoneBegin.Location.Equals(safetyZoneEnd.Location))
{
LinePath lp = new LinePath(new Coordinates[] { safetyZoneBegin.Location, safetyZoneEnd.Location });
LinePath lp1 = lp.ShiftLateral(-lane.Width / 2.0);
LinePath lp2 = lp.ShiftLateral(lane.Width / 2.0);
List<Coordinates> aszCoords = lp2;
aszCoords.AddRange(lp1);
this.SafetyPolygon = Polygon.GrahamScan(aszCoords);
}
else
{
}
}
/// <summary>
/// Maneuver for recovering from a lane blockage, used for lane changes as well
/// </summary>
/// <param name="lane"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicleSpeed"></param>
/// <param name="defcon"></param>
/// <param name="saudi"></param>
/// <returns></returns>
public Maneuver LaneRecoveryManeuver(ArbiterLane lane, VehicleState vehicleState, double vehicleSpeed,
INavigationalPlan plan, BlockageRecoveryState brs, bool failedVehiclesPersistentlyOnly, out bool waitForUTurn)
{
// get the blockage
ITacticalBlockage laneBlockageReport = brs.EncounteredState.TacticalBlockage;
// get the turn state
StayInLaneState sils = new StayInLaneState(lane, CoreCommon.CorePlanningState);
// set wait false
waitForUTurn = false;
#region Reverse
// check the state of the recovery for being the initial state
if (brs.Defcon == BlockageRecoveryDEFCON.INITIAL)
{
// determine if the reverse behavior is recommended
if (laneBlockageReport.BlockageReport.ReverseRecommended)
{
// notify
ArbiterOutput.Output("Initial encounter, reverse recommended, reversing");
// get reverse behavior
StayInLaneBehavior reverseRecovery = this.LaneReverseRecovery(lane, vehicleState, vehicleSpeed, brs.EncounteredState.TacticalBlockage.BlockageReport);
reverseRecovery.TimeStamp = vehicleState.Timestamp;
// get recovery state
BlockageRecoveryState brsT = new BlockageRecoveryState(
reverseRecovery, null, new StayInLaneState(lane, CoreCommon.CorePlanningState),
brs.Defcon = BlockageRecoveryDEFCON.REVERSE, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
brsT.CompletionState = brsT;
// reset blockages
BlockageHandler.SetDefaultBlockageCooldown();
// maneuver
return new Maneuver(reverseRecovery, brsT, TurnDecorators.HazardDecorator, vehicleState.Timestamp);
}
}
#endregion
#region uTurn
// check if uturn is available
ArbiterLane opp = this.tacticalUmbrella.roadTactical.forwardReasoning.GetClosestOpposing(lane, vehicleState);
// resoning
OpposingLateralReasoning olrTmp = new OpposingLateralReasoning(opp, SideObstacleSide.Driver);
if (opp != null && olrTmp.ExistsExactlyHere(vehicleState) && opp.IsInside(vehicleState.Front) && opp.LanePath().GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front) < TahoeParams.VL * 3.0)
{
// check possible to reach goal given block partition way
RoadPlan uTurnNavTest = CoreCommon.Navigation.PlanRoadOppositeWithoutPartition(
lane.GetClosestPartition(vehicleState.Front),
opp.GetClosestPartition(vehicleState.Front),
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
true,
vehicleState.Front,
true);
// flag to try the uturn
bool uturnTry = false;
// all polygons to include
List<Polygon> allPolys = BlockageTactical.AllForwardPolygons(vehicleState, failedVehiclesPersistentlyOnly);// .AllObstacleAndStoppedVehiclePolygons(vehicleState);
// test blockage takes up segment
double numLanesLeft = lane.NumberOfLanesLeft(vehicleState.Front, true);
double numLanesRight = lane.NumberOfLanesRight(vehicleState.Front, true);
LinePath leftBound = lane.LanePath().ShiftLateral((lane.Width * numLanesLeft) + (lane.Width / 2.0));
LinePath rightBound = lane.LanePath().ShiftLateral((lane.Width * numLanesRight) + (lane.Width / 2.0));
bool segmentBlockage = BlockageTester.TestBlockage(allPolys, leftBound, rightBound);
uturnTry = segmentBlockage;
// check if we should try the uturn
if(uturnTry)
{
// check uturn timer
if(uTurnTimer.ElapsedMilliseconds / 1000.0 > 2.0)
{
#region Determine Checkpoint
// check route feasible
if (uTurnNavTest.BestPlan.laneWaypointOfInterest.RouteTime < double.MaxValue - 1
&& uTurnNavTest.BestPlan.laneWaypointOfInterest.TimeCostToPoint < double.MaxValue - 1)
{
ArbiterOutput.Output("valid route to checkpoint by rerouting, uturning");
}
// otherwise remove the next checkpoint
else
{
// notiify
ArbiterOutput.Output("NO valid route to checkpoint by rerouting");
// get the next cp
ArbiterCheckpoint cp1 = CoreCommon.Mission.MissionCheckpoints.Peek();
// get distance to blockage
double blockageDistance = 15.0;
// check cp is in our lane
if (cp1.WaypointId.AreaSubtypeId.Equals(lane.LaneId))
{
// check distance to cp1
double distanceToCp1 = lane.DistanceBetween(vehicleState.Front, CoreCommon.RoadNetwork.ArbiterWaypoints[cp1.WaypointId].Position);
// check that this is an already inserted waypoint
if (cp1.Type == CheckpointType.Inserted)
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as inserted type of checkpoint");
ArbiterCheckpoint ac2 = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac2.WaypointId.ToString() + " as blocked type of checkpoint");
}
// closer to us than the blockage
else if (distanceToCp1 < blockageDistance)
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as between us and the blockage ~ 15m away");
}
// very close to blockage on the other side
else if (distanceToCp1 < blockageDistance + 5.0)
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as on the other side of blockage ~ 15m away");
}
// further away from the blockage
else
{
// check over all checkpoints if there exists a checkpoint cp2 in the opposing lane within 5m along our lane
ArbiterCheckpoint cp2 = null;
foreach (ArbiterWaypoint oppAw in opp.WaypointList)
{
// check checkpoint
if (oppAw.IsCheckpoint)
{
// distance between us and that waypoint
double distanceToCp2 = lane.DistanceBetween(vehicleState.Front, oppAw.Position);
// check along distance < 5.0m
if (Math.Abs(distanceToCp1 - distanceToCp2) < 5.0)
{
// set cp
cp2 = new ArbiterCheckpoint(oppAw.CheckpointId, oppAw.WaypointId, CheckpointType.Inserted);
}
}
}
// check close cp exists
if (cp2 != null)
{
// remove cp1 and replace with cp2
ArbiterOutput.Output("inserting checkpoint: " + cp2.WaypointId.ToString() + " before checkpoint: " + cp1.WaypointId.ToString() + " as can be replaced with adjacent opposing cp2");
//CoreCommon.Mission.MissionCheckpoints.Dequeue();
// get current checkpoints
ArbiterCheckpoint[] updatedAcs = new ArbiterCheckpoint[CoreCommon.Mission.MissionCheckpoints.Count + 1];
updatedAcs[0] = cp2;
CoreCommon.Mission.MissionCheckpoints.CopyTo(updatedAcs, 1);
updatedAcs[1].Type = CheckpointType.Blocked;
CoreCommon.Mission.MissionCheckpoints = new Queue<ArbiterCheckpoint>(new List<ArbiterCheckpoint>(updatedAcs));
}
// otherwise remove cp1
else
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane");
}
}
}
// otherwise we remove the checkpoint
else
{
// remove cp
ArbiterCheckpoint ac = CoreCommon.Mission.MissionCheckpoints.Dequeue();
ArbiterOutput.Output("removed checkpoint: " + ac.WaypointId.ToString() + " as cp not further down our lane");
}
}
#endregion
#region Plan Uturn
// notify
ArbiterOutput.Output("Segment blocked, uturn available");
// nav penalties
ArbiterLanePartition alpClose = lane.GetClosestPartition(vehicleState.Front);
CoreCommon.Navigation.AddHarshBlockageAcrossSegment(alpClose, vehicleState.Front);
// uturn
LinePath lpTmp = new LinePath(new Coordinates[] { vehicleState.Front, opp.LanePath().GetClosestPoint(vehicleState.Front).Location });
Coordinates vector = lpTmp[1] - lpTmp[0];
lpTmp[1] = lpTmp[1] + vector.Normalize(opp.Width / 2.0);
lpTmp[0] = lpTmp[0] - vector.Normalize(lane.Width / 2.0);
LinePath lb = lpTmp.ShiftLateral(15.0);
LinePath rb = lpTmp.ShiftLateral(-15.0);
List<Coordinates> uturnPolyCOords = new List<Coordinates>();
uturnPolyCOords.AddRange(lb);
uturnPolyCOords.AddRange(rb);
uTurnState uts = new uTurnState(opp, Polygon.GrahamScan(uturnPolyCOords));
// reset blockages
BlockageHandler.SetDefaultBlockageCooldown();
// reset the timers
this.Reset();
// go to uturn
return new Maneuver(uts.Resume(vehicleState, 2.0), uts, TurnDecorators.NoDecorators, vehicleState.Timestamp);
#endregion
}
// uturn timer not enough
else
{
// check timer running
if(!uTurnTimer.IsRunning)
{
uTurnTimer.Stop();
uTurnTimer.Reset();
uTurnTimer.Start();
}
// if gets here, need to wait
double time = uTurnTimer.ElapsedMilliseconds / 1000.0;
ArbiterOutput.Output("uTurn behavior evaluated to success, cooling down for: " + time.ToString("f2") + " out of 2");
waitForUTurn = true;
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
else
{
waitForUTurn = false;
this.Reset();
}
}
else
{
waitForUTurn = false;
this.Reset();
}
#endregion
#region Recovery Escalation
// plan forward reasoning
this.tacticalUmbrella.roadTactical.forwardReasoning.ForwardManeuver(lane, vehicleState, (RoadPlan)plan, new List<ITacticalBlockage>(), new List<ArbiterWaypoint>());
// check clear on right, or if it does nto exist here
ILateralReasoning rightLateral = this.tacticalUmbrella.roadTactical.rightLateralReasoning;
bool rightClear = !rightLateral.Exists || !rightLateral.ExistsExactlyHere(vehicleState) ||
rightLateral.AdjacentAndRearClear(vehicleState);
// check clear on left, or if it does nto exist here
ILateralReasoning leftLateral = this.tacticalUmbrella.roadTactical.leftLateralReasoning;
bool leftClear = !leftLateral.Exists || !leftLateral.ExistsExactlyHere(vehicleState) ||
leftLateral.AdjacentAndRearClear(vehicleState);
if(leftClear && leftLateral is OpposingLateralReasoning)
leftClear = leftLateral.ForwardClear(vehicleState, TahoeParams.VL * 3.0, 2.24,
new LaneChangeInformation(LaneChangeReason.FailedForwardVehicle, null),
lane.LanePath().AdvancePoint(lane.LanePath().GetClosestPoint(vehicleState.Front), TahoeParams.VL * 3.0).Location);
// check both clear to widen
bool widenOk = rightClear && leftClear;
// Notify
ArbiterOutput.Output("Blockage tactical recovery escalation: rightClear: " + rightClear.ToString() + ", leftCler: " + leftClear.ToString());
// if we can't widen for some reason just go through with no widen
StayInLaneBehavior silb = this.LaneRecoveryBehavior(lane, vehicleState, vehicleSpeed, plan, brs, brs.EncounteredState);
silb.TimeStamp = vehicleState.Timestamp;
// check widen
if (widenOk)
{
// output
ArbiterOutput.Output("Lane Blockage Recovery: Adjacent areas clear");
// mini icrease width
silb.LaneWidth = silb.LaneWidth + TahoeParams.T;
// check execute none saudi
CompletionReport l0Cr;
bool l0TestOk = CoreCommon.Communications.TestExecute(silb, out l0Cr);
// check mini ok
if (l0TestOk)
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen ok");
// update the current state
BlockageRecoveryState brsL0 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL0, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
else
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test Tahoe.T lane widen failed, moving to large widen");
#region Change Lanes
// check not in change lanes
if (brs.Defcon != BlockageRecoveryDEFCON.CHANGELANES_FORWARD && brs.Defcon != BlockageRecoveryDEFCON.WIDENBOUNDS)
{
// check normal change lanes reasoning
bool shouldWait;
IState laneChangeCompletionState;
ChangeLaneBehavior changeLanesBehavior = this.ChangeLanesRecoveryBehavior(lane, vehicleState, out shouldWait, out laneChangeCompletionState);
// check change lanes behaviore exists
if (changeLanesBehavior != null)
{
ArbiterOutput.Output("Lane Blockage Recovery: Found adjacent lane available change lanes beahvior: " + changeLanesBehavior.ToShortString() + ", " + changeLanesBehavior.ShortBehaviorInformation());
// update the current state
BlockageRecoveryState brsCL = new BlockageRecoveryState(
changeLanesBehavior, laneChangeCompletionState, sils, BlockageRecoveryDEFCON.CHANGELANES_FORWARD, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(changeLanesBehavior, brsCL, changeLanesBehavior.ChangeLeft ? TurnDecorators.LeftTurnDecorator : TurnDecorators.RightTurnDecorator, vehicleState.Timestamp);
}
// check should wait
else if (shouldWait)
{
ArbiterOutput.Output("Lane Blockage Recovery: Should wait for the forward lane, waiting");
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
}
#endregion
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Fell Through Forward Change Lanes");
// increase width
silb.LaneWidth = silb.LaneWidth * 3.0;
// check execute l1
CompletionReport l1Cr;
bool l1TestOk = CoreCommon.Communications.TestExecute(silb, out l1Cr);
// check ok
if (l1TestOk)
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen ok");
// update the current state
BlockageRecoveryState brsL1 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL1, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
// go to l2 for all the marbles
else
{
// notify
ArbiterOutput.Output("Lane Blockage Recovery: Test 3LW lane large widen failed, moving to 3LW, L1 Saudi");
ShutUpAndDoItDecorator saudi2 = new ShutUpAndDoItDecorator(SAUDILevel.L1);
List<BehaviorDecorator> saudi2bds = new List<BehaviorDecorator>(new BehaviorDecorator[] { saudi2 });
silb.Decorators = saudi2bds;
BlockageRecoveryState brsL2 = new BlockageRecoveryState(
silb, sils, sils, BlockageRecoveryDEFCON.WIDENBOUNDS, brs.EncounteredState, BlockageRecoverySTATUS.EXECUTING);
return new Maneuver(silb, brsL2, saudi2bds, vehicleState.Timestamp);
}
}
}
#endregion
// fallout goes back to lane state
return new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp);
}
private ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp)
{
// check the lane model probability
if (laneModel.Probability < lane_probability_reject_threshold) {
// we're rejecting this, just return a path lane model
return new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
}
// project the lane model's path into the rndf path's timestamp
RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp);
LinePath laneModelPath = laneModel.LanePath.Transform(relTransform);
// iterate through the waypoints in the RNDF path and project onto the lane model
// the first one that is over the threshold, we consider the waypoint before as a potential ending point
LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath();
// flag indicating if any of the waypoint tests failed because of the devation was too high
bool anyDeviationTooHigh = false;
// number of waypoints accepted
int numWaypointsAccepted = 0;
// get the vehicle's position on the rndf path
LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint;
// get the vehicle's position on the lane model
LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint;
// get the last point we want to consider on the lane model
LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist);
// start walking forward through the waypoints on the rndf path
// this loop will implicitly exit when we're past the end of the lane model as the waypoints
// will stop being close to the lane model (GetClosestPoint returns the end point if we're past the
// end of the path)
for (int i = rndfZeroPoint.Index+1; i < rndfPath.Count; i++) {
// get the waypoint
Coordinates rndfWaypoint = rndfPath[i];
// get the closest point on the lane model
LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint);
// compute the distance between the two
double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location);
// if this is above the deviation threshold, leave the loop
if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint) {
// if we're at the end of the lane model path, we don't want to consider this a rejection
if (laneModelClosestPoint < laneModelFarthestPoint) {
// mark that at least on deviation was too high
anyDeviationTooHigh = true;
}
break;
}
// increment the number of waypoint accepted
numWaypointsAccepted++;
// update the end point of where we're valid as the local road model was OK up to this point
laneModelDeviationEndPoint = laneModelClosestPoint;
}
// go through and figure out how far out the variance is within tolerance
LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath();
// walk forward from this point until the end of the lane mode path
for (int i = laneModelZeroPoint.Index+1; i < laneModelPath.Count; i++) {
// check if we're within the variance toleration
if (laneModel.LaneYVariance[i] <= y_var_reject_threshold) {
// we are, update the point on path
laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i);
}
else {
// we are out of tolerance, break out of the loop
break;
}
}
// now figure out everything out
// determine waypoint rejection status
WaypointRejectionResult waypointRejectionResult;
if (laneModelDeviationEndPoint.Valid) {
// if the point is valid, that we had at least one waypoint that was ok
// check if any waypoints were rejected
if (anyDeviationTooHigh) {
// some waypoint was ok, so we know that at least one waypoint was accepted
waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted;
}
else {
// no waypoint triggered a rejection, but at least one was good
waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted;
}
}
else {
// the point is not valid, so we either had no waypoints or we had all rejections
if (anyDeviationTooHigh) {
// the first waypoint was rejected, so all are rejected
waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected;
}
else {
// the first waypoint (if any) was past the end of the lane model
waypointRejectionResult = WaypointRejectionResult.NoWaypoints;
}
}
// criteria for determining if this path is valid:
// - if some or all waypoints were accepted, than this is probably a good path
// - if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted
// - if there were no waypoints, this is a potentially dangerous situation since we can't reject
// or confirm the local road model. for now, we'll assume that it is correct but this may need to change
// if we start handling intersections in this framework
// - if all waypoints were rejected, than we don't use the local road model
// - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of
// the lane model was in tolerance
// now build out the lane model
ILaneModel finalLaneModel;
// check if we rejected all waypoints or no lane model points satisified the variance threshold
if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid) {
// want to just use the path lane model
finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth);
}
else {
// we'll use the lane model
// need to build up the center line as well as left and right bounds
// to build up the center line, use the lane model as far as we feel comfortable (limited by either variance
// or by rejections) and then use the rndf lane after that.
LinePath centerLine = new LinePath();
// figure out the max distance
// if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model
if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints) {
laneModelDeviationEndPoint = laneModelFarthestPoint;
}
// figure out the closer of the end points
LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint;
bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint;
// add the lane model to the center line
centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint));
// create a list to hold the width expansion values
List<double> widthValue = new List<double>();
// make the width expansion values the width of the path plus the 1-sigma values
for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index+centerLine.Count; i++) {
widthValue.Add(laneModel.Width/2.0 + Math.Sqrt(laneModel.LaneYVariance[i]));
}
// now figure out how to add the rndf path
// get the projection of the lane model end point on the rndf path
LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location);
// if the closest point is past the end of rndf path, then we don't want to tack anything on
if (rndfPathStartPoint != rndfPath.EndPoint) {
// get the last segment of the new center line
Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector;
// get the last point of the new center line
Coordinates laneModelEndLoc = laneModelEndPoint.Location;
// now figure out the distance to the next waypoint
LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath();
// figure out if we're ending at a waypoint or not
if (endAtWaypoint) {
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index+1);
// if the distance from the start point to the next point is less than rndf_dist_min, then
// use the waypont after
double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint);
if (dist < rndf_dist_min) {
if (rndfPathStartPoint.Index < rndfPath.Count-2) {
rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2);
}
else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min) {
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
else {
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min*2);
}
}
}
else {
// track the last angle we had
double lastAngle = double.NaN;
// walk down the rndf path until we find a valid point
for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step) {
// advance from the start point by dist
double distTemp = dist;
rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp);
// if the distTemp is > 0, then we're past the end of the path
if (distTemp > 0) {
// if we're immediately past the end, we don't want to tack anything on
if (dist == rndf_dist_min) {
rndfNextPoint = LinePath.PointOnPath.Invalid;
}
break;
}
// check the angle made by the last segment of center line and the segment
// formed between the end point of the center line and this new point
double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location-laneModelEndLoc).Normalize()));
// check if the angle satisfies the threshold or we're increasing the angle
if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle)) {
// break out of the loop, we're done searching
break;
}
lastAngle = angle;
}
}
// tack on the rndf starting at next point going to the end
if (rndfNextPoint.Valid) {
LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint);
centerLine.AddRange(subPath);
// insert the lane model end point into the sub path
subPath.Insert(0, laneModelEndLoc);
// get the angles
List<Pair<int, double>> angles = subPath.GetIntersectionAngles(0, subPath.Count-1);
// add the width of the path inflated by the angles
for (int i = 0; i < angles.Count; i++) {
// calculate the width expansion factor
// 90 deg, 3x width
// 45 deg, 1.5x width
// 0 deg, 1x width
double widthFactor = Math.Pow(angles[i].Right/(Math.PI/2.0), 2)*2 + 1;
// add the width value
widthValue.Add(widthFactor*laneModel.Width/2);
}
// add the final width
widthValue.Add(laneModel.Width/2);
}
// set the rndf path start point to be the point we used
rndfPathStartPoint = rndfNextPoint;
}
// for now, calculate the left and right bounds the same way we do for the path lane model
// TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty
LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray());
// get the next shifts
for (int i = 0; i < widthValue.Count; i++) { widthValue[i] = -widthValue[i]; }
LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray());
// build the final lane model
finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp);
}
SendLaneModelToUI(finalLaneModel, rndfPathTimestamp);
// output the fit result
return finalLaneModel;
}
public static bool TestBlockage(IList<Polygon> obstaclePolygons, LinePath leftBound, LinePath rightBound, double expandDist, double trackWidth)
{
try
{
Circle robotCircle = new Circle(expandDist, Vector2.Zero);
Polygon robotPoly = robotCircle.ToPolygon(24);
List<BlockageData> obstacles = new List<BlockageData>(obstaclePolygons.Count);
foreach (Polygon poly in obstaclePolygons)
{
Polygon convolvedPoly = null;
try
{
convolvedPoly = Polygon.ConvexMinkowskiConvolution(robotPoly, poly);
}
catch (Exception)
{
// minkowski convolution failed, just expand that shit with the gaheezy inflate method
convolvedPoly = poly.Inflate(expandDist);
}
// add the entry to the obstacle collection
BlockageData data = new BlockageData(convolvedPoly);
obstacles.Add(data);
}
// shrink in the lanes by a half robot width
leftBound = leftBound.ShiftLateral(-trackWidth / 2.0);
rightBound = rightBound.ShiftLateral(-trackWidth / 2.0);
Queue<BlockageData> testQueue = new Queue<BlockageData>();
foreach (BlockageData obs in obstacles)
{
if (obs.convolvedPolygon.DoesIntersect(leftBound))
{
// check if this hits the right bound
if (obs.convolvedPolygon.DoesIntersect(rightBound))
{
// this extends across the entire lane, the segment is blocked
return true;
}
else
{
testQueue.Enqueue(obs);
obs.searchMark = true;
}
}
}
while (testQueue.Count > 0)
{
BlockageData obs = testQueue.Dequeue();
foreach (BlockageData neighbor in obstacles)
{
if (!neighbor.searchMark && Polygon.TestConvexIntersection(obs.convolvedPolygon, neighbor.convolvedPolygon))
{
if (neighbor.convolvedPolygon.DoesIntersect(rightBound))
return true;
testQueue.Enqueue(neighbor);
neighbor.searchMark = true;
}
}
}
return false;
}
catch (Exception) { }
return false;
}
/// <summary>
/// Plans what maneuer we should take next
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState,
SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, List <ITacticalBlockage> blockages)
{
#region Waiting At Intersection Exit
if (planningState is WaitingAtIntersectionExitState)
{
// state
WaitingAtIntersectionExitState waies = (WaitingAtIntersectionExitState)planningState;
// get intersection plan
IntersectionPlan ip = (IntersectionPlan)navigationalPlan;
// nullify turn reasoning
this.TurnReasoning = null;
#region Intersection Monitor Updates
// check correct intersection monitor
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(waies.exitWaypoint.AreaSubtypeWaypointId) &&
(IntersectionTactical.IntersectionMonitor == null ||
!IntersectionTactical.IntersectionMonitor.OurMonitor.Waypoint.Equals(waies.exitWaypoint)))
{
// create new intersection monitor
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
waies.exitWaypoint,
CoreCommon.RoadNetwork.IntersectionLookup[waies.exitWaypoint.AreaSubtypeWaypointId],
vehicleState,
ip.BestOption);
}
// update if exists
if (IntersectionTactical.IntersectionMonitor != null)
{
// update monitor
IntersectionTactical.IntersectionMonitor.Update(vehicleState);
// print current
ArbiterOutput.Output(IntersectionTactical.IntersectionMonitor.IntersectionStateString());
}
#endregion
#region Desired Behavior
// get best option from previously saved
IConnectAreaWaypoints icaw = null;
if (waies.desired != null)
{
ArbiterInterconnect tmpInterconnect = waies.desired;
if (waies.desired.InitialGeneric is ArbiterWaypoint)
{
ArbiterWaypoint init = (ArbiterWaypoint)waies.desired.InitialGeneric;
if (init.NextPartition != null && init.NextPartition.Final.Equals(tmpInterconnect.FinalGeneric))
{
icaw = init.NextPartition;
}
else
{
icaw = waies.desired;
}
}
else
{
icaw = waies.desired;
}
}
else
{
icaw = ip.BestOption;
waies.desired = icaw.ToInterconnect;
}
#endregion
#region Turn Feasibility Reasoning
// check uturn
if (waies.desired.TurnDirection == ArbiterTurnDirection.UTurn)
{
waies.turnTestState = TurnTestState.Completed;
}
// check already determined feasible
if (waies.turnTestState == TurnTestState.Unknown ||
waies.turnTestState == TurnTestState.Failed)
{
#region Determine Behavior to Accomplish Turn
// get default turn behavior
TurnBehavior testTurnBehavior = this.DefaultTurnBehavior(icaw);
// set saudi decorator
if (waies.saudi != SAUDILevel.None)
{
testTurnBehavior.Decorators.Add(new ShutUpAndDoItDecorator(waies.saudi));
}
// set to ignore all vehicles
testTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });
#endregion
#region Check Turn Feasible
// check if we have completed
CompletionReport turnCompletionReport;
bool completedTest = CoreCommon.Communications.TestExecute(testTurnBehavior, out turnCompletionReport); //CoreCommon.Communications.AsynchronousTestHasCompleted(testTurnBehavior, out turnCompletionReport, true);
// if we have completed the test
if (completedTest || ((TrajectoryBlockedReport)turnCompletionReport).BlockageType != BlockageType.Dynamic)
{
#region Can Complete
// check success
if (turnCompletionReport.Result == CompletionResult.Success)
{
// set completion state of the turn
waies.turnTestState = TurnTestState.Completed;
}
#endregion
#region No Saudi Level, Found Initial Blockage
// otherwise we cannot do the turn, check if saudi is still none
else if (waies.saudi == SAUDILevel.None)
{
// notify
ArbiterOutput.Output("Increased Saudi Level of Turn to L1");
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Failed;
}
#endregion
#region Already at L1 Saudi
else if (waies.saudi == SAUDILevel.L1)
{
// notify
ArbiterOutput.Output("Turn with Saudi L1 Level failed");
// get an intersection plan without this interconnect
IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect(
waies.exitWaypoint,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
waies.desired);
// check that the plan exists
if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) &&
testPlan.BestRouteTime < double.MaxValue - 1.0)
{
// get the desired interconnect
ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect;
#region Check that the reset interconnect is feasible
// test the reset interconnect
TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset);
// set to ignore all vehicles
testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });
// check if we have completed
CompletionReport turnResetCompletionReport;
bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport);
// check to see if this is feasible
if (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95)
{
// notify
ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to current interconnect: " + waies.desired.ToString());
// set the interconnect as being blocked
CoreCommon.Navigation.AddInterconnectBlockage(waies.desired);
// reset all
waies.desired = reset;
waies.turnTestState = TurnTestState.Completed;
waies.saudi = SAUDILevel.None;
waies.useTurnBounds = true;
IntersectionMonitor.ResetDesired(reset);
}
#endregion
#region No Lane Bounds
// otherwise try without lane bounds
else
{
// notify
ArbiterOutput.Output("Had to fallout to using no turn bounds");
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Completed;
waies.useTurnBounds = false;
}
#endregion
}
#region No Lane Bounds
// otherwise try without lane bounds
else
{
// up the saudi level, set as turn failed and no other option
waies.saudi = SAUDILevel.L1;
waies.turnTestState = TurnTestState.Unknown;
waies.useTurnBounds = false;
}
#endregion
}
#endregion
// want to reset ourselves
return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
}
#endregion
#region Entry Monitor Blocked
// checks the entry monitor vehicle for failure
if (IntersectionMonitor != null && IntersectionMonitor.EntryAreaMonitor != null &&
IntersectionMonitor.EntryAreaMonitor.Vehicle != null && IntersectionMonitor.EntryAreaMonitor.Failed)
{
ArbiterOutput.Output("Entry area blocked");
// get an intersection plan without this interconnect
IntersectionPlan testPlan = CoreCommon.Navigation.PlanIntersectionWithoutInterconnect(
waies.exitWaypoint,
CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId],
waies.desired,
true);
// check that the plan exists
if (!testPlan.BestOption.ToInterconnect.Equals(waies.desired) &&
testPlan.BestRouteTime < double.MaxValue - 1.0)
{
// get the desired interconnect
ArbiterInterconnect reset = testPlan.BestOption.ToInterconnect;
#region Check that the reset interconnect is feasible
// test the reset interconnect
TurnBehavior testResetTurnBehavior = this.DefaultTurnBehavior(reset);
// set to ignore all vehicles
testResetTurnBehavior.VehiclesToIgnore = new List <int>(new int[] { -1 });
// check if we have completed
CompletionReport turnResetCompletionReport;
bool completedResetTest = CoreCommon.Communications.TestExecute(testResetTurnBehavior, out turnResetCompletionReport);
// check to see if this is feasible
if (reset.TurnDirection == ArbiterTurnDirection.UTurn || (completedResetTest && turnResetCompletionReport is SuccessCompletionReport && reset.Blockage.ProbabilityExists < 0.95))
{
// notify
ArbiterOutput.Output("Found clear interconnect: " + reset.ToString() + " adding blockage to all possible turns into final");
// set all the interconnects to the final as being blocked
if (((ITraversableWaypoint)waies.desired.FinalGeneric).IsEntry)
{
foreach (ArbiterInterconnect toBlock in ((ITraversableWaypoint)waies.desired.FinalGeneric).Entries)
{
CoreCommon.Navigation.AddInterconnectBlockage(toBlock);
}
}
// check if exists previous partition to block
if (waies.desired.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint finWaypoint = (ArbiterWaypoint)waies.desired.FinalGeneric;
if (finWaypoint.PreviousPartition != null)
{
CoreCommon.Navigation.AddBlockage(finWaypoint.PreviousPartition, finWaypoint.Position, false);
}
}
// reset all
waies.desired = reset;
waies.turnTestState = TurnTestState.Completed;
waies.saudi = SAUDILevel.None;
waies.useTurnBounds = true;
IntersectionMonitor.ResetDesired(reset);
// want to reset ourselves
return(new Maneuver(new HoldBrakeBehavior(), CoreCommon.CorePlanningState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
#endregion
}
else
{
ArbiterOutput.Output("Entry area blocked, but no otehr valid route found");
}
}
#endregion
// check if can traverse
if (IntersectionTactical.IntersectionMonitor == null || IntersectionTactical.IntersectionMonitor.CanTraverse(icaw, vehicleState))
{
#region If can traverse the intersection
// quick check not interconnect
if (!(icaw is ArbiterInterconnect))
{
icaw = icaw.ToInterconnect;
}
// get interconnect
ArbiterInterconnect ai = (ArbiterInterconnect)icaw;
// clear all old completion reports
CoreCommon.Communications.ClearCompletionReports();
// check if uturn
if (ai.InitialGeneric is ArbiterWaypoint && ai.FinalGeneric is ArbiterWaypoint && ai.TurnDirection == ArbiterTurnDirection.UTurn)
{
// go into turn
List <ArbiterLane> involvedLanes = new List <ArbiterLane>();
involvedLanes.Add(((ArbiterWaypoint)ai.InitialGeneric).Lane);
involvedLanes.Add(((ArbiterWaypoint)ai.FinalGeneric).Lane);
uTurnState nextState = new uTurnState(((ArbiterWaypoint)ai.FinalGeneric).Lane,
IntersectionToolkit.uTurnBounds(vehicleState, involvedLanes));
nextState.Interconnect = ai;
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
else
{
if (ai.FinalGeneric is ArbiterWaypoint)
{
ArbiterWaypoint finalWaypoint = (ArbiterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.TurnInfo(finalWaypoint, out finalPath, out leftLL, out rightLL);
// go into turn
IState nextState = new TurnState(ai, ai.TurnDirection, finalWaypoint.Lane, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds);
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
else
{
// final perimeter waypoint
ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)ai.FinalGeneric;
// get turn params
LinePath finalPath;
LineList leftLL;
LineList rightLL;
IntersectionToolkit.ZoneTurnInfo(ai, apw, out finalPath, out leftLL, out rightLL);
// go into turn
IState nextState = new TurnState(ai, ai.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(2.5), waies.saudi, waies.useTurnBounds);
// hold brake
Behavior b = new HoldBrakeBehavior();
// return maneuver
return(new Maneuver(b, nextState, nextState.DefaultStateDecorators, vehicleState.Timestamp));
}
}
#endregion
}
// otherwise need to wait
else
{
IState next = waies;
return(new Maneuver(new HoldBrakeBehavior(), next, next.DefaultStateDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Stopping At Exit
else if (planningState is StoppingAtExitState)
{
// cast to exit stopping
StoppingAtExitState saes = (StoppingAtExitState)planningState;
saes.currentPosition = vehicleState.Front;
// get intersection plan
IntersectionPlan ip = (IntersectionPlan)navigationalPlan;
// if has an intersection
if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(saes.waypoint.AreaSubtypeWaypointId))
{
// create new intersection monitor
IntersectionTactical.IntersectionMonitor = new IntersectionMonitor(
saes.waypoint,
CoreCommon.RoadNetwork.IntersectionLookup[saes.waypoint.AreaSubtypeWaypointId],
vehicleState,
ip.BestOption);
// update it
IntersectionTactical.IntersectionMonitor.Update(vehicleState);
}
else
{
IntersectionTactical.IntersectionMonitor = null;
}
// otherwise update the stop parameters
saes.currentPosition = vehicleState.Front;
Behavior b = saes.Resume(vehicleState, CoreCommon.Communications.GetVehicleSpeed().Value);
return(new Maneuver(b, saes, saes.DefaultStateDecorators, vehicleState.Timestamp));
}
#endregion
#region In uTurn
else if (planningState is uTurnState)
{
// get state
uTurnState uts = (uTurnState)planningState;
// check if in other lane
if (CoreCommon.Communications.HasCompleted((new UTurnBehavior(null, null, null, null)).GetType()))
{
// quick check
if (uts.Interconnect != null && uts.Interconnect.FinalGeneric is ArbiterWaypoint)
{
// get the closest partition to the new lane
ArbiterLanePartition alpClose = uts.TargetLane.GetClosestPartition(vehicleState.Front);
// waypoints
ArbiterWaypoint partitionInitial = alpClose.Initial;
ArbiterWaypoint uturnEnd = (ArbiterWaypoint)uts.Interconnect.FinalGeneric;
// check initial past end
if (partitionInitial.WaypointId.Number > uturnEnd.WaypointId.Number)
{
// get waypoints inclusive
List <ArbiterWaypoint> inclusive = uts.TargetLane.WaypointsInclusive(uturnEnd, partitionInitial);
bool found = false;
// loop through
foreach (ArbiterWaypoint aw in inclusive)
{
if (!found && aw.WaypointId.Equals(CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId))
{
// notiofy
ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as passed over in uturn");
// remove
CoreCommon.Mission.MissionCheckpoints.Dequeue();
// set found
found = true;
}
}
}
// default check
else if (uts.Interconnect.FinalGeneric.Equals(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId]))
{
// notiofy
ArbiterOutput.Output("removed checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().CheckpointNumber.ToString() + " as end of uturn");
// remove
CoreCommon.Mission.MissionCheckpoints.Dequeue();
}
}
// check if the uturn is for a blockage
else if (uts.Interconnect == null)
{
// get final lane
ArbiterLane targetLane = uts.TargetLane;
// check has opposing
if (targetLane.Way.Segment.Lanes.Count > 1)
{
// check the final checkpoint is in our lane
if (CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.AreaSubtypeId.Equals(targetLane.LaneId))
{
// check that the final checkpoint is within the uturn polygon
if (uts.Polygon != null &&
uts.Polygon.IsInside(CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId].Position))
{
// remove the checkpoint
ArbiterOutput.Output("Found checkpoint: " + CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId.ToString() + " inside blockage uturn area, dequeuing");
CoreCommon.Mission.MissionCheckpoints.Dequeue();
}
}
}
}
// stay in target lane
IState nextState = new StayInLaneState(uts.TargetLane, new Probability(0.8, 0.2), true, CoreCommon.CorePlanningState);
Behavior b = new StayInLaneBehavior(uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0), new List <int>(), uts.TargetLane.LanePath(), uts.TargetLane.Width, uts.TargetLane.NumberOfLanesLeft(vehicleState.Front, true), uts.TargetLane.NumberOfLanesRight(vehicleState.Front, true));
return(new Maneuver(b, nextState, TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
// otherwise continue uturn
else
{
// get polygon
Polygon p = uts.Polygon;
// add polygon to observable
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(p, ArbiterInformationDisplayObjectType.uTurnPolygon));
// check the type of uturn
if (!uts.singleLaneUturn)
{
// get ending path
LinePath endingPath = uts.TargetLane.LanePath();
// next state is current
IState nextState = uts;
// behavior
Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0));
// maneuver
return(new Maneuver(b, nextState, null, vehicleState.Timestamp));
}
else
{
// get ending path
LinePath endingPath = uts.TargetLane.LanePath().Clone();
endingPath = endingPath.ShiftLateral(-2.0); //uts.TargetLane.Width);
// add polygon to observable
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(endingPath, ArbiterInformationDisplayObjectType.leftBound));
// next state is current
IState nextState = uts;
// behavior
Behavior b = new UTurnBehavior(p, endingPath, uts.TargetLane.LaneId, new ScalarSpeedCommand(2.0));
// maneuver
return(new Maneuver(b, nextState, null, vehicleState.Timestamp));
}
}
}
#endregion
#region In Turn
else if (planningState is TurnState)
{
// get state
TurnState ts = (TurnState)planningState;
// add bounds to observable
if (ts.LeftBound != null && ts.RightBound != null)
{
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.LeftBound, ArbiterInformationDisplayObjectType.leftBound));
CoreCommon.CurrentInformation.DisplayObjects.Add(new ArbiterInformationDisplayObject(ts.RightBound, ArbiterInformationDisplayObjectType.rightBound));
}
// create current turn reasoning
if (this.TurnReasoning == null)
{
this.TurnReasoning = new TurnReasoning(ts.Interconnect,
IntersectionTactical.IntersectionMonitor != null ? IntersectionTactical.IntersectionMonitor.EntryAreaMonitor : null);
}
// get primary maneuver
Maneuver primary = this.TurnReasoning.PrimaryManeuver(vehicleState, blockages, ts);
// get secondary maneuver
Maneuver?secondary = this.TurnReasoning.SecondaryManeuver(vehicleState, (IntersectionPlan)navigationalPlan);
// return the manevuer
return(secondary.HasValue ? secondary.Value : primary);
}
#endregion
#region Itnersection Startup
else if (planningState is IntersectionStartupState)
{
// state and plan
IntersectionStartupState iss = (IntersectionStartupState)planningState;
IntersectionStartupPlan isp = (IntersectionStartupPlan)navigationalPlan;
// initial path
LinePath vehiclePath = new LinePath(new Coordinates[] { vehicleState.Rear, vehicleState.Front });
List <ITraversableWaypoint> feasibleEntries = new List <ITraversableWaypoint>();
// vehicle polygon forward of us
Polygon vehicleForward = vehicleState.ForwardPolygon;
// best waypoint
ITraversableWaypoint best = null;
double bestCost = Double.MaxValue;
// given feasible choose best, no feasible choose random
if (feasibleEntries.Count == 0)
{
foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values)
{
if (vehicleForward.IsInside(itw.Position))
{
feasibleEntries.Add(itw);
}
}
if (feasibleEntries.Count == 0)
{
foreach (ITraversableWaypoint itw in iss.Intersection.AllEntries.Values)
{
feasibleEntries.Add(itw);
}
}
}
// get best
foreach (ITraversableWaypoint itw in feasibleEntries)
{
if (isp.NodeTimeCosts.ContainsKey(itw))
{
KeyValuePair <ITraversableWaypoint, double> lookup = new KeyValuePair <ITraversableWaypoint, double>(itw, isp.NodeTimeCosts[itw]);
if (best == null || lookup.Value < bestCost)
{
best = lookup.Key;
bestCost = lookup.Value;
}
}
}
// get something going to this waypoint
ArbiterInterconnect interconnect = null;
if (best.IsEntry)
{
ArbiterInterconnect closest = null;
double closestDistance = double.MaxValue;
foreach (ArbiterInterconnect ai in best.Entries)
{
double dist = ai.InterconnectPath.GetClosestPoint(vehicleState.Front).Location.DistanceTo(vehicleState.Front);
if (closest == null || dist < closestDistance)
{
closest = ai;
closestDistance = dist;
}
}
interconnect = closest;
}
else if (best is ArbiterWaypoint && ((ArbiterWaypoint)best).PreviousPartition != null)
{
interconnect = ((ArbiterWaypoint)best).PreviousPartition.ToInterconnect;
}
// get state
if (best is ArbiterWaypoint)
{
// go to this turn state
LinePath finalPath;
LineList leftBound;
LineList rightBound;
IntersectionToolkit.TurnInfo((ArbiterWaypoint)best, out finalPath, out leftBound, out rightBound);
return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, ((ArbiterWaypoint)best).Lane,
finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
else
{
// go to this turn state
LinePath finalPath;
LineList leftBound;
LineList rightBound;
IntersectionToolkit.ZoneTurnInfo(interconnect, (ArbiterPerimeterWaypoint)best, out finalPath, out leftBound, out rightBound);
return(new Maneuver(new HoldBrakeBehavior(), new TurnState(interconnect, interconnect.TurnDirection, null,
finalPath, leftBound, rightBound, new ScalarSpeedCommand(2.0)), TurnDecorators.NoDecorators, vehicleState.Timestamp));
}
}
#endregion
#region Unknown
else
{
throw new Exception("Unknown planning state in intersection tactical plan: " + planningState.ToString());
}
#endregion
}