From 03706408d374a025982071ab5414694a2742d8ca Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Tue, 23 Jan 2018 12:54:01 -0800 Subject: [PATCH] Added check for ros master on startup. More work on video coordinator. --- ground_station/.idea/workspace.xml | 95 +++++++++++-------- .../StartupSystems/ROSMasterChecker.py | 45 +++++++++ .../Framework/StartupSystems/__init__.py | 0 .../VideoSystems/RoverVideoCoordinator.py | 65 ++++++++++--- .../VideoSystems/RoverVideoReceiver.py | 29 +++--- ground_station/RoverGroundStation.py | 66 ++++++++----- 6 files changed, 209 insertions(+), 91 deletions(-) create mode 100644 ground_station/Framework/StartupSystems/ROSMasterChecker.py create mode 100644 ground_station/Framework/StartupSystems/__init__.py diff --git a/ground_station/.idea/workspace.xml b/ground_station/.idea/workspace.xml index c6756fc..f381a36 100644 --- a/ground_station/.idea/workspace.xml +++ b/ground_station/.idea/workspace.xml @@ -24,8 +24,8 @@ - - + + @@ -36,8 +36,8 @@ - - + + @@ -54,22 +54,10 @@ - - - - - - - - - - - - - + @@ -78,11 +66,11 @@ - + - - + + @@ -90,6 +78,18 @@ + + + + + + + + + + + + @@ -102,10 +102,11 @@ @@ -209,7 +210,7 @@ AAAAsC92Kuu3oLrABnhEDJd2fqLpSLKMDnz1RoGljcqcyx6rWWvT/BV+wcLNHGnvYk97nqnk/pKm4cGIiar9MRU3nZkNwyjJxPVtt3haC8iDCZdLJZ6CmktPYqrohRIav09rau96cPFO/I//1EIneIP/1ljcNiibrP6ukAATwUH19NLQaGz1/EYXXVq+qowSGeFE7dAFp107pLkmub5CsKj/8zF+PCqHoF8AEDn937Vn/JpXGWVGlieBMxMrucAQD8JqIA== diff --git a/ground_station/Framework/StartupSystems/ROSMasterChecker.py b/ground_station/Framework/StartupSystems/ROSMasterChecker.py new file mode 100644 index 0000000..e229167 --- /dev/null +++ b/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 + diff --git a/ground_station/Framework/StartupSystems/__init__.py b/ground_station/Framework/StartupSystems/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ground_station/Framework/VideoSystems/RoverVideoCoordinator.py b/ground_station/Framework/VideoSystems/RoverVideoCoordinator.py index 536ef6a..54352e9 100644 --- a/ground_station/Framework/VideoSystems/RoverVideoCoordinator.py +++ b/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 \ No newline at end of file + self.run_thread_flag = False + diff --git a/ground_station/Framework/VideoSystems/RoverVideoReceiver.py b/ground_station/Framework/VideoSystems/RoverVideoReceiver.py index e3e8b18..c2c96f9 100644 --- a/ground_station/Framework/VideoSystems/RoverVideoReceiver.py +++ b/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: diff --git a/ground_station/RoverGroundStation.py b/ground_station/RoverGroundStation.py index 1c2851e..74c2473 100755 --- a/ground_station/RoverGroundStation.py +++ b/ground_station/RoverGroundStation.py @@ -7,9 +7,11 @@ import sys from PyQt5 import QtWidgets, QtCore, QtGui, uic import signal import rospy +import logging import qdarkstyle # Custom Imports +import Framework.StartupSystems.ROSMasterChecker as ROSMasterChecker import Framework.LoggingSystems.Logger as Logger import Framework.VideoSystems.RoverVideoCoordinator as RoverVideoCoordinator @@ -55,6 +57,8 @@ class GroundStation(QtCore.QObject): LEFT_SCREEN_ID = 0 RIGHT_SCREEN_ID = 1 + exit_requested_signal = QtCore.pyqtSignal() + start_threads_signal = QtCore.pyqtSignal() connect_signals_and_slots_signal = QtCore.pyqtSignal() kill_threads_signal = QtCore.pyqtSignal() @@ -63,7 +67,11 @@ class GroundStation(QtCore.QObject): # noinspection PyArgumentList super(GroundStation, self).__init__(parent) - rospy.init_node("ground_station") + # ##### Setup the Logger Immediately ##### + self.logger_setup_class = Logger.Logger(console_output=True) # Doesn't need to be shared + + # ########## Get the Pick And Plate instance of the logger ########## + self.logger = logging.getLogger("groundstation") self.shared_objects = { "screens": {}, @@ -71,7 +79,7 @@ class GroundStation(QtCore.QObject): "threaded_classes": {} } - # ###### Instantiate Left And Right Screens ##### + # ###### Instantiate Left And Right Screens ###### self.shared_objects["screens"]["left_screen"] = \ self.create_application_window(UI_FILE_LEFT, "Rover Ground Station Left Screen", self.LEFT_SCREEN_ID) # type: ApplicationWindow @@ -80,34 +88,27 @@ class GroundStation(QtCore.QObject): self.create_application_window(UI_FILE_RIGHT, "Rover Ground Station Right Screen", self.RIGHT_SCREEN_ID) # type: ApplicationWindow - # ##### Instantiate Simple Classes ##### - self.logger_setup_class = Logger.Logger(console_output=True) # Doesn't need to be shared + # ###### Initialize the Ground Station Node ###### + rospy.init_node("ground_station") - # ##### Instantiate Threaded Classes ##### + # ##### Instantiate Regular Classes ###### + + # ##### Instantiate Threaded Classes ###### self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects)) - # self.__add_thread("Primary Video", - # RoverVideoReceiver.RoverVideoReceiver( - # self.shared_objects, - # self.shared_objects["screens"]["right_screen"].primary_video_label, - # "/cameras/main_navigation/")) - # self.__add_thread("Secondary Video", - # RoverVideoReceiverOld.VideoTest( - # self.shared_objects, - # self.shared_objects["screens"]["right_screen"].secondary_video_label, - # (640, 360), - # sub_path="/cameras/chassis/image_640x360/compressed")) - # self.__add_thread("Tertiary Video", - # RoverVideoReceiverOld.VideoTest( - # self.shared_objects, - # self.shared_objects["screens"]["right_screen"].tertiary_video_label, - # (640, 360), - # sub_path="/cameras/undercarriage/image_640x360/compressed")) - self.connect_signals_and_slots_signal.emit() self.__connect_signals_to_slots() self.start_threads_signal.emit() + def ___ros_master_running(self): + checker = ROSMasterChecker.ROSMasterChecker() + + if not checker.master_present(5): + self.logger.debug("ROS Master Not Found!!!! Exiting!!!") + QtGui.QGuiApplication.exit() + return False + return True + def __add_thread(self, thread_name, instance): self.shared_objects["threaded_classes"][thread_name] = instance instance.setup_signals(self.start_threads_signal, self.connect_signals_and_slots_signal, @@ -121,8 +122,8 @@ class GroundStation(QtCore.QObject): self.kill_threads_signal.emit() # Wait for Threads - for thread in self.threads["threaded_classes"]: - self.threads["threaded_classes"][thread].wait() + for thread in self.shared_objects["threaded_classes"]: + self.shared_objects["threaded_classes"][thread].wait() QtGui.QGuiApplication.exit() @@ -152,13 +153,26 @@ class GroundStation(QtCore.QObject): if __name__ == "__main__": signal.signal(signal.SIGINT, signal.SIG_DFL) # This allows the keyboard interrupt kill to work properly + # ########## Start the QApplication Framework ########## application = QtWidgets.QApplication(sys.argv) # Create the ase qt gui application application.setStyleSheet(qdarkstyle.load_stylesheet_pyqt5()) + # ########## Set Organization Details for QSettings ########## QtCore.QCoreApplication.setOrganizationName("OSURC") QtCore.QCoreApplication.setOrganizationDomain("http://osurobotics.club/") QtCore.QCoreApplication.setApplicationName("groundstation") - ground_station = GroundStation() + # ########## Check ROS Master Status ########## + master_checker = ROSMasterChecker.ROSMasterChecker() + if not master_checker.master_present(5): + message_box = QtWidgets.QMessageBox() + message_box.setWindowTitle("Rover Ground Station") + message_box.setText("Connection to ROS Master Failed!!!\n" + + "Ensure ROS master is running or check for network issues.") + message_box.exec_() + exit() + + # ########## Start Ground Station If Ready ########## + ground_station = GroundStation() application.exec_() # Execute launching of the application