caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-09 02:31:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Pan/tilt now works for both tower and chassis. Intelligently controllable and centerable using joystick. Added some error handling for map stuff. New firmware for chassis node.

Browse Source
This commit is contained in:
Corwin Perren
2018-07-19 18:42:55 -07:00
parent a573f72c1c
commit 39a7d482de
11 changed files with 455 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py
View File

@@ -16,7 +16,8 @@ from rover_control.msg import DriveCommandMessage, TowerPanTiltControlMessage
GAME_CONTROLLER_NAME = "Logitech Logitech Extreme 3D Pro"
DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_drive"
DEFAULT_PAN_TILT_COMMAND_TOPIC = "/rover_control/pan_tilt/control"
DEFAULT_TOWER_PAN_TILT_COMMAND_TOPIC = "/rover_control/tower/pan_tilt/control"
DEFAULT_CHASSIS_PAN_TILT_COMMAND_TOPIC = "/rover_control/chassis/pan_tilt/control"
DRIVE_COMMAND_HERTZ = 20
@@ -31,8 +32,11 @@ CAMERA_CHANGE_TIME = 0.2
GUI_ELEMENT_CHANGE_TIME = 0.2
CAMERA_TOGGLE_CHANGE_TIME = 0.35
PAN_TILT_X_AXIS_SCALAR = 3
PAN_TILT_Y_AXIS_SCALAR = 20
TOWER_PAN_TILT_X_AXIS_SCALAR = 2
TOWER_PAN_TILT_Y_AXIS_SCALAR = 15
CHASSIS_PAN_TILT_X_AXIS_SCALAR = 15
CHASSIS_PAN_TILT_Y_AXIS_SCALAR = 15
#####################################
@@ -67,12 +71,12 @@ class LogitechJoystick(QtCore.QThread):
"five_pressed": 0,
"six_pressed": 0,
"seven_pressed": 0,
"eight_pressed": 0,
"nine_pressed": 0,
"ten_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 = {
@@ -91,12 +95,12 @@ class LogitechJoystick(QtCore.QThread):
"BTN_TOP2": "five_pressed",
"BTN_PINKIE": "six_pressed",
"BTN_BASE": "seven_pressed",
"BTN_BASE2": "eight_pressed",
"BTN_BASE3": "nine_pressed",
"BTN_BASE4": "ten_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",
"BTN_BASE6": "twelve_pressed"
}
self.ready = False
@@ -125,6 +129,7 @@ class LogitechJoystick(QtCore.QThread):
events = self.gamepad.read()
for event in events:
# print event.code
if event.code in self.raw_mapping_to_class_mapping:
self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state
@@ -148,6 +153,7 @@ class JoystickControlSender(QtCore.QThread):
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.video_coordinator = self.shared_objects["threaded_classes"]["Video Coordinator"]
self.right_screen = self.shared_objects["screens"]["right_screen"]
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
@@ -167,7 +173,12 @@ class JoystickControlSender(QtCore.QThread):
# ########## Class Variables ##########
# Publishers
self.drive_command_publisher = rospy.Publisher(DEFAULT_DRIVE_COMMAND_TOPIC, DriveCommandMessage, queue_size=1)
self.tower_pan_tilt_command_publisher = rospy.Publisher(DEFAULT_PAN_TILT_COMMAND_TOPIC, TowerPanTiltControlMessage, queue_size=1)
self.tower_pan_tilt_command_publisher = rospy.Publisher(DEFAULT_TOWER_PAN_TILT_COMMAND_TOPIC,
TowerPanTiltControlMessage, queue_size=1)
self.chassis_pan_tilt_command_publisher = rospy.Publisher(DEFAULT_CHASSIS_PAN_TILT_COMMAND_TOPIC,
TowerPanTiltControlMessage, queue_size=1)
self.current_pan_tilt_selection = "no_pan_tilt"
self.last_hat_x_was_movement = False
self.last_hat_y_was_movement = False
@@ -183,7 +194,6 @@ class JoystickControlSender(QtCore.QThread):
def run(self):
while self.run_thread_flag:
start_time = time()
self.check_and_set_pause_state()
@@ -198,6 +208,8 @@ class JoystickControlSender(QtCore.QThread):
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)
self.video_coordinator.pan_tilt_selection_changed__signal.connect(self.on_pan_tilt_selection_changed__slot)
def check_and_set_pause_state(self):
thumb_pressed = self.joystick.controller_states["thumb_pressed"]
if thumb_pressed and (time() - self.last_pause_state_time) > PAUSE_STATE_CHANGE_TIME:
@@ -249,14 +261,31 @@ class JoystickControlSender(QtCore.QThread):
self.last_camera_toggle_time = time()
def publish_pan_tilt_control_commands(self):
button_eight = self.joystick.controller_states["seven_pressed"]
hat_x = self.joystick.controller_states["hat_x_axis"]
hat_y = self.joystick.controller_states["hat_y_axis"]
pan_tilt_message = TowerPanTiltControlMessage()
pan_tilt_message.relative_pan_adjustment = hat_x * PAN_TILT_X_AXIS_SCALAR
pan_tilt_message.relative_tilt_adjustment = -(hat_y * PAN_TILT_Y_AXIS_SCALAR)
if (hat_x == 0 and not self.last_hat_x_was_movement) and (
hat_y == 0 and not self.last_hat_y_was_movement) and not button_eight:
return
self.tower_pan_tilt_command_publisher.publish(pan_tilt_message)
self.last_hat_x_was_movement = True if hat_x != 0 else False
self.last_hat_y_was_movement = True if hat_y != 0 else False
pan_tilt_message = TowerPanTiltControlMessage()
if button_eight:
pan_tilt_message.should_center = 1
if self.current_pan_tilt_selection == "tower_pan_tilt":
pan_tilt_message.relative_pan_adjustment = hat_x * TOWER_PAN_TILT_X_AXIS_SCALAR
pan_tilt_message.relative_tilt_adjustment = -(hat_y * TOWER_PAN_TILT_Y_AXIS_SCALAR)
self.tower_pan_tilt_command_publisher.publish(pan_tilt_message)
elif self.current_pan_tilt_selection == "chassis_pan_tilt":
pan_tilt_message.relative_pan_adjustment = hat_x * CHASSIS_PAN_TILT_X_AXIS_SCALAR
pan_tilt_message.relative_tilt_adjustment = -(hat_y * CHASSIS_PAN_TILT_Y_AXIS_SCALAR)
self.chassis_pan_tilt_command_publisher.publish(pan_tilt_message)
def get_drive_message(self, throttle_axis):
drive_message = DriveCommandMessage()
@@ -278,6 +307,9 @@ class JoystickControlSender(QtCore.QThread):
return drive_message
def on_pan_tilt_selection_changed__slot(self, selection):
self.current_pan_tilt_selection = selection
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)
@@ -292,4 +324,4 @@ class JoystickControlSender(QtCore.QThread):
self.right_drive_progress_bar.setStyleSheet("background-color: transparent;")
def on_kill_threads_requested__slot(self):
self.run_thread_flag = False
self.run_thread_flag = False

28
software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py
View File

@@ -5,6 +5,7 @@
from PyQt5 import QtCore, QtWidgets, QtGui
from PIL.ImageQt import ImageQt
from PIL import Image
import numpy
import logging
@@ -145,20 +146,21 @@ class RoverMapCoordinator(QtCore.QThread):
def update_overlay(self):
if self.latitude and self.longitude:
longitude = self.latitude
latitude = self.longitude
if not numpy.isnan(self.latitude) and not numpy.isnan(self.longitude):
print self.longitude, " : ", self.latitude
navigation_list = self._get_table_elements(self.navigation_label)
# landmark_list = self._get_table_elements(self.landmark_label)
landmark_list = []
self.overlay_image = self.overlay_image_object.update_new_location(
latitude,
longitude,
70,
navigation_list,
landmark_list)
# self.overlay_image.save("something.png")
longitude = self.latitude
latitude = self.longitude
navigation_list = self._get_table_elements(self.navigation_label)
# landmark_list = self._get_table_elements(self.landmark_label)
landmark_list = []
self.overlay_image = self.overlay_image_object.update_new_location(
latitude,
longitude,
70,
navigation_list,
landmark_list)
# self.overlay_image.save("something.png")
def gps_position_updated_callback(self, data):
self.latitude = data.latitude

47
software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py
View File

@@ -36,6 +36,8 @@ class RoverVideoCoordinator(QtCore.QThread):
update_element_stylesheet__signal = QtCore.pyqtSignal()
pan_tilt_selection_changed__signal = QtCore.pyqtSignal(str)
def __init__(self, shared_objects):
super(RoverVideoCoordinator, self).__init__()
@@ -81,6 +83,9 @@ class RoverVideoCoordinator(QtCore.QThread):
self.msleep(3000)
# Setup cameras
self.main_nav_index = -1
self.chassis_index = -1
self.__get_cameras()
self.__setup_video_threads()
@@ -105,6 +110,8 @@ class RoverVideoCoordinator(QtCore.QThread):
def run(self):
self.logger.debug("Starting Video Coordinator Thread")
self.__broadcast_current_pan_tilt_selection()
while self.run_thread_flag:
self.__set_max_resolutions()
self.__toggle_background_cameras_if_needed()
@@ -115,6 +122,22 @@ class RoverVideoCoordinator(QtCore.QThread):
self.logger.debug("Stopping Video Coordinator Thread")
def __broadcast_current_pan_tilt_selection(self):
setting = None
if self.current_label_for_joystick_adjust == 0: # primary
setting = self.primary_label_current_setting
elif self.current_label_for_joystick_adjust == 1: # secondary
setting = self.secondary_label_current_setting
elif self.current_label_for_joystick_adjust == 2: # tertiary
setting = self.tertiary_label_current_setting
if setting == self.main_nav_index:
self.pan_tilt_selection_changed__signal.emit("tower_pan_tilt")
elif setting == self.chassis_index:
self.pan_tilt_selection_changed__signal.emit("chassis_pan_tilt")
else:
self.pan_tilt_selection_changed__signal.emit("no_pan_tilt")
def __set_max_resolutions(self):
if self.set_max_resolutions_flag:
self.camera_threads[self.valid_cameras[self.primary_label_current_setting]].set_hard_max_resolution(PRIMARY_LABEL_MAX)
@@ -178,7 +201,23 @@ class RoverVideoCoordinator(QtCore.QThread):
if camera in names:
names.remove(camera)
self.valid_cameras = list(names)
self.valid_cameras = []
current_count = 0
if "main_navigation" in names:
self.valid_cameras.append("main_navigation")
self.main_nav_index = current_count
current_count += 1
if "chassis" in names:
self.valid_cameras.append("chassis")
self.chassis_index = current_count
if "undercarriage" in names:
self.valid_cameras.append("undercarriage")
if "end_effector" in names:
self.valid_cameras.append("end_effector")
def __setup_video_threads(self):
for camera in self.valid_cameras:
@@ -275,16 +314,20 @@ class RoverVideoCoordinator(QtCore.QThread):
else:
self.current_label_for_joystick_adjust = new_selection
self.__broadcast_current_pan_tilt_selection()
self.update_element_stylesheet__signal.emit()
def on_camera_selection_for_current_gui_element_changed(self, direction):
if self.current_label_for_joystick_adjust == 0: # primary
if self.current_label_for_joystick_adjust == 0: # primary
self.primary_label_current_setting = (self.primary_label_current_setting + direction) % len(self.valid_cameras)
elif self.current_label_for_joystick_adjust == 1: # secondary
self.secondary_label_current_setting = (self.secondary_label_current_setting + direction) % len(self.valid_cameras)
elif self.current_label_for_joystick_adjust == 2: # tertiary
self.tertiary_label_current_setting = (self.tertiary_label_current_setting + direction) % len(self.valid_cameras)
self.__broadcast_current_pan_tilt_selection()
self.set_max_resolutions_flag = True
self.update_element_stylesheet__signal.emit()