/// <summary>
/// Updates vehicles
/// </summary>
/// <param name="vehicles"></param>
/// <param name="state"></param>
public void Update(SceneEstimatorTrackedClusterCollection vehicles, VehicleState state)
{
// vehicle population
this.PopulateValidVehicles(vehicles);
this.PopulateAreaVehicles();
this.UpdateQueuingMonitors(state.Timestamp);
}
private static Object ProcessTrackedClusterMsg(BinaryReader br)
{
br.BaseStream.Position = 0;
SceneEstimatorTrackedClusterCollection tcc = new SceneEstimatorTrackedClusterCollection();
int numClusters = br.ReadInt32();
tcc.clusters = new SceneEstimatorTrackedCluster[numClusters];
tcc.timestamp = br.ReadDouble();
Dictionary<SceneEstimatorTrackedCluster, List<Pair<UInt16, UInt16>>> partitions = new Dictionary<SceneEstimatorTrackedCluster, List<Pair<ushort, ushort>>>();
for (int i = 0; i < numClusters; i++)
{
tcc.clusters[i] = new SceneEstimatorTrackedCluster();
int numPoints = br.ReadInt32();
tcc.clusters[i].relativePoints = new Coordinates[numPoints];
int numClosestPartitions = br.ReadInt32();
tcc.clusters[i].closestPartitions = new SceneEstimatorClusterPartition[numClosestPartitions];
double closestX = (double)br.ReadSingle();
double closestY = (double)br.ReadSingle();
tcc.clusters[i].closestPoint = new Coordinates(closestX, closestY);
tcc.clusters[i].speed = br.ReadSingle();
tcc.clusters[i].speedValid = br.ReadBoolean();
tcc.clusters[i].relativeheading = br.ReadSingle();
tcc.clusters[i].absoluteHeading = br.ReadSingle();
tcc.clusters[i].headingValid = br.ReadBoolean();
tcc.clusters[i].targetClass = (SceneEstimatorTargetClass)br.ReadInt32();
tcc.clusters[i].id = br.ReadInt32();
tcc.clusters[i].statusFlag = (SceneEstimatorTargetStatusFlag)br.ReadInt32();
tcc.clusters[i].isStopped = br.ReadBoolean();
List<Pair<UInt16, UInt16>> partition = new List<Pair<ushort, ushort>>();
//create the partitions (still mapped)
for (int j = 0; j < numClosestPartitions; j++)
{
UInt16 partHashID = br.ReadUInt16();
UInt16 partProbFP = br.ReadUInt16();
partition.Add(new Pair<ushort, ushort>(partHashID, partProbFP));
}
partitions.Add(tcc.clusters[i], partition);
//create the points
for (int j = 0; j < numPoints; j++)
{
Int16 tmpx = br.ReadInt16();
Int16 tmpy = br.ReadInt16();
tcc.clusters[i].relativePoints[j].X = (double)tmpx / 100.0;
tcc.clusters[i].relativePoints[j].Y = (double)tmpy / 100.0;
}
}
//now get the map
int mapsize = br.ReadInt32();
Dictionary<UInt16, string> map = new Dictionary<ushort, string>();
for (int i = 0; i < mapsize; i++)
{
string s = br.ReadString();
UInt16 id = br.ReadUInt16();
map.Add(id, s);
}
//now reprocess the clusters and populate their closest partitions from the map we made
for (int i = 0; i < numClusters; i++)
{
if (partitions.ContainsKey(tcc.clusters[i]) == false) continue;
int numPart = partitions[tcc.clusters[i]].Count;
tcc.clusters[i].closestPartitions = new SceneEstimatorClusterPartition[numPart];
int j = 0;
SceneEstimatorTrackedCluster clust = tcc.clusters[i];
List<Pair<UInt16, UInt16>> death = partitions[clust];
foreach (Pair<UInt16, UInt16> entry in death)
{
//the pair is HASH ID and PROB(fp)
if (map.ContainsKey(entry.Left) == false) throw new InvalidDataException("Recieved an Invalid Mapping for Closest Partitions. Fatal.");
tcc.clusters[i].closestPartitions[j].partitionID = map[entry.Left];
tcc.clusters[i].closestPartitions[j].probability = (float)entry.Right / 65535.0f;
if (tcc.clusters[i].closestPartitions[j].probability > 1.0f)
{
Console.WriteLine("Warning: Overflow on probability of partition: " + tcc.clusters[i].closestPartitions[j].probability.ToString());
tcc.clusters[i].closestPartitions[j].probability = 1.0f;
}
else if (tcc.clusters[i].closestPartitions[j].probability < 0.0f)
{
Console.WriteLine("Warning: Underflow on probability of partition: " + tcc.clusters[i].closestPartitions[j].probability.ToString());
tcc.clusters[i].closestPartitions[j].probability = 0.0f;
}
//if (tcc.clusters[i].closestPartitions[j].probability != 0)
// Console.WriteLine("stuff! " + tcc.clusters[i].closestPartitions[j].probability.ToString());
j++;
}
}
if (br.BaseStream.Position != br.BaseStream.Length)
{
Console.WriteLine("Warning: Incomplete parse of received tracked cluster message. length is " + br.BaseStream.Length + ", go to " + br.BaseStream.Position + ".");
}
return tcc;
}
/// <summary>
/// Generic plan
/// </summary>
/// <param name="planningState"></param>
/// <param name="navigationalPlan"></param>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
/// <param name="blockage"></param>
/// <returns></returns>
public Maneuver Plan(IState planningState, INavigationalPlan navigationalPlan, VehicleState vehicleState,
SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles,
List<ITacticalBlockage> blockages, double vehicleSpeed)
{
// update stuff
this.Update(vehicles, vehicleState);
#region Plan Roads
if (planningState is TravelState)
{
Maneuver roadFinal = roadTactical.Plan(
planningState,
(RoadPlan)navigationalPlan,
vehicleState,
vehicles,
obstacles,
blockages,
vehicleSpeed);
// return
return roadFinal;
}
#endregion
#region Plan Intersection
else if (planningState is IntersectionState)
{
Maneuver intersectionFinal = intersectionTactical.Plan(
planningState,
navigationalPlan,
vehicleState,
vehicles,
obstacles,
blockages);
// return
return intersectionFinal;
}
#endregion
#region Plan Zone
else if (planningState is ZoneState)
{
Maneuver zoneFinal = zoneTactical.Plan(
planningState,
navigationalPlan,
vehicleState,
vehicles,
obstacles,
blockages);
// return
return zoneFinal;
}
#endregion
#region Plan Blockage
else if (planningState is BlockageState)
{
Maneuver blockageFinal = blockageTactical.Plan(
planningState,
vehicleState,
vehicleSpeed,
blockages,
navigationalPlan);
return blockageFinal;
}
#endregion
#region Unknown
else
{
throw new Exception("Unknown planning state type: " + planningState.GetType().ToString());
}
#endregion
}
/// <summary>
/// Populates mapping of vehicles we want to track given state
/// </summary>
/// <param name="vehicles"></param>
public void PopulateValidVehicles(SceneEstimatorTrackedClusterCollection vehicles)
{
TacticalDirector.NewVehicles = new List<VehicleAgent>();
TacticalDirector.OccludedVehicles = new Dictionary<int, VehicleAgent>();
if (TacticalDirector.ValidVehicles == null)
TacticalDirector.ValidVehicles = new Dictionary<int, VehicleAgent>();
List<int> toRemove = new List<int>();
foreach (VehicleAgent va in TacticalDirector.ValidVehicles.Values)
{
bool found = false;
for (int i = 0; i < vehicles.clusters.Length; i++)
{
if (vehicles.clusters[i].id.Equals(va.VehicleId))
found = true;
}
if (!found)
toRemove.Add(va.VehicleId);
}
foreach (int r in toRemove)
TacticalDirector.ValidVehicles.Remove(r);
for(int i = 0; i < vehicles.clusters.Length; i++)
{
SceneEstimatorTrackedCluster ov = vehicles.clusters[i];
bool clusterStopped = ov.isStopped;
if (ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_ACTIVE ||
(ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_PART && !clusterStopped) ||
(ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_FULL && !clusterStopped))
{
if (ov.targetClass == SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE)
{
if (TacticalDirector.ValidVehicles.ContainsKey(ov.id))
TacticalDirector.ValidVehicles[ov.id].StateMonitor.Observed = ov;
else
{
VehicleAgent ovNew = new VehicleAgent(ov);
TacticalDirector.ValidVehicles.Add(ovNew.VehicleId, ovNew);
TacticalDirector.NewVehicles.Add(ovNew);
}
}
else
{
if (TacticalDirector.ValidVehicles.ContainsKey(ov.id))
TacticalDirector.ValidVehicles.Remove(ov.id);
}
}
else
{
if (TacticalDirector.ValidVehicles.ContainsKey(ov.id))
TacticalDirector.ValidVehicles.Remove(ov.id);
}
if ((ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_PART && clusterStopped) ||
(ov.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_FULL && clusterStopped))
TacticalDirector.OccludedVehicles.Add(ov.id, new VehicleAgent(ov));
}
}
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);
}
}
}
private List<Obstacle> ProcessTrackedClusters(SceneEstimatorTrackedClusterCollection clusters, Rect vehicleBox)
{
List<Obstacle> obstacles = new List<Obstacle>(clusters.clusters.Length);
// get the list of previous id's
SortedList<int, Obstacle> previousID;
if (processedObstacles != null) {
previousID = new SortedList<int, Obstacle>(processedObstacles.obstacles.Count);
foreach (Obstacle obs in processedObstacles.obstacles) {
if (obs != null && obs.trackID != -1 && !previousID.ContainsKey(obs.trackID)) {
previousID.Add(obs.trackID, obs);
}
}
}
else {
previousID = new SortedList<int, Obstacle>();
}
List<Coordinates> goodPoints = new List<Coordinates>(1500);
Circle mergeCircle = new Circle(merge_expansion_size, Coordinates.Zero);
Polygon mergePolygon = mergeCircle.ToPolygon(24);
foreach (SceneEstimatorTrackedCluster cluster in clusters.clusters) {
// ignore deleted targets
if (cluster.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_DELETED || cluster.statusFlag == SceneEstimatorTargetStatusFlag.TARGET_STATE_OCCLUDED_FULL || cluster.relativePoints == null || cluster.relativePoints.Length < 3)
continue;
Obstacle obs = new Obstacle();
obs.trackID = cluster.id;
obs.speed = cluster.speed;
obs.speedValid = cluster.speedValid;
obs.occuluded = cluster.statusFlag != SceneEstimatorTargetStatusFlag.TARGET_STATE_ACTIVE;
// update the age
Obstacle prevTrack = null;
previousID.TryGetValue(cluster.id, out prevTrack);
goodPoints.Clear();
int numOccupancyDeleted = 0;
foreach (Coordinates pt in cluster.relativePoints) {
if (!vehicleBox.IsInside(pt)) {
if (useOccupancyGrid && Services.OccupancyGrid.GetOccupancy(pt) == OccupancyStatus.Free) {
occupancyDeletedCount++;
numOccupancyDeleted++;
}
else {
goodPoints.Add(pt);
}
}
}
if (goodPoints.Count < 3) {
continue;
}
IList<Polygon> polys;
if (obs.occuluded && numOccupancyDeleted > 0) {
polys = WrapAndSplit(goodPoints, 1, 2.5);
}
else {
polys = new Polygon[] { Polygon.GrahamScan(goodPoints) };
}
obs.absoluteHeadingValid = cluster.headingValid;
obs.absoluteHeading = cluster.absoluteHeading;
// set the obstacle polygon for calculate obstacle distance
Polygon obsPoly = Polygon.GrahamScan(goodPoints);
double targetDistance = GetObstacleDistance(obsPoly);
ObstacleClass impliedClass = ObstacleClass.DynamicUnknown;
switch (cluster.targetClass) {
case SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE:
if (cluster.isStopped) {
impliedClass = ObstacleClass.DynamicStopped;
}
else {
impliedClass = ObstacleClass.DynamicCarlike;
}
break;
case SceneEstimatorTargetClass.TARGET_CLASS_NOTCARLIKE:
impliedClass = ObstacleClass.DynamicNotCarlike;
break;
case SceneEstimatorTargetClass.TARGET_CLASS_UNKNOWN:
impliedClass = ObstacleClass.DynamicUnknown;
break;
}
if (prevTrack == null) {
obs.age = 1;
// we haven't seen this track before, determine what the implied class is
if (targetDistance < target_class_ignore_dist) {
impliedClass = ObstacleClass.DynamicUnknown;
}
}
else {
obs.age = prevTrack.age+1;
// if we've seen this target before and we've labelled it as unknown and it is labelled as car-like now, check the distance
if (prevTrack.obstacleClass == ObstacleClass.DynamicUnknown && targetDistance < target_class_ignore_dist && obs.age < target_class_ignore_age) {
impliedClass = ObstacleClass.DynamicUnknown;
}
}
// get the off-road percentage
double offRoadPercent = GetPercentOffRoad(obs.obstaclePolygon);
if (offRoadPercent > 0.65) {
obs.offroadAge = obs.age;
}
// now check if we're labelling the obstacle as car-like if it has been off-road in the last second
if ((impliedClass == ObstacleClass.DynamicCarlike || impliedClass == ObstacleClass.DynamicStopped) && (obs.age - obs.offroadAge) > 10 && obs.offroadAge > 0) {
// label as not car like
impliedClass = ObstacleClass.DynamicNotCarlike;
}
obs.obstacleClass = impliedClass;
foreach (Polygon poly in polys) {
Obstacle newObs = obs.ShallowClone();
newObs.obstaclePolygon = poly;
// determine what to do with the cluster
if (cluster.targetClass == SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE && !cluster.isStopped) {
// if the heading is valid, extrude the car polygon and predict forward
if (cluster.headingValid) {
newObs.extrudedPolygon = ExtrudeCarPolygon(newObs.obstaclePolygon, cluster.relativeheading);
}
}
try {
newObs.mergePolygon = Polygon.ConvexMinkowskiConvolution(mergePolygon, newObs.AvoidancePolygon);
}
catch (Exception) {
}
obstacles.Add(newObs);
}
}
return obstacles;
}
/// <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;
}
}
public void OnTrackedClustersReceived(SceneEstimatorTrackedClusterCollection clusters)
{
currentTrackedClusters = clusters;
newDataEvent.Set();
}
/// <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);
}
}
/// <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
}
/// <summary>
/// Get the vehicles from the world and put them into sensors form
/// </summary>
/// <param name="ours"></param>
/// <returns></returns>
public SceneEstimatorTrackedClusterCollection VehiclesFromWorld(SimVehicleId ours, double ts)
{
// vehicle list
List<SceneEstimatorTrackedCluster> vehicles = new List<SceneEstimatorTrackedCluster>();
// get our vehicle
SimVehicleState ourVS = null;
foreach (SimVehicleState svs in this.worldState.Vehicles.Values)
{
if (svs.VehicleID.Equals(ours))
{
ourVS = svs;
}
}
// generate "tracked" clusters
foreach (SimVehicleState svs in this.worldState.Vehicles.Values)
{
// don't inclue our vehicle
if (!svs.VehicleID.Equals(ours))
{
// construct cluster
SceneEstimatorTrackedCluster setc = new SceneEstimatorTrackedCluster();
// set heading valid
setc.headingValid = true;
// closest point
Coordinates closestPoint = this.ClosestToPolygon(this.VehiclePolygon(svs), ourVS.Position);
setc.closestPoint = closestPoint;
// stopped
bool isStopped = Math.Abs(svs.Speed) < 0.01;
setc.isStopped = isStopped;
// speed
float speed = (float)Math.Abs(svs.Speed);
setc.speed = speed;
setc.speedValid = svs.SpeedValid;
// absolute heading
float absHeading = (float)(svs.Heading.ArcTan);
setc.absoluteHeading = absHeading;
// relative heading
float relHeading = absHeading - (float)(ourVS.Heading.ArcTan);
setc.relativeheading = relHeading;
// set target class
setc.targetClass = SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE;
// set id
setc.id = svs.VehicleID.Number;
// cluster partitions
SceneEstimatorClusterPartition[] partitions = this.GetClusterPartitions(svs, ourVS);
setc.closestPartitions = partitions;
// state
setc.statusFlag = SceneEstimatorTargetStatusFlag.TARGET_STATE_ACTIVE;
// raw points
Coordinates[] points = this.VehiclePointsRelative(svs, ourVS.Position, ourVS.Heading);
setc.relativePoints = points;
// add
vehicles.Add(setc);
}
}
// array of clusters
SceneEstimatorTrackedClusterCollection setcc = new SceneEstimatorTrackedClusterCollection();
setcc.clusters = vehicles.ToArray();
setcc.timestamp = ts;
// return the clusters
return setcc;
}
/// <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>
/// 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 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>
/// Assigns vehicles to components
/// </summary>
/// <param name="vehicles"></param>
public void Assign(SceneEstimatorTrackedClusterCollection vehicles)
{
}