Added battery status monitoring. Also made wheel status senders/recievers.

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

@@ -147,7 +147,7 @@ class DriveControl(object):
                 self.second_motor.write_registers(0, second_motor_register_data)
 
             except Exception, error:
-                print "Error occurred:", error
+                pass
 
             self.new_control_message = False
 

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

@@ -91,11 +91,13 @@ class IrisController(object):
 
         self.drive_command_publisher = rospy.Publisher(self.drive_command_publisher_topic, DriveCommandMessage,
                                                        queue_size=1)
-        self.iris
-
+        self.iris_status_publisher = rospy.Publisher(self.iris_status_publisher_topic, IrisStatusMessage,
+                                                     queue_size=1)
 
         self.registers = []
 
+        self.iris_connected = False
+
         self.run()
 
     def __setup_minimalmodbus_for_485(self):
@@ -125,7 +127,7 @@ class IrisController(object):
         try:
             self.registers = self.iris.read_registers(0, len(MODBUS_REGISTERS))
         except Exception, error:
-            print error
+            self.iris_connected = False
 
     def broadcast_drive_if_current_mode(self):
         if self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["DRIVE_VS_ARM"]]] < SBUS_VALUES["SBUS_MID"]:
@@ -161,7 +163,10 @@ class IrisController(object):
             print "Arm"
 
     def broadcast_iris_status(self):
-        print self.registers[MODBUS_REGISTERS["VOLTAGE_24V"]]
+        status_message = IrisStatusMessage()
+        status_message.iris_connected = True
+        status_message.voltage_24v = self.registers[MODBUS_REGISTERS["VOLTAGE_24V"]]
+        self.iris_status_publisher.publish(status_message)
 
 
 if __name__ == "__main__":