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

Changed readouts for arm, added readouts for gripper w/modes and extra controls. GUI reorganization.

Browse Source
This commit is contained in:
Corwin Perren
2018-08-07 17:31:28 -07:00
parent 82f02eef66
commit 05b4c85ad2
10 changed files with 4541 additions and 1313 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
software/firmware/tower_pan_tilt/tower_pan_tilt.ino
View File

@@ -39,7 +39,7 @@ uint8_t message_count = 0;
// Pan/tilt hard limits // Pan/tilt hard limits
const int pan_min = 1470; const int pan_min = 1470;
const int pan_center = 1600; const int pan_center = 1605;
const int pan_max = 1725; const int pan_max = 1725;
const int tilt_min = 1020; const int tilt_min = 1020;
@@ -57,7 +57,7 @@ Servo pan_servo;
Servo tilt_servo; Servo tilt_servo;
void setup() { void setup() {
// Serial.begin(9600); Serial.begin(9600);
// while(!Serial); // while(!Serial);
setup_hardware(); setup_hardware();
@@ -151,4 +151,3 @@ void set_pan_tilt_adjustments() {
modbus_data[MODBUS_REGISTERS::TILT_ADJUST_NEGATIVE] = 0; modbus_data[MODBUS_REGISTERS::TILT_ADJUST_NEGATIVE] = 0;
} }
} }

252
software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py
View File

@@ -8,23 +8,103 @@ import logging
import rospy import rospy
from rover_arm.msg import ArmStatusMessage from rover_arm.msg import ArmStatusMessage
from rover_control.msg import GripperStatusMessage
##################################### #####################################
# Global Variables # Global Variables
##################################### #####################################
ARM_STATUS_TOPIC = "/rover_arm/status" ARM_STATUS_TOPIC = "/rover_arm/status"
GRIPPER_STATUS_TOPIC = "/rover_control/gripper/status"
COMMS_TO_STRING = {
0: "NO STATUS",
1: "COMMS OK",
2: "NO DEVICE",
4: "BUS ERROR",
8: "GEN COMM ERROR",
16: "PARAMETER ERROR",
32: "LENGTH ERROR"
}
TARGET_REACHED_BIT_POSITION = 1
STATUS_TO_STRING = {
1: "TARGET REACHED",
2: "ERROR RECOVERY",
3: "RUN",
4: "ENABLED",
5: "FAULT STOP",
6: "WARNING",
7: "STO ACTIVE",
8: "SERVO READY",
10: "BRAKING",
11: "HOMING",
12: "INITIALIZED",
13: "VOLT OK",
15: "PERMANENT STOP"
}
FAULT_TO_STRING = {
1: "TRACKING ERROR",
2: "OVER CURRENT",
# 3: "COMMUNICATION ERROR", # Was showing even though things were working???
4: "ENCODER FAILURE",
5: "OVER TEMP",
6: "UNDER VOLTAGE",
7: "OVER VOLTAGE",
8: "PROG OR MEM ERROR",
9: "HARDWARE ERROR",
10: "OVER VELOCITY",
11: "INIT ERROR",
12: "MOTION ERROR",
13: "RANGE ERROR",
14: "POWER STAGE FORCED OFF",
15: "HOST COMM ERROR"
}
##################################### #####################################
# Controller Class Definition # Controller Class Definition
##################################### #####################################
class ArmIndication(QtCore.QObject): class ArmIndication(QtCore.QObject):
base_position_updated__signal = QtCore.pyqtSignal(int) base_position_updated__signal = QtCore.pyqtSignal(float)
shoulder_position_updated__signal = QtCore.pyqtSignal(int) shoulder_position_updated__signal = QtCore.pyqtSignal(float)
elbow_position_updated__signal = QtCore.pyqtSignal(int) elbow_position_updated__signal = QtCore.pyqtSignal(float)
roll_position_updated__signal = QtCore.pyqtSignal(int) roll_position_updated__signal = QtCore.pyqtSignal(float)
wrist_pitch_position_updated__signal = QtCore.pyqtSignal(int) wrist_pitch_position_updated__signal = QtCore.pyqtSignal(float)
wrist_roll_position_updated__signal = QtCore.pyqtSignal(int) wrist_roll_position_updated__signal = QtCore.pyqtSignal(float)
base_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
shoulder_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
elbow_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
roll_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
wrist_pitch_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
wrist_roll_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
base_status_update_ready__signal = QtCore.pyqtSignal(str)
shoulder_status_update_ready__signal = QtCore.pyqtSignal(str)
elbow_status_update_ready__signal = QtCore.pyqtSignal(str)
roll_status_update_ready__signal = QtCore.pyqtSignal(str)
wrist_pitch_status_update_ready__signal = QtCore.pyqtSignal(str)
wrist_roll_status_update_ready__signal = QtCore.pyqtSignal(str)
base_faults_update_ready__signal = QtCore.pyqtSignal(str)
shoulder_faults_update_ready__signal = QtCore.pyqtSignal(str)
elbow_faults_update_ready__signal = QtCore.pyqtSignal(str)
roll_faults_update_ready__signal = QtCore.pyqtSignal(str)
wrist_pitch_faults_update_ready__signal = QtCore.pyqtSignal(str)
wrist_roll_faults_update_ready__signal = QtCore.pyqtSignal(str)
pinch_position_updated__signal = QtCore.pyqtSignal(int)
forefinger_position_updated__signal = QtCore.pyqtSignal(int)
thumb_position_updated__signal = QtCore.pyqtSignal(int)
middlefinger_position_updated__signal = QtCore.pyqtSignal(int)
pinch_current_updated__signal = QtCore.pyqtSignal(int)
forefinger_current_updated__signal = QtCore.pyqtSignal(int)
thumb_current_updated__signal = QtCore.pyqtSignal(int)
middlefinger_current_updated__signal = QtCore.pyqtSignal(int)
def __init__(self, shared_objects): def __init__(self, shared_objects):
super(ArmIndication, self).__init__() super(ArmIndication, self).__init__()
@@ -33,12 +113,45 @@ class ArmIndication(QtCore.QObject):
self.shared_objects = shared_objects self.shared_objects = shared_objects
self.right_screen = self.shared_objects["screens"]["right_screen"] self.right_screen = self.shared_objects["screens"]["right_screen"]
self.base_rotation_dial = self.right_screen.base_rotation_dial # type: QtWidgets.QDial self.base_position_lcd_number = self.right_screen.base_position_lcd_number # type: QtWidgets.QLCDNumber
self.shoulder_pitch_dial = self.right_screen.shoulder_pitch_dial # type: QtWidgets.QDial self.shoulder_position_lcd_number = self.right_screen.shoulder_position_lcd_number # type: QtWidgets.QLCDNumber
self.elbow_pitch_dial = self.right_screen.elbow_pitch_dial # type: QtWidgets.QDial self.elbow_position_lcd_number = self.right_screen.elbow_position_lcd_number # type: QtWidgets.QLCDNumber
self.elbow_roll_dial = self.right_screen.elbow_roll_dial # type: QtWidgets.QDial self.roll_position_lcd_number = self.right_screen.roll_position_lcd_number # type: QtWidgets.QLCDNumber
self.end_effector_pitch_dial = self.right_screen.end_effector_pitch_dial # type: QtWidgets.QDial self.wrist_pitch_position_lcd_number = self.right_screen.wrist_pitch_position_lcd_number # type: QtWidgets.QLCDNumber
self.end_effector_rotation_dial = self.right_screen.end_effector_rotation_dial # type: QtWidgets.QDial self.wrist_roll_position_lcd_number = self.right_screen.wrist_roll_position_lcd_number # type: QtWidgets.QLCDNumber
self.pinch_position_lcd_number = self.right_screen.pinch_position_lcd_number # type: QtWidgets.QLCDNumber
self.forefinger_position_lcd_number = self.right_screen.forefinger_position_lcd_number # type: QtWidgets.QLCDNumber
self.thumb_position_lcd_number = self.right_screen.thumb_position_lcd_number # type: QtWidgets.QLCDNumber
self.middlefinger_position_lcd_number = self.right_screen.middlefinger_position_lcd_number # type: QtWidgets.QLCDNumber
self.pinch_current_lcd_number = self.right_screen.pinch_current_lcd_number # type: QtWidgets.QLCDNumber
self.forefinger_current_lcd_number = self.right_screen.forefinger_current_lcd_number # type: QtWidgets.QLCDNumber
self.thumb_current_lcd_number = self.right_screen.thumb_current_lcd_number # type: QtWidgets.QLCDNumber
self.middlefinger_current_lcd_number = self.right_screen.middlefinger_current_lcd_number # type: QtWidgets.QLCDNumber
self.arm_controls_base_comms_label = self.right_screen.arm_controls_base_comms_label # type:QtWidgets.QLabel
self.arm_controls_base_status_label = self.right_screen.arm_controls_base_status_label # type:QtWidgets.QLabel
self.arm_controls_base_faults_label = self.right_screen.arm_controls_base_faults_label # type:QtWidgets.QLabel
self.arm_controls_shoulder_comms_label = self.right_screen.arm_controls_shoulder_comms_label # type:QtWidgets.QLabel
self.arm_controls_shoulder_status_label = self.right_screen.arm_controls_shoulder_status_label # type:QtWidgets.QLabel
self.arm_controls_shoulder_faults_label = self.right_screen.arm_controls_shoulder_faults_label # type:QtWidgets.QLabel
self.arm_controls_elbow_comms_label = self.right_screen.arm_controls_elbow_comms_label # type:QtWidgets.QLabel
self.arm_controls_elbow_status_label = self.right_screen.arm_controls_elbow_status_label # type:QtWidgets.QLabel
self.arm_controls_elbow_faults_label = self.right_screen.arm_controls_elbow_faults_label # type:QtWidgets.QLabel
self.arm_controls_roll_comms_label = self.right_screen.arm_controls_roll_comms_label # type:QtWidgets.QLabel
self.arm_controls_roll_status_label = self.right_screen.arm_controls_roll_status_label # type:QtWidgets.QLabel
self.arm_controls_roll_faults_label = self.right_screen.arm_controls_roll_faults_label # type:QtWidgets.QLabel
self.arm_controls_wrist_pitch_comms_label = self.right_screen.arm_controls_wrist_pitch_comms_label # type:QtWidgets.QLabel
self.arm_controls_wrist_pitch_status_label = self.right_screen.arm_controls_wrist_pitch_status_label # type:QtWidgets.QLabel
self.arm_controls_wrist_pitch_faults_label = self.right_screen.arm_controls_wrist_pitch_faults_label # type:QtWidgets.QLabel
self.arm_controls_wrist_roll_comms_label = self.right_screen.arm_controls_wrist_roll_comms_label # type:QtWidgets.QLabel
self.arm_controls_wrist_roll_status_label = self.right_screen.arm_controls_wrist_roll_status_label # type:QtWidgets.QLabel
self.arm_controls_wrist_roll_faults_label = self.right_screen.arm_controls_wrist_roll_faults_label # type:QtWidgets.QLabel
# ########## Get the settings instance ########## # ########## Get the settings instance ##########
self.settings = QtCore.QSettings() self.settings = QtCore.QSettings()
@@ -48,24 +161,115 @@ class ArmIndication(QtCore.QObject):
# ########## Class Variables ########## # ########## Class Variables ##########
self.arm_status_subscriber = rospy.Subscriber(ARM_STATUS_TOPIC, ArmStatusMessage, self.on_arm_status_update_received__callback) self.arm_status_subscriber = rospy.Subscriber(ARM_STATUS_TOPIC, ArmStatusMessage, self.on_arm_status_update_received__callback)
self.gripper_status_subscriver = rospy.Subscriber(GRIPPER_STATUS_TOPIC, GripperStatusMessage, self.on_gripper_status_update_received__callback)
# ########## Connect Signals and Slots ########## # ########## Connect Signals and Slots ##########
self.connect_signals_and_slots() self.connect_signals_and_slots()
def connect_signals_and_slots(self): def connect_signals_and_slots(self):
self.base_position_updated__signal.connect(self.base_rotation_dial.setValue) self.base_position_updated__signal.connect(self.base_position_lcd_number.display)
self.shoulder_position_updated__signal.connect(self.shoulder_pitch_dial.setValue) self.shoulder_position_updated__signal.connect(self.shoulder_position_lcd_number.display)
self.elbow_position_updated__signal.connect(self.elbow_pitch_dial.setValue) self.elbow_position_updated__signal.connect(self.elbow_position_lcd_number.display)
self.roll_position_updated__signal.connect(self.elbow_roll_dial.setValue) self.roll_position_updated__signal.connect(self.roll_position_lcd_number.display)
self.wrist_pitch_position_updated__signal.connect(self.end_effector_pitch_dial.setValue) self.wrist_pitch_position_updated__signal.connect(self.wrist_pitch_position_lcd_number.display)
self.wrist_roll_position_updated__signal.connect(self.end_effector_rotation_dial.setValue) self.wrist_roll_position_updated__signal.connect(self.wrist_roll_position_lcd_number.display)
self.base_comms_state_update_ready__signal.connect(self.arm_controls_base_comms_label.setText)
self.shoulder_comms_state_update_ready__signal.connect(self.arm_controls_shoulder_comms_label.setText)
self.elbow_comms_state_update_ready__signal.connect(self.arm_controls_elbow_comms_label.setText)
self.roll_comms_state_update_ready__signal.connect(self.arm_controls_roll_comms_label.setText)
self.wrist_pitch_comms_state_update_ready__signal.connect(self.arm_controls_wrist_pitch_comms_label.setText)
self.wrist_roll_comms_state_update_ready__signal.connect(self.arm_controls_wrist_roll_comms_label.setText)
self.base_status_update_ready__signal.connect(self.arm_controls_base_status_label.setText)
self.shoulder_status_update_ready__signal.connect(self.arm_controls_shoulder_status_label.setText)
self.elbow_status_update_ready__signal.connect(self.arm_controls_elbow_status_label.setText)
self.roll_status_update_ready__signal.connect(self.arm_controls_roll_status_label.setText)
self.wrist_pitch_status_update_ready__signal.connect(self.arm_controls_wrist_pitch_status_label.setText)
self.wrist_roll_status_update_ready__signal.connect(self.arm_controls_wrist_roll_status_label.setText)
self.base_faults_update_ready__signal.connect(self.arm_controls_base_faults_label.setText)
self.shoulder_faults_update_ready__signal.connect(self.arm_controls_shoulder_faults_label.setText)
self.elbow_faults_update_ready__signal.connect(self.arm_controls_elbow_faults_label.setText)
self.roll_faults_update_ready__signal.connect(self.arm_controls_roll_faults_label.setText)
self.wrist_pitch_faults_update_ready__signal.connect(self.arm_controls_wrist_pitch_faults_label.setText)
self.wrist_roll_faults_update_ready__signal.connect(self.arm_controls_wrist_roll_faults_label.setText)
self.pinch_position_updated__signal.connect(self.pinch_position_lcd_number.display)
self.forefinger_position_updated__signal.connect(self.forefinger_position_lcd_number.display)
self.thumb_position_updated__signal.connect(self.thumb_position_lcd_number.display)
self.middlefinger_position_updated__signal.connect(self.middlefinger_position_lcd_number.display)
self.pinch_current_updated__signal.connect(self.pinch_current_lcd_number.display)
self.forefinger_current_updated__signal.connect(self.forefinger_current_lcd_number.display)
self.thumb_current_updated__signal.connect(self.thumb_current_lcd_number.display)
self.middlefinger_current_updated__signal.connect(self.middlefinger_current_lcd_number.display)
def on_arm_status_update_received__callback(self, data): def on_arm_status_update_received__callback(self, data):
self.base_position_updated__signal.emit(int(data.base * 1000)) self.base_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.base_comm_status))
self.shoulder_position_updated__signal.emit(int(data.shoulder * 1000)) self.shoulder_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.shoulder_comm_status))
self.elbow_position_updated__signal.emit(int(data.elbow * 1000)) self.elbow_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.elbow_comm_status))
self.roll_position_updated__signal.emit(int(data.roll * 1000)) self.roll_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.roll_comm_status))
self.wrist_pitch_position_updated__signal.emit(int(data.wrist_pitch * 1000)) self.wrist_pitch_comms_state_update_ready__signal.emit(
self.wrist_roll_position_updated__signal.emit(int(data.wrist_roll * 1000)) self.process_comms_to_string(data.wrist_pitch_comm_status))
self.wrist_roll_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.wrist_roll_comm_status))
self.base_status_update_ready__signal.emit(self.process_statuses_to_string(data.base_status))
self.shoulder_status_update_ready__signal.emit(self.process_statuses_to_string(data.shoulder_status))
self.elbow_status_update_ready__signal.emit(self.process_statuses_to_string(data.elbow_status))
self.roll_status_update_ready__signal.emit(self.process_statuses_to_string(data.roll_status))
self.wrist_pitch_status_update_ready__signal.emit(self.process_statuses_to_string(data.wrist_pitch_status))
self.wrist_roll_status_update_ready__signal.emit(self.process_statuses_to_string(data.wrist_roll_status))
self.base_faults_update_ready__signal.emit(self.process_faults_to_string(data.base_faults))
self.shoulder_faults_update_ready__signal.emit(self.process_faults_to_string(data.shoulder_faults))
self.elbow_faults_update_ready__signal.emit(self.process_faults_to_string(data.elbow_faults))
self.roll_faults_update_ready__signal.emit(self.process_faults_to_string(data.roll_faults))
self.wrist_pitch_faults_update_ready__signal.emit(self.process_faults_to_string(data.wrist_pitch_faults))
self.wrist_roll_faults_update_ready__signal.emit(self.process_faults_to_string(data.wrist_roll_faults))
self.base_position_updated__signal.emit(data.base)
self.shoulder_position_updated__signal.emit(data.shoulder)
self.elbow_position_updated__signal.emit(data.elbow)
self.roll_position_updated__signal.emit(data.roll)
self.wrist_pitch_position_updated__signal.emit(data.wrist_pitch)
self.wrist_roll_position_updated__signal.emit(data.wrist_roll)
def on_gripper_status_update_received__callback(self, data):
data = data # type: GripperStatusMessage
self.pinch_position_updated__signal.emit(data.pinch_position_raw)
self.forefinger_position_updated__signal.emit(data.forefinger_position_raw)
self.thumb_position_updated__signal.emit(data.thumb_position_raw)
self.middlefinger_position_updated__signal.emit(data.middlefinger_position_raw)
self.pinch_current_updated__signal.emit(data.pinch_current)
self.forefinger_current_updated__signal.emit(data.forefinger_current)
self.thumb_current_updated__signal.emit(data.thumb_current)
self.middlefinger_current_updated__signal.emit(data.middlefinger_current)
@staticmethod
def process_faults_to_string(faults):
fault_output = ""
for bit_position in FAULT_TO_STRING:
if (1 << bit_position) & faults:
fault_output += FAULT_TO_STRING[bit_position] + "\n"
return fault_output[:-1]
@staticmethod
def process_statuses_to_string(statuses):
status_output = ""
for bit_position in STATUS_TO_STRING:
if (1 << bit_position) & statuses:
status_output += STATUS_TO_STRING[bit_position] + "\n"
return status_output[:-1]
@staticmethod
def process_comms_to_string(comms):
return COMMS_TO_STRING[comms] if comms in COMMS_TO_STRING else "UNKNOWN"

127
software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py
View File

@@ -160,6 +160,14 @@ class XBOXController(QtCore.QThread):
# Controller Class Definition # Controller Class Definition
##################################### #####################################
class XBOXControllerControlSender(QtCore.QThread): class XBOXControllerControlSender(QtCore.QThread):
xbox_control_arm_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
xbox_control_mining_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
gripper_mode_normal_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
gripper_mode_pinch_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
gripper_mode_wide_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
gripper_mode_scissor_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
XBOX_CONTROL_STATES = [ XBOX_CONTROL_STATES = [
"ARM", "ARM",
"MINING" "MINING"
@@ -172,17 +180,20 @@ class XBOXControllerControlSender(QtCore.QThread):
"SCISSOR": 4 "SCISSOR": 4
} }
xbox_control_arm_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
xbox_control_mining_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
def __init__(self, shared_objects): def __init__(self, shared_objects):
super(XBOXControllerControlSender, self).__init__() super(XBOXControllerControlSender, self).__init__()
# ########## Reference to class init variables ########## # ########## Reference to class init variables ##########
self.shared_objects = shared_objects self.shared_objects = shared_objects
self.left_screen = self.shared_objects["screens"]["left_screen"] self.left_screen = self.shared_objects["screens"]["left_screen"]
self.xbox_mode_arm_label = self.left_screen.xbox_mode_arm_label # type: QtWidgets.QLabel self.right_screen = self.shared_objects["screens"]["right_screen"]
self.xbox_mode_mining_label = self.left_screen.xbox_mode_mining_label # type: QtWidgets.QLabel self.xbox_mode_arm_label = self.right_screen.xbox_mode_arm_label # type: QtWidgets.QLabel
self.xbox_mode_mining_label = self.right_screen.xbox_mode_mining_label # type: QtWidgets.QLabel
self.gripper_mode_normal_label = self.right_screen.gripper_mode_normal_label # type: QtWidgets.QLabel
self.gripper_mode_pinch_label = self.right_screen.gripper_mode_pinch_label # type: QtWidgets.QLabel
self.gripper_mode_wide_label = self.right_screen.gripper_mode_wide_label # type: QtWidgets.QLabel
self.gripper_mode_scissor_label = self.right_screen.gripper_mode_scissor_label # type: QtWidgets.QLabel
# ########## Get the settings instance ########## # ########## Get the settings instance ##########
self.settings = QtCore.QSettings() self.settings = QtCore.QSettings()
@@ -205,14 +216,46 @@ class XBOXControllerControlSender(QtCore.QThread):
queue_size=1) queue_size=1)
self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1) self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
self.current_state = self.XBOX_CONTROL_STATES.index("ARM") self.xbox_current_control_state = self.XBOX_CONTROL_STATES.index("ARM")
self.xbox_control_state_just_changed = False self.xbox_control_state_just_changed = False
self.last_xbox_button_state = 0 self.last_xbox_button_state = 0
self.last_left_bumper_state = 0 self.last_left_bumper_state = 0
self.last_right_bumper_state = 0 self.last_right_bumper_state = 0
self.gripper_control_mode = self.GRIPPER_CONTROL_MODES["NORMAL"] self.gripper_control_mode = 0
self.gripper_control_mode_just_changed = False
self.send_new_gripper_mode = False
self.GRIPPER_DISPLAY_SIGNAL_MAPPING = {
self.GRIPPER_CONTROL_MODES["NORMAL"]: {
"SET": [self.gripper_mode_normal_stylesheet_update_ready__signal],
"UNSET": [self.gripper_mode_pinch_stylesheet_update_ready__signal,
self.gripper_mode_wide_stylesheet_update_ready__signal,
self.gripper_mode_scissor_stylesheet_update_ready__signal]
},
self.GRIPPER_CONTROL_MODES["TWO_FINGER_PINCH"]: {
"SET": [self.gripper_mode_pinch_stylesheet_update_ready__signal],
"UNSET": [self.gripper_mode_normal_stylesheet_update_ready__signal,
self.gripper_mode_wide_stylesheet_update_ready__signal,
self.gripper_mode_scissor_stylesheet_update_ready__signal]
},
self.GRIPPER_CONTROL_MODES["WIDE"]: {
"SET": [self.gripper_mode_wide_stylesheet_update_ready__signal],
"UNSET": [self.gripper_mode_pinch_stylesheet_update_ready__signal,
self.gripper_mode_normal_stylesheet_update_ready__signal,
self.gripper_mode_scissor_stylesheet_update_ready__signal]
},
self.GRIPPER_CONTROL_MODES["SCISSOR"]: {
"SET": [self.gripper_mode_scissor_stylesheet_update_ready__signal],
"UNSET": [self.gripper_mode_pinch_stylesheet_update_ready__signal,
self.gripper_mode_wide_stylesheet_update_ready__signal,
self.gripper_mode_normal_stylesheet_update_ready__signal]
}
}
def run(self): def run(self):
self.logger.debug("Starting Joystick Thread") self.logger.debug("Starting Joystick Thread")
@@ -222,9 +265,9 @@ class XBOXControllerControlSender(QtCore.QThread):
self.change_control_state_if_needed() self.change_control_state_if_needed()
if self.current_state == self.XBOX_CONTROL_STATES.index("ARM"): if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"):
self.process_and_send_arm_control() self.process_and_send_arm_control()
elif self.current_state == self.XBOX_CONTROL_STATES.index("MINING"): elif self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("MINING"):
self.send_mining_commands() self.send_mining_commands()
time_diff = time() - start_time time_diff = time() - start_time
@@ -237,43 +280,62 @@ class XBOXControllerControlSender(QtCore.QThread):
self.xbox_control_arm_stylesheet_update_ready__signal.connect(self.xbox_mode_arm_label.setStyleSheet) self.xbox_control_arm_stylesheet_update_ready__signal.connect(self.xbox_mode_arm_label.setStyleSheet)
self.xbox_control_mining_stylesheet_update_ready__signal.connect(self.xbox_mode_mining_label.setStyleSheet) self.xbox_control_mining_stylesheet_update_ready__signal.connect(self.xbox_mode_mining_label.setStyleSheet)
self.gripper_mode_normal_stylesheet_update_ready__signal.connect(self.gripper_mode_normal_label.setStyleSheet)
self.gripper_mode_pinch_stylesheet_update_ready__signal.connect(self.gripper_mode_pinch_label.setStyleSheet)
self.gripper_mode_wide_stylesheet_update_ready__signal.connect(self.gripper_mode_wide_label.setStyleSheet)
self.gripper_mode_scissor_stylesheet_update_ready__signal.connect(self.gripper_mode_scissor_label.setStyleSheet)
def change_control_state_if_needed(self): def change_control_state_if_needed(self):
xbox_state = self.controller.controller_states["xbox_button"] xbox_state = self.controller.controller_states["xbox_button"]
left_bumper_state = self.controller.controller_states["left_bumper"] left_bumper_state = self.controller.controller_states["left_bumper"]
right_bumper_state = self.controller.controller_states["right_bumper"] right_bumper_state = self.controller.controller_states["right_bumper"]
if self.last_xbox_button_state == 0 and xbox_state == 1: if self.last_xbox_button_state == 0 and xbox_state == 1:
self.current_state += 1 self.xbox_current_control_state += 1
self.current_state = self.current_state % len(self.XBOX_CONTROL_STATES) self.xbox_current_control_state = self.xbox_current_control_state % len(self.XBOX_CONTROL_STATES)
self.xbox_control_state_just_changed = True self.xbox_control_state_just_changed = True
self.last_xbox_button_state = 1 self.last_xbox_button_state = 1
elif self.last_xbox_button_state == 1 and xbox_state == 0: elif self.last_xbox_button_state == 1 and xbox_state == 0:
self.last_xbox_button_state = 0 self.last_xbox_button_state = 0
if self.last_left_bumper_state == 0 and left_bumper_state == 1:
self.gripper_control_mode = ((self.gripper_control_mode - 1) % len(self.GRIPPER_CONTROL_MODES))
# self.gripper_control_mode += 1
self.last_left_bumper_state = 1
elif self.last_left_bumper_state == 1 and left_bumper_state == 0:
self.last_left_bumper_state = 0
if self.last_right_bumper_state == 0 and right_bumper_state == 1:
self.gripper_control_mode = ((self.gripper_control_mode + 1) % len(self.GRIPPER_CONTROL_MODES))
# self.gripper_control_mode += 1
self.last_right_bumper_state = 1
elif self.last_right_bumper_state == 1 and right_bumper_state == 0:
self.last_right_bumper_state = 0
if self.xbox_control_state_just_changed: if self.xbox_control_state_just_changed:
self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_NONE) if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"):
self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_GREEN) self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_GREEN)
self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_NONE)
elif self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("MINING"):
self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_NONE)
self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_GREEN)
self.xbox_control_state_just_changed = False self.xbox_control_state_just_changed = False
if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"):
if self.last_left_bumper_state == 0 and left_bumper_state == 1:
self.gripper_control_mode = ((self.gripper_control_mode - 1) % len(self.GRIPPER_CONTROL_MODES))
self.gripper_control_mode_just_changed = True
self.last_left_bumper_state = 1
elif self.last_left_bumper_state == 1 and left_bumper_state == 0:
self.last_left_bumper_state = 0
if self.last_right_bumper_state == 0 and right_bumper_state == 1:
self.gripper_control_mode = ((self.gripper_control_mode + 1) % len(self.GRIPPER_CONTROL_MODES))
self.gripper_control_mode_just_changed = True
self.last_right_bumper_state = 1
elif self.last_right_bumper_state == 1 and right_bumper_state == 0:
self.last_right_bumper_state = 0
if self.gripper_control_mode_just_changed:
signal_map = self.GRIPPER_DISPLAY_SIGNAL_MAPPING[self.gripper_control_mode + 1]
for signal in signal_map["SET"]:
signal.emit(COLOR_GREEN)
for signal in signal_map["UNSET"]:
signal.emit(COLOR_NONE)
self.send_new_gripper_mode = True
self.gripper_control_mode_just_changed = False
def process_and_send_arm_control(self): def process_and_send_arm_control(self):
if self.xbox_control_state_just_changed:
self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_GREEN)
self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_NONE)
self.xbox_control_state_just_changed = False
arm_control_message = ArmControlMessage() arm_control_message = ArmControlMessage()
@@ -282,7 +344,7 @@ class XBOXControllerControlSender(QtCore.QThread):
gripper_control_message.gripper_mode = self.gripper_control_mode + 1 gripper_control_message.gripper_mode = self.gripper_control_mode + 1
should_publish_arm = False should_publish_arm = False
should_publish_gripper = False should_publish_gripper = True if self.send_new_gripper_mode else False
left_trigger = self.controller.controller_states["left_trigger"] left_trigger = self.controller.controller_states["left_trigger"]
right_trigger = self.controller.controller_states["right_trigger"] right_trigger = self.controller.controller_states["right_trigger"]
@@ -322,6 +384,7 @@ class XBOXControllerControlSender(QtCore.QThread):
if should_publish_gripper: if should_publish_gripper:
self.gripper_control_publisher.publish(gripper_control_message) self.gripper_control_publisher.publish(gripper_control_message)
self.send_new_gripper_mode = False
def send_mining_commands(self): def send_mining_commands(self):

180
software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py
View File

@@ -9,6 +9,7 @@ import rospy
from time import time from time import time
from rover_arm.msg import ArmControlMessage, ArmStatusMessage from rover_arm.msg import ArmControlMessage, ArmStatusMessage
from rover_control.msg import GripperControlMessage
##################################### #####################################
# Global Variables # Global Variables
@@ -17,54 +18,10 @@ ARM_RELATIVE_CONTROL_TOPIC = "/rover_arm/control/relative"
ARM_ABSOLUTE_CONTROL_TOPIC = "/rover_arm/control/absolute" ARM_ABSOLUTE_CONTROL_TOPIC = "/rover_arm/control/absolute"
ARM_STATUS_TOPIC = "/rover_arm/status" ARM_STATUS_TOPIC = "/rover_arm/status"
GRIPPER_CONTROL_TOPIC = "/rover_control/gripper/control"
THREAD_HERTZ = 5 THREAD_HERTZ = 5
COMMS_TO_STRING = {
0: "NO STATUS",
1: "COMMS OK",
2: "NO DEVICE",
4: "BUS ERROR",
8: "GEN COMM ERROR",
16: "PARAMETER ERROR",
32: "LENGTH ERROR"
}
TARGET_REACHED_BIT_POSITION = 1
STATUS_TO_STRING = {
1: "TARGET REACHED",
2: "ERROR RECOVERY",
3: "RUN",
4: "ENABLED",
5: "FAULT STOP",
6: "WARNING",
7: "STO ACTIVE",
8: "SERVO READY",
10: "BRAKING",
11: "HOMING",
12: "INITIALIZED",
13: "VOLT OK",
15: "PERMANENT STOP"
}
FAULT_TO_STRING = {
1: "TRACKING ERROR",
2: "OVER CURRENT",
# 3: "COMMUNICATION ERROR", # Was showing even though things were working???
4: "ENCODER FAILURE",
5: "OVER TEMP",
6: "UNDER VOLTAGE",
7: "OVER VOLTAGE",
8: "PROG OR MEM ERROR",
9: "HARDWARE ERROR",
10: "OVER VELOCITY",
11: "INIT ERROR",
12: "MOTION ERROR",
13: "RANGE ERROR",
14: "POWER STAGE FORCED OFF",
15: "HOST COMM ERROR"
}
POSITIONAL_TOLERANCE = 0.02 POSITIONAL_TOLERANCE = 0.02
# Order is [base, shoulder, elbow, roll, wrist_pitch, wrist_roll] # Order is [base, shoulder, elbow, roll, wrist_pitch, wrist_roll]
@@ -87,27 +44,6 @@ ARM_UNSTOW_PROCEDURE = [
# UbiquitiRadioSettings Class Definition # UbiquitiRadioSettings Class Definition
##################################### #####################################
class MiscArm(QtCore.QThread): class MiscArm(QtCore.QThread):
base_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
shoulder_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
elbow_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
roll_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
wrist_pitch_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
wrist_roll_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
base_status_update_ready__signal = QtCore.pyqtSignal(str)
shoulder_status_update_ready__signal = QtCore.pyqtSignal(str)
elbow_status_update_ready__signal = QtCore.pyqtSignal(str)
roll_status_update_ready__signal = QtCore.pyqtSignal(str)
wrist_pitch_status_update_ready__signal = QtCore.pyqtSignal(str)
wrist_roll_status_update_ready__signal = QtCore.pyqtSignal(str)
base_faults_update_ready__signal = QtCore.pyqtSignal(str)
shoulder_faults_update_ready__signal = QtCore.pyqtSignal(str)
elbow_faults_update_ready__signal = QtCore.pyqtSignal(str)
roll_faults_update_ready__signal = QtCore.pyqtSignal(str)
wrist_pitch_faults_update_ready__signal = QtCore.pyqtSignal(str)
wrist_roll_faults_update_ready__signal = QtCore.pyqtSignal(str)
def __init__(self, shared_objects): def __init__(self, shared_objects):
super(MiscArm, self).__init__() super(MiscArm, self).__init__()
@@ -123,28 +59,8 @@ class MiscArm(QtCore.QThread):
self.arm_controls_clear_faults_button = self.left_screen.arm_controls_clear_faults_button # type:QtWidgets.QPushButton self.arm_controls_clear_faults_button = self.left_screen.arm_controls_clear_faults_button # type:QtWidgets.QPushButton
self.arm_controls_reset_motor_drivers_button = self.left_screen.arm_controls_reset_motor_drivers_button # type:QtWidgets.QPushButton self.arm_controls_reset_motor_drivers_button = self.left_screen.arm_controls_reset_motor_drivers_button # type:QtWidgets.QPushButton
self.arm_controls_base_comms_label = self.left_screen.arm_controls_base_comms_label # type:QtWidgets.QLabel self.gripper_home_button = self.left_screen.gripper_home_button # type:QtWidgets.QPushButton
self.arm_controls_base_status_label = self.left_screen.arm_controls_base_status_label # type:QtWidgets.QLabel self.gripper_toggle_light_button = self.left_screen.gripper_toggle_light_button # type:QtWidgets.QPushButton
self.arm_controls_base_faults_label = self.left_screen.arm_controls_base_faults_label # type:QtWidgets.QLabel
self.arm_controls_shoulder_comms_label = self.left_screen.arm_controls_shoulder_comms_label # type:QtWidgets.QLabel
self.arm_controls_shoulder_status_label = self.left_screen.arm_controls_shoulder_status_label # type:QtWidgets.QLabel
self.arm_controls_shoulder_faults_label = self.left_screen.arm_controls_shoulder_faults_label # type:QtWidgets.QLabel
self.arm_controls_elbow_comms_label = self.left_screen.arm_controls_elbow_comms_label # type:QtWidgets.QLabel
self.arm_controls_elbow_status_label = self.left_screen.arm_controls_elbow_status_label # type:QtWidgets.QLabel
self.arm_controls_elbow_faults_label = self.left_screen.arm_controls_elbow_faults_label # type:QtWidgets.QLabel
self.arm_controls_roll_comms_label = self.left_screen.arm_controls_roll_comms_label # type:QtWidgets.QLabel
self.arm_controls_roll_status_label = self.left_screen.arm_controls_roll_status_label # type:QtWidgets.QLabel
self.arm_controls_roll_faults_label = self.left_screen.arm_controls_roll_faults_label # type:QtWidgets.QLabel
self.arm_controls_wrist_pitch_comms_label = self.left_screen.arm_controls_wrist_pitch_comms_label # type:QtWidgets.QLabel
self.arm_controls_wrist_pitch_status_label = self.left_screen.arm_controls_wrist_pitch_status_label # type:QtWidgets.QLabel
self.arm_controls_wrist_pitch_faults_label = self.left_screen.arm_controls_wrist_pitch_faults_label # type:QtWidgets.QLabel
self.arm_controls_wrist_roll_comms_label = self.left_screen.arm_controls_wrist_roll_comms_label # type:QtWidgets.QLabel
self.arm_controls_wrist_roll_status_label = self.left_screen.arm_controls_wrist_roll_status_label # type:QtWidgets.QLabel
self.arm_controls_wrist_roll_faults_label = self.left_screen.arm_controls_wrist_roll_faults_label # type:QtWidgets.QLabel
# ########## Get the settings instance ########## # ########## Get the settings instance ##########
self.settings = QtCore.QSettings() self.settings = QtCore.QSettings()
@@ -166,6 +82,8 @@ class MiscArm(QtCore.QThread):
self.arm_absolute_control_publisher = rospy.Publisher(ARM_ABSOLUTE_CONTROL_TOPIC, ArmControlMessage, self.arm_absolute_control_publisher = rospy.Publisher(ARM_ABSOLUTE_CONTROL_TOPIC, ArmControlMessage,
queue_size=1) queue_size=1)
self.gripper_control_publisher = rospy.Publisher(GRIPPER_CONTROL_TOPIC, GripperControlMessage, queue_size=1)
self.base_position = 0 self.base_position = 0
self.shoulder_position = 0 self.shoulder_position = 0
self.elbow_position = 0 self.elbow_position = 0
@@ -258,26 +176,8 @@ class MiscArm(QtCore.QThread):
self.arm_controls_clear_faults_button.clicked.connect(self.on_clear_faults_button_pressed__slot) self.arm_controls_clear_faults_button.clicked.connect(self.on_clear_faults_button_pressed__slot)
self.arm_controls_reset_motor_drivers_button.clicked.connect(self.on_reset_drivers_button_pressed__slot) self.arm_controls_reset_motor_drivers_button.clicked.connect(self.on_reset_drivers_button_pressed__slot)
self.base_comms_state_update_ready__signal.connect(self.arm_controls_base_comms_label.setText) self.gripper_home_button.clicked.connect(self.on_gripper_home_pressed)
self.shoulder_comms_state_update_ready__signal.connect(self.arm_controls_shoulder_comms_label.setText) self.gripper_toggle_light_button.clicked.connect(self.on_gripper_toggle_light_pressed)
self.elbow_comms_state_update_ready__signal.connect(self.arm_controls_elbow_comms_label.setText)
self.roll_comms_state_update_ready__signal.connect(self.arm_controls_roll_comms_label.setText)
self.wrist_pitch_comms_state_update_ready__signal.connect(self.arm_controls_wrist_pitch_comms_label.setText)
self.wrist_roll_comms_state_update_ready__signal.connect(self.arm_controls_wrist_roll_comms_label.setText)
self.base_status_update_ready__signal.connect(self.arm_controls_base_status_label.setText)
self.shoulder_status_update_ready__signal.connect(self.arm_controls_shoulder_status_label.setText)
self.elbow_status_update_ready__signal.connect(self.arm_controls_elbow_status_label.setText)
self.roll_status_update_ready__signal.connect(self.arm_controls_roll_status_label.setText)
self.wrist_pitch_status_update_ready__signal.connect(self.arm_controls_wrist_pitch_status_label.setText)
self.wrist_roll_status_update_ready__signal.connect(self.arm_controls_wrist_roll_status_label.setText)
self.base_faults_update_ready__signal.connect(self.arm_controls_base_faults_label.setText)
self.shoulder_faults_update_ready__signal.connect(self.arm_controls_shoulder_faults_label.setText)
self.elbow_faults_update_ready__signal.connect(self.arm_controls_elbow_faults_label.setText)
self.roll_faults_update_ready__signal.connect(self.arm_controls_roll_faults_label.setText)
self.wrist_pitch_faults_update_ready__signal.connect(self.arm_controls_wrist_pitch_faults_label.setText)
self.wrist_roll_faults_update_ready__signal.connect(self.arm_controls_wrist_roll_faults_label.setText)
def on_upright_zeroed_button_pressed__slot(self): def on_upright_zeroed_button_pressed__slot(self):
self.process_absolute_move_command([0 for _ in range(6)]) self.process_absolute_move_command([0 for _ in range(6)])
@@ -303,29 +203,23 @@ class MiscArm(QtCore.QThread):
def on_unstow_arm_button_pressed__slot(self): def on_unstow_arm_button_pressed__slot(self):
self.should_unstow_arm = True self.should_unstow_arm = True
def on_gripper_home_pressed(self):
message = GripperControlMessage()
message.gripper_mode = 1
message.gripper_position_absolute = -1
message.should_home = True
self.gripper_control_publisher.publish(message)
def on_gripper_toggle_light_pressed(self):
message = GripperControlMessage()
message.gripper_mode = 1
message.toggle_light = True
message.gripper_position_absolute = -1
self.gripper_control_publisher.publish(message)
def new_arm_status_message_received__callback(self, data): def new_arm_status_message_received__callback(self, data):
self.base_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.base_comm_status))
self.shoulder_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.shoulder_comm_status))
self.elbow_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.elbow_comm_status))
self.roll_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.roll_comm_status))
self.wrist_pitch_comms_state_update_ready__signal.emit(
self.process_comms_to_string(data.wrist_pitch_comm_status))
self.wrist_roll_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.wrist_roll_comm_status))
self.base_status_update_ready__signal.emit(self.process_statuses_to_string(data.base_status))
self.shoulder_status_update_ready__signal.emit(self.process_statuses_to_string(data.shoulder_status))
self.elbow_status_update_ready__signal.emit(self.process_statuses_to_string(data.elbow_status))
self.roll_status_update_ready__signal.emit(self.process_statuses_to_string(data.roll_status))
self.wrist_pitch_status_update_ready__signal.emit(self.process_statuses_to_string(data.wrist_pitch_status))
self.wrist_roll_status_update_ready__signal.emit(self.process_statuses_to_string(data.wrist_roll_status))
self.base_faults_update_ready__signal.emit(self.process_faults_to_string(data.base_faults))
self.shoulder_faults_update_ready__signal.emit(self.process_faults_to_string(data.shoulder_faults))
self.elbow_faults_update_ready__signal.emit(self.process_faults_to_string(data.elbow_faults))
self.roll_faults_update_ready__signal.emit(self.process_faults_to_string(data.roll_faults))
self.wrist_pitch_faults_update_ready__signal.emit(self.process_faults_to_string(data.wrist_pitch_faults))
self.wrist_roll_faults_update_ready__signal.emit(self.process_faults_to_string(data.wrist_roll_faults))
self.base_position = data.base self.base_position = data.base
self.shoulder_position = data.shoulder self.shoulder_position = data.shoulder
self.elbow_position = data.elbow self.elbow_position = data.elbow
@@ -333,30 +227,6 @@ class MiscArm(QtCore.QThread):
self.wrist_pitch_position = data.wrist_pitch self.wrist_pitch_position = data.wrist_pitch
self.wrist_roll_position = data.wrist_roll self.wrist_roll_position = data.wrist_roll
@staticmethod
def process_faults_to_string(faults):
fault_output = ""
for bit_position in FAULT_TO_STRING:
if (1 << bit_position) & faults:
fault_output += FAULT_TO_STRING[bit_position] + "\n"
return fault_output[:-1]
@staticmethod
def process_statuses_to_string(statuses):
status_output = ""
for bit_position in STATUS_TO_STRING:
if (1 << bit_position) & statuses:
status_output += STATUS_TO_STRING[bit_position] + "\n"
return status_output[:-1]
@staticmethod
def process_comms_to_string(comms):
return COMMS_TO_STRING[comms] if comms in COMMS_TO_STRING else "UNKNOWN"
def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal): def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
start_signal.connect(self.start) start_signal.connect(self.start)
signals_and_slots_signal.connect(self.connect_signals_and_slots) signals_and_slots_signal.connect(self.connect_signals_and_slots)

9
software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoReceiver.py
View File

@@ -69,10 +69,6 @@ class RoverVideoReceiver(QtCore.QThread):
# Subscription variables # Subscription variables
self.video_subscribers = [] self.video_subscribers = []
self.video_subscribers.append(rospy.Subscriber(self.topic_base_path + "/image_1280x720/compressed", CompressedImage, self.__image_data_received_callback))
self.video_subscribers.append(rospy.Subscriber(self.topic_base_path + "/image_640x360/compressed", CompressedImage, self.__image_data_received_callback))
self.video_subscribers.append(rospy.Subscriber(self.topic_base_path + "/image_256x144/compressed", CompressedImage, self.__image_data_received_callback))
# Publisher variables # Publisher variables
self.camera_control_publisher = rospy.Publisher(self.control_topic_path, CameraControlMessage, queue_size=1) self.camera_control_publisher = rospy.Publisher(self.control_topic_path, CameraControlMessage, queue_size=1)
@@ -118,6 +114,11 @@ class RoverVideoReceiver(QtCore.QThread):
# Initial class setup to make text images and get camera resolutions # Initial class setup to make text images and get camera resolutions
self.__create_camera_name_opencv_images() self.__create_camera_name_opencv_images()
# Attach subscribers now that everything is set up
self.video_subscribers.append(rospy.Subscriber(self.topic_base_path + "/image_1280x720/compressed", CompressedImage, self.__image_data_received_callback))
self.video_subscribers.append(rospy.Subscriber(self.topic_base_path + "/image_640x360/compressed", CompressedImage, self.__image_data_received_callback))
self.video_subscribers.append(rospy.Subscriber(self.topic_base_path + "/image_256x144/compressed", CompressedImage, self.__image_data_received_callback))
def run(self): def run(self):
self.logger.debug("Starting \"%s\" Camera Thread" % self.camera_title_name) self.logger.debug("Starting \"%s\" Camera Thread" % self.camera_title_name)

711
software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui
View File

@@ -1390,7 +1390,7 @@ N/A</string>
</size> </size>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>4</number> <number>1</number>
</property> </property>
<widget class="QWidget" name="tab_5"> <widget class="QWidget" name="tab_5">
<attribute name="title"> <attribute name="title">
@@ -1449,19 +1449,6 @@ N/A</string>
</property> </property>
</widget> </widget>
</item> </item>
<item row="2" column="0">
<spacer name="verticalSpacer_4">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="0"> <item row="0" column="0">
<widget class="QPushButton" name="arm_controls_stow_arm_button"> <widget class="QPushButton" name="arm_controls_stow_arm_button">
<property name="text"> <property name="text">
@@ -1489,6 +1476,22 @@ N/A</string>
</property> </property>
</widget> </widget>
</item> </item>
<item row="5" column="0">
<spacer name="verticalSpacer_4">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::MinimumExpanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout> </layout>
</item> </item>
</layout> </layout>
@@ -1538,6 +1541,9 @@ N/A</string>
<property name="orientation"> <property name="orientation">
<enum>Qt::Vertical</enum> <enum>Qt::Vertical</enum>
</property> </property>
<property name="sizeType">
<enum>QSizePolicy::MinimumExpanding</enum>
</property>
<property name="sizeHint" stdset="0"> <property name="sizeHint" stdset="0">
<size> <size>
<width>20</width> <width>20</width>
@@ -1548,6 +1554,19 @@ N/A</string>
</item> </item>
</layout> </layout>
</item> </item>
<item>
<spacer name="horizontalSpacer_15">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout> </layout>
</item> </item>
</layout> </layout>
@@ -1579,515 +1598,42 @@ N/A</string>
</font> </font>
</property> </property>
<property name="text"> <property name="text">
<string>Motor Driver Statuses</string> <string>Gripper Controls</string>
</property> </property>
</widget> </widget>
</item> </item>
<item> <item>
<layout class="QGridLayout" name="gridLayout_9"> <layout class="QGridLayout" name="gridLayout_9">
<item row="0" column="0"> <item row="0" column="0">
<layout class="QVBoxLayout" name="verticalLayout_6"> <layout class="QVBoxLayout" name="verticalLayout_9">
<item> <item>
<widget class="QLabel" name="label_20"> <widget class="QPushButton" name="gripper_home_button">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text"> <property name="text">
<string>Base</string> <string>Home</string>
</property> </property>
</widget> </widget>
</item> </item>
<item> <item>
<layout class="QFormLayout" name="formLayout"> <widget class="QPushButton" name="gripper_toggle_light_button">
<item row="0" column="0"> <property name="text">
<widget class="QLabel" name="label_21"> <string>Toggle Light</string>
<property name="text"> </property>
<string>Comms:</string> </widget>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="arm_controls_base_comms_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_23">
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_27">
<property name="text">
<string>Faults:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="arm_controls_base_status_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="arm_controls_base_faults_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item> </item>
</layout> </layout>
</item> </item>
<item row="0" column="1"> <item row="0" column="1">
<layout class="QVBoxLayout" name="verticalLayout_9"> <spacer name="horizontalSpacer_17">
<item> <property name="orientation">
<widget class="QLabel" name="label_32"> <enum>Qt::Horizontal</enum>
<property name="font"> </property>
<font> <property name="sizeHint" stdset="0">
<weight>75</weight> <size>
<bold>true</bold> <width>40</width>
</font> <height>20</height>
</property> </size>
<property name="text"> </property>
<string>Shoulder</string> </spacer>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_2">
<item row="0" column="0">
<widget class="QLabel" name="label_33">
<property name="text">
<string>Comms:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="arm_controls_shoulder_comms_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_35">
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_36">
<property name="text">
<string>Faults:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="arm_controls_shoulder_status_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="arm_controls_shoulder_faults_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="0" column="2">
<layout class="QVBoxLayout" name="verticalLayout_10">
<item>
<widget class="QLabel" name="label_39">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Elbow</string>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_3">
<item row="0" column="0">
<widget class="QLabel" name="label_40">
<property name="text">
<string>Comms:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="arm_controls_elbow_comms_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_42">
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_43">
<property name="text">
<string>Faults:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="arm_controls_elbow_status_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="arm_controls_elbow_faults_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="1" column="0">
<layout class="QVBoxLayout" name="verticalLayout_11">
<item>
<widget class="QLabel" name="label_46">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Roll</string>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_4">
<item row="0" column="0">
<widget class="QLabel" name="label_47">
<property name="text">
<string>Comms:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="arm_controls_roll_comms_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_49">
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_50">
<property name="text">
<string>Faults:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="arm_controls_roll_status_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="arm_controls_roll_faults_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="1" column="1">
<layout class="QVBoxLayout" name="verticalLayout_12">
<item>
<widget class="QLabel" name="label_53">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Wrist Pitch</string>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_5">
<item row="0" column="0">
<widget class="QLabel" name="label_54">
<property name="text">
<string>Comms:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="arm_controls_wrist_pitch_comms_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_56">
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_57">
<property name="text">
<string>Faults:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="arm_controls_wrist_pitch_status_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="arm_controls_wrist_pitch_faults_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="1" column="2">
<layout class="QVBoxLayout" name="verticalLayout_13">
<item>
<widget class="QLabel" name="label_60">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Wrist Roll</string>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_6">
<item row="0" column="0">
<widget class="QLabel" name="label_61">
<property name="text">
<string>Comms:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="arm_controls_wrist_roll_comms_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_63">
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_64">
<property name="text">
<string>Faults:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="arm_controls_wrist_roll_status_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="arm_controls_wrist_roll_faults_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item> </item>
</layout> </layout>
</item> </item>
@@ -3191,157 +2737,6 @@ N/A</string>
</property> </property>
</spacer> </spacer>
</item> </item>
<item>
<widget class="Line" name="line_9">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_17">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_18">
<property name="spacing">
<number>6</number>
</property>
<item>
<widget class="QLabel" name="label_34">
<property name="font">
<font>
<pointsize>12</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Xbox Mode</string>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_16">
<property name="spacing">
<number>0</number>
</property>
<item>
<widget class="QLabel" name="xbox_mode_arm_label">
<property name="minimumSize">
<size>
<width>120</width>
<height>120</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>120</width>
<height>120</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 1px solid black;</string>
</property>
<property name="text">
<string>Arm</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_17">
<property name="spacing">
<number>0</number>
</property>
<item>
<widget class="QLabel" name="xbox_mode_mining_label">
<property name="minimumSize">
<size>
<width>120</width>
<height>120</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>120</width>
<height>120</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">border: 1px solid black;</string>
</property>
<property name="text">
<string>Mining</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer_7">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item>
<spacer name="horizontalSpacer_18">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="Line" name="line_10">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_15">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item> <item>
<widget class="QWidget" name="widget_3" native="true"> <widget class="QWidget" name="widget_3" native="true">
<property name="sizePolicy"> <property name="sizePolicy">

2123
software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui
View File

File diff suppressed because it is too large Load Diff

2439
software/ros_packages/ground_station/src/Resources/Ui/right_screen_old.ui Normal file
View File

File diff suppressed because it is too large Load Diff

7
software/ros_packages/rover_control/src/effectors_control/effectors_control.py
View File

@@ -80,6 +80,8 @@ DEFAULT_GRIPPER_REGISTERS = [
0, # Light off 0, # Light off
] ]
GRIPPER_UNIVERSAL_POSITION_MAX = 10000
# ##### Mining Defines ##### # ##### Mining Defines #####
MINING_MODBUS_REGISTERS = { MINING_MODBUS_REGISTERS = {
"LIFT_SET_POSITIVE": 0, "LIFT_SET_POSITIVE": 0,
@@ -97,6 +99,7 @@ MINING_MODBUS_REGISTERS = {
"MEASURED_WEIGHT": 11 "MEASURED_WEIGHT": 11
} }
MINING_POSITIONAL_THRESHOLD = 20 MINING_POSITIONAL_THRESHOLD = 20
# ##### Science Defines ##### # ##### Science Defines #####
@@ -316,10 +319,10 @@ class EffectorsControl(object):
gripper_relative = self.gripper_control_message.gripper_position_relative gripper_relative = self.gripper_control_message.gripper_position_relative
if -1 < gripper_absolute < UINT16_MAX: if -1 < gripper_absolute < UINT16_MAX:
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = gripper_absolute self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(gripper_absolute, 0), GRIPPER_UNIVERSAL_POSITION_MAX)
else: else:
new_position = self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] + gripper_relative new_position = self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] + gripper_relative
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(new_position, 0), 10000) self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(new_position, 0), GRIPPER_UNIVERSAL_POSITION_MAX)
self.gripper_node.write_registers(0, self.gripper_registers) self.gripper_node.write_registers(0, self.gripper_registers)

1
software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch
View File

@@ -156,6 +156,7 @@
{name: "/rover_control/tower/status/co2", compress: false, rate: 1.0}, {name: "/rover_control/tower/status/co2", compress: false, rate: 1.0},
{name: "/rover_odometry/imu/data", compress: false, rate: 10.0}, {name: "/rover_odometry/imu/data", compress: false, rate: 10.0},
{name: "/rover_control/mining/status", compress: false, rate: 5.0}, {name: "/rover_control/mining/status", compress: false, rate: 5.0},
{name: "/rover_control/gripper/status", compress: false, rate: 5.0},
{name: "/rdf/data", compress: false, rate: 50.0} {name: "/rdf/data", compress: false, rate: 50.0}
] ]
</rosparam> </rosparam>