/// <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) { }