Added check for ros master on startup. More work on video coordinator.

ground_station/Framework/StartupSystems/ROSMasterChecker.py

@@ -0,0 +1,45 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtGui, QtWidgets
+import time
+import logging
+import socket
+
+import rospy
+
+# Custom Imports
+
+#####################################
+# Global Variables
+#####################################
+
+
+#####################################
+# RoverVideoReceiver Class Definition
+#####################################
+class ROSMasterChecker(QtCore.QThread):
+    def __init__(self):
+        super(ROSMasterChecker, self).__init__()
+
+        # ########## Class Variables ##########
+        self.ros_master_present = False
+
+        self.start_time = time.time()
+        self.start()
+
+    def run(self):
+        try:
+            master = rospy.get_master()
+            master.getPid()
+            self.ros_master_present = True
+        except socket.error:
+            return
+
+    def master_present(self, timeout):
+        while self.isRunning() and (time.time() - self.start_time) < timeout:
+            self.msleep(100)
+
+        return self.ros_master_present
+

ground_station/Framework/VideoSystems/RoverVideoCoordinator.py

@@ -14,15 +14,17 @@ from cv_bridge import CvBridge
 from sensor_msgs.msg import CompressedImage
 
 # Custom Imports
+import RoverVideoReceiver
 
 #####################################
 # Global Variables
 #####################################
-FONT = cv2.FONT_HERSHEY_TRIPLEX
+CAMERA_TOPIC_PATH = "/cameras"
+EXCLUDED_CAMERAS = ["zed"]
 
 
 #####################################
-# RoverVideoReceiver Class Definition
+# RoverVideoCoordinator Class Definition
 #####################################
 class RoverVideoCoordinator(QtCore.QThread):
     def __init__(self, shared_objects):
@@ -44,24 +46,59 @@ class RoverVideoCoordinator(QtCore.QThread):
         # ########## Thread Flags ##########
         self.run_thread_flag = True
 
+        self.setup_cameras_flag = True
+
+        # ########## Class Variables ##########
+        self.global_start_signal = None
+        self.global_connect_signals_and_slots_signal = None
+        self.global_kill_signal = None
+
+        self.valid_cameras = []
+
+        self.camera_threads = {}
+
+        # ########## Setup cameras ##########
+        self.__get_cameras()
+        self.__setup_video_threads()
+
     def run(self):
         self.logger.debug("Starting Video Coordinator Thread")
 
-        topics = rospy.get_published_topics("/cameras")
-
-        for group in topics:
-            main_topic = group[0]
-            last_section_topic = main_topic.split("/")[-1]
-            if "image_" in main_topic and "zed" not in main_topic and last_section_topic == "compressed" :
-                print group[0]
-
         while self.run_thread_flag:
+            if self.setup_cameras_flag:
 
+                self.setup_cameras_flag = False
             self.msleep(100)
 
+        self.__wait_for_camera_threads()
+
         self.logger.debug("Stopping Video Coordinator Thread")
 
-    def _get_cameras(self):
+    def __get_cameras(self):
+        topics = rospy.get_published_topics(CAMERA_TOPIC_PATH)
+
+        names = []
+
+        for topics_group in topics:
+            main_topic = topics_group[0]
+            camera_name = main_topic.split("/")[2]
+            names.append(camera_name)
+
+        names = set(names)
+
+        for camera in EXCLUDED_CAMERAS:
+            if camera in names:
+                names.remove(camera)
+
+        self.valid_cameras = list(names)
+
+    def __setup_video_threads(self):
+        for camera in self.valid_cameras:
+            self.camera_threads[camera] = RoverVideoReceiver.RoverVideoReceiver(camera)
+
+    def __wait_for_camera_threads(self):
+        for camera in self.camera_threads:
+            self.camera_threads[camera].wait()
 
     def connect_signals_and_slots(self):
         pass
@@ -71,5 +108,9 @@ class RoverVideoCoordinator(QtCore.QThread):
         signals_and_slots_signal.connect(self.connect_signals_and_slots)
         kill_signal.connect(self.on_kill_threads_requested__slot)
 
+        for camera in self.camera_threads:
+            self.camera_threads[camera].setup_signals(start_signal, signals_and_slots_signal, kill_signal)
+
     def on_kill_threads_requested__slot(self):
-        self.run_thread_flag = False
+        self.run_thread_flag = False
+

ground_station/Framework/VideoSystems/RoverVideoReceiver.py

@@ -28,13 +28,11 @@ class RoverVideoReceiver(QtCore.QThread):
     publish_message_signal = QtCore.pyqtSignal()
     image_ready_signal = QtCore.pyqtSignal()
 
-    def __init__(self, shared_objects, video_display_label, topic_path):
+    def __init__(self, camera_name):
         super(RoverVideoReceiver, self).__init__()
 
         # ########## Reference to class init variables ##########
-        self.shared_objects = shared_objects
-        self.video_display_label = video_display_label  # type:QtWidgets.QLabel
-        self.topic_path = topic_path
+        self.camera_name = camera_name
 
         # ########## Get the settings instance ##########
         self.settings = QtCore.QSettings()
@@ -46,14 +44,13 @@ class RoverVideoReceiver(QtCore.QThread):
         self.run_thread_flag = True
 
         # ########## Class Variables ##########
+        self.camera_title_name = self.camera_name.replace("_", " ").title()
+
+        self.topic_path = "/cameras/" + self.camera_name + "/image_640x360/compressed"
+
         # Subscription variables
-        # self.video_subscriber = rospy.Subscriber(self.topic_path, CompressedImage,
-        #                                          self.__image_data_received_callback)  # type: rospy.Subscriber
-
-        self.subscription_queue_size = 10
-
-        # Steam name variable
-        self.video_name = self.topic_path.split("/")[2].replace("_", " ").title()
+        self.video_subscriber = rospy.Subscriber(self.topic_path, CompressedImage,
+                                                 self.__image_data_received_callback)  # type: rospy.Subscriber
 
         # Image variables
         self.raw_image = None
@@ -69,7 +66,7 @@ class RoverVideoReceiver(QtCore.QThread):
         self.__set_local_callbacks()
 
     def run(self):
-        self.logger.debug("Starting \"%s\" Thread")
+        self.logger.debug("Starting \"%s\" Camera Thread" % self.camera_title_name)
 
         while self.run_thread_flag:
             if self.video_enabled:
@@ -79,7 +76,7 @@ class RoverVideoReceiver(QtCore.QThread):
 
             self.msleep(18)
 
-        self.logger.debug("Stopping \"%s\" Thread")
+        self.logger.debug("Stopping \"%s\" Camera Thread" % self.camera_title_name)
 
     def __show_video_enabled(self):
         if self.new_frame:
@@ -98,14 +95,16 @@ class RoverVideoReceiver(QtCore.QThread):
             self.new_frame = False
 
     def __on_image_update_ready(self):
-        self.video_display_label.setPixmap(self.pixmap)
+        pass
+        # self.video_display_label.setPixmap(self.pixmap)
 
     def __image_data_received_callback(self, raw_image):
         self.raw_image = raw_image
         self.new_frame = True
 
     def __set_local_callbacks(self):
-        self.video_display_label.mouseDoubleClickEvent = self.toggle_video_display
+        pass
+        # self.video_display_label.mouseDoubleClickEvent = self.toggle_video_display
 
     def toggle_video_display(self, _):
         if self.video_enabled: