diff --git a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py
index a39b2ff..c28c46c 100644
--- a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py
+++ b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py
@@ -8,14 +8,23 @@ from std_msgs.msg import Empty
REQUEST_UPDATE_TOPIC = "/rover_status/update_requested"
-class SensorCore(QtCore.QThread):
+CAMERA_TOPIC_NAME = "/rover_status/camera_status"
+BOGIE_TOPIC_NAME = "/rover_status/bogie_status"
+FRSKY_TOPIC_NAME = "/rover_status/frsky_status"
+GPS_TOPIC_NAME = "/rover_status/gps_status"
+JETSON_TOPIC_NAME = "/rover_status/jetson_status"
+MISC_TOPIC_NAME = "/rover_status/misc_status"
- def __init__(self, screen):
+
+class SensorCore(QtCore.QThread):
+ def __init__(self, shared_objects):
super(SensorCore, self).__init__()
- self.not_abort = True
+ self.run_thread_flag = True
- self.screen_main_window = screen
+ # ########## Reference to class init variables ##########
+ self.shared_objects = shared_objects
+ self.screen_main_window = self.shared_objects["screens"]["left_screen"]
self.cpu_read = self.screen_main_window.lineEdit # type: QtWidgets.QLabel
self.ram_read = self.screen_main_window.lineEdit_2 # type: QtWidgets.QLabel
@@ -36,18 +45,13 @@ class SensorCore(QtCore.QThread):
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_status = rospy.Subscriber(CAMERA_TOPIC_NAME, CameraStatuses, self.__camera_callback)
+ self.bogie_status = rospy.Subscriber(BOGIE_TOPIC_NAME, BogieStatuses, self.__bogie_callback)
+ self.frsky_status = rospy.Subscriber(FRSKY_TOPIC_NAME, FrSkyStatus, self.__frsky_callback)
+ self.gps_status = rospy.Subscriber(GPS_TOPIC_NAME, GPSInfo, self.__gps_callback)
+ self.jetson_status = rospy.Subscriber(JETSON_TOPIC_NAME, JetsonInfo, self.__jetson_callback)
+ self.misc_status = rospy.Subscriber(MISC_TOPIC_NAME, MiscStatuses, self.__misc_callback)
self.camera_msg = CameraStatuses()
self.bogie_msg = BogieStatuses()
@@ -56,9 +60,7 @@ class SensorCore(QtCore.QThread):
self.jetson_msg = JetsonInfo()
self.misc_msg = MiscStatuses()
- self.req = rospy.Publisher(REQUEST_UPDATE_TOPIC, Empty, queue_size=1)
-
- self.req.publish(Empty())
+ rospy.Publisher(REQUEST_UPDATE_TOPIC, Empty, queue_size=1).publish(Empty())
def __camera_callback(self, data):
self.camera_msg.camera_zed = data.camera_zed
@@ -161,22 +163,20 @@ class SensorCore(QtCore.QThread):
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
+ while self.run_thread_flag:
+ self.msleep(100)
def connect_signals_and_slots(self):
pass
+ def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
+ start_signal.connect(self.start)
+ signals_and_slots_signal.connect(self.connect_signals_and_slots)
+ kill_signal.connect(self.on_kill_threads_requested__slot)
+
+ def on_kill_threads_requested__slot(self):
+ self.run_thread_flag = False
+
if __name__ == '__main__':
rover_statuses = SensorCore()
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 1c655f5..2fa285b 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,16 +39,7 @@
0
-
- 0
-
-
- 0
-
-
- 0
-
-
+
0
-
@@ -77,22 +68,13 @@
-
- 0
-
-
- 0
-
-
- 0
-
-
+
0
0
- -
+
-
0
@@ -107,16 +89,6 @@
- -
-
-
- background-color: darkgreen;
-
-
- <html><head/><body><p align="center"><span style=" font-weight:600;">CPU %</span></p></body></html>
-
-
-
-
@@ -256,48 +228,6 @@ Connected
- -
-
-
-
- 0
- 0
-
-
-
-
- 0
- 0
-
-
-
-
- 9999999
- 9999999
-
-
-
-
- 10
- 75
- true
-
-
-
- false
-
-
- background-color:darkgreen;
-
-
- GPS
-Fix
-
-
- Qt::AlignCenter
-
-
-
-
@@ -375,48 +305,6 @@ Connected
Rear Bogie
-Connected
-
-
- Qt::AlignCenter
-
-
-
- -
-
-
-
- 0
- 0
-
-
-
-
- 0
- 0
-
-
-
-
- 9999999
- 9999999
-
-
-
-
- 10
- 75
- true
-
-
-
- background-color:darkgreen;
-
-
- QFrame::NoFrame
-
-
- Rover
Connected
@@ -466,47 +354,6 @@ Connected
- -
-
-
-
- 0
- 0
-
-
-
-
- 0
- 0
-
-
-
-
- 9999999
- 9999999
-
-
-
-
- 10
- 75
- true
-
-
-
- background-color:darkred;
-
-
- QFrame::NoFrame
-
-
- ZED Connected
-
-
- Qt::AlignCenter
-
-
-
-
@@ -594,9 +441,6 @@ Connected
- -
-
-
-
@@ -655,6 +499,144 @@ Connected
+ -
+
+
+ background-color: darkgreen;
+
+
+ <html><head/><body><p align="center"><span style=" font-weight:600;">CPU %</span></p></body></html>
+
+
+
+ -
+
+
+
+ 0
+ 0
+
+
+
+
+ 0
+ 0
+
+
+
+
+ 9999999
+ 9999999
+
+
+
+
+ 10
+ 75
+ true
+
+
+
+ background-color:darkgreen;
+
+
+ QFrame::NoFrame
+
+
+ Rover
+Connected
+
+
+ Qt::AlignCenter
+
+
+
+ -
+
+
+
+ 0
+ 0
+
+
+
+
+ 0
+ 0
+
+
+
+
+ 9999999
+ 9999999
+
+
+
+
+ 10
+ 75
+ true
+
+
+
+ false
+
+
+ background-color:darkgreen;
+
+
+ GPS
+Fix
+
+
+ Qt::AlignCenter
+
+
+
+ -
+
+
+
+ 0
+ 0
+
+
+
+
+ 0
+ 0
+
+
+
+
+ 9999999
+ 9999999
+
+
+
+
+ 10
+ 75
+ true
+
+
+
+ background-color:darkred;
+
+
+ QFrame::NoFrame
+
+
+ ZED Connected
+
+
+ Qt::AlignCenter
+
+
+
+ -
+
+
@@ -816,16 +798,7 @@ Connected
-
- 0
-
-
- 0
-
-
- 0
-
-
+
0
diff --git a/software/ros_packages/ground_station/src/ground_station.py b/software/ros_packages/ground_station/src/ground_station.py
index b286a1d..5656522 100644
--- a/software/ros_packages/ground_station/src/ground_station.py
+++ b/software/ros_packages/ground_station/src/ground_station.py
@@ -18,6 +18,7 @@ import Framework.MapSystems.RoverMapCoordinator as RoverMapCoordinator
import Framework.JoystickControlSystems.JoystickControlSender as JoystickControlSender
import Framework.NavigationSystems.SpeedAndHeadingIndication as SpeedAndHeading
import Framework.ArmSystems.ArmIndication as ArmIndication
+import Framework.StatusSystems.StatusCore as StatusCore
#####################################
# Global Variables
@@ -103,6 +104,7 @@ class GroundStation(QtCore.QObject):
self.__add_thread("Joystick Sender", JoystickControlSender.JoystickControlSender(self.shared_objects))
self.__add_thread("Speed and Heading", SpeedAndHeading.SpeedAndHeadingIndication(self.shared_objects))
self.__add_thread("Arm Indication", ArmIndication.ArmIndication(self.shared_objects))
+ self.__add_thread("Rover Status", StatusCore.SensorCore(self.shared_objects))
self.connect_signals_and_slots_signal.emit()
self.__connect_signals_to_slots()