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 Process(object param)
{
curTimestamp = Services.RelativePose.CurrentTimestamp;
if (totalTimer.Elapsed > TimeSpan.FromMinutes(2))
{
Type type;
if (pulloutMode)
{
type = typeof(ZoneParkingPullOutBehavior);
}
else
{
type = typeof(ZoneParkingBehavior);
}
Services.BehaviorManager.ForwardCompletionReport(new SuccessCompletionReport(type));
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
return;
}
if (phase == PlanningPhase.Initial)
{
if (!pulloutMode && IsPassCompleted())
{
Type type = typeof(ZoneParkingBehavior);
Services.BehaviorManager.ForwardCompletionReport(new SuccessCompletionReport(type));
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
return;
}
totalStopTimer = Stopwatch.StartNew();
timer = Stopwatch.StartNew();
// determine if there are any moving obstacles within 20 m
if (CheckAllClear())
{
phase = PlanningPhase.CoolDown;
}
else
{
phase = PlanningPhase.WaitingForClear;
}
}
else if (phase == PlanningPhase.CoolDown)
{
if (CheckAllClear())
{
if (timer.ElapsedMilliseconds > 2000 || totalStopTimer.ElapsedMilliseconds > 20000)
{
// transition to parking phase
InitializeParking();
phase = PlanningPhase.Parking;
totalStopTimer.Stop();
timer.Stop();
}
}
else
{
// we're not clear, transition to waiting for clear
phase = PlanningPhase.WaitingForClear;
timer.Reset();
timer.Start();
}
}
else if (phase == PlanningPhase.WaitingForClear)
{
if (CheckAllClear())
{
// we're all clear to go, transition to cool-down
phase = PlanningPhase.CoolDown;
timer.Reset();
timer.Start();
}
else
{
// check if the timer has elapsed
if (timer.ElapsedMilliseconds > 10000 || totalStopTimer.ElapsedMilliseconds > 20000)
{
InitializeParking();
phase = PlanningPhase.Parking;
totalStopTimer.Stop();
timer.Stop();
}
}
}
else if (phase == PlanningPhase.Parking)
{
bool reverse = false;
if (movingOrder != null)
{
reverse = !movingOrder.Forward;
}
List <Obstacle> obstacles = GetObstacles(curTimestamp);
double collisionDist = GetObstacleCollisionDistance(obstacles) - 0.3;
double feelerDist = CheckFeelerDist(reverse, obstacles) - 0.3;
double remainingDist = GetRemainingStopDistance();
if ((!finalPass && collisionDist < remainingDist && collisionDist < 5) || feelerDist < remainingDist)
{
// wait for the stuff to clear
if (collisionDist < remainingDist)
{
Console.WriteLine("collision violation: cd - {0}, rd {1}", collisionDist, remainingDist);
}
if (feelerDist < remainingDist)
{
Console.WriteLine("feeler violation: fd - {0}, rd {1}", feelerDist, remainingDist);
}
if (!pulloutMode && IsPassCompleted())
{
Type type;
if (pulloutMode)
{
type = typeof(ZoneParkingPullOutBehavior);
}
else
{
type = typeof(ZoneParkingBehavior);
}
Services.BehaviorManager.ForwardCompletionReport(new SuccessCompletionReport(type));
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
}
if (!timer.IsRunning)
{
timer.Start();
}
else if (timer.ElapsedMilliseconds > 2000)
{
// re-initialize parking
InitializeParking();
timer.Stop();
}
Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetHoldBrakeCommand());
passCompleted = true;
}
else if (passCompleted)
{
timer.Stop();
timer.Reset();
ExecutePass();
}
}
}
public void Process(object param)
{
if (cancelled)
{
return;
}
if (!passCompleted && !checkMode)
{
return;
}
if (param != null && param is UTurnBehavior)
{
HandleBehavior((UTurnBehavior)param);
}
ITrackingCommand trackingCommand = null;
if (pass == UTurnPass.Initializing)
{
pass = CheckInitialAlignment();
}
else if (CheckFinalExit())
{
pass = UTurnPass.Final;
}
// determine which pass we just finished
if (pass == UTurnPass.Backward)
{
if (passCompleted)
{
// build the forward pass and run it
bool finalPass;
trackingCommand = BuildForwardPass(out finalPass);
if (finalPass)
{
pass = UTurnPass.Final;
}
else
{
pass = UTurnPass.Forward;
}
passCompleted = false;
}
else if (checkMode)
{
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
double remainingDist = GetRemainingStopDistance(curTimestamp);
double collisionDist = GetObstacleCollisionDistance(false, curvature, remainingDist, GetObstacles(curTimestamp));
if (collisionDist < remainingDist && collisionDist < 3)
{
Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetHoldBrakeCommand());
passCompleted = true;
}
}
}
else if (pass == UTurnPass.Forward)
{
if (passCompleted)
{
trackingCommand = BuildBackwardPass();
pass = UTurnPass.Backward;
passCompleted = false;
}
else if (checkMode)
{
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
double remainingDist = GetRemainingStopDistance(curTimestamp);
double collisionDist = GetObstacleCollisionDistance(true, curvature, remainingDist, GetObstacles(curTimestamp));
if (collisionDist < remainingDist && collisionDist < 3)
{
Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetHoldBrakeCommand());
passCompleted = true;
}
}
}
else if (pass == UTurnPass.Final)
{
// execute a stay in lane behavior
Services.BehaviorManager.Execute(new StayInLaneBehavior(finalLane, finalSpeedCommand, null), null, false);
passCompleted = false;
Services.BehaviorManager.ForwardCompletionReport(new SuccessCompletionReport(typeof(UTurnBehavior)));
}
if (trackingCommand != null)
{
Services.TrackingManager.QueueCommand(trackingCommand);
}
}
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));
}
}
}
}