Changes from URC (#513)

* Make auton forwarding catch all exceptions * odom constants change * reference coordinate update * dont reset search upon recovery * dont output perception loop stats * Fix offline map max native zoom again * Switch to fast mode toggle for SA * Adjust configs * Change servo angle * change coordinate back to Michigan --------- Co-authored-by: Justin Yu <[email protected]> Co-authored-by: Rocker <[email protected]> Co-authored-by: Ankith Udupa <[email protected]>
umrover · Jun 7, 2023 · 3f2d50b · 3f2d50b
1 parent 0ecfa33
commit 3f2d50b
Show file tree

Hide file tree

Showing 9 changed files with 46 additions and 41 deletions.
diff --git a/config/esw.yaml b/config/esw.yaml
@@ -338,7 +338,7 @@ science:
     - pushed: 60
       start: 90
     - pushed: 0
-      start: 50
+      start: 80
   info:
     mcu_active_timeout_s: 2
     mcu_reset_period_s: 20

diff --git a/config/navigation.yaml b/config/navigation.yaml
@@ -7,12 +7,12 @@ drive:
     turning_p: 3.0
     driving_p: 20.0
   odom:
-    max_driving_effort: 0.5
-    min_driving_effort: -0.5
-    max_turning_effort: 0.3
-    min_turning_effort: -0.3
+    max_driving_effort: 0.7
+    min_driving_effort: -0.7
+    max_turning_effort: 0.5
+    min_turning_effort: -0.5
     turning_p: 3.0
-    driving_p: 5.0
+    driving_p: 10.0
   lookahead_distance: 1.0
 
 gate:

diff --git a/config/teleop.yaml b/config/teleop.yaml
@@ -49,7 +49,7 @@ teleop:
       slow_mode_multiplier: 0.5
     scoop:
       multiplier: 1
-      slow_mode_multiplier: 0.5
+      slow_mode_multiplier: 1.0
     microscope:
       multiplier: 1
       slow_mode_multiplier: 0.5

diff --git a/src/navigation/search.py b/src/navigation/search.py
@@ -108,10 +108,12 @@ def __init__(
         )
         self.traj: Optional[SearchTrajectory] = None
         self.prev_target: Optional[np.ndarray] = None
+        self.is_recovering = False
 
     def reset(self) -> None:
-        self.traj = None
-        self.prev_target = None
+        if not self.is_recovering:
+            self.traj = None
+            self.prev_target = None
 
     def evaluate(self, ud):
         # Check if a path has been generated, and it's associated with the same
@@ -143,7 +145,10 @@ def evaluate(self, ud):
 
         if self.context.rover.stuck:
             self.context.rover.previous_state = SearchStateTransitions.continue_search.name  # type: ignore
+            self.is_recovering = True
             return SearchStateTransitions.recovery_state.name  # type: ignore
+        else:
+            self.is_recovering = False
 
         self.context.search_point_publisher.publish(
             GPSPointList([convert_cartesian_to_gps(pt) for pt in self.traj.coordinates])

diff --git a/src/teleop/auton_enable_forward.py b/src/teleop/auton_enable_forward.py
@@ -64,7 +64,7 @@ def handle_message(self, msg: EnableAuton) -> None:
                 self.msg = msg
 
             # Reconnect service client upon failure.
-            except rospy.ServiceException as e:
+            except Exception as e:
                 rospy.logerr(f"Could not forward enable auton message: {e}")
 
                 self.service_client.close()

diff --git a/src/teleop/gui/src/components/AutonRoverMap.vue b/src/teleop/gui/src/components/AutonRoverMap.vue
@@ -111,7 +111,7 @@ const onlineTileOptions = {
   subdomains: ["mt0", "mt1", "mt2", "mt3"]
 };
 const offlineTileOptions = {
-  maxNativeZoom: 14,
+  maxNativeZoom: 16,
   maxZoom: 100
 };
 

diff --git a/src/teleop/gui/src/components/BasicRoverMap.vue b/src/teleop/gui/src/components/BasicRoverMap.vue
@@ -68,7 +68,7 @@ const onlineTileOptions = {
   subdomains: ["mt0", "mt1", "mt2", "mt3"],
 };
 const offlineTileOptions = {
-  maxNativeZoom: 14,
+  maxNativeZoom: 16,
   maxZoom: 100,
 };
 

diff --git a/src/teleop/jetson/jetson_teleop.py b/src/teleop/jetson/jetson_teleop.py
@@ -332,8 +332,8 @@ def sa_control_callback(self, msg: Joy) -> None:
                     * self.filter_xbox_button(msg.buttons, "right_bumper", "left_bumper"),
                 ]
 
-                slow_mode_activated = msg.buttons[self.xbox_mappings["a"]] or msg.buttons[self.xbox_mappings["b"]]
-                if slow_mode_activated:
+                fast_mode_activated = msg.buttons[self.xbox_mappings["a"]] or msg.buttons[self.xbox_mappings["b"]]
+                if not fast_mode_activated:
                     for i, name in enumerate(self.SA_NAMES):
                         # When going up (vel > 0) with SA joint 2, we DON'T want slow mode.
                         if not (name == "sa_joint_2" and self.sa_cmd.velocity[i] > 0):

diff --git a/src/util/loop_profiler.hpp b/src/util/loop_profiler.hpp
@@ -31,28 +31,28 @@ class LoopProfiler {
      * @brief Call this at the beginning of each loop iteration.
      */
     void beginLoop() {
-        if (mTick % mPrintTick == 0) {
-            Clock::duration averageLoopDuration{};
-            for (auto& [_, durations]: mEventReadings) {
-                averageLoopDuration += std::accumulate(durations.begin(), durations.end(), Clock::duration{}) / durations.size();
-            }
-            // Print update time for the entire loop
-            size_t threadId = std::hash<std::thread::id>{}(std::this_thread::get_id());
-            auto averageLoopMs = std::chrono::duration_cast<DisplayUnits>(averageLoopDuration);
-            int hz = averageLoopMs.count() ? DisplayUnits::period::den / averageLoopMs.count() : -1;
-            ROS_INFO_STREAM("[" << mName << "] [" << threadId << "] Total: "
-                                << averageLoopMs.count() << "ms"
-                                << " (" << hz << " Hz)");
-            // Print update times for each loop event
-            for (auto& [name, durations]: mEventReadings) {
-                Clock::duration averageEventDuration = std::accumulate(durations.begin(), durations.end(), Clock::duration{}) / durations.size();
-                auto averageEventMs = std::chrono::duration_cast<DisplayUnits>(averageEventDuration);
-                ROS_INFO_STREAM("\t" << name << ": " << averageEventMs.count() << "ms");
-                durations.clear();
-            }
-        }
+        // if (mTick % mPrintTick == 0) {
+        //     Clock::duration averageLoopDuration{};
+        //     for (auto& [_, durations]: mEventReadings) {
+        //         averageLoopDuration += std::accumulate(durations.begin(), durations.end(), Clock::duration{}) / durations.size();
+        //     }
+        //     // Print update time for the entire loop
+        //     size_t threadId = std::hash<std::thread::id>{}(std::this_thread::get_id());
+        //     auto averageLoopMs = std::chrono::duration_cast<DisplayUnits>(averageLoopDuration);
+        //     int hz = averageLoopMs.count() ? DisplayUnits::period::den / averageLoopMs.count() : -1;
+        //     ROS_INFO_STREAM("[" << mName << "] [" << threadId << "] Total: "
+        //                         << averageLoopMs.count() << "ms"
+        //                         << " (" << hz << " Hz)");
+        //     // Print update times for each loop event
+        //     for (auto& [name, durations]: mEventReadings) {
+        //         Clock::duration averageEventDuration = std::accumulate(durations.begin(), durations.end(), Clock::duration{}) / durations.size();
+        //         auto averageEventMs = std::chrono::duration_cast<DisplayUnits>(averageEventDuration);
+        //         ROS_INFO_STREAM("\t" << name << ": " << averageEventMs.count() << "ms");
+        //         durations.clear();
+        //     }
+        // }
 
-        mTick++;
+        // mTick++;
     }
 
     /**
@@ -63,11 +63,11 @@ class LoopProfiler {
      * @param name
      */
     void measureEvent(std::string const& name) {
-        Clock::time_point now = Clock::now();
-        if (mLastEpochTime) {
-            Clock::duration duration = now - mLastEpochTime.value();
-            mEventReadings[name].push_back(duration);
-        }
-        mLastEpochTime = now;
+        // Clock::time_point now = Clock::now();
+        // if (mLastEpochTime) {
+        //     Clock::duration duration = now - mLastEpochTime.value();
+        //     mEventReadings[name].push_back(duration);
+        // }
+        // mLastEpochTime = now;
     }
 };