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;
}
DateTime start = HighResDateTime.Now;
OperationalVehicleState vs = Services.StateProvider.GetVehicleState();
LinePath curBasePath = basePath;
LinePath curLeftBound = leftBound;
LinePath curRightBound = rightBound;
// transform the base path to the current iteration
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
if (pathTime != curTimestamp)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(pathTime, curTimestamp);
curBasePath = curBasePath.Transform(relTransform);
curLeftBound = curLeftBound.Transform(relTransform);
curRightBound = curRightBound.Transform(relTransform);
}
// get the distance between the zero point and the start point
double distToStart = Coordinates.Zero.DistanceTo(curBasePath.GetPoint(curBasePath.StartPoint));
// get the sub-path between 5 and 25 meters ahead
double startDist = TahoeParams.FL;
LinePath.PointOnPath startPoint = curBasePath.AdvancePoint(curBasePath.ZeroPoint, ref startDist);
double endDist = 30;
LinePath.PointOnPath endPoint = curBasePath.AdvancePoint(startPoint, ref endDist);
if (startDist > 0)
{
// we've reached the end
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
return;
}
// get the sub-path
LinePath subPath = curBasePath.SubPath(startPoint, endPoint);
// add (0,0) as the starting point
subPath.Insert(0, new Coordinates(0, 0));
// do the same for the left, right bound
startDist = TahoeParams.FL;
endDist = 40;
startPoint = curLeftBound.AdvancePoint(curLeftBound.ZeroPoint, startDist);
endPoint = curLeftBound.AdvancePoint(startPoint, endDist);
curLeftBound = curLeftBound.SubPath(startPoint, endPoint);
startPoint = curRightBound.AdvancePoint(curRightBound.ZeroPoint, startDist);
endPoint = curRightBound.AdvancePoint(startPoint, endDist);
curRightBound = curRightBound.SubPath(startPoint, endPoint);
if (cancelled)
{
return;
}
Services.UIService.PushRelativePath(subPath, curTimestamp, "subpath");
Services.UIService.PushRelativePath(curLeftBound, curTimestamp, "left bound");
Services.UIService.PushRelativePath(curRightBound, curTimestamp, "right bound");
// run a path smoothing iteration
lock (this) {
planner = new PathPlanner();
}
////////////////////////////////////////////////////////////////////////////////////////////////////
// start of obstacle manager - hik
bool obstacleManagerEnable = true;
PathPlanner.SmoothingResult result;
if (obstacleManagerEnable == true)
{
// generate fake obstacles (for simulation testing only)
double obsSize = 10.5 / 2;
List <Coordinates> obstaclePoints = new List <Coordinates>();
List <Obstacle> obstacleClusters = new List <Obstacle>();
// fake left obstacles (for simulation only)
int totalLeftObstacles = Math.Min(0, curLeftBound.Count - 1);
for (int i = 0; i < totalLeftObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
// fake right obstacles (for simulation only)
int totalRightObstacles = Math.Min(0, curRightBound.Count - 1);
for (int i = 0; i < totalRightObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
// fake center obstacles (for simulation only)
int totalCenterObstacles = Math.Min(0, subPath.Count - 1);
for (int i = 2; i < totalCenterObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(10000, 10000));
obstaclePoints.Add(new Coordinates(10000, 10001));
obstaclePoints.Add(new Coordinates(10001, 10000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(1000, 1000));
obstaclePoints.Add(new Coordinates(1000, 1001));
obstaclePoints.Add(new Coordinates(1001, 1000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(-1000, -1000));
obstaclePoints.Add(new Coordinates(-1000, -1001));
obstaclePoints.Add(new Coordinates(-1001, -1000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
foreach (Obstacle obs in obstacleClusters)
{
obs.cspacePolygon = new Polygon(obs.obstaclePolygon.points);
}
// find obstacle path and left/right classification
LinePath obstaclePath = new LinePath();
//Boolean successFlag;
//double laneWidthAtPathEnd = 10.0;
//#warning this currently doesn't work
/*obstacleManager.ProcessObstacles(subPath, new LinePath[] { curLeftBound }, new LinePath[] { curRightBound },
* obstacleClusters, laneWidthAtPathEnd,
* out obstaclePath, out successFlag);
*/
// prepare left and right lane bounds
double laneMinSpacing = 0.1;
double laneDesiredSpacing = 0.5;
double laneAlphaS = 10000;
List <Boundary> leftBounds = new List <Boundary>();
List <Boundary> rightBounds = new List <Boundary>();
leftBounds.Add(new Boundary(curLeftBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));
rightBounds.Add(new Boundary(curRightBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));
// sort out obstacles as left and right
double obstacleMinSpacing = 0.1;
double obstacleDesiredSpacing = 1.0;
double obstacleAlphaS = 10000;
Boundary bound;
int totalObstacleClusters = obstacleClusters.Count;
for (int i = 0; i < totalObstacleClusters; i++)
{
if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Left)
{
// obstacle cluster is to the left of obstacle path
bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
obstacleDesiredSpacing, obstacleAlphaS, true);
bound.CheckFrontBumper = true;
leftBounds.Add(bound);
}
else if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Right)
{
// obstacle cluster is to the right of obstacle path
bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
obstacleDesiredSpacing, obstacleAlphaS, true);
bound.CheckFrontBumper = true;
rightBounds.Add(bound);
}
else
{
// obstacle cluster is outside grid, hence ignore obstacle cluster
}
}
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
// PlanPath function call with obstacle path and obstacles
result = planner.PlanPath(obstaclePath, obstaclePath, leftBounds, rightBounds,
0, maxSpeed, vs.speed, null, curTimestamp, false);
stopwatch.Stop();
Console.WriteLine("============================================================");
Console.WriteLine("With ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
Console.WriteLine("============================================================");
}
else
{
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
// original PlanPath function call
result = planner.PlanPath(subPath, curLeftBound, curRightBound,
0, maxSpeed, vs.speed, null, curTimestamp);
stopwatch.Stop();
Console.WriteLine("============================================================");
Console.WriteLine("Without ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
Console.WriteLine("============================================================");
}
// end of obstacle manager - hik
////////////////////////////////////////////////////////////////////////////////////////////////////
//PathPlanner.PlanningResult result = planner.PlanPath(subPath, curLeftBound, curRightBound, 0, maxSpeed, vs.speed, null, curTimestamp);
//SmoothedPath path = new SmoothedPath(pathTime);
lock (this) {
planner = null;
}
if (cancelled)
{
return;
}
if (result.result == UrbanChallenge.PathSmoothing.SmoothResult.Sucess)
{
// start tracking the path
Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetSmoothedPathVelocityCommand(result.path));
//Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetConstantSteeringConstantSpeedCommand(-.5, 2));
/*TrackingCommand cmd = new TrackingCommand(
* new FeedbackSpeedCommandGenerator(new ConstantSpeedGenerator(2, null)),
* new SinSteeringCommandGenerator(),
* true);
* Services.TrackingManager.QueueCommand(cmd);*/
// send the path's we're tracking to the UI
Services.UIService.PushRelativePath(result.path, curTimestamp, "smoothed path");
cancelled = true;
}
}
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));
}
}
}
}
