commit ead4da6c9f603c3fa666bb4abbfbdf6c5a536a4a
author James Kuszmaul <jabukuszmaul@gmail.com> Sun Oct 24 17:36:54 2021 -0700
committer Austin Schuh <austin.linux@gmail.com> Sat Nov 06 16:59:30 2021 -0700
tree eea6cabef9eb0cc5217a14b2890824d39d69dd03
parent ba77b7e5880ad3d93a1f4b6be2864f877a17a7be

Don't reinitialize auto after it runs once

This does two things:
* Fixes bug where we could reinitialize the localizer accidentally at
  the start of teleop.
* Defers replanning until 5 seconds after being disabled.

Change-Id: Ia7c85166cd5e849c36cf81b79b732f5c4bbda57a
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>

y2020/actors/autonomous_actor.cc

diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 46a7542..077e87e 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -53,6 +53,8 @@
   });
 
   button_poll_ = event_loop->AddTimer([this]() {
+    const aos::monotonic_clock::time_point now =
+        this->event_loop()->context().monotonic_event_time;
     if (robot_state_fetcher_.Fetch()) {
       if (robot_state_fetcher_->user_button()) {
         user_indicated_safe_to_reset_ = true;
@@ -63,14 +65,24 @@
       if (joystick_state_fetcher_->has_alliance() &&
           (joystick_state_fetcher_->alliance() != alliance_)) {
         alliance_ = joystick_state_fetcher_->alliance();
-        Replan();
+        is_planned_ = false;
+        // Only kick the planning out by 2 seconds. If we end up enabled in that
+        // second, then we will kick it out further based on the code below.
+        replan_timer_->Setup(now + std::chrono::seconds(2));
+      }
+      if (joystick_state_fetcher_->enabled()) {
+        if (!is_planned_) {
+          // Only replan once we've been disabled for 5 seconds.
+          replan_timer_->Setup(now + std::chrono::seconds(5));
+        }
       }
     }
   });
 }
 
 void AutonomousActor::MaybeSendStartingPosition() {
-  if (user_indicated_safe_to_reset_ && !sent_starting_position_) {
+  if (is_planned_ && user_indicated_safe_to_reset_ &&
+      !sent_starting_position_) {
     CHECK(starting_position_);
     SendStartingPosition(starting_position_.value());
   }
@@ -107,6 +119,9 @@
   } else {
     starting_position_ = Eigen::Vector3d::Zero();
   }
+
+  is_planned_ = true;
+
   MaybeSendStartingPosition();
 }
 
@@ -128,10 +143,11 @@
     AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
     SendStartingPosition(starting_position_.value());
   }
-
-  // Queue up a replan to occur as soon as this action completes.
-  // TODO(james): Modify this so we don't replan during teleop.
-  replan_timer_->Setup(monotonic_now());
+  // Clear this so that we don't accidentally resend things as soon as we replan
+  // later.
+  user_indicated_safe_to_reset_ = false;
+  is_planned_ = false;
+  starting_position_.reset();
 
   AOS_LOG(INFO, "Params are %d\n", params->mode());
   if (alliance_ == aos::Alliance::kInvalid) {

y2020/actors/autonomous_actor.h

diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 7cbabb0..7b60ca4 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -91,6 +91,8 @@
   bool user_indicated_safe_to_reset_ = false;
   bool sent_starting_position_ = false;
 
+  bool is_planned_ = false;
+
   std::optional<Eigen::Vector3d> starting_position_;
 };