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 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 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);
}
}
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);
}
}