Added dummy movement for arm position. Changed rover_drive and iris_controller to handle full dropouts of the usb device (hopefully)...

software/ros_packages/rover_control/src/drive_control/drive_control.py

@@ -55,6 +55,8 @@ MOTOR_DRIVER_DEFAULT_MESSAGE = [
 
 UINT16_MAX = 65535
 
+BOGIE_LAST_SEEN_TIMEOUT = 2  # seconds
+
 
 #####################################
 # DriveControl Class Definition
@@ -75,9 +77,10 @@ class DriveControl(object):
 
         self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
 
-        self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID)
-        self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID)
-        self.__setup_minimalmodbus_for_485()
+        self.first_motor = None
+        self.second_motor = None
+
+        self.connect_to_bogie()
 
         self.drive_control_subscriber = \
             rospy.Subscriber(self.drive_control_subscriber_topic, DriveControlMessage, self.drive_control_callback)
@@ -87,6 +90,8 @@ class DriveControl(object):
         self.drive_control_message = DriveControlMessage()
         self.new_control_message = False
 
+        self.bogie_last_seen = time()
+
         self.run()
 
     def __setup_minimalmodbus_for_485(self):
@@ -111,10 +116,18 @@ class DriveControl(object):
             except Exception, error:
                 print "Error occurred:", error
 
+            if (time() - self.bogie_last_seen) > BOGIE_LAST_SEEN_TIMEOUT:
+                return  # Exit so respawn can take over
+
             time_diff = time() - start_time
 
             sleep(max(self.wait_time - time_diff, 0))
 
+    def connect_to_bogie(self):
+        self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID)
+        self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID)
+        self.__setup_minimalmodbus_for_485()
+
     def send_drive_control_message(self):
         if self.new_control_message:
             drive_control = self.drive_control_message
@@ -156,6 +169,9 @@ class DriveControl(object):
         except Exception:
             status.second_motor_connected = False
 
+        if status.first_motor_connected or status.second_motor_connected:
+            self.bogie_last_seen = time()
+
         if status.first_motor_connected:
             status.first_motor_current = first_motor_status[0] / 1000.0
             status.first_motor_fault = first_motor_status[1]

software/ros_packages/rover_control/src/iris_controller/iris_controller.py

@@ -111,6 +111,7 @@ class IrisController(object):
 
             except Exception, error:
                 print "Error occurred:", error
+                return
 
             time_diff = time() - start_time