diff --git a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py new file mode 100644 index 0000000..a39b2ff --- /dev/null +++ b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python + +import rospy +from rover_status.msg import * +from PyQt5 import QtWidgets, QtCore, QtGui, uic +from std_msgs.msg import Empty + +REQUEST_UPDATE_TOPIC = "/rover_status/update_requested" + + +class SensorCore(QtCore.QThread): + + def __init__(self, screen): + super(SensorCore, self).__init__() + + self.not_abort = True + + self.screen_main_window = screen + + self.cpu_read = self.screen_main_window.lineEdit # type: QtWidgets.QLabel + self.ram_read = self.screen_main_window.lineEdit_2 # type: QtWidgets.QLabel + + self.rover_conn = self.screen_main_window.rover # type: QtWidgets.QLabel + self.frsky = self.screen_main_window.frsky # type: QtWidgets.QLabel + self.nav_mouse = self.screen_main_window.nav_mouse # type: QtWidgets.QLabel + self.joystick = self.screen_main_window.joystick # type: QtWidgets.QLabel + self.gps = self.screen_main_window.gps # type: QtWidgets.QLabel + self.zed = self.screen_main_window.zed # type: QtWidgets.QLabel + self.main_cam = self.screen_main_window.main_cam # type: QtWidgets.QLabel + self.chassis_cam = self.screen_main_window.chassis_cam # type: QtWidgets.QLabel + self.under_cam = self.screen_main_window.under_cam # type: QtWidgets.QLabel + self.bogie_right = self.screen_main_window.right_bogie # type: QtWidgets.QLabel + self.bogie_left = self.screen_main_window.left_bogie # type: QtWidgets.QLabel + self.bogie_rear = self.screen_main_window.rear_bogie # type: QtWidgets.QLabel + self.clock = self.screen_main_window.clock # type: QtWidgets.QLCDNumber + self.cpu = self.screen_main_window.cpu # type: QtWidgets.QLabel + self.ram = self.screen_main_window.ram # type: QtWidgets.QLabel + + + rospy.init_node('SensorCore') + + # self.pub = rospy.Publisher('rover_statuses_chatter', RoverSysStatus, queue_size=10) + + # Subscription examples on pulling data from system_statuses_node.py + rospy.Subscriber('camera_system_status_chatter', CameraStatuses, self.__camera_callback) + rospy.Subscriber('bogie_system_status_chatter', BogieStatuses, self.__bogie_callback) + rospy.Subscriber('FrSky_system_status_chatter', FrSkyStatus, self.__frsky_callback) + rospy.Subscriber('GPS_system_status_chatter', GPSInfo, self.__gps_callback) + rospy.Subscriber('jetson_system_status_chatter', JetsonInfo, self.__jetson_callback) + rospy.Subscriber('misc_system_status_chatter', MiscStatuses, self.__misc_callback) + + self.camera_msg = CameraStatuses() + self.bogie_msg = BogieStatuses() + self.FrSky_msg = FrSkyStatus() + self.GPS_msg = GPSInfo() + self.jetson_msg = JetsonInfo() + self.misc_msg = MiscStatuses() + + self.req = rospy.Publisher(REQUEST_UPDATE_TOPIC, Empty, queue_size=1) + + self.req.publish(Empty()) + + def __camera_callback(self, data): + self.camera_msg.camera_zed = data.camera_zed + self.camera_msg.camera_undercarriage = data.camera_undercarriage + self.camera_msg.camera_chassis = data.camera_chassis + self.camera_msg.camera_main_navigation = data.camera_main_navigation + + if self.camera_msg.camera_zed is False: + self.zed.setStyleSheet("background-color: red;") + else: + self.zed.setStyleSheet("") + + if self.camera_msg.camera_undercarriage is False: + self.under_cam.setStyleSheet("background-color: red;") + else: + self.under_cam.setStyleSheet("") + + if self.camera_msg.camera_chassis is False: + self.chassis_cam.setStyleSheet("background-color: red;") + else: + self.chassis_cam.setStyleSheet("") + + if self.camera_msg.camera_main_navigation is False: + self.main_cam.setStyleSheet("background-color: red;") + else: + self.main_cam.setStyleSheet("") + + def __frsky_callback(self, data): + self.FrSky_msg.FrSky_controller_connection_status = data.FrSky_controller_connection_status + + if self.FrSky_msg.FrSky_controller_connection_status is False: + self.frsky.setStyleSheet("background-color: red;") + else: + self.frsky.setStyleSheet("") + + def __bogie_callback(self, data): + self.bogie_msg.bogie_connection_1 = data.bogie_connection_1 + self.bogie_msg.bogie_connection_2 = data.bogie_connection_2 + self.bogie_msg.bogie_connection_3 = data.bogie_connection_3 + + if self.bogie_msg.bogie_connection_1 is False: + self.bogie_right.setStyleSheet("background-color: red;") + else: + self.bogie_right.setStyleSheet("") + + if self.bogie_msg.bogie_connection_2 is False: + self.bogie_left.setStyleSheet("background-color: red;") + else: + self.bogie_left.setStyleSheet("") + + if self.bogie_msg.bogie_connection_3 is False: + self.bogie_rear.setStyleSheet("background-color: red;") + else: + self.bogie_rear.setStyleSheet("") + + def __jetson_callback(self, data): + self.jetson_msg.jetson_CPU = data.jetson_CPU + + # self.cpu_read.setText(str(self.jetson_msg.jetson_CPU)) + self.cpu.setText(str(self.jetson_msg.jetson_CPU)) + if self.jetson_msg.jetson_CPU > 79: + self.cpu.setStyleSheet("background-color: orange;") + elif self.jetson_msg.jetson_CPU > 89: + self.cpu.setStyleSheet("background-color: red;") + else: + self.cpu.setStyleSheet("") + + self.jetson_msg.jetson_RAM = data.jetson_RAM + self.ram.setText(str(self.jetson_msg.jetson_RAM)) + if self.jetson_msg.jetson_RAM > 79: + self.ram.setStyleSheet("background-color: orange;") + elif self.jetson_msg.jetson_RAM > 89: + self.ram.setStyleSheet("background-color: red;") + else: + self.ram.setStyleSheet("") + + self.jetson_msg.jetson_EMMC = data.jetson_EMMC + self.jetson_msg.jetson_NVME_SSD = data.jetson_NVME_SSD + #rospy.loginfo(self.jetson_msg) + + def __gps_callback(self, data): + self.GPS_msg.UTC_GPS_time = data.UTC_GPS_time + self.GPS_msg.GPS_connection_status = data.GPS_connection_status + + if self.GPS_msg.GPS_connection_status is False: + self.gps.setStyleSheet("background-color: red") + else: + self.gps.setStyleSheet("") + + def __misc_callback(self, data): + self.misc_msg.arm_connection_status = data.arm_connection_status + self.misc_msg.arm_end_effector_connection_statuses = data.arm_end_effector_connection_statuses + self.misc_msg.sample_containment_connection_status = data.sample_containment_connection_status + self.misc_msg.tower_connection_status = data.tower_connection_status + self.misc_msg.chassis_pan_tilt_connection_status = data.chassis_pan_tilt_connection_status + + def __display_time(self): + time = QtCore.QTime.currentTime() + temp = time.toString('hh:mm:ss') + self.clock.display(temp) + + def run(self): + rospy.Subscriber('camera_system_status_chatter', CameraStatuses, self.__camera_callback) + rospy.Subscriber('bogie_system_status_chatter', BogieStatuses, self.__bogie_callback) + rospy.Subscriber('FrSky_system_status_chatter', FrSkyStatus, self.__frsky_callback) + rospy.Subscriber('GPS_system_status_chatter', GPSInfo, self.__gps_callback) + rospy.Subscriber('jetson_system_status_chatter', JetsonInfo, self.__jetson_callback) + rospy.Subscriber('misc_system_status_chatter', MiscStatuses, self.__misc_callback) + #self.gui_element = self.jetson_msg.jetson_CPU + #print(self.jetson_msg.jetson_CPU) + rospy.spin() + + def on_kill_threads_requested__slot(self): + self.not_abort = False + + def connect_signals_and_slots(self): + pass + + +if __name__ == '__main__': + rover_statuses = SensorCore() + rover_statuses.run() diff --git a/software/ros_packages/ground_station/src/Framework/StatusSystems/Timer.py b/software/ros_packages/ground_station/src/Framework/StatusSystems/Timer.py new file mode 100644 index 0000000..b734925 --- /dev/null +++ b/software/ros_packages/ground_station/src/Framework/StatusSystems/Timer.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python2 + +import sys + +from PyQt5 import Qt +from PyQt5.uic import loadUi + +# [ms] +TICK_TIME = 2**6 + + +class Timer(Qt.QMainWindow): + def __init__(self, parent=None): + super().__init__(parent) + + self.reset.clicked.connect(self.do_reset) + self.start.clicked.connect(self.do_start) + + self.timer = Qt.QTimer() + self.timer.setInterval(TICK_TIME) + self.timer.timeout.connect(self.tick) + + self.do_reset() + + def keyPressEvent(self, event): + if event.key() == Qt.Qt.Key_Escape: + self.close() + else: + super().keyPressEvent(event) + + def display(self): + self.lcd.display("%d:%05.2f" % (self.time // 60, self.time % 60)) + + @Qt.pyqtSlot() + def tick(self): + self.time += TICK_TIME/1000 + self.display() + + @Qt.pyqtSlot() + def do_start(self): + self.timer.start() + self.start.setText("Pause") + self.start.clicked.disconnect() + self.start.clicked.connect(self.do_pause) + + @Qt.pyqtSlot() + def do_pause(self): + self.timer.stop() + self.start.setText("Start") + self.start.clicked.disconnect() + self.start.clicked.connect(self.do_start) + + @Qt.pyqtSlot() + def do_reset(self): + self.time = 0 + self.display() + + +app = Qt.QApplication(sys.argv) + +timer = Timer() +timer.show() + +app.exec_() diff --git a/software/ros_packages/ground_station/src/Framework/StatusSystems/__init__.py b/software/ros_packages/ground_station/src/Framework/StatusSystems/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui index 3ecb508..1c655f5 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui @@ -39,7 +39,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -68,19 +77,58 @@ - + + 0 + + + 0 + + + 0 + + 0 0 - + 0 - - + + + + background-color: darkgreen; + + + placeholder + + + + + + + background-color: darkgreen; + + + <html><head/><body><p align="center"><span style=" font-weight:600;">CPU %</span></p></body></html> + + + + + + + background-color: darkgreen; + + + placeholder + + + + + 0 @@ -121,8 +169,8 @@ Connected - - + + 0 @@ -166,8 +214,8 @@ Connected - - + + 0 @@ -208,8 +256,8 @@ Connected - - + + 0 @@ -250,8 +298,8 @@ Fix - - + + 0 @@ -292,8 +340,8 @@ Connected - - + + 0 @@ -334,8 +382,8 @@ Connected - - + + 0 @@ -376,8 +424,8 @@ Connected - - + + 0 @@ -418,8 +466,8 @@ Connected - - + + 0 @@ -459,8 +507,8 @@ Connected - - + + 0 @@ -501,8 +549,8 @@ Connected - - + + 0 @@ -546,8 +594,11 @@ Connected - - + + + + + 0 @@ -591,6 +642,19 @@ Connected + + + + + + + background-color: darkgreen; + + + <html><head/><body><p align="center"><span style=" font-weight:600;">RAM</span></p></body></html> + + + @@ -752,7 +816,16 @@ Connected - + + 0 + + + 0 + + + 0 + + 0 diff --git a/software/ros_packages/rover_status/msg/RoverSysStatus.msg b/software/ros_packages/rover_status/msg/RoverSysStatus.msg index d181327..b5f45a7 100644 --- a/software/ros_packages/rover_status/msg/RoverSysStatus.msg +++ b/software/ros_packages/rover_status/msg/RoverSysStatus.msg @@ -1,19 +1,20 @@ int8 UTC_GPS_time -int8 bogie_connection_1 -int8 bogie_connection_2 -int8 bogie_connection_3 -int8 arm_connection_status -int8 arm_end_effector_connection_statuses -int8 camera_zed -int8 camera_undercarriage -int8 camera_chassis -int8 camera_main_navigation -int8 sample_containment_connection_status -int8 tower_connection_status -int8 chassis_pan_tilt_connection_status +bool bogie_connection_1 +bool bogie_connection_2 +bool bogie_connection_3 +bool arm_connection_status +bool arm_end_effector_connection_statuses +bool camera_zed +bool camera_undercarriage +bool camera_chassis +bool camera_main_navigation +bool sample_containment_connection_status +bool tower_connection_status +bool chassis_pan_tilt_connection_status int8 GPS_connection_status -int8 jetson_CPU -int8 jetson_RAM -int8 jetson_EMMC -int8 jetson_NVME_SSD -int8 FrSky_controller_connection_status \ No newline at end of file +float64 jetson_CPU +float64 jetson_RAM +float64 jetson_EMMC +float64 jetson_NVME_SSD +float64 jetson_GPU_temp +bool FrSky_controller_connection_status \ 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 da09ba7..0660d3c 100755 --- a/software/ros_packages/rover_status/src/system_statuses_node.py +++ b/software/ros_packages/rover_status/src/system_statuses_node.py @@ -158,7 +158,7 @@ class SystemStatuses: .replace("\n", "").replace("+", "").split() self.jetson_msg.jetson_GPU_temp = float(parsed_temps[4]) except subprocess.CalledProcessError: - print 'sensors call failed (potential reason: on VM)' + print('sensors call failed (potential reason: on VM)') self.jetson_msg.jetson_GPU_temp = -1.0 # EMMC and NVMe_SSD used % calculation