//Initializes the level for reset
void Awake()
{
_registry = new List <RegisterResetObject>();
_reset = false;
DeathScript.ResetLevel += _beginReset;
_planningPhase = Camera.main.GetComponent <PlanningPhase>();
_inventory = GameObject.FindGameObjectWithTag("Player").GetComponent <Inventory>();
}
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);
ZoneParkingBehavior cb = (ZoneParkingBehavior)b;
HandleBehavior(cb);
this.totalTimer = Stopwatch.StartNew();
phase = PlanningPhase.Initial;
}
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 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 override void Process(object param)
{
if (!BeginProcess()) {
return;
}
// check if we were given a parameter
if (param != null && param is SupraLaneBehavior) {
HandleBehavior((SupraLaneBehavior)param);
}
RelativeTransform relTransform = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp);
LinePath curStartingLane = startingLanePath.Transform(relTransform);
LinePath curEndingLane = endingLanePath.Transform(relTransform);
if (planningPhase == PlanningPhase.StartingLane) {
planningPhase = PlanStartingLane(curStartingLane, curEndingLane);
}
else if (planningPhase == PlanningPhase.BeginEnteringTurn) {
planningPhase = PlanBeginningTurn(curStartingLane, curEndingLane);
}
else if (planningPhase == PlanningPhase.EnteringTurn) {
planningPhase = PlanEnteringTurn(curStartingLane, curEndingLane);
}
else if (planningPhase == PlanningPhase.BeginMidTurn) {
planningPhase = PlanBeginMidTurn(curStartingLane, curEndingLane);
}
else if (planningPhase == PlanningPhase.MidTurn) {
planningPhase = PlanMidTurn(curStartingLane, curEndingLane);
}
else if (planningPhase == PlanningPhase.EndingLane) {
planningPhase = PlanEndingLane(curEndingLane);
}
}
public override void Initialize(Behavior b)
{
base.Initialize(b);
SupraLaneBehavior cb = (SupraLaneBehavior)b;
Services.UIService.PushAbsolutePath(cb.StartingLanePath, cb.TimeStamp, "original path1");
Services.UIService.PushAbsolutePath(cb.EndingLanePath, cb.TimeStamp, "original path2");
HandleBehavior(cb);
// determine which phase we're in
// get the closest point on the starting lane
LinePath.PointOnPath startingLanePoint = startingLanePath.ZeroPoint;
double remainingDistance = startingLanePath.DistanceBetween(startingLanePoint, startingLanePath.EndPoint);
curTimestamp = behaviorTimestamp;
// immediately switch to the mid turn mode if the remaining distance is less than the front-length
// this prevents problems with the lane linearization extending out past the end of the lane
if (startingLanePoint == startingLanePath.EndPoint) {
this.planningPhase = PlanningPhase.EndingLane;
}
else if (remainingDistance < TahoeParams.FL) {
this.planningPhase = PlanningPhase.BeginMidTurn;
}
else if (remainingDistance < GetPlanningDistance()) {
this.planningPhase = PlanningPhase.BeginEnteringTurn;
}
else {
this.planningPhase = PlanningPhase.StartingLane;
}
}
