diff --git a/software/ros_packages/ground_station/src/Framework/JoystickControlSystems/JoystickControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py similarity index 100% rename from software/ros_packages/ground_station/src/Framework/JoystickControlSystems/JoystickControlSender.py rename to software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py new file mode 100644 index 0000000..8afd46f --- /dev/null +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py @@ -0,0 +1,134 @@ +##################################### +# Imports +##################################### +# Python native imports +from PyQt5 import QtCore +import logging +from time import time +import spnav + +import rospy + +##################################### +# Global Variables +##################################### +THREAD_HERTZ = 15 + + +##################################### +# Controller Class Definition +##################################### +class SpaceNavControlSender(QtCore.QThread): + def __init__(self, shared_objects): + super(SpaceNavControlSender, self).__init__() + + # ########## Reference to class init variables ########## + self.shared_objects = shared_objects + self.right_screen = self.shared_objects["screens"]["left_screen"] + + # ########## 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.spnav_states = { + "linear_x": 0, + "linear_y": 0, + "linear_z": 0, + + "angular_x": 0, + "angular_y": 0, + "angular_z": 0, + + "panel_pressed": 0, + "fit_pressed": 0, + + "shift_pressed": 0, + "alt_pressed": 0, + "ctrl_pressed": 0, + "esc_pressed": 0, + + "1_pressed": 0, + "2_pressed": 0, + + "minus_pressed": 0, + "plus_pressed": 0, + + "t_pressed": 0, + "l_pressed": 0, + "2d_pressed": 0, + "r_pressed": 0, + "f_pressed": 0 + } + + self.event_mapping_to_button_mapping = { + 11: "panel_pressed", + 10: "fit_pressed", + + 8: "shift_pressed", + 7: "alt_pressed", + 9: "ctrl_pressed", + 6: "esc_pressed", + + 0: "1_pressed", + 1: "2_pressed", + + 13: "minus_pressed", + 12: "plus_pressed", + + 2: "t_pressed", + 3: "l_pressed", + 14: "2d_pressed", + 4: "r_pressed", + 5: "f_pressed" + } + + def run(self): + spnav.spnav_open() + + while self.run_thread_flag: + + start_time = time() + + self.process_spnav_events() + + time_diff = time() - start_time + + self.msleep(max(int(self.wait_time - time_diff), 0)) + + def process_spnav_events(self): + event = spnav.spnav_poll_event() + + if event: + if event.ev_type == spnav.SPNAV_EVENT_MOTION: + # FIXME: Make sure these divisors are correct. Should be for most. Check linear_z, is weird + self.spnav_states["linear_x"] = event.translation[0] / 350.0 + self.spnav_states["linear_y"] = event.translation[2] / 350.0 + self.spnav_states["linear_z"] = event.translation[1] / 350.0 + + self.spnav_states["angular_x"] = event.rotation[2] / 350.0 + self.spnav_states["angular_y"] = event.rotation[0] / 350.0 + self.spnav_states["angular_z"] = event.rotation[1] / 350.0 + + # print "x", self.spnav_states["linear_x"], "\t", "y", self.spnav_states["linear_y"], "\t", "z", self.spnav_states["linear_z"] + # print "x", self.spnav_states["angular_x"], "\t", "y", self.spnav_states["angular_y"], "\t", "z", self.spnav_states["angular_z"] + else: + self.spnav_states[self.event_mapping_to_button_mapping[event.bnum]] = event.press + + def connect_signals_and_slots(self): + pass + + 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/JoystickControlSystems/__init__.py b/software/ros_packages/ground_station/src/Framework/InputSystems/__init__.py similarity index 100% rename from software/ros_packages/ground_station/src/Framework/JoystickControlSystems/__init__.py rename to software/ros_packages/ground_station/src/Framework/InputSystems/__init__.py diff --git a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py index a7b7ebe..d33a124 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py @@ -428,7 +428,7 @@ class OverlayImage(object): def _draw_rover(self, lat, lon, angle=0): x, y = self._get_cartesian(lat, lon) - print x,y + # print x,y # Center of the circle on the indicator is (12.5, 37.5) x = x - 50 y = y - 50 diff --git a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui index 8380cda..ad0c539 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui @@ -132,6 +132,28 @@ + + Qt::AlignCenter + + + + + + + + 0 + 75 + + + + + 16777215 + 75 + + + + Qt::Vertical + @@ -154,6 +176,28 @@ 75 + + QFrame::NoFrame + + + + + + + + 0 + 75 + + + + + 16777215 + 75 + + + + Qt::Vertical + @@ -176,6 +220,28 @@ 75 + + QFrame::NoFrame + + + + + + + + 0 + 75 + + + + + 16777215 + 75 + + + + Qt::Vertical + diff --git a/software/ros_packages/ground_station/src/ground_station.py b/software/ros_packages/ground_station/src/ground_station.py index 4acf527..a21febb 100644 --- a/software/ros_packages/ground_station/src/ground_station.py +++ b/software/ros_packages/ground_station/src/ground_station.py @@ -15,12 +15,13 @@ 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.JoystickControlSystems.JoystickControlSender as JoystickControlSender +import Framework.InputSystems.JoystickControlSender as JoystickControlSender import Framework.NavigationSystems.SpeedAndHeadingIndication as SpeedAndHeading import Framework.NavigationSystems.WaypointsCoordinator as WaypointsCoordinator import Framework.ArmSystems.ArmIndication as ArmIndication import Framework.StatusSystems.StatusCore as StatusCore import Framework.SettingsSystems.UbiquitiRadioSettings as UbiquitiRadioSettings +import Framework.InputSystems.SpaceNavControlSender as SpaceNavControlSender ##################################### # Global Variables @@ -109,6 +110,7 @@ class GroundStation(QtCore.QObject): self.__add_thread("Rover Status", StatusCore.SensorCore(self.shared_objects)) self.__add_thread("Ubiquiti Radio Settings", UbiquitiRadioSettings.UbiquitiRadioSettings(self.shared_objects)) self.__add_thread("Waypoints Coordinator", WaypointsCoordinator.WaypointsCoordinator(self.shared_objects)) + self.__add_thread("Spacenav Sender", SpaceNavControlSender.SpaceNavControlSender(self.shared_objects)) self.connect_signals_and_slots_signal.emit() self.__connect_signals_to_slots()