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__":

software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch

@@ -75,7 +75,7 @@
             <param name="port" value="17018" />
         </node>
 
-        <node name="gps_status_udp" pkg="nimbro_topic_transport" type="tcp_receiver" output="screen">
+        <node name="udp_statuses" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
             <param name="port" value="17019" />
         </node>
 

software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch

@@ -102,7 +102,7 @@
             <param name="destination_addr" value="$(arg target)" />
             <param name="destination_port" value="17013" />
             <rosparam param="topics">
-                [{name: "/rover_status/bogie_status", compress: false, rate: 1.0}]
+                [{name: "/rover_status/wheel_status", compress: false, rate: 1.0}]
             </rosparam>
         </node>
 
@@ -146,11 +146,14 @@
             </rosparam>
         </node>
 
-        <node name="gps_status_udp_sender" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
+        <node name="udp_statuses_sender" pkg="nimbro_topic_transport" type="udp_sender" output="screen">
             <param name="destination_addr" value="$(arg target)" />
             <param name="destination_port" value="17019" />
             <rosparam param="topics">
-                [{name: "/rover_status/gps_status", compress: true, rate: 5.0}]
+                [
+                {name: "/rover_status/gps_status", compress: true, rate: 5.0},
+                {name: "/rover_status/battery_status", compress: false, rate: 1.0}
+                ]
             </rosparam>
         </node>
     </group>

software/ros_packages/rover_status/CMakeLists.txt

@@ -56,6 +56,7 @@ find_package(catkin REQUIRED COMPONENTS
 
 add_message_files(
     FILES
+    BatteryStatusMessage.msg
     RoverSysStatus.msg
     WheelStatuses.msg
     FrSkyStatus.msg

software/ros_packages/rover_status/msg/BatteryStatusMessage.msg

@@ -0,0 +1 @@
+float32 battery_voltage

software/ros_packages/rover_status/src/system_statuses_node.py

@@ -8,8 +8,8 @@ import os.path
 import psutil
 import pynmea2
 import subprocess
-from rover_status.msg import CameraStatuses, WheelStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo
+from rover_status.msg import BatteryStatusMessage, CameraStatuses, WheelStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo
-from rover_control.msg import DriveCommandMessage, DriveStatusMessage
+from rover_control.msg import DriveCommandMessage, DriveStatusMessage, IrisStatusMessage
 from std_msgs.msg import Empty
 from nmea_msgs.msg import Sentence
 from time import time
@@ -19,6 +19,7 @@ from time import time
 # Global Variables
 #####################################
 # Publishers
+DEFAULT_BATTERY_TOPIC_NAME = "battery_status"
 DEFAULT_CAMERA_TOPIC_NAME = "camera_status"
 DEFAULT_WHEEL_TOPIC_NAME = "wheel_status"
 DEFAULT_FRSKY_TOPIC_NAME = "frsky_status"
@@ -27,9 +28,11 @@ DEFAULT_JETSON_TOPIC_NAME = "jetson_status"
 DEFAULT_MISC_TOPIC_NAME = "misc_status"
 
 MAX_JETSON_UPDATE_HERTZ = 0.2
+MAX_IRIS_UPDATE_HERTZ = 0.2
 
 # Subscribers
 DEFAULT_REQUEST_UPDATE_TOPIC_NAME = "update_requested"
+DEFAULT_IRIS_STATUS_TOPIC_NAME = "/rover_control/iris_status"
 DEFAULT_BOGIE_LEFT_TOPIC_NAME = '/rover_control/drive_status/left'
 DEFAULT_BOGIE_RIGHT_TOPIC_NAME = '/rover_control/drive_status/right'
 DEFAULT_BOGIE_REAR_TOPIC_NAME = '/rover_control/drive_status/rear'
@@ -59,6 +62,7 @@ class SystemStatuses:
         rospy.init_node('SystemStatuses')
 
         # Get Topic Names
+        self.battery_topic_name = rospy.get_param("~pub_battery_status_topic", DEFAULT_BATTERY_TOPIC_NAME)
         self.camera_topic_name = rospy.get_param("~pub_camera_status_topic", DEFAULT_CAMERA_TOPIC_NAME)
         self.wheel_topic_name = rospy.get_param("~pub_wheel_status_topic", DEFAULT_WHEEL_TOPIC_NAME)
         self.frsky_topic_name = rospy.get_param("~pub_frsky_status_topic", DEFAULT_FRSKY_TOPIC_NAME)
@@ -69,12 +73,15 @@ class SystemStatuses:
         # Subscribers
         self.request_update_topic_name = rospy.get_param("~sub_request_update_status_topic",
                                                          DEFAULT_REQUEST_UPDATE_TOPIC_NAME)
+
+        self.iris_status_topic_name = rospy.get_param("~sub_iris_status_topic", DEFAULT_IRIS_STATUS_TOPIC_NAME)
         self.bogie_left_topic_name = rospy.get_param("~sub_bogie_left_topic", DEFAULT_BOGIE_LEFT_TOPIC_NAME)
         self.bogie_right_topic_name = rospy.get_param("~sub_bogie_right_topic", DEFAULT_BOGIE_RIGHT_TOPIC_NAME)
         self.bogie_rear_topic_name = rospy.get_param("~sub_bogie_rear_topic", DEFAULT_BOGIE_REAR_TOPIC_NAME)
         self.gps_nmea_topic_name = rospy.get_param("~sub_gps_nmea_topic", DEFAULT_GPS_NMEA_TOPIC_NAME)
 
         # init all publisher functions
+        self.pub_battery = rospy.Publisher(self.battery_topic_name, BatteryStatusMessage, queue_size=1)
         self.pub_camera = rospy.Publisher(self.camera_topic_name, CameraStatuses, queue_size=1)
         self.pub_wheel = rospy.Publisher(self.wheel_topic_name, WheelStatuses, queue_size=1)
         self.pub_FrSky = rospy.Publisher(self.frsky_topic_name, FrSkyStatus, queue_size=1)
@@ -90,6 +97,7 @@ class SystemStatuses:
         self.manual_update_requested = False
 
         # init all message variables
+        self.battery_message = BatteryStatusMessage()
         self.camera_msg = CameraStatuses()
         self.wheel_msg = WheelStatuses()
         self.FrSky_msg = FrSkyStatus()
@@ -106,6 +114,7 @@ class SystemStatuses:
         self.__update_all_previous_values()
 
         self.last_jetson_message_sent = time()
+        self.last_iris_message_sent = time()
 
     # init all RoverSysMessage values
     def __pull_new_message_values(self):
@@ -138,6 +147,9 @@ class SystemStatuses:
 
     # Instantiates all subscriber methods
     def __instantiate_subscribers(self):
+        # Iris Status Subscriber
+        self.iris_status_sub = rospy.Subscriber(self.iris_status_topic_name, IrisStatusMessage, self.__iris_status_callback)
+
         # Bogie Wheel Subscribers
         self.bogie_left_sub = rospy.Subscriber(self.bogie_left_topic_name, DriveStatusMessage, self.__left_wheel_callback)
         self.bogie_right_sub = rospy.Subscriber(self.bogie_right_topic_name, DriveStatusMessage, self.__right_wheel_callback)
@@ -145,6 +157,9 @@ class SystemStatuses:
         # GPS NMEA subscriber
         self.gps_nmea_sub = rospy.Subscriber(self.gps_nmea_topic_name, Sentence, self.__set_gps_info)
 
+    def __iris_status_callback(self, data):
+        self.battery_message.battery_voltage = data.voltage_24v
+
     def __left_wheel_callback(self, data):
         self.wheel_msg.front_left = data.first_motor_connected
         self.wheel_msg.middle_left = data.second_motor_connected
@@ -228,6 +243,7 @@ class SystemStatuses:
 
     # Used mainly for init, sets all previous values in one go
     def __update_all_previous_values(self):
+        self.__set_previous_battery_values()
         self.__set_previous_camera_values()
         self.__set_previous_jetson_values()
         self.__set_previous_frsky_value()
@@ -274,6 +290,9 @@ class SystemStatuses:
         self.previous_sample_containment_connection_status = self.misc_msg.sample_containment_connection_status
         self.previous_tower_connection_status = self.misc_msg.tower_connection_status
 
+    def __set_previous_battery_values(self):
+        self.previous_battery_voltage = self.battery_message.battery_voltage
+
     def on_update_requested(self,  _):
         self.manual_update_requested = True
 
@@ -283,6 +302,13 @@ class SystemStatuses:
             # Update all message values
             self.__pull_new_message_values()
 
+            if (self.battery_message.battery_voltage != self.previous_battery_voltage or
+                    self.manual_update_requested):
+                if (time() - self.last_iris_message_sent) > (1.0 / MAX_IRIS_UPDATE_HERTZ):
+                    self.__set_previous_battery_values()
+                    self.pub_battery.publish(self.battery_message)
+                    self.last_iris_message_sent = time()
+
             # Camera Check -- if current value is now different from previous value,
             # update the previous value for cameras and publish to listening Subscribers
             if (self.camera_msg.camera_zed != self.previous_camera_zed or