public SensedObsacleDisplay()
{
untrackedClusters = new SceneEstimatorUntrackedClusterCollection();
untrackedClusters.clusters = new SceneEstimatorUntrackedCluster[] { };
trackedClusters = new SceneEstimatorTrackedClusterCollection();
trackedClusters.clusters = new SceneEstimatorTrackedCluster[] { };
}
/// <summary>
/// Called when message sent to us
/// </summary>
/// <param name="channelName"></param>
/// <param name="message"></param>
public void MessageArrived(string channelName, object message)
{
if (channelName == "ArbiterSceneEstimatorPositionChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is VehicleState)
{
// cast and set
vehicleState = (VehicleState)message;
}
else if (channelName == "ObservedObstacleChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is SceneEstimatorUntrackedClusterCollection)
{
// cast and set
observedObstacles = (SceneEstimatorUntrackedClusterCollection)message;
}
else if (channelName == "ObservedVehicleChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is SceneEstimatorTrackedClusterCollection)
{
// cast and set
observedVehicles = (SceneEstimatorTrackedClusterCollection)message;
}
else if (channelName == "VehicleSpeedChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is double)
{
// cast and set
vehicleSpeed = (double)message;
}
else if (channelName == "ArbiterOutputChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is string)
{
// output
RemoraOutput.WriteLine((string)message, OutputType.Arbiter);
}
else if (channelName == "ArbiterInformationChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is ArbiterInformation)
{
// set info
RemoraCommon.aiInformation.information = (ArbiterInformation)message;
}
else if (channelName == "SideObstacleChannel" + RemoraCommon.Communicator.RemotingSuffix &&
message is SideObstacles)
{
SideObstacles sideSickObstacles = (SideObstacles)message;
if (sideSickObstacles.side == SideObstacleSide.Driver)
{
this.sideSickObstaclesDriver = sideSickObstacles;
}
else
{
this.sideSickObstaclesPass = sideSickObstacles;
}
}
}
/// <summary>
/// Called when message sent to us
/// </summary>
/// <param name="channelName"></param>
/// <param name="message"></param>
public void MessageArrived(string channelName, object message)
{
try
{
if (channelName == "ArbiterSceneEstimatorPositionChannel" + this.remotingSuffix &&
message is VehicleState)
{
// cast and set
vehicleState = (VehicleState)message;
}
else if (channelName == "ObservedObstacleChannel" + this.remotingSuffix &&
message is SceneEstimatorUntrackedClusterCollection)
{
// cast and set
observedObstacles = (SceneEstimatorUntrackedClusterCollection)message;
}
else if (channelName == "ObservedVehicleChannel" + this.remotingSuffix &&
message is SceneEstimatorTrackedClusterCollection)
{
// check if not ignoring vehicles
if (!Arbiter.Core.ArbiterSettings.Default.IgnoreVehicles)
{
// cast and set
observedVehicles = (SceneEstimatorTrackedClusterCollection)message;
}
}
else if (channelName == "VehicleSpeedChannel" + this.remotingSuffix &&
message is double)
{
// cast and set
vehicleSpeed = (double)message;
}
else if (channelName == "SideObstacleChannel" + this.remotingSuffix &&
message is SideObstacles)
{
SideObstacles sideSickObstacles = (SideObstacles)message;
if (sideSickObstacles.side == SideObstacleSide.Driver)
{
this.sideSickObstaclesDriver = sideSickObstacles;
}
else
{
this.sideSickObstaclesPass = sideSickObstacles;
}
}
}
catch (Exception ex)
{
ArbiterOutput.Output("Error receiving message: " + ex.ToString());
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="remotingSuffix"></param>
public MessagingListener(string remotingSuffix)
{
this.remotingSuffix = remotingSuffix;
// check if sim mode HACK
bool simMode = global::UrbanChallenge.Arbiter.Core.ArbiterSettings.Default.SimMode;
if (!simMode)
{
SceneEstimatorTrackedClusterCollection setc = new SceneEstimatorTrackedClusterCollection();
setc.clusters = new SceneEstimatorTrackedCluster[] { };
this.observedVehicles = setc;
SceneEstimatorUntrackedClusterCollection seutc = new SceneEstimatorUntrackedClusterCollection();
seutc.clusters = new SceneEstimatorUntrackedCluster[] { };
this.observedObstacles = seutc;
}
}
static void Main(string[] args)
{
SimSensor sensor = new SimSensor(Math.PI / 4, -Math.PI / 4, 1 * Math.PI / 180, SensorType.Scan, 30);
SimObstacleState obsState = new SimObstacleState();
obsState.Position = new Coordinates(5, 5);
obsState.Length = 4;
obsState.Width = 2;
obsState.Heading = new Coordinates(1, 0);
Polygon[] poly = new Polygon[] { obsState.ToPolygon() };
SceneEstimatorUntrackedClusterCollection clusters = new SceneEstimatorUntrackedClusterCollection();
sensor.GetHits(poly, Coordinates.Zero, 0, clusters);
SimulatorClient client = new SimulatorClient();
client.BeginClient();
}
/// <summary>
/// Turn sim obstacle states into obstacles
/// </summary>
/// <returns></returns>
public SceneEstimatorUntrackedClusterCollection ObstaclesFromWorld(SimVehicleId ours, Coordinates vehiclePosition, double vehicleHeading, double ts)
{
SceneEstimatorUntrackedClusterCollection seucc = new SceneEstimatorUntrackedClusterCollection();
seucc.timestamp = (double)SimulatorClient.GetCurrentTimestamp;
List <Polygon> obstaclePolygons = new List <Polygon>(worldState.Obstacles.Count);
foreach (SimObstacleState obstacle in worldState.Obstacles.Values)
{
if (obstacle.ObstacleId.Number != ours.Number)
{
obstaclePolygons.Add(obstacle.ToPolygon());
}
}
// use the lidar to get the hits
lidar.GetHits(obstaclePolygons, vehiclePosition, vehicleHeading, seucc);
lidar2.GetHits(obstaclePolygons, vehiclePosition, vehicleHeading, seucc);
return(seucc);
}
public SceneEstimatorUCC_RXEventArgs(SceneEstimatorUntrackedClusterCollection ucc)
{
this.ucc = ucc;
}
public void GetHits(IList <Polygon> obstacles, Coordinates vehicleLoc, double vehicleHeading, SceneEstimatorUntrackedClusterCollection clusters)
{
// for now, only support the Scan sensor type
if (sensorType != SensorType.Scan)
{
throw new NotSupportedException("Only Scan sensor type is supported at the moment");
}
Matrix3 transform = Matrix3.Rotation(-vehicleHeading) * Matrix3.Translation(-vehicleLoc.X, -vehicleLoc.Y);
// for each obstacle, get the bounding circle, see if it's in range
List <Polygon> possibleTargets = new List <Polygon>(obstacles.Count);
for (int i = 0; i < obstacles.Count; i++)
{
Circle boundingCircle = obstacles[i].BoundingCircle;
double minDist = boundingCircle.center.DistanceTo(vehicleLoc) - boundingCircle.r;
if (minDist < range)
{
possibleTargets.Add(obstacles[i].Transform(transform));
}
}
// now start doing the actual rays
int numRays = (int)Math.Round((leftAngularExtent - rightAngularExtent) / beamResolution);
double beamStep = (leftAngularExtent - rightAngularExtent) / numRays;
numRays++;
HitData[] rays = new HitData[numRays];
for (int rayNum = 0; rayNum < numRays; rayNum++)
{
double angle = rightAngularExtent + rayNum * beamStep;
Coordinates unitVec = Coordinates.FromAngle(angle);
LineSegment l = new LineSegment(Coordinates.Zero, unitVec * range);
rays[rayNum].line = l;
rays[rayNum].distance = double.MaxValue;
rays[rayNum].obstacleIndex = -1;
rays[rayNum].pt = Coordinates.NaN;
rays[rayNum].unitVec = unitVec;
}
// go through and intersect with each polygon
for (int rayInd = 0; rayInd < numRays; rayInd++)
{
HitData hd = rays[rayInd];
for (int i = 0; i < possibleTargets.Count; i++)
{
Coordinates[] pts;
double[] K;
if (possibleTargets[i].Intersect(hd.line, out pts, out K))
{
// find the min distance
double minK = K[0];
int minKind = 0;
for (int j = 1; j < K.Length; j++)
{
if (K[j] < minK)
{
minK = K[j];
minKind = j;
}
}
// get the minimum point
double dist = minK * range;
if (dist < hd.distance)
{
hd.distance = dist;
hd.obstacleIndex = i;
hd.pt = pts[minKind];
}
}
}
rays[rayInd] = hd;
}
Random rand = new Random();
// we have all the hits, form into clusters
int indPrev = -1;
List <Coordinates> clusterPoints = new List <Coordinates>();
List <SceneEstimatorUntrackedCluster> newClusters = new List <SceneEstimatorUntrackedCluster>();
for (int rayNum = 0; rayNum < numRays; rayNum++)
{
HitData hd = rays[rayNum];
if (hd.obstacleIndex != indPrev && clusterPoints.Count > 0)
{
// test if any points are within 10 meters
bool noneTooClose = true;
if (noneTooClose)
{
SceneEstimatorUntrackedCluster cluster = new SceneEstimatorUntrackedCluster();
cluster.points = clusterPoints.ToArray();
newClusters.Add(cluster);
clusterPoints.Clear();
}
}
indPrev = hd.obstacleIndex;
if (hd.obstacleIndex == -1)
{
continue;
}
// add noise to the point
double noisyRange = hd.distance + (2 * rand.NextDouble() - 1) * .25;
Coordinates pt = hd.unitVec * noisyRange;
clusterPoints.Add(pt);
}
if (indPrev != -1 && clusterPoints.Count > 0)
{
SceneEstimatorUntrackedCluster cluster = new SceneEstimatorUntrackedCluster();
cluster.points = clusterPoints.ToArray();
newClusters.Add(cluster);
}
SceneEstimatorUntrackedCluster[] allClusters;
if (clusters.clusters != null && clusters.clusters.Length > 0)
{
allClusters = new SceneEstimatorUntrackedCluster[clusters.clusters.Length + newClusters.Count];
Array.Copy(clusters.clusters, allClusters, clusters.clusters.Length);
newClusters.CopyTo(allClusters, clusters.clusters.Length);
}
else
{
allClusters = newClusters.ToArray();
}
clusters.clusters = allClusters;
}
/// <summary>
/// Update sensor states
/// </summary>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
public void Update(VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, double speed, LocalRoadEstimate lre, PathRoadModel prm)
{
this.vehicleStateChannel.PublishUnreliably(vehicleState);
this.observedVehicleChannel.PublishUnreliably(vehicles);
this.observedObstacleChannel.PublishUnreliably(obstacles);
this.vehicleSpeedChannel.PublishUnreliably(speed);
this.localRoadChannel.PublishUnreliably(lre);
if (prm != null)
{
this.localRoadChannel.PublishUnreliably(prm);
}
}
private List <Obstacle> ProcessUntrackedClusters(SceneEstimatorUntrackedClusterCollection clusters, List <Obstacle> trackedObstacles, Rect vehicleBox)
{
List <Obstacle> obstacles = new List <Obstacle>();
SortedList <Obstacle, List <Coordinates> > point_splits = new SortedList <Obstacle, List <Coordinates> >();
List <Coordinates> unclaimed_points = new List <Coordinates>(1500);
foreach (SceneEstimatorUntrackedCluster cluster in clusters.clusters)
{
// clear out stored variables
point_splits.Clear();
unclaimed_points.Clear();
// now determine if the point belongs to an old obstacle
ObstacleClass targetClass;
if (cluster.clusterClass == SceneEstimatorClusterClass.SCENE_EST_HighObstacle)
{
targetClass = ObstacleClass.StaticLarge;
}
else
{
targetClass = ObstacleClass.StaticSmall;
}
// only add points that are not within a tracked obstacle's extruded polygon
for (int i = 0; i < cluster.points.Length; i++)
{
Coordinates pt = cluster.points[i];
// iterate over all tracked cluster
bool did_hit = false;
if (useOccupancyGrid && Services.OccupancyGrid.GetOccupancy(pt) == OccupancyStatus.Free)
{
occupancyDeletedCount++;
did_hit = true;
}
else if (vehicleBox.IsInside(pt))
{
did_hit = true;
}
else if (trackedObstacles != null)
{
foreach (Obstacle trackedObs in trackedObstacles)
{
if (trackedObs.extrudedPolygon != null && trackedObs.extrudedPolygon.BoundingCircle.IsInside(pt) && trackedObs.extrudedPolygon.IsInside(pt))
{
did_hit = true;
break;
}
}
}
// if there was a hit, skip this point
if (!did_hit)
{
unclaimed_points.Add(pt);
}
//if (did_hit)
// continue;
//Obstacle oldObstacle = FindIntersectingCluster(pt, targetClass, previousObstacles);
//if (oldObstacle != null) {
// List<Coordinates> obstacle_points;
// if (!point_splits.TryGetValue(oldObstacle, out obstacle_points)) {
// obstacle_points = new List<Coordinates>(100);
// point_splits.Add(oldObstacle, obstacle_points);
// }
// obstacle_points.Add(pt);
//}
//else {
// unclaimed_points.Add(pt);
//}
}
// we've split up all the points appropriately
// now construct the obstacles
// we'll start with the obstacle belonging to an existing polygon
//foreach (KeyValuePair<Obstacle, List<Coordinates>> split in point_splits) {
// if (split.Value != null && split.Value.Count >= 3) {
// // the obstacle will inherit most of the properties of the old obstacle
// Obstacle obs = new Obstacle();
// obs.age = split.Key.age+1;
// obs.obstacleClass = split.Key.obstacleClass;
// // don't bother doing a split operation on these clusters -- they have already been split
// obs.obstaclePolygon = Polygon.GrahamScan(split.Value);
// obstacles.Add(obs);
// }
//}
// handle the unclaimed points
IList <Polygon> polygons = WrapAndSplit(unclaimed_points, split_area_threshold, split_length_threshold);
foreach (Polygon poly in polygons)
{
// create a new obstacle
Obstacle obs = new Obstacle();
obs.age = 1;
obs.obstacleClass = targetClass;
obs.obstaclePolygon = poly;
obstacles.Add(obs);
}
}
// test all old obstacles and see if they intersect any new obstacles
// project the previous static obstacles to the current time frame
if (processedObstacles != null)
{
try {
// get the relative transform
List <Obstacle> carryOvers = new List <Obstacle>();
Circle mergeCircle = new Circle(merge_expansion_size, Coordinates.Zero);
Polygon mergePolygon = mergeCircle.ToPolygon(24);
RelativeTransform transform = Services.RelativePose.GetTransform(processedObstacles.timestamp, clusters.timestamp);
foreach (Obstacle prevObs in processedObstacles.obstacles)
{
if (prevObs.obstacleClass == ObstacleClass.StaticLarge || prevObs.obstacleClass == ObstacleClass.StaticSmall)
{
prevObs.obstaclePolygon = prevObs.obstaclePolygon.Transform(transform);
prevObs.age++;
if (prevObs.age < 20)
{
Coordinates centroid = prevObs.obstaclePolygon.GetCentroid();
double dist = GetObstacleDistance(prevObs.obstaclePolygon);
double angle = centroid.ArcTan;
if (dist < 30 && dist > 6 && Math.Abs(centroid.Y) < 15 && Math.Abs(angle) < Math.PI / 2.0)
{
try {
prevObs.mergePolygon = Polygon.ConvexMinkowskiConvolution(mergePolygon, prevObs.obstaclePolygon);
if (!TestIntersection(prevObs.mergePolygon, obstacles))
{
bool dropObstacle = false;
for (int i = 0; i < prevObs.obstaclePolygon.Count; i++)
{
Coordinates pt = prevObs.obstaclePolygon[i];
// iterate over all tracked cluster
if (vehicleBox.IsInside(pt))
{
dropObstacle = true;
}
else if (useOccupancyGrid && externalUseOccupancyGrid && Services.OccupancyGrid.GetOccupancy(pt) == OccupancyStatus.Free)
{
dropObstacle = true;
}
else if (trackedObstacles != null)
{
foreach (Obstacle trackedObs in trackedObstacles)
{
if (trackedObs.obstacleClass == ObstacleClass.DynamicCarlike)
{
Polygon testPoly = trackedObs.extrudedPolygon ?? trackedObs.mergePolygon;
if (testPoly != null && testPoly.BoundingCircle.IsInside(pt) && testPoly.IsInside(pt))
{
dropObstacle = true;
break;
}
}
}
}
if (dropObstacle)
{
break;
}
}
if (!dropObstacle)
{
carryOvers.Add(prevObs);
}
}
}
catch (Exception) {
}
}
}
}
}
obstacles.AddRange(carryOvers);
}
catch (Exception) {
}
}
// create the merge polygon for all these duder
/*Circle mergeCircle = new Circle(merge_expansion_size, Coordinates.Zero);
* Polygon mergePolygon = mergeCircle.ToPolygon(24);
*
* foreach (Obstacle obs in obstacles) {
* obs.mergePolygon = Polygon.ConvexMinkowskiConvolution(mergePolygon, obs.obstaclePolygon);
* }*/
return(obstacles);
}
/// <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
}
public void OnUntrackedClustersReceived(SceneEstimatorUntrackedClusterCollection clusters)
{
currentUntrackedClusters = clusters;
newDataEvent.Set();
}
/// <summary>
/// Plans what maneuer we should take next
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, RoadPlan navigationalPlan, VehicleState vehicleState,
SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles,
List <ITacticalBlockage> blockages, double vehicleSpeed)
{
// assign vehicles to their lanes
this.roadMonitor.Assign(vehicles);
// navigation tasks
this.taskReasoning.navigationPlan = navigationalPlan;
#region Stay in lane
// maneuver given we are in a lane
if (planningState is StayInLaneState)
{
// get state
StayInLaneState sils = (StayInLaneState)planningState;
// check reasoning if needs to be different
if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(sils.Lane))
{
if (sils.Lane.LaneOnLeft == null)
{
this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver);
}
else if (sils.Lane.LaneOnLeft.Way.Equals(sils.Lane.Way))
{
this.leftLateralReasoning = new LateralReasoning(sils.Lane.LaneOnLeft, SideObstacleSide.Driver);
}
else
{
this.leftLateralReasoning = new OpposingLateralReasoning(sils.Lane.LaneOnLeft, SideObstacleSide.Driver);
}
if (sils.Lane.LaneOnRight == null)
{
this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger);
}
else if (sils.Lane.LaneOnRight.Way.Equals(sils.Lane.Way))
{
this.rightLateralReasoning = new LateralReasoning(sils.Lane.LaneOnRight, SideObstacleSide.Passenger);
}
else
{
this.rightLateralReasoning = new OpposingLateralReasoning(sils.Lane.LaneOnRight, SideObstacleSide.Passenger);
}
this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, sils.Lane);
}
// populate navigation with road plan
taskReasoning.SetRoadPlan(navigationalPlan, sils.Lane);
// as penalties for lane changes already taken into account, can just look at
// best lane plan to figure out what to do
TypeOfTasks bestTask = taskReasoning.Best;
// get the forward lane plan
Maneuver forwardManeuver = forwardReasoning.ForwardManeuver(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore);
// get the secondary
Maneuver?secondaryManeuver = forwardReasoning.AdvancedSecondary(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore, bestTask); //forwardReasoning.SecondaryManeuver(sils.Lane, vehicleState, navigationalPlan, blockages, sils.WaypointsToIgnore, bestTask);
// check behavior type for uturn
if (secondaryManeuver.HasValue && secondaryManeuver.Value.PrimaryBehavior is UTurnBehavior)
{
return(secondaryManeuver.Value);
}
// check if we wish to change lanes here
if (bestTask != TypeOfTasks.Straight)
{
// parameters
LaneChangeParameters parameters;
secondaryManeuver = this.forwardReasoning.AdvancedDesiredLaneChangeManeuver(sils.Lane, bestTask == TypeOfTasks.Left ? true : false, navigationalPlan.BestPlan.laneWaypointOfInterest.PointOfInterest,
navigationalPlan, vehicleState, blockages, sils.WaypointsToIgnore, new LaneChangeInformation(LaneChangeReason.Navigation, this.forwardReasoning.ForwardMonitor.ForwardVehicle.CurrentVehicle), secondaryManeuver, out parameters);
}
// final maneuver
Maneuver finalManeuver = secondaryManeuver.HasValue ? secondaryManeuver.Value : forwardManeuver;
// set opposing vehicle flag
if (false && this.leftLateralReasoning != null && this.leftLateralReasoning is OpposingLateralReasoning && finalManeuver.PrimaryBehavior is StayInLaneBehavior)
{
StayInLaneBehavior silb = (StayInLaneBehavior)finalManeuver.PrimaryBehavior;
OpposingLateralReasoning olr = (OpposingLateralReasoning)this.leftLateralReasoning;
olr.ForwardMonitor.ForwardVehicle.Update(olr.lane, vehicleState);
if (olr.ForwardMonitor.ForwardVehicle.CurrentVehicle != null)
{
ForwardVehicleTrackingControl fvtc = olr.ForwardMonitor.ForwardVehicle.GetControl(olr.lane, vehicleState);
BehaviorDecorator[] bds = new BehaviorDecorator[finalManeuver.PrimaryBehavior.Decorators.Count];
finalManeuver.PrimaryBehavior.Decorators.CopyTo(bds);
finalManeuver.PrimaryBehavior.Decorators = new List <BehaviorDecorator>(bds);
silb.Decorators.Add(new OpposingLaneDecorator(fvtc.xSeparation, olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed));
ArbiterOutput.Output("Added Opposing Lane Decorator: " + fvtc.xSeparation.ToString("F2") + "m, " + olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f2") + "m/s");
}
finalManeuver.PrimaryBehavior = silb;
}
// return the final
return(finalManeuver);
}
#endregion
#region Stay in supra lane
else if (CoreCommon.CorePlanningState is StayInSupraLaneState)
{
// get state
StayInSupraLaneState sisls = (StayInSupraLaneState)planningState;
// check reasoning
if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(sisls.Lane))
{
if (sisls.Lane.Initial.LaneOnLeft == null)
{
this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver);
}
else if (sisls.Lane.Initial.LaneOnLeft.Way.Equals(sisls.Lane.Initial.Way))
{
this.leftLateralReasoning = new LateralReasoning(sisls.Lane.Initial.LaneOnLeft, SideObstacleSide.Driver);
}
else
{
this.leftLateralReasoning = new OpposingLateralReasoning(sisls.Lane.Initial.LaneOnLeft, SideObstacleSide.Driver);
}
if (sisls.Lane.Initial.LaneOnRight == null)
{
this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger);
}
else if (sisls.Lane.Initial.LaneOnRight.Way.Equals(sisls.Lane.Initial.Way))
{
this.rightLateralReasoning = new LateralReasoning(sisls.Lane.Initial.LaneOnRight, SideObstacleSide.Passenger);
}
else
{
this.rightLateralReasoning = new OpposingLateralReasoning(sisls.Lane.Initial.LaneOnRight, SideObstacleSide.Passenger);
}
this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, sisls.Lane);
}
// populate navigation with road plan
taskReasoning.SetSupraRoadPlan(navigationalPlan, sisls.Lane);
// as penalties for lane changes already taken into account, can just look at
// best lane plan to figure out what to do
// TODO: NOTE THAT THIS BEST TASK SHOULD BE IN THE SUPRA LANE!! (DO WE NEED THIS)
TypeOfTasks bestTask = taskReasoning.Best;
// get the forward lane plan
Maneuver forwardManeuver = forwardReasoning.ForwardManeuver(sisls.Lane, vehicleState, navigationalPlan, blockages, sisls.WaypointsToIgnore);
// get hte secondary
Maneuver?secondaryManeuver = forwardReasoning.AdvancedSecondary(sisls.Lane, vehicleState, navigationalPlan, blockages, new List <ArbiterWaypoint>(), bestTask); //forwardReasoning.SecondaryManeuver(sisls.Lane, vehicleState, navigationalPlan, blockages, sisls.WaypointsToIgnore, bestTask);
// final maneuver
Maneuver finalManeuver = secondaryManeuver.HasValue ? secondaryManeuver.Value : forwardManeuver;
// check if stay in lane
if (false && this.leftLateralReasoning != null && this.leftLateralReasoning is OpposingLateralReasoning && finalManeuver.PrimaryBehavior is SupraLaneBehavior)
{
SupraLaneBehavior silb = (SupraLaneBehavior)finalManeuver.PrimaryBehavior;
OpposingLateralReasoning olr = (OpposingLateralReasoning)this.leftLateralReasoning;
olr.ForwardMonitor.ForwardVehicle.Update(olr.lane, vehicleState);
if (olr.ForwardMonitor.ForwardVehicle.CurrentVehicle != null)
{
ForwardVehicleTrackingControl fvtc = olr.ForwardMonitor.ForwardVehicle.GetControl(olr.lane, vehicleState);
BehaviorDecorator[] bds = new BehaviorDecorator[finalManeuver.PrimaryBehavior.Decorators.Count];
finalManeuver.PrimaryBehavior.Decorators.CopyTo(bds);
finalManeuver.PrimaryBehavior.Decorators = new List <BehaviorDecorator>(bds);
silb.Decorators.Add(new OpposingLaneDecorator(fvtc.xSeparation, olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed));
ArbiterOutput.Output("Added Opposing Lane Decorator: " + fvtc.xSeparation.ToString("F2") + "m, " + olr.ForwardMonitor.ForwardVehicle.CurrentVehicle.Speed.ToString("f2") + "m/s");
}
finalManeuver.PrimaryBehavior = silb;
}
// return the final
return(finalManeuver);
// notify
/*if (secondaryManeuver.HasValue)
* ArbiterOutput.Output("Secondary Maneuver");
*
* // check for forward's secondary maneuver for desired behavior other than going straight
* if (secondaryManeuver.HasValue)
* {
* // return the secondary maneuver
* return secondaryManeuver.Value;
* }
* // otherwise our default behavior and posibly desired is going straight
* else
* {
* // return default forward maneuver
* return forwardManeuver;
* }*/
}
#endregion
#region Change Lanes State
// maneuver given we are changing lanes
else if (planningState is ChangeLanesState)
{
// get state
ChangeLanesState cls = (ChangeLanesState)planningState;
LaneChangeReasoning lcr = new LaneChangeReasoning();
Maneuver final = lcr.PlanLaneChange(cls, vehicleState, navigationalPlan, blockages, new List <ArbiterWaypoint>());
#warning need to filter through waypoints to ignore so don't get stuck by a stop line
//Maneuver final = new Maneuver(cls.Resume(vehicleState, vehicleSpeed), cls, cls.DefaultStateDecorators, vehicleState.Timestamp);
// return the final planned maneuver
return(final);
/*if (!cls.parameters..TargetIsOnComing)
* {
* // check reasoning
* if (this.forwardReasoning == null || !this.forwardReasoning.Lane.Equals(cls.TargetLane))
* {
* if (cls.TargetLane.LaneOnLeft.Way.Equals(cls.TargetLane.Way))
* this.leftLateralReasoning = new LateralReasoning(cls.TargetLane.LaneOnLeft);
* else
* this.leftLateralReasoning = new OpposingLateralReasoning(cls.TargetLane.LaneOnLeft);
*
* if (cls.TargetLane.LaneOnRight.Way.Equals(cls.TargetLane.Way))
* this.rightLateralReasoning = new LateralReasoning(cls.TargetLane.LaneOnRight);
* else
* this.rightLateralReasoning = new OpposingLateralReasoning(cls.TargetLane.LaneOnRight);
*
* this.forwardReasoning = new ForwardReasoning(this.leftLateralReasoning, this.rightLateralReasoning, cls.TargetLane);
* }
*
*
* // get speed command
* double speed;
* double distance;
* this.forwardReasoning.ForwardMonitor.StoppingParams(new ArbiterWaypoint(cls.TargetUpperBound.pt, null), cls.TargetLane, vehicleState.Front, vehicleState.ENCovariance, out speed, out distance);
* SpeedCommand sc = new ScalarSpeedCommand(Math.Max(speed, 0.0));
* cls.distanceLeft = distance;
*
* // get behavior
* ChangeLaneBehavior clb = new ChangeLaneBehavior(cls.InitialLane.LaneId, cls.TargetLane.LaneId, cls.InitialLane.LaneOnLeft != null && cls.InitialLane.LaneOnLeft.Equals(cls.TargetLane),
* distance, sc, new List<int>(), cls.InitialLane.PartitionPath, cls.TargetLane.PartitionPath, cls.InitialLane.Width, cls.TargetLane.Width);
*
* // plan over the target lane
* //Maneuver targetManeuver = forwardReasoning.ForwardManeuver(cls.TargetLane, vehicleState, !cls.TargetIsOnComing, blockage, cls.InitialLaneState.IgnorableWaypoints);
*
* // plan over the initial lane
* //Maneuver initialManeuver = forwardReasoning.ForwardManeuver(cls.InitialLane, vehicleState, !cls.InitialIsOncoming, blockage, cls.InitialLaneState.IgnorableWaypoints);
*
* // generate the change lanes command
* //Maneuver final = laneChangeReasoning.PlanLaneChange(cls, initialManeuver, targetManeuver);
*
* }
* else
* {
* throw new Exception("Change lanes into oncoming not supported yet by road tactical");
* }*/
}
#endregion
#region Opposing Lanes State
// maneuver given we are in an opposing lane
else if (planningState is OpposingLanesState)
{
// get state
OpposingLanesState ols = (OpposingLanesState)planningState;
ArbiterWayId opposingWay = ols.OpposingWay;
ols.SetClosestGood(vehicleState);
ols.ResetLaneAgent = false;
// check reasoning
if (this.opposingReasoning == null || !this.opposingReasoning.Lane.Equals(ols.OpposingLane))
{
if (ols.OpposingLane.LaneOnRight == null)
{
this.leftLateralReasoning = new LateralReasoning(null, SideObstacleSide.Driver);
}
else if (!ols.OpposingLane.LaneOnRight.Way.Equals(ols.OpposingLane.Way))
{
this.leftLateralReasoning = new LateralReasoning(ols.OpposingLane.LaneOnRight, SideObstacleSide.Driver);
}
else
{
this.leftLateralReasoning = new OpposingLateralReasoning(ols.OpposingLane.LaneOnRight, SideObstacleSide.Driver);
}
if (ols.OpposingLane.LaneOnLeft == null)
{
this.rightLateralReasoning = new LateralReasoning(null, SideObstacleSide.Passenger);
}
else if (!ols.OpposingLane.LaneOnLeft.Way.Equals(ols.OpposingLane.Way))
{
this.rightLateralReasoning = new LateralReasoning(ols.OpposingLane.LaneOnLeft, SideObstacleSide.Passenger);
}
else
{
this.rightLateralReasoning = new OpposingLateralReasoning(ols.OpposingLane.LaneOnLeft, SideObstacleSide.Passenger);
}
this.opposingReasoning = new OpposingReasoning(this.leftLateralReasoning, this.rightLateralReasoning, ols.OpposingLane);
}
// get the forward lane plan
Maneuver forwardManeuver = this.opposingReasoning.ForwardManeuver(ols.OpposingLane, ols.ClosestGoodLane, vehicleState, navigationalPlan, blockages);
// get the secondary maneuver
Maneuver?secondaryManeuver = null;
if (ols.ClosestGoodLane != null)
{
secondaryManeuver = this.opposingReasoning.SecondaryManeuver(ols.OpposingLane, ols.ClosestGoodLane, vehicleState, blockages, ols.EntryParameters);
}
// check for reasonings secondary maneuver for desired behavior other than going straight
if (secondaryManeuver != null)
{
// return the secondary maneuver
return(secondaryManeuver.Value);
}
// otherwise our default behavior and posibly desired is going straight
else
{
// return default forward maneuver
return(forwardManeuver);
}
}
#endregion
#region not imp
/*
#region Uturn
*
* // we are making a uturn
* else if (planningState is uTurnState)
* {
* // get the uturn state
* uTurnState uts = (uTurnState)planningState;
*
* // get the final lane we wish to be in
* ArbiterLane targetLane = uts.TargetLane;
*
* // get operational state
* Type operationalBehaviorType = CoreCommon.Communications.GetCurrentOperationalBehavior();
*
* // check if we have completed the uturn
* bool complete = operationalBehaviorType == typeof(StayInLaneBehavior);
*
* // default next behavior
* Behavior nextBehavior = new StayInLaneBehavior(targetLane.LaneId, new ScalarSpeedCommand(CoreCommon.OperationalStopSpeed), new List<int>());
* nextBehavior.Decorators = TurnDecorators.NoDecorators;
*
* // check if complete
* if (complete)
* {
* // stay in lane
* List<ArbiterLaneId> aprioriLanes = new List<ArbiterLaneId>();
* aprioriLanes.Add(targetLane.LaneId);
* return new Maneuver(nextBehavior, new StayInLaneState(targetLane), null, null, aprioriLanes, true);
* }
* // otherwise keep same
* else
* {
* // set abort behavior
* ((StayInLaneBehavior)nextBehavior).SpeedCommand = new ScalarSpeedCommand(0.0);
*
* // maneuver
* return new Maneuver(uts.DefaultBehavior, uts, nextBehavior, new StayInLaneState(targetLane));
* }
* }
*
#endregion*/
#endregion
#region Unknown
// unknown state
else
{
throw new Exception("Unknown Travel State type: planningState: " + planningState.ToString() + "\n with type: " + planningState.GetType().ToString());
}
#endregion
}
/// <summary>
/// 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
}
/// <summary>
/// Generic plan
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState,
SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles,
List <ITacticalBlockage> blockages, double vehicleSpeed)
{
// update stuff
this.Update(vehicles, vehicleState);
#region Plan Roads
if (planningState is TravelState)
{
Maneuver roadFinal = roadTactical.Plan(
planningState,
(RoadPlan)navigationalPlan,
vehicleState,
vehicles,
obstacles,
blockages,
vehicleSpeed);
// return
return(roadFinal);
}
#endregion
#region Plan Intersection
else if (planningState is IntersectionState)
{
Maneuver intersectionFinal = intersectionTactical.Plan(
planningState,
navigationalPlan,
vehicleState,
vehicles,
obstacles,
blockages);
// return
return(intersectionFinal);
}
#endregion
#region Plan Zone
else if (planningState is ZoneState)
{
Maneuver zoneFinal = zoneTactical.Plan(
planningState,
navigationalPlan,
vehicleState,
vehicles,
obstacles,
blockages);
// return
return(zoneFinal);
}
#endregion
#region Plan Blockage
else if (planningState is BlockageState)
{
Maneuver blockageFinal = blockageTactical.Plan(
planningState,
vehicleState,
vehicleSpeed,
blockages,
navigationalPlan);
return(blockageFinal);
}
#endregion
#region Unknown
else
{
throw new Exception("Unknown planning state type: " + planningState.GetType().ToString());
}
#endregion
}
public static Object Deserialize(Stream stream, string channelName)
{
BinaryReader br = new BinaryReader(stream);
SceneEstimatorMessageID msgtype = (SceneEstimatorMessageID)br.ReadInt32();
switch (msgtype)
{
case SceneEstimatorMessageID.SCENE_EST_Info:
Console.WriteLine("SE Info:");
break;
case SceneEstimatorMessageID.SCENE_EST_Bad:
Console.WriteLine("SE BAD:");
break;
case SceneEstimatorMessageID.SCENE_EST_ParticleRender:
break;
case SceneEstimatorMessageID.SCENE_EST_PositionEstimate:
break;
case SceneEstimatorMessageID.SCENE_EST_Stopline:
break;
case SceneEstimatorMessageID.SCENE_EST_TrackedClusters:
{
if (channelName != SceneEstimatorObstacleChannelNames.TrackedClusterChannelName &&
channelName != SceneEstimatorObstacleChannelNames.AnyClusterChannelName)
{
break;
}
int chunkNum = (int)br.ReadByte();
int numTotChunks = (int)br.ReadByte();
totPackets++;
if (chunkNum == 0) //first packet...
{
if (expChunkNum != chunkNum)
{
badPackets++;
}
int lenToRead = (int)(br.BaseStream.Length - br.BaseStream.Position);
lock (trackStorageLock)
{
trackedClusterStorage = new MemoryStream(65000);
trackedClusterStorage.Write(br.ReadBytes(lenToRead), 0, lenToRead);
}
if (chunkNum == numTotChunks - 1) //we are done
{
expChunkNum = 0;
//Console.WriteLine("Got Single Frame Packet OK " + " of " + numTotChunks);
Object o = null;
lock (trackStorageLock)
{
try
{
o = ProcessTrackedClusterMsg(new BinaryReader(trackedClusterStorage));
}
catch (Exception ex)
{
Console.WriteLine("EXCEPTION: " + ex.Message);
}
goodPackets++;
}
if (o != null)
{
return(o);
}
}
else
{
expChunkNum = 1;
Console.WriteLine("Got Mulii Frame Packet " + expChunkNum + " of " + numTotChunks);
}
}
else if (chunkNum == expChunkNum) //nth packet....
{
lock (trackStorageLock)
{
int lenToRead = (int)(br.BaseStream.Length - br.BaseStream.Position);
trackedClusterStorage.Write(br.ReadBytes(lenToRead), 0, lenToRead);
}
if (chunkNum == numTotChunks - 1) //we are done
{
expChunkNum = 0;
Object o = null;
lock (trackStorageLock)
{
Console.WriteLine("Got Mulii Frame Packet OK Len:" + trackedClusterStorage.Length + " Chunks " + numTotChunks);
try
{
o = ProcessTrackedClusterMsg(new BinaryReader(trackedClusterStorage));
}
catch (Exception ex)
{
Console.WriteLine("EXCEPTION: " + ex.Message);
}
trackedClusterStorage = new MemoryStream(65000);
goodPackets++;
}
return(o);
}
else
{
expChunkNum++;
Console.WriteLine("Got Mulii Frame Packet " + expChunkNum + " of " + numTotChunks);
}
}
else //misaligned packet! yikes
{
lock (trackStorageLock)
{
trackedClusterStorage = new MemoryStream();
}
expChunkNum = 0;
badPackets++;
Console.WriteLine("Bad Packet! ChunkNum: " + chunkNum + " ExpChunkNum: " + expChunkNum + " Total: " + numTotChunks);
}
if (totPackets % 100 == 0)
{
Console.WriteLine("PACKET STATS: GOOD: " + goodPackets + " BAD: " + badPackets);
}
break;
}
case SceneEstimatorMessageID.SCENE_EST_UntrackedClusters:
{
if (channelName != SceneEstimatorObstacleChannelNames.UntrackedClusterChannelName &&
channelName != SceneEstimatorObstacleChannelNames.AnyClusterChannelName)
{
break;
}
SceneEstimatorUntrackedClusterCollection ucc = new SceneEstimatorUntrackedClusterCollection();
int numClusters = br.ReadInt32();
ucc.clusters = new SceneEstimatorUntrackedCluster[numClusters];
ucc.timestamp = br.ReadDouble();
for (int i = 0; i < numClusters; i++)
{
int numPoints = br.ReadInt32();
ucc.clusters[i].clusterClass = (SceneEstimatorClusterClass)br.ReadInt32();
ucc.clusters[i].points = new Coordinates[numPoints];
for (int j = 0; j < numPoints; j++)
{
Int16 tmpx = br.ReadInt16();
Int16 tmpy = br.ReadInt16();
ucc.clusters[i].points[j].X = (double)tmpx / 100.0;
ucc.clusters[i].points[j].Y = (double)tmpy / 100.0;
}
}
if (br.BaseStream.Position != br.BaseStream.Length)
{
Console.WriteLine("Warning: Incomplete parse of received untracked cluster message. length is " + br.BaseStream.Length + ", go to " + br.BaseStream.Position + ".");
}
return(ucc);
}
case (SceneEstimatorMessageID)LocalRoadModelMessageID.LOCAL_ROAD_MODEL:
if ((channelName != LocalRoadModelChannelNames.LocalRoadModelChannelName) && (channelName != LocalRoadModelChannelNames.LMLocalRoadModelChannelName))
{
break;
}
LocalRoadModel lrm = new LocalRoadModel();
lrm.timestamp = br.ReadDouble();
lrm.probabilityRoadModelValid = br.ReadSingle();
lrm.probabilityCenterLaneExists = br.ReadSingle();
lrm.probabilityLeftLaneExists = br.ReadSingle();
lrm.probabilityRightLaneExists = br.ReadSingle();
lrm.laneWidthCenter = br.ReadSingle();
lrm.laneWidthCenterVariance = br.ReadSingle();
lrm.laneWidthLeft = br.ReadSingle();
lrm.laneWidthLeftVariance = br.ReadSingle();
lrm.laneWidthRight = br.ReadSingle();
lrm.laneWidthRightVariance = br.ReadSingle();
int numCenterPoints = br.ReadInt32();
int numLeftPoints = br.ReadInt32();
int numRightPoints = br.ReadInt32();
lrm.LanePointsCenter = new Coordinates[numCenterPoints];
lrm.LanePointsCenterVariance = new float[numCenterPoints];
lrm.LanePointsLeft = new Coordinates[numLeftPoints];
lrm.LanePointsLeftVariance = new float[numLeftPoints];
lrm.LanePointsRight = new Coordinates[numRightPoints];
lrm.LanePointsRightVariance = new float[numRightPoints];
for (int i = 0; i < numCenterPoints; i++)
{
//fixed point conversion!
Int16 x = br.ReadInt16();
Int16 y = br.ReadInt16();
UInt16 var = br.ReadUInt16();
lrm.LanePointsCenter[i] = new Coordinates((double)x / 100.0, (double)y / 100.0);
lrm.LanePointsCenterVariance[i] = ((float)var / 100.0f) * ((float)var / 100.0f);
}
for (int i = 0; i < numLeftPoints; i++)
{
//fixed point conversion!
Int16 x = br.ReadInt16();
Int16 y = br.ReadInt16();
UInt16 var = br.ReadUInt16();
lrm.LanePointsLeft[i] = new Coordinates((double)x / 100.0, (double)y / 100.0);
lrm.LanePointsLeftVariance[i] = ((float)var / 100.0f) * ((float)var / 100.0f);
}
for (int i = 0; i < numRightPoints; i++)
{
//fixed point conversion!
Int16 x = br.ReadInt16();
Int16 y = br.ReadInt16();
UInt16 var = br.ReadUInt16();
lrm.LanePointsRight[i] = new Coordinates((double)x / 100.0, (double)y / 100.0);
lrm.LanePointsRightVariance[i] = ((float)var / 100.0f) * ((float)var / 100.0f);
}
int offset = (LocalRoadModel.MAX_LANE_POINTS * 3) - (numCenterPoints + numRightPoints + numLeftPoints);
offset *= 6; //this is just the size of each "point"
if ((br.BaseStream.Position + offset) != br.BaseStream.Length)
{
Console.WriteLine("Warning: Incomplete parse of received local road model message. length is " + br.BaseStream.Length + ", go to " + br.BaseStream.Position + ".");
}
return(lrm);
default:
throw new InvalidDataException("Invalid Scene Estimator Message Received: " + msgtype);
}
return(null);
}
private void ObstacleThread()
{
for (;;)
{
try {
SceneEstimatorUntrackedClusterCollection newUntrackedClusters = Interlocked.Exchange(ref currentUntrackedClusters, null);
SceneEstimatorTrackedClusterCollection newTrackedClusters = Interlocked.Exchange(ref currentTrackedClusters, null);
if (newUntrackedClusters == null && newTrackedClusters == null)
{
if (!Services.DebuggingService.StepMode)
{
newDataEvent.WaitOne();
}
else
{
Services.DebuggingService.WaitOnSequencer(typeof(ObstaclePipeline));
}
continue;
}
// check if we have a matching pair
if (newUntrackedClusters != null)
{
queuedUntrackedClusters = newUntrackedClusters;
}
if (newTrackedClusters != null)
{
haveReceivedTrackedClusters = true;
queuedTrackedClusters = newTrackedClusters;
}
if (queuedUntrackedClusters == null || (haveReceivedTrackedClusters && (queuedTrackedClusters == null || queuedTrackedClusters.timestamp != queuedUntrackedClusters.timestamp)))
{
continue;
}
Rect vehicleBox = DetermineVehicleEraseBox();
// load in the appropriate stuff to the occupancy grid
useOccupancyGrid = (Services.OccupancyGrid != null && !Services.OccupancyGrid.IsDisposed);
if (useOccupancyGrid)
{
double occup_ts = Services.OccupancyGrid.LoadNewestGrid();
if (occup_ts < 0)
{
useOccupancyGrid = false;
}
else
{
double delta_ts = occup_ts - queuedUntrackedClusters.timestamp;
Services.Dataset.ItemAs <double>("occupancy delta ts").Add(delta_ts, queuedUntrackedClusters.timestamp);
}
}
occupancyDeletedCount = 0;
List <Obstacle> trackedObstacles;
if (queuedTrackedClusters == null)
{
trackedObstacles = new List <Obstacle>();
}
else
{
trackedObstacles = ProcessTrackedClusters(queuedTrackedClusters, vehicleBox);
}
List <Obstacle> untrackedObstacles = ProcessUntrackedClusters(queuedUntrackedClusters, trackedObstacles, vehicleBox);
List <Obstacle> finalObstacles = FinalizeProcessing(trackedObstacles, untrackedObstacles, queuedUntrackedClusters.timestamp);
processedObstacles = new ObstacleCollection(queuedUntrackedClusters.timestamp, finalObstacles);
Services.Dataset.ItemAs <int>("occupancy deleted count").Add(occupancyDeletedCount, queuedUntrackedClusters.timestamp);
queuedUntrackedClusters = null;
queuedTrackedClusters = null;
if (Services.DebuggingService.StepMode)
{
Services.DebuggingService.SetCompleted(typeof(ObstaclePipeline));
}
Services.Dataset.MarkOperation("obstacle rate", LocalCarTimeProvider.LocalNow);
}
catch (Exception ex) {
OperationalTrace.WriteError("error processing obstacles: {0}", ex);
}
}
}
