diff --git a/software/ros_packages/rover_control/src/drive_control/drive_control.py b/software/ros_packages/rover_control/src/drive_control/drive_control.py
index 09187fe..0da5859 100755
--- a/software/ros_packages/rover_control/src/drive_control/drive_control.py
+++ b/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
diff --git a/software/ros_packages/rover_control/src/iris_controller/iris_controller.py b/software/ros_packages/rover_control/src/iris_controller/iris_controller.py
index 73c579b..4100c4a 100755
--- a/software/ros_packages/rover_control/src/iris_controller/iris_controller.py
+++ b/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__":
diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch
index 96c18d9..9dee88a 100644
--- a/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch
+++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch
@@ -75,7 +75,7 @@
-
+
diff --git a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch
index 35c5d3c..7954592 100644
--- a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch
+++ b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch
@@ -102,7 +102,7 @@
- [{name: "/rover_status/bogie_status", compress: false, rate: 1.0}]
+ [{name: "/rover_status/wheel_status", compress: false, rate: 1.0}]
@@ -146,11 +146,14 @@
-
+
- [{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}
+ ]
diff --git a/software/ros_packages/rover_status/CMakeLists.txt b/software/ros_packages/rover_status/CMakeLists.txt
index 368e849..bd5dbb5 100644
--- a/software/ros_packages/rover_status/CMakeLists.txt
+++ b/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
diff --git a/software/ros_packages/rover_status/msg/BatteryStatusMessage.msg b/software/ros_packages/rover_status/msg/BatteryStatusMessage.msg
new file mode 100644
index 0000000..cb41631
--- /dev/null
+++ b/software/ros_packages/rover_status/msg/BatteryStatusMessage.msg
@@ -0,0 +1 @@
+float32 battery_voltage
\ No newline at end of file
diff --git a/software/ros_packages/rover_status/src/system_statuses_node.py b/software/ros_packages/rover_status/src/system_statuses_node.py
index 461edad..f4f2309 100755
--- a/software/ros_packages/rover_status/src/system_statuses_node.py
+++ b/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_control.msg import DriveCommandMessage, DriveStatusMessage
+from rover_status.msg import BatteryStatusMessage, CameraStatuses, WheelStatuses, FrSkyStatus, GPSInfo, MiscStatuses, JetsonInfo
+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