Joystick camera cycling and disabling fully working!

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

@@ -24,8 +24,11 @@ X_AXIS_DEADBAND = 0.05
 
 THROTTLE_MIN = 0.05
 
-CAMERA_CHANGE_TIME = 0.25
+PAUSE_STATE_CHANGE_TIME = 0.5
-GUI_ELEMENT_CHANGE_TIME = 0.25
+
+CAMERA_CHANGE_TIME = 0.2
+GUI_ELEMENT_CHANGE_TIME = 0.2
+CAMERA_TOGGLE_CHANGE_TIME = 0.35
 
 
 #####################################
@@ -134,6 +137,7 @@ class JoystickControlSender(QtCore.QThread):
 
     change_gui_element_selection__signal = QtCore.pyqtSignal(int)
     change_camera_selection__signal = QtCore.pyqtSignal(int)
+    toggle_selected_gui_camera__signal = QtCore.pyqtSignal()
 
     def __init__(self, shared_objects):
         super(JoystickControlSender, self).__init__()
@@ -164,8 +168,10 @@ class JoystickControlSender(QtCore.QThread):
 
         self.drive_paused = True
 
+        self.last_pause_state_time = time()
         self.last_gui_element_change_time = time()
         self.last_camera_change_time = time()
+        self.last_camera_toggle_time = time()
 
     def run(self):
         while self.run_thread_flag:
@@ -185,10 +191,11 @@ class JoystickControlSender(QtCore.QThread):
         self.set_right_drive_output__signal.connect(self.right_drive_progress_bar.setValue)
 
     def check_and_set_pause_state(self):
-        if self.joystick.controller_states["thumb_pressed"]:
+        thumb_pressed = self.joystick.controller_states["thumb_pressed"]
+        if thumb_pressed and (time() - self.last_pause_state_time) > PAUSE_STATE_CHANGE_TIME:
             self.drive_paused = not self.drive_paused
-            self.msleep(350)
             self.show_changed_pause_state()
+            self.last_pause_state_time = time()
 
     def __update_and_publish(self):
         self.publish_drive_command()
@@ -212,6 +219,7 @@ class JoystickControlSender(QtCore.QThread):
         self.drive_command_publisher.publish(drive_message)
 
     def publish_camera_control_commands(self):
+        trigger_pressed = self.joystick.controller_states["trigger_pressed"]
         three_pressed = self.joystick.controller_states["three_pressed"]
         four_pressed = self.joystick.controller_states["four_pressed"]
         five_pressed = self.joystick.controller_states["five_pressed"]
@@ -227,6 +235,10 @@ class JoystickControlSender(QtCore.QThread):
             self.change_gui_element_selection__signal.emit(change)
             self.last_gui_element_change_time = time()
 
+        if trigger_pressed and (time() - self.last_camera_toggle_time) > CAMERA_TOGGLE_CHANGE_TIME:
+            self.toggle_selected_gui_camera__signal.emit()
+            self.last_camera_toggle_time = time()
+
     def get_drive_message(self, throttle_axis):
         drive_message = DriveCommandMessage()
 

software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py

@@ -62,7 +62,7 @@ class RoverMapCoordinator(QtCore.QThread):
                                                          44.567161,
                                                          -123.278432,
                                                          18,
-                                                         'terrain',
+                                                         'satellite',
                                                          None, 20)
 
     def _map_setup(self):
@@ -71,7 +71,7 @@ class RoverMapCoordinator(QtCore.QThread):
                                                          44.567161,
                                                          -123.278432,
                                                          18,
-                                                         'terrain',
+                                                         'satellite',
                                                          None, 20)
         self.overlay_image_object = (
             RoverMap.OverlayImage(44.567161, -123.278432,

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

@@ -28,6 +28,8 @@ TERTIARY_LABEL_MAX = (640, 360)
 class RoverVideoCoordinator(QtCore.QThread):
     pixmap_ready_signal = QtCore.pyqtSignal(str)
 
+    update_element_stylesheet__signal = QtCore.pyqtSignal()
+
     def __init__(self, shared_objects):
         super(RoverVideoCoordinator, self).__init__()
 
@@ -130,15 +132,18 @@ class RoverVideoCoordinator(QtCore.QThread):
 
     def __update_gui_element_selection(self):
         if self.gui_selection_update_needed:
-            elements_to_reset = range(len(self.index_to_label_element))
+            self.update_element_stylesheet__signal.emit()
-            elements_to_reset.remove(self.current_label_for_joystick_adjust)
 
-            for index in elements_to_reset:
+    def __on_gui_element_stylesheet_update__slot(self):
-                self.index_to_label_element[index].setStyleSheet("")
+        elements_to_reset = range(len(self.index_to_label_element))
+        elements_to_reset.remove(self.current_label_for_joystick_adjust)
 
-            self.index_to_label_element[self.current_label_for_joystick_adjust].setStyleSheet("border: 2px solid orange")
+        for index in elements_to_reset:
+            self.index_to_label_element[index].setStyleSheet("background-color:black;")
 
-            self.gui_selection_update_needed = False
+        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)
@@ -180,6 +185,10 @@ class RoverVideoCoordinator(QtCore.QThread):
             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)
+        self.shared_objects["threaded_classes"]["Joystick Sender"].toggle_selected_gui_camera__signal.connect(
+            self.on_gui_selected_camera_toggled)
+
+        self.update_element_stylesheet__signal.connect(self.__on_gui_element_stylesheet_update__slot)
 
     def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
         start_signal.connect(self.start)
@@ -254,14 +263,34 @@ class RoverVideoCoordinator(QtCore.QThread):
         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 self.current_label_for_joystick_adjust == 0: # primary
+            self.primary_label_current_setting = (self.primary_label_current_setting + direction) % len(self.valid_cameras)
+        elif self.current_label_for_joystick_adjust == 1:  # secondary
+            self.secondary_label_current_setting = (self.secondary_label_current_setting + direction) % len(self.valid_cameras)
+        elif self.current_label_for_joystick_adjust == 2:  # tertiary
+            self.tertiary_label_current_setting = (self.tertiary_label_current_setting + direction) % len(self.valid_cameras)
 
-        if new_label_setting < 0:
+        self.set_max_resolutions_flag = True
-            self.index_to_label_current_setting[self.current_label_for_joystick_adjust] = len(self.valid_cameras) - 1
+
-        elif new_label_setting == len(self.valid_cameras):
+    def on_gui_selected_camera_toggled(self):
-            self.index_to_label_current_setting[self.current_label_for_joystick_adjust] = 0
+        if self.current_label_for_joystick_adjust == 0: # primary
-        else:
+            if self.primary_label_current_setting in self.disabled_cameras:
-            self.index_to_label_current_setting[self.current_label_for_joystick_adjust] = new_label_setting
+                self.disabled_cameras.remove(self.primary_label_current_setting)
+            else:
+                self.disabled_cameras.append(self.primary_label_current_setting)
+            self.camera_threads[self.valid_cameras[self.primary_label_current_setting]].toggle_video_display()
+        elif self.current_label_for_joystick_adjust == 1:  # secondary
+            if self.secondary_label_current_setting in self.disabled_cameras:
+                self.disabled_cameras.remove(self.secondary_label_current_setting)
+            else:
+                self.disabled_cameras.append(self.secondary_label_current_setting)
+            self.camera_threads[self.valid_cameras[self.secondary_label_current_setting]].toggle_video_display()
+        elif self.current_label_for_joystick_adjust == 2:  # tertiary
+            if self.tertiary_label_current_setting in self.disabled_cameras:
+                self.disabled_cameras.remove(self.tertiary_label_current_setting)
+            else:
+                self.disabled_cameras.append(self.tertiary_label_current_setting)
+            self.camera_threads[self.valid_cameras[self.tertiary_label_current_setting]].toggle_video_display()
 
     def on_kill_threads_requested__slot(self):
         self.run_thread_flag = False

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

@@ -1,134 +0,0 @@
-import sys
-from PyQt5 import QtWidgets, QtCore, QtGui, uic
-import signal
-import rospy
-import time
-from cv_bridge import CvBridge, CvBridgeError
-import cv2
-import qimage2ndarray
-import numpy as np
-
-from geometry_msgs.msg import Twist
-from sensor_msgs.msg import CompressedImage
-#from sensor_msgs.msg import Image, CompressedImage
-
-
-class VideoTest(QtCore.QThread):
-    publish_message_signal = QtCore.pyqtSignal()
-    image_ready_signal = QtCore.pyqtSignal()
-
-    def __init__(self, shared_objects, screen_label, video_size=None, sub_path=None):
-        super(VideoTest, self).__init__()
-
-        self.not_abort = True
-
-        self.shared_objects = shared_objects
-
-        self.right_screen_label = screen_label  # type: QtGui.QPixmap
-        self.video_size = video_size
-
-        self.message = None
-
-        self.publisher = rospy.Subscriber(sub_path, CompressedImage, self.__receive_message)
-
-        self.raw_image = None
-        self.cv_image = None
-        self.pixmap = None
-        self.bridge = CvBridge()
-        # self.bridge.com
-
-        self.new_frame = False
-        self.frame_count = 0
-        self.last_frame_time = time.time()
-        self.fps = 0
-
-        self.name = sub_path.split("/")[2].replace("_", " ").title()
-
-        self.font = cv2.FONT_HERSHEY_TRIPLEX
-
-        thickness = 1
-        baseline = 0
-
-        text_size = cv2.getTextSize(self.name, self.font, thickness, baseline)
-        print text_size
-
-        text_width, text_height = text_size[0]
-
-        width = text_width + 10
-        height = text_height + 20
-
-        self.blank_image = np.zeros((height, width, 3), np.uint8)
-        cv2.putText(self.blank_image, self.name, ((width - text_width) / 2, int((height * 2) / 3)), self.font, 1, (255, 255, 255), 1, cv2.LINE_AA)
-        self.blank_image = cv2.resize(self.blank_image, (width / 2, height / 2))
-
-    def run(self):
-        # TODO: Thread starting message here
-        y_offset = 0
-        x_offset = 0
-
-        while self.not_abort:
-            if self.raw_image and self.new_frame:
-                self.cv_image = self.bridge.compressed_imgmsg_to_cv2(self.raw_image, "rgb8")
-
-                self.cv_image = self.__show_fps(self.cv_image)
-
-                self.cv_image[y_offset:y_offset + self.blank_image.shape[0], x_offset:x_offset + self.blank_image.shape[1]] = self.blank_image
-
-                if self.video_size:
-                    self.cv_image = cv2.resize(self.cv_image, self.video_size)
-                self.pixmap = QtGui.QPixmap.fromImage(qimage2ndarray.array2qimage(self.cv_image))
-                self.image_ready_signal.emit()
-                self.new_frame = False
-
-            if (time.time() - self.last_frame_time) >= 0.5:
-                self.fps = int(self.frame_count / (time.time() - self. last_frame_time))
-
-                self.last_frame_time = time.time()
-                self.frame_count = 0
-
-            self.msleep(18)
-        # TODO: Thread ending message here
-
-    def __show_fps(self, image):
-        thickness = 1
-        baseline = 0
-
-        fps_string = str(self.fps)
-
-        text_size = cv2.getTextSize(fps_string, self.font, thickness, baseline)
-
-        text_width, text_height = text_size[0]
-
-        width = text_width + 10
-        height = text_height + 20
-
-        fps_image = np.zeros((height, width, 3), np.uint8)
-
-        cv2.putText(fps_image, fps_string, ((width - text_width) / 2, int((height * 2) / 3)), self.font, 1, (255, 255, 255), 1, cv2.LINE_AA)
-        fps_image = cv2.resize(fps_image, (width / 2, height / 2))
-
-        y_offset = 0
-        x_offset = (image.shape[1] - fps_image.shape[1]) / 2
-
-        image[y_offset:y_offset + fps_image.shape[0], x_offset:x_offset + fps_image.shape[1]] = fps_image
-
-        return image
-
-    def __on_image_update_ready(self):
-        self.right_screen_label.setPixmap(self.pixmap)
-
-    def __receive_message(self, message):
-        self.raw_image = message
-        self.new_frame = True
-        self.frame_count += 1
-
-    def connect_signals_and_slots(self):
-        self.image_ready_signal.connect(self.__on_image_update_ready)
-
-    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.not_abort = False

software/ros_packages/rover_main/launch/rover/control.launch

@@ -26,6 +26,5 @@
         </node>
 
         <node name="drive_coordinator" pkg="rover_control" type="drive_coordinator.py" output="screen"/>
-
     </group>
 </launch>