public void Initialize(Behavior b) { SimpleStayInLaneBehavior sb = (SimpleStayInLaneBehavior)b; // store the base path basePath = ConvertPath(sb.BasePath); // get the lower, upper bound leftBound = FindBoundary(sb.LaneWidth/2); rightBound = FindBoundary(-sb.LaneWidth/2); // convert everything to be vehicle relative AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(); pathTime = absTransform.Timestamp; basePath = basePath.Transform(absTransform); leftBound = leftBound.Transform(absTransform); rightBound = rightBound.Transform(absTransform); // send the left and right bounds Services.UIService.PushRelativePath(leftBound, pathTime, "left bound"); Services.UIService.PushRelativePath(rightBound, pathTime, "right bound"); maxSpeed = sb.MaxSpeed; obstacleManager = new ObstacleManager(); // hik }
public override void Initialize(Behavior b) { Services.ObstaclePipeline.ExtraSpacing = 0; Services.ObstaclePipeline.UseOccupancyGrid = true; base.Initialize(b); }
public override void OnBehaviorReceived(Behavior b) { if (b is ChangeLaneBehavior) { ChangeLaneBehavior cb = (ChangeLaneBehavior)b; if (cb.StartLane.Equals(startingLaneID) && cb.TargetLane.Equals(endingLaneID)) { LinePath startingLanePath = cb.BackupStartLanePath; LinePath endingLanePath = cb.BackupTargetLanePath; Services.UIService.PushAbsolutePath(startingLanePath, cb.TimeStamp, "original path1"); Services.UIService.PushAbsolutePath(endingLanePath, cb.TimeStamp, "original path2"); Services.BehaviorManager.QueueParam(cb); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in change lanes -- got change lanes behavior with same start/end lane"); } else { // this is a new behavior Services.BehaviorManager.Execute(b, null, false); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in change lanes -- got change lanes with different start/end lane ({0}/{1})", cb.StartLane, cb.TargetLane); } } else { Services.BehaviorManager.Execute(b, null, false); } }
public override void ExecuteBehavior(UrbanChallenge.Behaviors.Behavior b) { if (displaySignals) { if (b.Decorators != null) { Console.Write("Decorators: "); foreach (BehaviorDecorator bd in b.Decorators) { if (bd is TurnSignalDecorator) { TurnSignalDecorator tsd = (TurnSignalDecorator)bd; Console.Write(tsd.Signal.ToString() + ", "); } else { Console.Write("Unrecognized, "); } } Console.Write("\n"); } else { Console.WriteLine("Decorators: null"); } } }
public override void OnBehaviorReceived(Behavior b) { // check if the transition is valid if (b is StayInLaneBehavior) { StayInLaneBehavior sb = (StayInLaneBehavior)b; // check if the lane id's match if (object.Equals(sb.TargetLane, laneID)) { // check if the speed command changed BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "received new stay in lane speed command: {0}", sb.SpeedCommand); // queue the new speed command to the behavior Services.BehaviorManager.QueueParam(sb); } else { BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "recevied new stay in lane behavior, different target lane: {0}", sb); // the lane id's changed // re-initialize the behavior Services.BehaviorManager.Execute(b, null, false); } } else { BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane behavior, received {0}", b); // this is cool to execute Services.BehaviorManager.Execute(b, null, false); } }
public void Initialize(Behavior b) { Services.ObstaclePipeline.ExtraSpacing = 0; Services.ObstaclePipeline.UseOccupancyGrid = true; // set the tracking manager to run a constant braking behavior Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetHoldBrakeCommand()); }
public override void OnBehaviorReceived(Behavior b) { if (b is ZoneTravelingBehavior) { Services.BehaviorManager.QueueParam(b); } else { Services.BehaviorManager.Execute(b, null, false); } }
/// <summary> /// Constructor /// </summary> /// <param name="localRoute"></param> /// <param name="bestFullRoute"></param> /// <param name="vehicleState"></param> /// <param name="maneuver"></param> public AiRemoteListener(Routes localRoute, FullRoute bestFullRoute, VehicleState vehicleState, Behavior behavior, IPath path, DynamicObstacles dynamicObstacles, StaticObstacles staticObstacles) { this.LocalRoute = localRoute; this.BestFullRoute = bestFullRoute; this.VehicleState = vehicleState; this.behavior = behavior; this.path = path; this.DynamicObstacles = dynamicObstacles; this.StaticObstacles = staticObstacles; }
public override void Initialize(Behavior b) { base.Initialize(b); ZoneParkingBehavior cb = (ZoneParkingBehavior)b; HandleBehavior(cb); this.totalTimer = Stopwatch.StartNew(); phase = PlanningPhase.Initial; }
public override void Initialize(Behavior b) { base.Initialize(b); // extract the stay in lane behavior StayInLaneBehavior sb = (StayInLaneBehavior)b; Services.UIService.PushAbsolutePath(sb.BackupPath, sb.TimeStamp, "original path1"); Services.UIService.PushAbsolutePath(new LineList(), sb.TimeStamp, "original path2"); HandleBehavior(sb); }
/// <summary> /// Constructor /// </summary> /// <param name="currentArbiterState"></param> /// <param name="internalCarMode"></param> /// <param name="fullRoute"></param> /// <param name="routeTime"></param> /// <param name="actionPoint"></param> /// <param name="currentGoal"></param> /// <param name="goals"></param> /// <param name="behavior"></param> public ArbiterInformation(string currentArbiterState, CarMode internalCarMode, FullRoute fullRoute, double routeTime, RndfWaypointID actionPoint, RndfWaypointID currentGoal, Queue<RndfWaypointID> goals, Behavior behavior, VehicleState planningState) { this.CurrentArbiterState = currentArbiterState; this.InternalCarMode = internalCarMode; this.FullRoute = fullRoute; this.RouteTime = routeTime; this.ActionPoint = actionPoint; this.CurrentGoal = currentGoal; this.Goals = goals; this.Behavior = behavior; this.PlanningState = planningState; }
public override void Initialize(Behavior b) { base.Initialize(b); Services.ObstaclePipeline.ExtraSpacing = 0.5; ZoneTravelingBehavior cb = (ZoneTravelingBehavior)b; HandleBehavior(cb); // set up a bogus speed command HandleSpeedCommand(recommendedSpeed); }
public void Initialize(Behavior b) { UTurnBehavior cb = (UTurnBehavior)b; Services.ObstaclePipeline.ExtraSpacing = 0; Services.ObstaclePipeline.UseOccupancyGrid = true; // get the absolute state at the time of the behavior AbsolutePose absPose = Services.StateProvider.GetAbsolutePose(cb.TimeStamp); originalPoint = absPose.xy; HandleBehavior(cb); this.passCompleted = true; this.pass = UTurnPass.Initializing; this.cancelled = false; }
public override void Initialize(Behavior b) { base.Initialize(b); Services.ObstaclePipeline.ExtraSpacing = 0; Services.ObstaclePipeline.UseOccupancyGrid = true; ChangeLaneBehavior cb = (ChangeLaneBehavior)b; Services.UIService.PushAbsolutePath(cb.BackupStartLanePath, cb.TimeStamp, "original path1"); Services.UIService.PushAbsolutePath(cb.BackupTargetLanePath, cb.TimeStamp, "original path2"); startTimestamp = b.TimeStamp; HandleBehavior(cb); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in change lanes -- initializing, start timestamp {0}", b.TimeStamp); }
public void OnBehaviorReceived(Behavior b) { if (b is UTurnBehavior) { UTurnBehavior cb = (UTurnBehavior)b; Services.BehaviorManager.QueueParam(cb); } else { Services.BehaviorManager.Execute(b, null, false); } }
public void OnBehaviorReceived(Behavior b) { Trace.CorrelationManager.StartLogicalOperation("OnBehaviorReceived"); Services.Dataset.MarkOperation("behavior rate", LocalCarTimeProvider.LocalNow); try { // write the trace output if (TraceSource.Switch.ShouldTrace(TraceEventType.Verbose)) { // write a verbose output TraceSource.TraceEvent(TraceEventType.Verbose, 1, "recevied behavior: {0}", b); } else { TraceSource.TraceEvent(TraceEventType.Information, 1, "received behavior: {0}", b.GetType()); } if (b != null) { HandleDecorators(b.Decorators); } if (b is NullBehavior) return; if (currentBehavior != null) { try { currentBehavior.OnBehaviorReceived(b); } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Warning, 2, "current behavior ({0}) threw exception in OnBehaviorReceived: {1}", currentBehavior, ex); throw; } } else { Execute(b, null, false); } } finally { Trace.CorrelationManager.StopLogicalOperation(); } }
/// <summary> /// Execute the Behavior /// </summary> /// <param name="behavior"></param> public abstract void ExecuteBehavior(Behavior behavior, Common.Coordinates location, RndfWaypointID lowerBound, RndfWaypointID upperBound);
public void OnBehaviorReceived(Behavior b) { // just have the behavior manager execute the new behavior Services.BehaviorManager.Execute(b, null, false); }
public void OnBehaviorReceived(Behavior b) { // check if this is a valid state transition if (b is HoldBrakeBehavior || b is SimpleStayInLaneBehavior) { Services.BehaviorManager.Execute(b, null, true); } else { throw new InvalidBehaviorException(); } }
/// <summary> /// Sets and immediately begins executing the supplied behavior, blocks and returns completion report /// </summary> /// <param name="b">Behavior to execute</param> public abstract CompletionReport TestExecuteBehavior(Behavior b);
public override void OnBehaviorReceived(Behavior b) { if ((pulloutMode && b is ZoneParkingPullOutBehavior) || (!pulloutMode && !(b is ZoneParkingPullOutBehavior))) { Services.BehaviorManager.QueueParam(b); } else { Services.BehaviorManager.Execute(b, null, false); } }
private void BehaviorProc() { Trace.CorrelationManager.StartLogicalOperation("Behavior Loop"); // set the default trace source for this thread OperationalTrace.ThreadTraceSource = TraceSource; OperationalDataSource previousTimeout = OperationalDataSource.None; using (MMWaitableTimer timer = new MMWaitableTimer((uint)(Settings.BehaviorPeriod*1000))) { while (true) { if (Services.DebuggingService.StepMode) { Services.DebuggingService.WaitOnSequencer(typeof(BehaviorManager)); } else { // wait for the timer timer.WaitEvent.WaitOne(); } if (watchdogEvent != null) { try { watchdogEvent.Set(); } catch (Exception) { } } Services.Dataset.MarkOperation("planning rate", LocalCarTimeProvider.LocalNow); TraceSource.TraceEvent(TraceEventType.Verbose, 0, "starting behavior loop"); object param = null; IOperationalBehavior newBehavior = null; Behavior behaviorParam = null; OperationalDataSource timeoutSource = OperationalDataSource.Pose; bool forceHoldBrake = false; try { if (OperationalBuilder.BuildMode != BuildMode.Listen && TimeoutMonitor.HandleRecoveryState()) { forceHoldBrake = true; } } catch (Exception ex) { OperationalTrace.WriteError("error in recovery logic: {0}", ex); } if (OperationalBuilder.BuildMode == BuildMode.Realtime && TimeoutMonitor.AnyTimedOut(ref timeoutSource)) { if (timeoutSource != previousTimeout) { OperationalTrace.WriteError("data source {0} has timed out", timeoutSource); previousTimeout = timeoutSource; } // simply queue up a hold brake Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetHoldBrakeCommand(45)); } else { if (previousTimeout != OperationalDataSource.None) { OperationalTrace.WriteWarning("data source {0} is back online", previousTimeout); previousTimeout = OperationalDataSource.None; } if (forceHoldBrake || (Services.Operational != null && Services.Operational.GetCarMode() == CarMode.Human)) { queuedBehavior = null; queuedParam = null; queuedOrigBehavior = null; if (!(currentBehavior is HoldBrake)) { newBehavior = new HoldBrake(); param = null; queuedOrigBehavior = new HoldBrakeBehavior(); } } else { lock (queueLock) { // get the current queue param param = queuedParam; //TraceSource.TraceEvent(TraceEventType.Verbose, 0, "queued param: {0}", queuedParam == null ? "<null>" : queuedParam.ToString()); // chekc if there is a queued behavior newBehavior = queuedBehavior; queuedBehavior = null; behaviorParam = queuedOrigBehavior; queuedOrigBehavior = null; } } if (newBehavior != null) { // dispose of the old behavior if (currentBehavior != null && currentBehavior is IDisposable) { ((IDisposable)currentBehavior).Dispose(); } Trace.CorrelationManager.StartLogicalOperation("initialize"); TraceSource.TraceEvent(TraceEventType.Verbose, 8, "executing initialize on {0}", newBehavior.GetType().Name); // swap in the new behavior and initialize currentBehavior = newBehavior; try { currentBehavior.Initialize(behaviorParam); TraceSource.TraceEvent(TraceEventType.Verbose, 8, "initialize completed on {0}", currentBehavior.GetType().Name); } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Warning, 4, "exception thrown when initializing behavior: {0}", ex); //throw; } finally { Trace.CorrelationManager.StopLogicalOperation(); } if (behaviorParam != null) { this.currentBehaviorType = behaviorParam.GetType(); } else { this.currentBehaviorType = null; } } else { //TraceSource.TraceEvent(TraceEventType.Verbose, 11, "queued behavior was null"); } // process the current behavior Trace.CorrelationManager.StartLogicalOperation("process"); try { if (currentBehavior != null) { Services.Dataset.ItemAs<string>("behavior string").Add(currentBehavior.GetName(), LocalCarTimeProvider.LocalNow); DateTime start = HighResDateTime.Now; currentBehavior.Process(param); TimeSpan diff = HighResDateTime.Now-start; lock (cycleTimeQueue) { cycleTimeQueue.Add(diff.TotalSeconds, LocalCarTimeProvider.LocalNow.ts); } } } catch (Exception ex) { // just ignore any exceptions for now // should log this somehow TraceSource.TraceEvent(TraceEventType.Warning, 5, "exception thrown when processing behavior: {0}", ex); } finally { Trace.CorrelationManager.StopLogicalOperation(); } } TraceSource.TraceEvent(TraceEventType.Verbose, 0, "ending behavior loop"); if (Services.DebuggingService.StepMode) { Services.DebuggingService.SetCompleted(typeof(BehaviorManager)); } } } }
/// <summary> /// Sets and immediately begins executing the supplied behavior. /// </summary> /// <param name="b">Behavior to execute</param> public abstract void ExecuteBehavior(Behavior b);
public override void ExecuteBehavior(Behavior behavior, Common.Coordinates location, RndfWaypointID lowerBound, RndfWaypointID upperBound) { if(behavior is UTurnBehavior) { // } // check to see if we are at the upper bound RndfWayPoint upperWaypoint = channelListener.RndfNetwork.Waypoints[upperBound]; if (!(behavior is TurnBehavior) && upperWaypoint.NextLanePartition != null && location.DistanceTo(upperWaypoint.Position) < 3) { lowerBound = upperWaypoint.WaypointID; upperBound = upperWaypoint.NextLanePartition.FinalWaypoint.WaypointID; } Console.WriteLine(" > Received Instruction to Execute Behavior: " + behavior.ToString()); if (behavior is StayInLaneBehavior) { StayInLaneBehavior stayInLane = (StayInLaneBehavior)behavior; if (stayInLane.SpeedCommand is StopLineLaneSpeedCommand) { StopLineLaneSpeedCommand speedCommand = (StopLineLaneSpeedCommand)stayInLane.SpeedCommand; if (speedCommand.Distance < 2) { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = location; LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1); List<LaneEstimate> laneEstimates = new List<LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 0; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); ///Console.WriteLine(" > Published Position"); } else { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[upperBound].Position; LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1); List<LaneEstimate> laneEstimates = new List<LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 3; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); ///Console.WriteLine(" > Published Position"); } } else if (stayInLane.SpeedCommand is StopLaneSpeedCommand) { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = location; LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1); List<LaneEstimate> laneEstimates = new List<LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = -5; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); ///Console.WriteLine(" > Published Position"); } else if(stayInLane.SpeedCommand is DefaultLaneSpeedCommand) { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[upperBound].Position; LaneEstimate laneEstimate = new LaneEstimate(lowerBound.LaneID, lowerBound.LaneID, 1); List<LaneEstimate> laneEstimates = new List<LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 3; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); //Console.WriteLine(" > Published Position"); } else { throw new ArgumentException("Unknown Lane Speed Type", "stayInLane.SpeedCommand"); } } // TODO: include midway point else if (behavior is TurnBehavior) { TurnBehavior currentBehavior = (TurnBehavior)behavior; RndfWayPoint exitWaypoint = channelListener.RndfNetwork.Waypoints[currentBehavior.ExitPoint]; if (location.DistanceTo(exitWaypoint.Position) < 0.1) { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); RndfWayPoint entryWaypoint = channelListener.RndfNetwork.Waypoints[currentBehavior.EntryPoint]; Common.Coordinates change = entryWaypoint.Position - exitWaypoint.Position; Common.Coordinates midpoint = exitWaypoint.Position + change/2; LaneEstimate laneEstimate = new LaneEstimate(currentBehavior.ExitPoint.LaneID, currentBehavior.EntryPoint.LaneID, 1); vehicleState.xyPosition = midpoint; List<LaneEstimate> laneEstimates = new List<LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 3; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); } else { // Create a fake vehicle state VehicleState vehicleState = new VehicleState(); vehicleState.xyPosition = channelListener.RndfNetwork.Waypoints[currentBehavior.EntryPoint].Position; LaneEstimate laneEstimate = new LaneEstimate(currentBehavior.EntryPoint.LaneID, currentBehavior.EntryPoint.LaneID, 1); List<LaneEstimate> laneEstimates = new List<LaneEstimate>(); laneEstimates.Add(laneEstimate); vehicleState.speed = 3; vehicleState.vehicleRndfState = new VehicleRndfState(laneEstimates); this.PublishVehicleState(vehicleState); //Console.WriteLine(" > Published Position"); } } else { throw new ArgumentException("Unknown Behavior Type", "behavior"); } //Console.WriteLine("Sent Back Position \n"); }
public CompletionReport TestBehavior(Behavior b) { IOperationalBehavior operBehavior = null; try { testMode = true; Console.WriteLine("Testing behavior " + b.ToString() + " -- type " + b.GetType().Name); cachedCompletionReport = new SuccessCompletionReport(b.GetType()); HandleDecorators(b.Decorators); operBehavior = MapBehavior(b); operBehavior.Initialize(b); operBehavior.Process(null); Console.WriteLine("Result: " + cachedCompletionReport.ToString()); cachedCompletionReport.BehaviorId = b.UniqueId(); return cachedCompletionReport; } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Error, 0, "error testing behavior {0}: {1}", b.GetType(), ex); throw; } finally { if (operBehavior != null && operBehavior is IDisposable) { ((IDisposable)operBehavior).Dispose(); } } }
public void Execute(IOperationalBehavior b, object param, bool immediate) { lock (queueLock) { // if we want to immediately execute the next behavior, then we need to cancel the current one if (immediate && currentBehavior != null) { TraceSource.TraceEvent(TraceEventType.Verbose, 9, "in Execute, perfoming cancel on current behavior"); try { currentBehavior.Cancel(); } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Error, 3, "cancelling the current behavior threw an exception: {0}", ex); throw; } } // queue up the new behavior queuedBehavior = b; queuedParam = param; queuedOrigBehavior = null; } }
/// <summary> /// Maneuver to execute /// </summary> /// <param name="path">Path to Follow</param> /// <param name="behavior">Type of Behavior</param> public Maneuver(Behavior behavior, IState next) { this.next = next; this.behavior = behavior; }
public void Execute(Behavior b, object param, bool immediate) { lock (queueLock) { Execute(MapBehavior(b), param, immediate); queuedOrigBehavior = b; } }
/// <summary> /// Constructor /// </summary> /// <param name="behavior">Behavior to accomplish</param> /// <param name="path">General path to take to accomplish Behavior</param> public ArbiterExitData(Behavior behavior, IPath path) { this.behavior = behavior; this.path = path; }
/// <summary> /// Constructor /// </summary> /// <param name="recoveryBehaviorType"></param> /// <param name="completionState"></param> /// <param name="abortState"></param> /// <param name="defcon"></param> public BlockageRecoveryState(Behavior recoveryBehavior, IState completionState, IState abortState, BlockageRecoveryDEFCON defcon, EncounteredBlockageState ebs, BlockageRecoverySTATUS status) { this.RecoveryBehavior = recoveryBehavior; this.CompletionState = completionState; this.AbortState = abortState; this.Defcon = defcon; this.RecoveryStatus = status; this.EncounteredState = ebs; }
private IOperationalBehavior MapBehavior(Behavior b) { BehaviorBuidler builder; if (behaviorMap.TryGetValue(b.GetType(), out builder)) { return builder(); } else { TraceSource.TraceEvent(TraceEventType.Warning, 2, "behavior type {0} does not have a mapping", b.GetType()); throw new ArgumentException("Behavior type " + b.GetType().Name + " does not have a mapping"); } }