Added initial drive sender for ground station

software/ground_station/Framework/DriveSystems/RoverDriveSender.py

@@ -1,50 +1,180 @@
-import sys
-from PyQt5 import QtWidgets, QtCore, QtGui, uic
-import signal
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets
+import logging
+from inputs import devices, GamePad
+from time import time
+
 import rospy
-import time
-from cv_bridge import CvBridge, CvBridgeError
-import cv2
-import qimage2ndarray
-import numpy as np
+from rover_control.msg import DriveCommandMessage
 
-from geometry_msgs.msg import Twist
-from sensor_msgs.msg import CompressedImage
-#from sensor_msgs.msg import Image, CompressedImage
+#####################################
+# Global Variables
+#####################################
+GAME_CONTROLLER_NAME = "Logitech Logitech Extreme 3D Pro"
+
+DEFAULT_DRIVE_COMMAND_TOPIC = "command_control/groundstation_drive"
 
 
-class DriveTest(QtCore.QThread):
-    publish_message_signal = QtCore.pyqtSignal()
-
+#####################################
+# Controller Class Definition
+#####################################
+class LogitechJoystick(QtCore.QThread):
     def __init__(self):
-        super(DriveTest, self).__init__()
+        super(LogitechJoystick, self).__init__()
 
-        self.not_abort = True
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+        self.setup_controller_flag = True
+        self.data_acquisition_and_broadcast_flag = True
+        self.controller_acquired = False
 
-        self.message = None
-        self.publisher = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
+        # ########## Class Variables ##########
+        self.gamepad = None  # type: GamePad
 
-        rospy.init_node("test")
+        self.controller_states = {
+            "left_stick_x_axis": 0,
+            "left_stick_y_axis": 0,
+            "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,
+
+            "right_trigger_z_axis": 0,
+            "right_bumper_pressed": 0,
+
+            "dpad_x": 0,
+            "dpad_y": 0,
+
+            "select_pressed": 0,
+            "start_pressed": 0,
+            "home_pressed": 0,
+
+            "a_pressed": 0,
+            "b_pressed": 0,
+            "x_pressed": 0,
+            "y_pressed": 0
+        }
+
+        self.raw_mapping_to_class_mapping = {
+            "ABS_X": "left_stick_x_axis",
+            "ABS_Y": "left_stick_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": "right_trigger_z_axis",
+            "BTN_TR": "right_bumper_pressed",
+
+            "ABS_HAT0X": "dpad_x",
+            "ABS_HAT0Y": "dpad_y",
+
+            "BTN_SELECT": "select_pressed",
+            "BTN_START": "start_pressed",
+            "BTN_MODE": "home_pressed",
+
+            "BTN_SOUTH": "a_pressed",
+            "BTN_EAST": "b_pressed",
+            "BTN_NORTH": "x_pressed",
+            "BTN_WEST": "y_pressed"
+        }
+        self.ready = False
+
+        self.start()
 
     def run(self):
-        # TODO: Thread starting message here
-        while self.not_abort:
-            self.message = Twist()
 
-            self.message.linear.x = 1.0
-            self.message.angular.z = 1.0
+        while self.run_thread_flag:
+            if self.setup_controller_flag:
+                self.controller_acquired = self.__setup_controller()
+                self.setup_controller_flag = False
+            if self.data_acquisition_and_broadcast_flag:
+                self.__get_controller_data()
 
-            self.publisher.publish(self.message)
+    def __setup_controller(self):
+        for device in devices.gamepads:
+            if device.name == GAME_CONTROLLER_NAME:
+                self.gamepad = device
 
+                return True
+        return False
+
+    def __get_controller_data(self):
+        if self.controller_acquired:
+            events = self.gamepad.read()
+
+            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):
+    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
+
+        # ########## 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
+
+        self.joystick = LogitechJoystick()
+        while not self.joystick.ready:
             self.msleep(100)
-        # TODO: Thread ending message here
 
-    def __publish_message(self):
+        # ########## Class Variables ##########
+        # Publishers
+        self.drive_command_publisher = rospy.Publisher(DEFAULT_DRIVE_COMMAND_TOPIC, DriveCommandMessage, queue_size=1)
+
+    def run(self):
+        while self.run_thread_flag:
+
+            start_time = time()
+
+            self.__update_and_publish()
+
+            time_diff = time() - start_time
+
+            self.msleep(max(self.wait_time - time_diff, 0))
+
+    def connect_signals_and_slots(self):
         pass
 
-    def setup_start_and_kill_signals(self, start_signal, signals_and_slots_signal, kill_signal):
+    def __update_and_publish(self):
+
+        # axis = self.joystick.controller_states["left_stick_y_axis"]
+        print self.joystick.controller_states
+
+    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
+        self.run_thread_flag = False

software/ground_station/Framework/MapSystems/RoverMapCoordinator.py

@@ -92,5 +92,4 @@ class RoverMapCoordinator(QtCore.QThread):
         kill_signal.connect(self.on_kill_threads_requested_slot)
 
     def pixmap_ready__slot(self):
-        self.logger.info("Made it")
         self.mapping_label.setPixmap(self.map_pixmap)

software/ground_station/Resources/Ui/right_screen.ui

@@ -120,6 +120,9 @@
                <property name="text">
                 <string/>
                </property>
+               <property name="alignment">
+                <set>Qt::AlignCenter</set>
+               </property>
               </widget>
              </item>
             </layout>

software/ground_station/RoverGroundStation.py

@@ -17,6 +17,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
 
 #####################################
 # Global Variables
@@ -99,6 +100,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.connect_signals_and_slots_signal.emit()
         self.__connect_signals_to_slots()
         self.start_threads_signal.emit()