diff --git a/software/ground_station/Framework/DriveSystems/RoverDriveSender.py b/software/ground_station/Framework/DriveSystems/RoverDriveSender.py
index 363808a..6b44a2b 100644
--- a/software/ground_station/Framework/DriveSystems/RoverDriveSender.py
+++ b/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
diff --git a/software/ground_station/Framework/MapSystems/RoverMapCoordinator.py b/software/ground_station/Framework/MapSystems/RoverMapCoordinator.py
index e2686a8..a9e1278 100644
--- a/software/ground_station/Framework/MapSystems/RoverMapCoordinator.py
+++ b/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)
diff --git a/software/ground_station/Resources/Ui/right_screen.ui b/software/ground_station/Resources/Ui/right_screen.ui
index 57df7bf..de7a2c0 100644
--- a/software/ground_station/Resources/Ui/right_screen.ui
+++ b/software/ground_station/Resources/Ui/right_screen.ui
@@ -120,6 +120,9 @@
+
+ Qt::AlignCenter
+
diff --git a/software/ground_station/RoverGroundStation.py b/software/ground_station/RoverGroundStation.py
index 2e7894a..e954d3b 100755
--- a/software/ground_station/RoverGroundStation.py
+++ b/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()