diff --git a/software/ros_packages/ground_station/src/Framework/JoystickControlSystems/JoystickControlSender.py b/software/ros_packages/ground_station/src/Framework/JoystickControlSystems/JoystickControlSender.py index 852c9ea..70e0f66 100644 --- a/software/ros_packages/ground_station/src/Framework/JoystickControlSystems/JoystickControlSender.py +++ b/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 -GUI_ELEMENT_CHANGE_TIME = 0.25 +PAUSE_STATE_CHANGE_TIME = 0.5 + +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() diff --git a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py index bd43307..f6e52a2 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py +++ b/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, diff --git a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py b/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py index fe874f9..b091b2d 100644 --- a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py +++ b/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)) - elements_to_reset.remove(self.current_label_for_joystick_adjust) + self.update_element_stylesheet__signal.emit() - for index in elements_to_reset: - self.index_to_label_element[index].setStyleSheet("") + def __on_gui_element_stylesheet_update__slot(self): + 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.index_to_label_current_setting[self.current_label_for_joystick_adjust] = len(self.valid_cameras) - 1 - 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 + self.set_max_resolutions_flag = True + + def on_gui_selected_camera_toggled(self): + if self.current_label_for_joystick_adjust == 0: # primary + if self.primary_label_current_setting in self.disabled_cameras: + 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 diff --git a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoReceiverOld.py b/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoReceiverOld.py deleted file mode 100644 index 7bc1435..0000000 --- a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoReceiverOld.py +++ /dev/null @@ -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 \ No newline at end of file diff --git a/software/ros_packages/rover_main/launch/rover/control.launch b/software/ros_packages/rover_main/launch/rover/control.launch index b7feb7c..7ace600 100644 --- a/software/ros_packages/rover_main/launch/rover/control.launch +++ b/software/ros_packages/rover_main/launch/rover/control.launch @@ -26,6 +26,5 @@ -