[esw] Drive watchdog bug fix (#978)

jetson/odrive_bridge/src/__main__.py

@@ -15,15 +15,15 @@
 
 def main():
     global lcm_, modrive, left_speed, right_speed, legal_controller, \
-        vel_msg, state_msg, usblock, speedlock, start_time, watchdog, odrive_bridge
+        vel_msg, state_msg, usb_lock, speed_lock, start_time, watchdog, odrive_bridge
 
     lcm_, left_speed, right_speed, start_time, legal_controller, \
         vel_msg, state_msg = lcm.LCM(), 0.0, 0.0, t.clock(), int(sys.argv[1]), DriveVelData(), DriveStateData()
 
-    speedlock = threading.Lock()
-    usblock = threading.Lock()
+    speed_lock = threading.Lock()
+    usb_lock = threading.Lock()
 
-    threading._start_new_thread(lcmThreaderMan, ())
+    threading._start_new_thread(lcm_publisher_thread, ())
     odrive_bridge = OdriveBridge()
     # starting state is DisconnectedState()
     # start up sequence is called, disconnected-->disarm-->arm
@@ -38,38 +38,41 @@ def main():
                 print("loss of comms")
                 prev_comms = False
 
-            speedlock.acquire()
+            speed_lock.acquire()
             left_speed = right_speed = 0
-            speedlock.release()
+            speed_lock.release()
         else:
             if not prev_comms:
                 prev_comms = True
                 print("regained comms")
 
         try:
             odrive_bridge.update()
-        except (fibre.protocol.ChannelBrokenException, AttributeError):
-            print("odrive has been unplugged")
-            if usblock.locked():
-                usblock.release()
+        except Exception as e:
+            print("odrive has been unplugged, exception caught as:", e)
+            if usb_lock.locked():
+                usb_lock.release()
 
-            usblock.acquire()
+            usb_lock.acquire()
             odrive_bridge.on_event(Event.DISCONNECTED_ODRIVE)
-            usblock.release()
+            usb_lock.release()
 
     exit()
 
 
-def lcmThreaderMan():
-    lcm_1 = lcm.LCM()
-    lcm_1.subscribe("/drive_vel_cmd", drive_vel_cmd_callback)
+def lcm_publisher_thread():
+    lcm_pub = lcm.LCM()
+    lcm_pub.subscribe("/drive_vel_cmd", drive_vel_cmd_callback)
     while True:
-        lcm_1.handle()
+        lcm_pub.handle()
+        global start_time
+        start_time = t.clock()
         try:
             publish_encoder_msg()
-        except (NameError, AttributeError, fibre.protocol.ChannelBrokenException):
-            if usblock.locked():
-                usblock.release()
+        except Exception as e:
+            print("Exception caught as:", e)
+            if usb_lock.locked():
+                usb_lock.release()
 
 
 states = ["DisconnectedState", "DisarmedState", "ArmedState", "ErrorState"]
@@ -177,8 +180,8 @@ def on_event(self, event):
         if (event == Event.ODRIVE_ERROR):
             try:
                 modrive.reboot()  # only runs after initial pairing
-            except:
-                print('channel error caught')
+            except Exception as e:
+                print('Exception caught as:', e)
 
             return DisconnectedState()
 
@@ -194,7 +197,7 @@ def __init__(self):
         """
         global modrive
         self.state = DisconnectedState()  # default is disarmed
-        self.encoder_time = self.left_speed = self.right_speed = 0.0
+        self.left_speed = self.right_speed = 0.0
 
     def connect(self):
         global modrive, legal_controller
@@ -212,11 +215,10 @@ def connect(self):
         odrive = odv.find_any(serial_number=id)
 
         print("found odrive")
-        usblock.acquire()
+        usb_lock.acquire()
         modrive = Modrive(odrive)  # arguments = odr
         modrive.set_current_lim(modrive.CURRENT_LIM)
-        usblock.release()
-        self.encoder_time = t.time()
+        usb_lock.release()
 
     def on_event(self, event):
         """
@@ -234,47 +236,47 @@ def on_event(self, event):
     def update(self):
         if (str(self.state) == "ArmedState"):
             try:
-                usblock.acquire()
+                usb_lock.acquire()
                 errors = modrive.check_errors()
                 modrive.watchdog_feed()
-                usblock.release()
+                usb_lock.release()
 
-            except (fibre.protocol.ChannelBrokenException, AttributeError):
-                if usblock.locked():
-                    usblock.release()
+            except Exception as e:
+                if usb_lock.locked():
+                    usb_lock.release()
                 errors = 0
-                usblock.acquire()
+                usb_lock.acquire()
                 self.on_event(Event.DISCONNECTED_ODRIVE)
-                usblock.release()
-                print("unable to check errors of unplugged odrive")
+                usb_lock.release()
+                print("odrive unplugged, update failed with exception:", e)
 
             if errors:
-                usblock.acquire()
+                usb_lock.acquire()
                 self.on_event(Event.ODRIVE_ERROR)
-                usblock.release()
+                usb_lock.release()
                 return
 
-            global speedlock, left_speed, right_speed
+            global speed_lock, left_speed, right_speed
 
-            speedlock.acquire()
+            speed_lock.acquire()
             self.left_speed, self.right_speed = left_speed, right_speed
-            speedlock.release()
+            speed_lock.release()
 
-            usblock.acquire()
+            usb_lock.acquire()
             modrive.set_vel("LEFT", self.left_speed)
             modrive.set_vel("RIGHT", self.right_speed)
-            usblock.release()
+            usb_lock.release()
 
         elif (str(self.state) == "DisconnectedState"):
             self.connect()
-            usblock.acquire()
+            usb_lock.acquire()
             self.on_event(Event.ARM_CMD)
-            usblock.release()
+            usb_lock.release()
 
         elif (str(self.state) == "ErrorState"):
-            usblock.acquire()
+            usb_lock.acquire()
             self.on_event(Event.ODRIVE_ERROR)
-            usblock.release()
+            usb_lock.release()
 
     def get_state(self):
         return str(self.state)
@@ -294,13 +296,13 @@ def publish_state_msg(msg, state):
 
 
 def publish_encoder_helper(axis):
-    global modrive, legal_controller, usblock
+    global modrive, legal_controller, usb_lock
     msg = DriveVelData()
 
-    usblock.acquire()
+    usb_lock.acquire()
     msg.current_amps = modrive.get_iq_measured(axis)
     msg.vel_percent = modrive.get_vel_estimate(axis)
-    usblock.release()
+    usb_lock.release()
 
     motor_map = {("LEFT", 0): 0, ("RIGHT", 0): 1,
                  ("LEFT", 1): 2, ("RIGHT", 1): 3,
@@ -320,16 +322,17 @@ def drive_vel_cmd_callback(channel, msg):
     # set the odrive's velocity to the float specified in the message
     # no state change
 
-    global speedlock, odrive_bridge
+    global speed_lock, odrive_bridge
     try:
         cmd = DriveVelCmd.decode(msg)
         if (odrive_bridge.get_state() == "ArmedState"):
             global left_speed, right_speed
 
-            speedlock.acquire()
+            speed_lock.acquire()
             left_speed, right_speed = cmd.left, cmd.right
-            speedlock.release()
-    except NameError:
+            speed_lock.release()
+    except Exception as e:
+        print("Exception caught as:", e)
         pass
 
 
@@ -339,11 +342,13 @@ def drive_vel_cmd_callback(channel, msg):
 
 class Modrive:
     CURRENT_LIM = 4
+    # scales normalized inputs to max physical speed of rover in turn/s
+    SPEED_MULTIPLIER = 50
 
     def __init__(self, odr):
         self.odrive = odr
-        self.front_axis = self.odrive.axis0
-        self.back_axis = self.odrive.axis1
+        self.left_axis = self.odrive.axis0
+        self.right_axis = self.odrive.axis1
         self.set_current_lim(self.CURRENT_LIM)
 
     # viable to set initial state to idle?
@@ -356,22 +361,22 @@ def __getattr__(self, attr):
     def enable_watchdog(self):
         try:
             print("Enabling watchdog")
-            self.front_axis.config.watchdog_timeout = 0.1
-            self.back_axis.config.watchdog_timeout = 0.1
+            self.left_axis.config.watchdog_timeout = 0.1
+            self.right_axis.config.watchdog_timeout = 0.1
             self.watchdog_feed()
-            self.front_axis.config.enable_watchdog = True
-            self.back_axis.config.enable_watchdog = True
+            self.left_axis.config.enable_watchdog = True
+            self.right_axis.config.enable_watchdog = True
         except Exception as e:
             print("Failed in enable_watchdog. Error:")
             print(e)
 
     def disable_watchdog(self):
         try:
             print("Disabling watchdog")
-            self.front_axis.config.watchdog_timeout = 0
-            self.back_axis.config.watchdog_timeout = 0
-            self.front_axis.config.enable_watchdog = False
-            self.back_axis.config.enable_watchdog = False
+            self.left_axis.config.watchdog_timeout = 0
+            self.right_axis.config.watchdog_timeout = 0
+            self.left_axis.config.enable_watchdog = False
+            self.right_axis.config.enable_watchdog = False
         except fibre.protocol.ChannelBrokenException:
             print("Failed in disable_watchdog. Unplugged")
 
@@ -380,15 +385,15 @@ def reset_watchdog(self):
             print("Resetting watchdog")
             self.disable_watchdog()
             # clears errors cleanly
-            self.front_axis.error = self.back_axis.error = 0
+            self.left_axis.error = self.right_axis.error = 0
             self.enable_watchdog()
         except fibre.protocol.ChannelBrokenException:
             print("Failed in disable_watchdog. Unplugged")
 
     def watchdog_feed(self):
         try:
-            self.front_axis.watchdog_feed()
-            self.back_axis.watchdog_feed()
+            self.left_axis.watchdog_feed()
+            self.right_axis.watchdog_feed()
         except fibre.protocol.ChannelBrokenException:
             print("Failed in watchdog_feed. Unplugged")
 
@@ -407,29 +412,28 @@ def arm(self):
         self.set_velocity_ctrl()
 
     def set_current_lim(self, lim):
-        self.front_axis.motor.config.current_lim = lim
-        self.back_axis.motor.config.current_lim = lim
+        self.left_axis.motor.config.current_lim = lim
+        self.right_axis.motor.config.current_lim = lim
 
     def _set_control_mode(self, mode):
-        self.front_axis.controller.config.control_mode = mode
-        self.back_axis.controller.config.control_mode = mode
+        self.left_axis.controller.config.control_mode = mode
+        self.right_axis.controller.config.control_mode = mode
 
     def set_velocity_ctrl(self):
         self._set_control_mode(CONTROL_MODE_VELOCITY_CONTROL)
 
     def get_iq_measured(self, axis):
         # measured current [Amps]
         if (axis == "LEFT"):
-            return self.front_axis.motor.current_control.Iq_measured
+            return self.left_axis.motor.current_control.Iq_measured
         elif(axis == "RIGHT"):
-            return self.back_axis.motor.current_control.Iq_measured
+            return self.right_axis.motor.current_control.Iq_measured
 
     def get_vel_estimate(self, axis):
-        # divide by 1.5 to scale by percent
         if (axis == "LEFT"):
-            return self.front_axis.encoder.vel_estimate
+            return self.left_axis.encoder.vel_estimate
         elif(axis == "RIGHT"):
-            return self.back_axis.encoder.vel_estimate
+            return self.right_axis.encoder.vel_estimate
 
     def idle(self):
         self._requested_state(AXIS_STATE_IDLE)
@@ -438,17 +442,17 @@ def closed_loop_ctrl(self):
         self._requested_state(AXIS_STATE_CLOSED_LOOP_CONTROL)
 
     def _requested_state(self, state):
-        self.back_axis.requested_state = state
-        self.front_axis.requested_state = state
+        self.right_axis.requested_state = state
+        self.left_axis.requested_state = state
 
     def set_vel(self, axis, vel):
         if (axis == "LEFT"):
-            self.front_axis.controller.input_vel = -vel * 50
+            self.left_axis.controller.input_vel = -vel * self.SPEED_MULTIPLIER
         elif axis == "RIGHT":
-            self.back_axis.controller.input_vel = vel * 50
+            self.right_axis.controller.input_vel = vel * self.SPEED_MULTIPLIER
 
     def get_current_state(self):
-        return (self.front_axis.current_state, self.back_axis.current_state)
+        return (self.left_axis.current_state, self.right_axis.current_state)
 
     def check_errors(self):
-        return self.front_axis.error + self.back_axis.error
+        return self.left_axis.error + self.right_axis.error