Renamed drive sender to joystick sender. Added qt broadcast of button presses for camera control. Added receiver to control gui elements. Need to be tested

software/ros_packages/ground_station/src/Framework/JoystickControlSystems/JoystickControlSender.py

@@ -24,6 +24,9 @@ X_AXIS_DEADBAND = 0.05
 
 THROTTLE_MIN = 0.05
 
+CAMERA_CHANGE_TIME = 0.25
+GUI_ELEMENT_CHANGE_TIME = 0.25
+
 
 #####################################
 # Controller Class Definition
@@ -124,13 +127,16 @@ class LogitechJoystick(QtCore.QThread):
 #####################################
 # Controller Class Definition
 #####################################
-class RoverDriveSender(QtCore.QThread):
+class JoystickControlSender(QtCore.QThread):
     set_speed_limit__signal = QtCore.pyqtSignal(int)
     set_left_drive_output__signal = QtCore.pyqtSignal(int)
     set_right_drive_output__signal = QtCore.pyqtSignal(int)
 
+    change_gui_element_selection__signal = QtCore.pyqtSignal(int)
+    change_camera_selection__signal = QtCore.pyqtSignal(int)
+
     def __init__(self, shared_objects):
-        super(RoverDriveSender, self).__init__()
+        super(JoystickControlSender, self).__init__()
 
         # ########## Reference to class init variables ##########
         self.shared_objects = shared_objects
@@ -158,6 +164,9 @@ class RoverDriveSender(QtCore.QThread):
 
         self.drive_paused = True
 
+        self.last_gui_element_change_time = time()
+        self.last_camera_change_time = time()
+
     def run(self):
         while self.run_thread_flag:
 
@@ -182,6 +191,10 @@ class RoverDriveSender(QtCore.QThread):
             self.show_changed_pause_state()
 
     def __update_and_publish(self):
+        self.publish_drive_command()
+        self.publish_camera_control_commands()
+
+    def publish_drive_command(self):
         throttle_axis = max((255 - self.joystick.controller_states["throttle_axis"]) / 255.0, THROTTLE_MIN)
 
         if self.drive_paused:
@@ -198,6 +211,22 @@ class RoverDriveSender(QtCore.QThread):
 
         self.drive_command_publisher.publish(drive_message)
 
+    def publish_camera_control_commands(self):
+        three_pressed = self.joystick.controller_states["three_pressed"]
+        four_pressed = self.joystick.controller_states["four_pressed"]
+        five_pressed = self.joystick.controller_states["five_pressed"]
+        six_pressed = self.joystick.controller_states["six_pressed"]
+
+        if (five_pressed or six_pressed) and (time() - self.last_camera_change_time) > CAMERA_CHANGE_TIME:
+            change = -1 if five_pressed else 1
+            self.change_camera_selection__signal.emit(change)
+            self.last_camera_change_time = time()
+
+        if (three_pressed or four_pressed) and (time() - self.last_gui_element_change_time) > GUI_ELEMENT_CHANGE_TIME:
+            change = -1 if three_pressed else 1
+            self.change_gui_element_selection__signal.emit(change)
+            self.last_gui_element_change_time = time()
+
     def get_drive_message(self, throttle_axis):
         drive_message = DriveCommandMessage()
 

software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py

@@ -38,6 +38,12 @@ class RoverVideoCoordinator(QtCore.QThread):
         self.secondary_video_display_label = self.right_screen.secondary_video_label  # type:QtWidgets.QLabel
         self.tertiary_video_display_label = self.right_screen.tertiary_video_label  # type:QtWidgets.QLabel
 
+        self.index_to_label_element = {
+            0: self.primary_video_display_label,
+            1: self.secondary_video_display_label,
+            2: self.tertiary_video_display_label
+        }
+
         # ########## Get the settings instance ##########
         self.settings = QtCore.QSettings()
 
@@ -57,13 +63,14 @@ class RoverVideoCoordinator(QtCore.QThread):
 
         reset_camera_message = CameraControlMessage()
         reset_camera_message.enable_small_broadcast = True
+
         # Reset default cameras
         rospy.Publisher("/cameras/chassis/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message)
         rospy.Publisher("/cameras/undercarriage/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message)
         rospy.Publisher("/cameras/main_navigation/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message)
         rospy.Publisher("/cameras/end_effector/camera_control", CameraControlMessage, queue_size=1).publish(reset_camera_message)
 
-        self.msleep(3000)
+        # self.msleep(3000)
 
         # Setup cameras
         self.__get_cameras()
@@ -73,9 +80,14 @@ class RoverVideoCoordinator(QtCore.QThread):
         self.secondary_label_current_setting = min(self.primary_label_current_setting + 1, len(self.valid_cameras))
         self.tertiary_label_current_setting = min(self.secondary_label_current_setting + 1, len(self.valid_cameras))
 
-        self.primary_label_max_resolution = -1
+        self.index_to_label_current_setting = {
-        self.secondary_label_max_resolution = -1
+            0, self.primary_label_current_setting,
-        self.tertiary_label_max_resolution = -1
+            1, self.secondary_label_current_setting,
+            2, self.tertiary_label_current_setting
+        }
+
+        self.current_label_for_joystick_adjust = 0
+        self.gui_selection_update_needed = True
 
         self.set_max_resolutions_flag = True
 
@@ -87,6 +99,7 @@ class RoverVideoCoordinator(QtCore.QThread):
         while self.run_thread_flag:
             self.__set_max_resolutions()
             self.__toggle_background_cameras_if_needed()
+            self.__update_gui_element_selection()
             self.msleep(10)
 
         self.__wait_for_camera_threads()
@@ -115,6 +128,18 @@ class RoverVideoCoordinator(QtCore.QThread):
             elif camera_index in enabled and camera_index not in self.disabled_cameras and not self.camera_threads[camera_name].video_enabled:
                 self.camera_threads[camera_name].toggle_video_display()
 
+    def __update_gui_element_selection(self):
+        if self.gui_selection_update_needed:
+            elements_to_reset = range(len(self.valid_cameras))
+            elements_to_reset.remove(self.current_label_for_joystick_adjust)
+
+            for index in elements_to_reset:
+                self.index_to_label_element[index].setStyleSheet("background-color: transparent;")
+
+            self.index_to_label_element[self.current_label_for_joystick_adjust].setStyleSheet("border: 2px solid orange")
+
+            self.gui_selection_update_needed = False
+
     def __get_cameras(self):
         topics = rospy.get_published_topics(CAMERA_TOPIC_PATH)
 
@@ -151,6 +176,11 @@ class RoverVideoCoordinator(QtCore.QThread):
         self.secondary_video_display_label.mousePressEvent = self.__change_display_source_secondary_mouse_press_event
         self.tertiary_video_display_label.mousePressEvent = self.__change_display_source_tertiary_mouse_press_event
 
+        self.shared_objects["threaded_classes"]["Joystick Sender"].change_gui_element_selection__signal.connect(
+            self.on_camera_gui_element_selection_changed)
+        self.shared_objects["threaded_classes"]["Joystick Sender"].change_camera_selection__signal.connect(
+            self.on_camera_selection_for_current_gui_element_changed)
+
     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)
@@ -211,5 +241,27 @@ class RoverVideoCoordinator(QtCore.QThread):
             except:
                 pass
 
+    def on_camera_gui_element_selection_changed(self, direction):
+        new_selection = self.current_label_for_joystick_adjust + direction
+
+        if new_selection < 0:
+            self.current_label_for_joystick_adjust = len(self.valid_cameras)
+        elif new_selection > len(self.valid_cameras):
+            self.current_label_for_joystick_adjust = 0
+        else:
+            self.current_label_for_joystick_adjust = new_selection
+
+        self.gui_selection_update_needed = True
+
+    def on_camera_selection_for_current_gui_element_changed(self, direction):
+        new_label_setting = self.index_to_label_current_setting[self.current_label_for_joystick_adjust] + direction
+
+        if new_label_setting < 0:
+            self.index_to_label_current_setting[self.current_label_for_joystick_adjust] = len(self.valid_cameras)
+        elif new_label_setting > len(self.valid_cameras):
+            self.index_to_label_current_setting[self.current_label_for_joystick_adjust] = 0
+        else:
+            self.index_to_label_current_setting[self.current_label_for_joystick_adjust] = new_label_setting
+
     def on_kill_threads_requested__slot(self):
         self.run_thread_flag = False

software/ros_packages/ground_station/src/ground_station.py

@@ -16,7 +16,7 @@ import Framework.StartupSystems.ROSMasterChecker as ROSMasterChecker
 import Framework.LoggingSystems.Logger as Logger
 import Framework.VideoSystems.RoverVideoCoordinator as RoverVideoCoordinator
 import Framework.MapSystems.RoverMapCoordinator as RoverMapCoordinator
-import Framework.DriveSystems.RoverDriveSender as RoverDriveSender
+import Framework.JoystickControlSystems.JoystickControlSender as JoystickControlSender
 
 #####################################
 # Global Variables
@@ -99,7 +99,7 @@ class GroundStation(QtCore.QObject):
         # ##### Instantiate Threaded Classes ######
         self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects))
         # self.__add_thread("Map Coordinator", RoverMapCoordinator.RoverMapCoordinator(self.shared_objects))
-        self.__add_thread("Rover Drive Sender", RoverDriveSender.RoverDriveSender(self.shared_objects))
+        self.__add_thread("Joystick Sender", JoystickControlSender.JoystickControlSender(self.shared_objects))
         self.connect_signals_and_slots_signal.emit()
         self.__connect_signals_to_slots()
         self.start_threads_signal.emit()