diff --git a/software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py b/software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py new file mode 100644 index 0000000..c0fa63c --- /dev/null +++ b/software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py @@ -0,0 +1,96 @@ +# coding=utf-8 +##################################### +# Imports +##################################### +# Python native imports +from PyQt5 import QtCore, QtWidgets, QtGui +import logging +from time import time + +##################################### +# Global Variables +##################################### +THREAD_HERTZ = 10 + +ROTATION_SPEED_MODIFIER = 0.015 + + +##################################### +# Controller Class Definition +##################################### +class ArmIndication(QtCore.QThread): + arm_joint_position_updated__signal = QtCore.pyqtSignal(int) + + def __init__(self, shared_objects): + super(ArmIndication, self).__init__() + + # ########## Reference to class init variables ########## + self.shared_objects = shared_objects + self.right_screen = self.shared_objects["screens"]["right_screen"] + + self.base_rotation_dial = self.right_screen.base_rotation_dial # type: QtWidgets.QDial + self.shoulder_pitch_dial = self.right_screen.shoulder_pitch_dial # type: QtWidgets.QDial + self.elbow_pitch_dial = self.right_screen.elbow_pitch_dial # type: QtWidgets.QDial + self.elbow_roll_dial = self.right_screen.elbow_roll_dial # type: QtWidgets.QDial + self.end_effector_pitch_dial = self.right_screen.end_effector_pitch_dial # type: QtWidgets.QDial + self.end_effector_rotation_dial = self.right_screen.end_effector_rotation_dial # type: QtWidgets.QDial + + # ########## Get the settings instance ########## + self.settings = QtCore.QSettings() + + # ########## Get the Pick And Plate instance of the logger ########## + self.logger = logging.getLogger("groundstation") + + # ########## Thread Flags ########## + self.run_thread_flag = True + + # ########## Class Variables ########## + self.wait_time = 1.0 / THREAD_HERTZ + + self.current_position_delta = 0 + self.shown_position = 0 + + def run(self): + while self.run_thread_flag: + start_time = time() + + self.change_position_if_needed() + + time_diff = time() - start_time + + self.msleep(max(int(self.wait_time - time_diff), 0)) + + def change_position_if_needed(self): + self.shown_position += self.current_position_delta * ROTATION_SPEED_MODIFIER + self.shown_position %= 360 + + self.arm_joint_position_updated__signal.emit(self.shown_position) + + def __on_position_change_requested__slot(self, event): + if event.button() == QtCore.Qt.LeftButton: + self.current_position_delta = 1 + elif event.button() == QtCore.Qt.RightButton: + self.current_position_delta = 0 + + def connect_signals_and_slots(self): + self.arm_joint_position_updated__signal.connect(self.base_rotation_dial.setValue) + self.arm_joint_position_updated__signal.connect(self.shoulder_pitch_dial.setValue) + self.arm_joint_position_updated__signal.connect(self.elbow_pitch_dial.setValue) + self.arm_joint_position_updated__signal.connect(self.elbow_roll_dial.setValue) + self.arm_joint_position_updated__signal.connect(self.end_effector_pitch_dial.setValue) + self.arm_joint_position_updated__signal.connect(self.end_effector_rotation_dial.setValue) + + self.base_rotation_dial.mousePressEvent = self.__on_position_change_requested__slot + self.shoulder_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot + self.elbow_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot + self.elbow_roll_dial.mousePressEvent = self.__on_position_change_requested__slot + self.end_effector_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot + self.end_effector_rotation_dial.mousePressEvent = self.__on_position_change_requested__slot + + 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.run_thread_flag = False diff --git a/software/ros_packages/ground_station/src/Framework/ArmSystems/__init__.py b/software/ros_packages/ground_station/src/Framework/ArmSystems/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py b/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py index f408e89..cc4a711 100644 --- a/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py +++ b/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py @@ -60,9 +60,6 @@ class SpeedAndHeadingIndication(QtCore.QThread): self.current_heading_shown_rotation_angle = 0 self.rotation_direction = 1 - # compass_image = PIL.Image.open("Resources/Images/compass.png").resize((300, 300)).rotate(45) # PIL.Image - # self.shared_objects["screens"]["right_screen"].heading_compass_label.setPixmap(QtGui.QPixmap.fromImage(ImageQt(compass_image))) - def run(self): self.on_heading_changed__slot(self.current_heading) @@ -107,16 +104,14 @@ class SpeedAndHeadingIndication(QtCore.QThread): def __on_heading_clicked__slot(self, event): new_heading = self.current_heading if event.button() == QtCore.Qt.LeftButton: - new_heading = (self.current_heading + 10) % 360 + new_heading = (self.current_heading + 5) % 360 elif event.button() == QtCore.Qt.RightButton: - new_heading = (self.current_heading - 10) % 360 + new_heading = (self.current_heading - 5) % 360 self.on_heading_changed__slot(new_heading) self.new_speed_update_ready__signal.emit("%.2f" % (random() * 2.5)) self.heading_text_update_ready__signal.emit(str(new_heading) + "°") - - def on_new_compass_image_ready__slot(self): self.heading_compass_label.setPixmap(self.compass_pixmap) 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 83e605a..ed7d282 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 @@ -150,7 +150,7 @@ - + 305 @@ -158,10 +158,10 @@ - -180 + 0 - 180 + 360 10 @@ -217,7 +217,7 @@ - + 305 @@ -225,10 +225,10 @@ - -180 + 0 - 180 + 360 10 @@ -258,7 +258,7 @@ 90.000000000000000 - true + false @@ -268,19 +268,6 @@ - - - - Qt::Horizontal - - - - 40 - 20 - - - - @@ -301,7 +288,7 @@ - + 305 @@ -309,10 +296,10 @@ - -180 + 0 - 180 + 360 10 @@ -342,24 +329,82 @@ 90.000000000000000 - true + false - - - Qt::Horizontal - - - - 40 - 20 - - - + + + + + + + + 14 + 75 + true + + + + Elbow Roll + + + Qt::AlignCenter + + + + + + + + 305 + 185 + + + + 0 + + + 360 + + + 10 + + + 0 + + + 0 + + + true + + + Qt::Horizontal + + + false + + + false + + + true + + + 90.000000000000000 + + + false + + + + + + @@ -385,7 +430,7 @@ - + 305 @@ -393,10 +438,10 @@ - -180 + 0 - 180 + 360 10 @@ -426,7 +471,7 @@ 90.000000000000000 - true + false @@ -452,7 +497,7 @@ - + 305 @@ -460,10 +505,10 @@ - -180 + 0 - 180 + 360 10 @@ -493,7 +538,7 @@ 90.000000000000000 - true + false diff --git a/software/ros_packages/ground_station/src/ground_station.py b/software/ros_packages/ground_station/src/ground_station.py index b7a33ad..b286a1d 100644 --- a/software/ros_packages/ground_station/src/ground_station.py +++ b/software/ros_packages/ground_station/src/ground_station.py @@ -17,6 +17,7 @@ import Framework.VideoSystems.RoverVideoCoordinator as RoverVideoCoordinator import Framework.MapSystems.RoverMapCoordinator as RoverMapCoordinator import Framework.JoystickControlSystems.JoystickControlSender as JoystickControlSender import Framework.NavigationSystems.SpeedAndHeadingIndication as SpeedAndHeading +import Framework.ArmSystems.ArmIndication as ArmIndication ##################################### # Global Variables @@ -101,6 +102,8 @@ class GroundStation(QtCore.QObject): self.__add_thread("Map Coordinator", RoverMapCoordinator.RoverMapCoordinator(self.shared_objects)) self.__add_thread("Joystick Sender", JoystickControlSender.JoystickControlSender(self.shared_objects)) self.__add_thread("Speed and Heading", SpeedAndHeading.SpeedAndHeadingIndication(self.shared_objects)) + self.__add_thread("Arm Indication", ArmIndication.ArmIndication(self.shared_objects)) + self.connect_signals_and_slots_signal.emit() self.__connect_signals_to_slots() self.start_threads_signal.emit() diff --git a/software/ros_packages/rover_control/src/drive_control/drive_control.py b/software/ros_packages/rover_control/src/drive_control/drive_control.py index 8879f4f..09187fe 100755 --- a/software/ros_packages/rover_control/src/drive_control/drive_control.py +++ b/software/ros_packages/rover_control/src/drive_control/drive_control.py @@ -55,6 +55,8 @@ MOTOR_DRIVER_DEFAULT_MESSAGE = [ UINT16_MAX = 65535 +BOGIE_LAST_SEEN_TIMEOUT = 2 # seconds + ##################################### # DriveControl Class Definition @@ -75,9 +77,10 @@ class DriveControl(object): self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ) - self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID) - self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID) - self.__setup_minimalmodbus_for_485() + self.first_motor = None + self.second_motor = None + + self.connect_to_bogie() self.drive_control_subscriber = \ rospy.Subscriber(self.drive_control_subscriber_topic, DriveControlMessage, self.drive_control_callback) @@ -87,6 +90,8 @@ class DriveControl(object): self.drive_control_message = DriveControlMessage() self.new_control_message = False + self.bogie_last_seen = time() + self.run() def __setup_minimalmodbus_for_485(self): @@ -111,10 +116,18 @@ class DriveControl(object): except Exception, error: print "Error occurred:", error + if (time() - self.bogie_last_seen) > BOGIE_LAST_SEEN_TIMEOUT: + return # Exit so respawn can take over + time_diff = time() - start_time sleep(max(self.wait_time - time_diff, 0)) + def connect_to_bogie(self): + self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID) + self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID) + self.__setup_minimalmodbus_for_485() + def send_drive_control_message(self): if self.new_control_message: drive_control = self.drive_control_message @@ -156,6 +169,9 @@ class DriveControl(object): except Exception: status.second_motor_connected = False + if status.first_motor_connected or status.second_motor_connected: + self.bogie_last_seen = time() + if status.first_motor_connected: status.first_motor_current = first_motor_status[0] / 1000.0 status.first_motor_fault = first_motor_status[1] 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 86ef1ad..69ab713 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 @@ -111,6 +111,7 @@ class IrisController(object): except Exception, error: print "Error occurred:", error + return time_diff = time() - start_time diff --git a/software/ros_packages/rover_main/launch/rover/control.launch b/software/ros_packages/rover_main/launch/rover/control.launch index 7ace600..f5042ed 100644 --- a/software/ros_packages/rover_main/launch/rover/control.launch +++ b/software/ros_packages/rover_main/launch/rover/control.launch @@ -1,17 +1,17 @@ - + - + - + @@ -19,7 +19,7 @@ - + diff --git a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch index 7891458..713f58e 100644 --- a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch @@ -106,7 +106,7 @@ {name: "/rover_status/camera_status", compress: true, rate: 1.0}, {name: "/rover_status/frsky_status", compress: true, rate: 1.0}, {name: "/rover_status/gps_status", compress: true, rate: 1.0}, - {name: "/rover_status/jetson_status", compress: true, rate: 5.0}, + {name: "/rover_status/jetson_status", compress: true, rate: 0.2}, {name: "/rover_status/misc_status", compress: true, rate: 1.0}]