From 6d9ed5d43887ab4b5aece48bd4158527a7dd6231 Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Thu, 1 Mar 2018 17:14:34 -0800 Subject: [PATCH] Fixed some gui elements. Added pause / speed control to remote drive. --- .../DriveSystems/RoverDriveSender.py | 151 ++-- .../MapSystems/RoverMapCoordinator.py | 6 +- .../src/Resources/Ui/right_screen.ui | 707 +++++++++++++++++- .../ground_station/src/ground_station.py | 4 +- .../control_coordinators/drive_coordinator.py | 1 - .../src/iris_controller/iris_controller.py | 1 + 6 files changed, 772 insertions(+), 98 deletions(-) diff --git a/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py b/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py index e323554..8f1551a 100644 --- a/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py +++ b/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py @@ -19,6 +19,11 @@ DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_dri DRIVE_COMMAND_HERTZ = 15 +Y_AXIS_DEADBAND = 0.015 +X_AXIS_DEADBAND = 0.025 + +THROTTLE_MIN = 0.05 + ##################################### # Controller Class Definition @@ -37,59 +42,51 @@ class LogitechJoystick(QtCore.QThread): self.gamepad = None # type: GamePad self.controller_states = { - "left_stick_x_axis": 0, + "x_axis": 512, "y_axis": 512, - "left_stick_center_pressed": 0, - - "right_stick_x_axis": 0, - "right_stick_y_axis": 0, - "right_stick_center_pressed": 0, - - "left_trigger_z_axis": 0, - "left_bumper_pressed": 0, - "z_axis": 128, - "right_bumper_pressed": 0, + "throttle_axis": 128, - "dpad_x": 0, - "dpad_y": 0, + "hat_x_axis": 0, + "hat_y_axis": 0, - "select_pressed": 0, - "start_pressed": 0, - "home_pressed": 0, + "trigger_pressed": 0, + "thumb_pressed": 0, + "three_pressed": 0, + "four_pressed": 0, + "five_pressed": 0, + "six_pressed": 0, - "a_pressed": 0, - "b_pressed": 0, - "x_pressed": 0, - "y_pressed": 0 + "eleven_pressed": 0, + "twelve_pressed": 0, + "thirteen_pressed": 0, + "fourteen_pressed": 0, + "fifteen_pressed": 0, + "sixteen_pressed": 0, } self.raw_mapping_to_class_mapping = { - "ABS_X": "left_stick_x_axis", + "ABS_X": "x_axis", "ABS_Y": "y_axis", - "BTN_THUMBL": "left_stick_center_pressed", - - "ABS_RX": "right_stick_x_axis", - "ABS_RY": "right_stick_y_axis", - "BTN_THUMBR": "right_stick_center_pressed", - - "ABS_Z": "left_trigger_z_axis", - "BTN_TL": "left_bumper_pressed", - "ABS_RZ": "z_axis", - "BTN_TR": "right_bumper_pressed", + "ABS_THROTTLE": "throttle_axis", - "ABS_HAT0X": "dpad_x", - "ABS_HAT0Y": "dpad_y", + "ABS_HAT0X": "hat_x_axis", + "ABS_HAT0Y": "hat_y_axis", - "BTN_SELECT": "select_pressed", - "BTN_START": "start_pressed", - "BTN_MODE": "home_pressed", + "BTN_TRIGGER": "trigger_pressed", + "BTN_THUMB": "thumb_pressed", + "BTN_THUMB2": "three_pressed", + "BTN_TOP": "four_pressed", + "BTN_TOP2": "five_pressed", + "BTN_PINKIE": "six_pressed", - "BTN_SOUTH": "a_pressed", - "BTN_EAST": "b_pressed", - "BTN_NORTH": "x_pressed", - "BTN_WEST": "y_pressed" + "BTN_BASE5": "eleven_pressed", + "BTN_BASE6": "twelve_pressed", + "BTN_BASE3": "thirteen_pressed", + "BTN_BASE4": "fourteen_pressed", + "BTN_BASE": "fifteen_pressed", + "BTN_BASE2": "sixteen_pressed", } self.ready = False @@ -121,23 +118,27 @@ class LogitechJoystick(QtCore.QThread): for event in events: if event.code in self.raw_mapping_to_class_mapping: self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state + self.ready = True - # print "Logitech: %s" % self.controller_states ##################################### # Controller Class Definition ##################################### class RoverDriveSender(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) + def __init__(self, shared_objects): super(RoverDriveSender, self).__init__() # ########## Reference to class init variables ########## self.shared_objects = shared_objects self.right_screen = self.shared_objects["screens"]["right_screen"] - self.primary_video_display_label = self.right_screen.primary_video_label # type:QtWidgets.QLabel - 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.speed_limit_progress_bar = self.right_screen.speed_limit_progress_bar # type: QtWidgets.QProgressBar + self.left_drive_progress_bar = self.right_screen.left_drive_progress_bar # type: QtWidgets.QProgressBar + self.right_drive_progress_bar = self.right_screen.right_drive_progress_bar # type: QtWidgets.QProgressBar # ########## Get the settings instance ########## self.settings = QtCore.QSettings() @@ -154,34 +155,82 @@ class RoverDriveSender(QtCore.QThread): # Publishers self.drive_command_publisher = rospy.Publisher(DEFAULT_DRIVE_COMMAND_TOPIC, DriveCommandMessage, queue_size=1) - self.wait_time = 1 / DRIVE_COMMAND_HERTZ + self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ + + self.drive_paused = True def run(self): while self.run_thread_flag: start_time = time() + self.check_and_set_pause_state() self.__update_and_publish() time_diff = time() - start_time - self.msleep(max(self.wait_time - time_diff, 0)) + self.msleep(max(int(self.wait_time - time_diff), 0)) def connect_signals_and_slots(self): - pass + self.set_speed_limit__signal.connect(self.speed_limit_progress_bar.setValue) + self.set_left_drive_output__signal.connect(self.left_drive_progress_bar.setValue) + 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"]: + self.drive_paused = not self.drive_paused + self.msleep(350) + self.show_changed_pause_state() def __update_and_publish(self): + throttle_axis = max((255 - self.joystick.controller_states["throttle_axis"]) / 255.0, THROTTLE_MIN) + + if self.drive_paused: + drive_message = DriveCommandMessage() + else: + drive_message = self.get_drive_message(throttle_axis) + + left_output = abs(drive_message.drive_twist.linear.x - drive_message.drive_twist.angular.z) + right_output = abs(drive_message.drive_twist.linear.x + drive_message.drive_twist.angular.z) + + self.set_speed_limit__signal.emit(throttle_axis * 100) + self.set_left_drive_output__signal.emit(left_output * 100) + self.set_right_drive_output__signal.emit(right_output * 100) + + self.drive_command_publisher.publish(drive_message) + + def get_drive_message(self, throttle_axis): drive_message = DriveCommandMessage() - drive_message.drive_twist.linear.x = -(self.joystick.controller_states["y_axis"] - 512) / 1024.0 - drive_message.drive_twist.angular.z = -(self.joystick.controller_states["z_axis"] - 128) / 255.0 - self.drive_command_publisher.publish(drive_message) - # print self.joystick.controller_states["y_axis"], self.joystick.controller_states["z_axis"] + y_axis = throttle_axis * (-(self.joystick.controller_states["y_axis"] - 512) / 512.0) + z_axis = throttle_axis * (-(self.joystick.controller_states["z_axis"] - 128) / 128.0) + x_axis = throttle_axis * (-(self.joystick.controller_states["x_axis"] - 512) / 512.0) + + if abs(y_axis) < Y_AXIS_DEADBAND: + y_axis = 0 + + if abs(x_axis) < X_AXIS_DEADBAND and y_axis == 0: + twist = z_axis + else: + twist = x_axis if y_axis >= 0 else -x_axis + + drive_message.drive_twist.linear.x = y_axis + drive_message.drive_twist.angular.z = twist + + return drive_message 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 show_changed_pause_state(self): + if self.drive_paused: + self.left_drive_progress_bar.setStyleSheet("background-color:darkred;") + self.right_drive_progress_bar.setStyleSheet("background-color:darkred;") + else: + self.left_drive_progress_bar.setStyleSheet("background-color: transparent;") + self.right_drive_progress_bar.setStyleSheet("background-color: transparent;") + def on_kill_threads_requested__slot(self): self.run_thread_flag = False 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 804eb9f..bd63b19 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py @@ -76,13 +76,13 @@ class RoverMapCoordinator(QtCore.QThread): RoverMap.OverlayImage(44.567161, -123.278432, self.google_maps_object.northwest, self.google_maps_object.southeast, - self.google_maps_object.big_image[0], - self.google_maps_object.big_image[1], + self.google_maps_object.big_image.size[0], + self.google_maps_object.big_image.size[1], 1280, 720)) def _get_map_image(self): self.map_image = self.google_maps_object.display_image - self.map_image.paste(self.overlay_image_object.display_image) + # self.map_image.paste(self.overlay_image_object.display_image) # get overlay here qim = ImageQt(self.map_image) self.map_pixmap = QtGui.QPixmap.fromImage(qim) diff --git a/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui index 44a668e..889c6df 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui @@ -42,7 +42,16 @@ QLayout::SetDefaultConstraint - + + 0 + + + 0 + + + 0 + + 0 @@ -75,9 +84,425 @@ 720 - - background-color:black; - + + + + + + + + 22 + 75 + true + + + + Arm Joint Positions + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + + 14 + 75 + true + + + + Base Rotation + + + Qt::AlignCenter + + + + + + + + 305 + 185 + + + + -180 + + + 180 + + + 10 + + + 0 + + + 0 + + + true + + + Qt::Horizontal + + + false + + + false + + + true + + + 90.000000000000000 + + + true + + + + + + + + + + + + 14 + 75 + true + + + + Shoulder Pitch + + + Qt::AlignCenter + + + + + + + + 305 + 185 + + + + -180 + + + 180 + + + 10 + + + 0 + + + 0 + + + true + + + Qt::Horizontal + + + false + + + false + + + true + + + 90.000000000000000 + + + true + + + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + 14 + 75 + true + + + + Elbow Pitch + + + Qt::AlignCenter + + + + + + + + 305 + 185 + + + + -180 + + + 180 + + + 10 + + + 0 + + + 0 + + + true + + + Qt::Horizontal + + + false + + + false + + + true + + + 90.000000000000000 + + + true + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + + 14 + 75 + true + + + + End Effector Pitch + + + Qt::AlignCenter + + + + + + + + 305 + 185 + + + + -180 + + + 180 + + + 10 + + + 0 + + + 0 + + + true + + + Qt::Horizontal + + + false + + + false + + + true + + + 90.000000000000000 + + + true + + + + + + + + + + + + 14 + 75 + true + + + + End Effector Rotation + + + Qt::AlignCenter + + + + + + + + 305 + 185 + + + + -180 + + + 180 + + + 10 + + + 0 + + + 0 + + + true + + + Qt::Horizontal + + + false + + + false + + + true + + + 90.000000000000000 + + + true + + + + + + + + + + + Qt::Vertical + + + + 20 + 3 + + + + + @@ -108,25 +533,134 @@ 360 - - background-color:black;; - - + + 0 + + + 0 + + + 0 + + 0 0 - - - + + + 0 - - Qt::AlignCenter - - + + + + 6 + + + 6 + + + 0 + + + + + + 17 + 75 + true + + + + Heading + + + Qt::AlignCenter + + + + + + + + 14 + + + + 258° + + + Qt::AlignCenter + + + + + + + + 17 + 75 + true + + + + Next Goal + + + Qt::AlignCenter + + + + + + + + 14 + + + + 260° + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + 275 + 275 + + + + + + + Qt::AlignCenter + + + + @@ -152,6 +686,21 @@ + + 6 + + + 6 + + + 0 + + + 6 + + + 0 + @@ -170,19 +719,35 @@ - - - - 22 - - - - 2.4 m/s - - - Qt::AlignCenter - - + + + + + + 22 + + + + 0.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 22 + + + + m/s + + + + @@ -202,13 +767,19 @@ - + + + 100 + 50 Qt::AlignCenter + + true + Qt::Horizontal @@ -238,18 +809,72 @@ - - - 0 - - - - - - - 0 - - + + + + + + 75 + true + + + + Left + + + + + + + background-color:darkred; + + + 0 + + + 100 + + + 0 + + + true + + + + + + + + 75 + true + + + + Right + + + + + + + background-color:darkred; + + + 0 + + + 100 + + + 0 + + + true + + + + diff --git a/software/ros_packages/ground_station/src/ground_station.py b/software/ros_packages/ground_station/src/ground_station.py index 8655744..79cbf26 100644 --- a/software/ros_packages/ground_station/src/ground_station.py +++ b/software/ros_packages/ground_station/src/ground_station.py @@ -98,14 +98,14 @@ 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("Map Coordinator", RoverMapCoordinator.RoverMapCoordinator(self.shared_objects)) self.__add_thread("Rover Drive Sender", RoverDriveSender.RoverDriveSender(self.shared_objects)) self.connect_signals_and_slots_signal.emit() self.__connect_signals_to_slots() self.start_threads_signal.emit() compass_image = PIL.Image.open("Resources/Images/compass.png").resize((300, 300)) # PIL.Image - self.shared_objects["screens"]["right_screen"].compass_label.setPixmap(QtGui.QPixmap.fromImage(ImageQt(compass_image))) + self.shared_objects["screens"]["right_screen"].compass_label.setPixmap(QtGui.QPixmap.fromImage(ImageQt(compass_image))) def ___ros_master_running(self): checker = ROSMasterChecker.ROSMasterChecker() diff --git a/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py index 1083e37..6ead8d7 100755 --- a/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py +++ b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py @@ -117,7 +117,6 @@ class DriveCoordinator(object): self.left_bogie_publisher.publish(left_drive) self.right_bogie_publisher.publish(right_drive) - def iris_drive_command_callback(self, drive_command): self.drive_commands["iris"] = drive_command return diff --git a/software/ros_packages/rover_control/src/iris_controller/iris_controller.py b/software/ros_packages/rover_control/src/iris_controller/iris_controller.py index caebd08..1d1bb88 100755 --- a/software/ros_packages/rover_control/src/iris_controller/iris_controller.py +++ b/software/ros_packages/rover_control/src/iris_controller/iris_controller.py @@ -118,6 +118,7 @@ class IrisController(object): def read_registers(self): self.registers = self.iris.read_registers(0, len(MODBUS_REGISTERS)) + # print self.registers def broadcast_drive_if_current_mode(self): if self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["DRIVE_VS_ARM"]]] < SBUS_VALUES["SBUS_MID"]: