From c9c2d29942d0a20384821dea90a8aa6fe3b6bbed Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Thu, 8 Mar 2018 16:41:16 -0800 Subject: [PATCH] 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 --- .../JoystickControlSender.py} | 33 +++++++++- .../__init__.py | 0 .../VideoSystems/RoverVideoCoordinator.py | 60 +++++++++++++++++-- .../ground_station/src/ground_station.py | 4 +- 4 files changed, 89 insertions(+), 8 deletions(-) rename software/ros_packages/ground_station/src/Framework/{DriveSystems/RoverDriveSender.py => JoystickControlSystems/JoystickControlSender.py} (85%) rename software/ros_packages/ground_station/src/Framework/{DriveSystems => JoystickControlSystems}/__init__.py (100%) diff --git a/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py b/software/ros_packages/ground_station/src/Framework/JoystickControlSystems/JoystickControlSender.py similarity index 85% rename from software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py rename to software/ros_packages/ground_station/src/Framework/JoystickControlSystems/JoystickControlSender.py index eebfd87..852c9ea 100644 --- a/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py +++ b/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() diff --git a/software/ros_packages/ground_station/src/Framework/DriveSystems/__init__.py b/software/ros_packages/ground_station/src/Framework/JoystickControlSystems/__init__.py similarity index 100% rename from software/ros_packages/ground_station/src/Framework/DriveSystems/__init__.py rename to software/ros_packages/ground_station/src/Framework/JoystickControlSystems/__init__.py 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 4042329..bfb3a3d 100644 --- a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py +++ b/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.secondary_label_max_resolution = -1 - self.tertiary_label_max_resolution = -1 + self.index_to_label_current_setting = { + 0, self.primary_label_current_setting, + 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 diff --git a/software/ros_packages/ground_station/src/ground_station.py b/software/ros_packages/ground_station/src/ground_station.py index 58cc126..bde6fb2 100644 --- a/software/ros_packages/ground_station/src/ground_station.py +++ b/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()