mirror of
https://github.com/OSURoboticsClub/Rover_2017_2018.git
synced 2025-11-08 10:11:14 +00:00
Changed readouts for arm, added readouts for gripper w/modes and extra controls. GUI reorganization.
This commit is contained in:
@@ -39,7 +39,7 @@ uint8_t message_count = 0;
|
||||
|
||||
// Pan/tilt hard limits
|
||||
const int pan_min = 1470;
|
||||
const int pan_center = 1600;
|
||||
const int pan_center = 1605;
|
||||
const int pan_max = 1725;
|
||||
|
||||
const int tilt_min = 1020;
|
||||
@@ -57,7 +57,7 @@ Servo pan_servo;
|
||||
Servo tilt_servo;
|
||||
|
||||
void setup() {
|
||||
// Serial.begin(9600);
|
||||
Serial.begin(9600);
|
||||
// while(!Serial);
|
||||
setup_hardware();
|
||||
|
||||
@@ -151,4 +151,3 @@ void set_pan_tilt_adjustments() {
|
||||
modbus_data[MODBUS_REGISTERS::TILT_ADJUST_NEGATIVE] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -8,23 +8,103 @@ import logging
|
||||
import rospy
|
||||
|
||||
from rover_arm.msg import ArmStatusMessage
|
||||
from rover_control.msg import GripperStatusMessage
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
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
|
||||
#####################################
|
||||
class ArmIndication(QtCore.QObject):
|
||||
base_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
shoulder_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
elbow_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
roll_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
wrist_pitch_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
wrist_roll_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
base_position_updated__signal = QtCore.pyqtSignal(float)
|
||||
shoulder_position_updated__signal = QtCore.pyqtSignal(float)
|
||||
elbow_position_updated__signal = QtCore.pyqtSignal(float)
|
||||
roll_position_updated__signal = QtCore.pyqtSignal(float)
|
||||
wrist_pitch_position_updated__signal = QtCore.pyqtSignal(float)
|
||||
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):
|
||||
super(ArmIndication, self).__init__()
|
||||
@@ -33,12 +113,45 @@ class ArmIndication(QtCore.QObject):
|
||||
self.shared_objects = shared_objects
|
||||
self.right_screen = self.shared_objects["screens"]["right_screen"]
|
||||
|
||||
self.base_rotation_dial = self.right_screen.base_rotation_dial # type: QtWidgets.QDial
|
||||
self.shoulder_pitch_dial = self.right_screen.shoulder_pitch_dial # type: QtWidgets.QDial
|
||||
self.elbow_pitch_dial = self.right_screen.elbow_pitch_dial # type: QtWidgets.QDial
|
||||
self.elbow_roll_dial = self.right_screen.elbow_roll_dial # type: QtWidgets.QDial
|
||||
self.end_effector_pitch_dial = self.right_screen.end_effector_pitch_dial # type: QtWidgets.QDial
|
||||
self.end_effector_rotation_dial = self.right_screen.end_effector_rotation_dial # type: QtWidgets.QDial
|
||||
self.base_position_lcd_number = self.right_screen.base_position_lcd_number # type: QtWidgets.QLCDNumber
|
||||
self.shoulder_position_lcd_number = self.right_screen.shoulder_position_lcd_number # type: QtWidgets.QLCDNumber
|
||||
self.elbow_position_lcd_number = self.right_screen.elbow_position_lcd_number # type: QtWidgets.QLCDNumber
|
||||
self.roll_position_lcd_number = self.right_screen.roll_position_lcd_number # type: QtWidgets.QLCDNumber
|
||||
self.wrist_pitch_position_lcd_number = self.right_screen.wrist_pitch_position_lcd_number # type: QtWidgets.QLCDNumber
|
||||
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 ##########
|
||||
self.settings = QtCore.QSettings()
|
||||
@@ -48,24 +161,115 @@ class ArmIndication(QtCore.QObject):
|
||||
|
||||
# ########## Class Variables ##########
|
||||
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 ##########
|
||||
self.connect_signals_and_slots()
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
self.base_position_updated__signal.connect(self.base_rotation_dial.setValue)
|
||||
self.shoulder_position_updated__signal.connect(self.shoulder_pitch_dial.setValue)
|
||||
self.elbow_position_updated__signal.connect(self.elbow_pitch_dial.setValue)
|
||||
self.roll_position_updated__signal.connect(self.elbow_roll_dial.setValue)
|
||||
self.wrist_pitch_position_updated__signal.connect(self.end_effector_pitch_dial.setValue)
|
||||
self.wrist_roll_position_updated__signal.connect(self.end_effector_rotation_dial.setValue)
|
||||
self.base_position_updated__signal.connect(self.base_position_lcd_number.display)
|
||||
self.shoulder_position_updated__signal.connect(self.shoulder_position_lcd_number.display)
|
||||
self.elbow_position_updated__signal.connect(self.elbow_position_lcd_number.display)
|
||||
self.roll_position_updated__signal.connect(self.roll_position_lcd_number.display)
|
||||
self.wrist_pitch_position_updated__signal.connect(self.wrist_pitch_position_lcd_number.display)
|
||||
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):
|
||||
self.base_position_updated__signal.emit(int(data.base * 1000))
|
||||
self.shoulder_position_updated__signal.emit(int(data.shoulder * 1000))
|
||||
self.elbow_position_updated__signal.emit(int(data.elbow * 1000))
|
||||
self.roll_position_updated__signal.emit(int(data.roll * 1000))
|
||||
self.wrist_pitch_position_updated__signal.emit(int(data.wrist_pitch * 1000))
|
||||
self.wrist_roll_position_updated__signal.emit(int(data.wrist_roll * 1000))
|
||||
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_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"
|
||||
|
||||
|
||||
|
||||
@@ -160,6 +160,14 @@ class XBOXController(QtCore.QThread):
|
||||
# Controller Class Definition
|
||||
#####################################
|
||||
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 = [
|
||||
"ARM",
|
||||
"MINING"
|
||||
@@ -172,17 +180,20 @@ class XBOXControllerControlSender(QtCore.QThread):
|
||||
"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):
|
||||
super(XBOXControllerControlSender, self).__init__()
|
||||
|
||||
# ########## Reference to class init variables ##########
|
||||
self.shared_objects = shared_objects
|
||||
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.xbox_mode_mining_label = self.left_screen.xbox_mode_mining_label # type: QtWidgets.QLabel
|
||||
self.right_screen = self.shared_objects["screens"]["right_screen"]
|
||||
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 ##########
|
||||
self.settings = QtCore.QSettings()
|
||||
@@ -205,14 +216,46 @@ class XBOXControllerControlSender(QtCore.QThread):
|
||||
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.last_xbox_button_state = 0
|
||||
self.last_left_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):
|
||||
self.logger.debug("Starting Joystick Thread")
|
||||
@@ -222,9 +265,9 @@ class XBOXControllerControlSender(QtCore.QThread):
|
||||
|
||||
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()
|
||||
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()
|
||||
|
||||
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_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):
|
||||
xbox_state = self.controller.controller_states["xbox_button"]
|
||||
left_bumper_state = self.controller.controller_states["left_bumper"]
|
||||
right_bumper_state = self.controller.controller_states["right_bumper"]
|
||||
|
||||
if self.last_xbox_button_state == 0 and xbox_state == 1:
|
||||
self.current_state += 1
|
||||
self.current_state = self.current_state % len(self.XBOX_CONTROL_STATES)
|
||||
self.xbox_current_control_state += 1
|
||||
self.xbox_current_control_state = self.xbox_current_control_state % len(self.XBOX_CONTROL_STATES)
|
||||
self.xbox_control_state_just_changed = True
|
||||
self.last_xbox_button_state = 1
|
||||
elif self.last_xbox_button_state == 1 and xbox_state == 0:
|
||||
self.last_xbox_button_state = 0
|
||||
|
||||
if self.xbox_control_state_just_changed:
|
||||
if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"):
|
||||
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
|
||||
|
||||
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 += 1
|
||||
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 += 1
|
||||
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.xbox_control_state_just_changed:
|
||||
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
|
||||
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):
|
||||
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()
|
||||
|
||||
@@ -282,7 +344,7 @@ class XBOXControllerControlSender(QtCore.QThread):
|
||||
gripper_control_message.gripper_mode = self.gripper_control_mode + 1
|
||||
|
||||
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"]
|
||||
right_trigger = self.controller.controller_states["right_trigger"]
|
||||
@@ -322,6 +384,7 @@ class XBOXControllerControlSender(QtCore.QThread):
|
||||
|
||||
if should_publish_gripper:
|
||||
self.gripper_control_publisher.publish(gripper_control_message)
|
||||
self.send_new_gripper_mode = False
|
||||
|
||||
def send_mining_commands(self):
|
||||
|
||||
|
||||
@@ -9,6 +9,7 @@ import rospy
|
||||
from time import time
|
||||
|
||||
from rover_arm.msg import ArmControlMessage, ArmStatusMessage
|
||||
from rover_control.msg import GripperControlMessage
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
@@ -17,54 +18,10 @@ ARM_RELATIVE_CONTROL_TOPIC = "/rover_arm/control/relative"
|
||||
ARM_ABSOLUTE_CONTROL_TOPIC = "/rover_arm/control/absolute"
|
||||
ARM_STATUS_TOPIC = "/rover_arm/status"
|
||||
|
||||
GRIPPER_CONTROL_TOPIC = "/rover_control/gripper/control"
|
||||
|
||||
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
|
||||
|
||||
# Order is [base, shoulder, elbow, roll, wrist_pitch, wrist_roll]
|
||||
@@ -87,27 +44,6 @@ ARM_UNSTOW_PROCEDURE = [
|
||||
# UbiquitiRadioSettings Class Definition
|
||||
#####################################
|
||||
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):
|
||||
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_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.arm_controls_base_status_label = self.left_screen.arm_controls_base_status_label # type:QtWidgets.QLabel
|
||||
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
|
||||
self.gripper_home_button = self.left_screen.gripper_home_button # type:QtWidgets.QPushButton
|
||||
self.gripper_toggle_light_button = self.left_screen.gripper_toggle_light_button # type:QtWidgets.QPushButton
|
||||
|
||||
# ########## Get the settings instance ##########
|
||||
self.settings = QtCore.QSettings()
|
||||
@@ -166,6 +82,8 @@ class MiscArm(QtCore.QThread):
|
||||
self.arm_absolute_control_publisher = rospy.Publisher(ARM_ABSOLUTE_CONTROL_TOPIC, ArmControlMessage,
|
||||
queue_size=1)
|
||||
|
||||
self.gripper_control_publisher = rospy.Publisher(GRIPPER_CONTROL_TOPIC, GripperControlMessage, queue_size=1)
|
||||
|
||||
self.base_position = 0
|
||||
self.shoulder_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_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.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.gripper_home_button.clicked.connect(self.on_gripper_home_pressed)
|
||||
self.gripper_toggle_light_button.clicked.connect(self.on_gripper_toggle_light_pressed)
|
||||
|
||||
def on_upright_zeroed_button_pressed__slot(self):
|
||||
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):
|
||||
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):
|
||||
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.shoulder_position = data.shoulder
|
||||
self.elbow_position = data.elbow
|
||||
@@ -333,30 +227,6 @@ class MiscArm(QtCore.QThread):
|
||||
self.wrist_pitch_position = data.wrist_pitch
|
||||
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):
|
||||
start_signal.connect(self.start)
|
||||
signals_and_slots_signal.connect(self.connect_signals_and_slots)
|
||||
|
||||
@@ -69,10 +69,6 @@ class RoverVideoReceiver(QtCore.QThread):
|
||||
# Subscription variables
|
||||
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
|
||||
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
|
||||
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):
|
||||
self.logger.debug("Starting \"%s\" Camera Thread" % self.camera_title_name)
|
||||
|
||||
|
||||
@@ -1390,7 +1390,7 @@ N/A</string>
|
||||
</size>
|
||||
</property>
|
||||
<property name="currentIndex">
|
||||
<number>4</number>
|
||||
<number>1</number>
|
||||
</property>
|
||||
<widget class="QWidget" name="tab_5">
|
||||
<attribute name="title">
|
||||
@@ -1449,19 +1449,6 @@ N/A</string>
|
||||
</property>
|
||||
</widget>
|
||||
</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">
|
||||
<widget class="QPushButton" name="arm_controls_stow_arm_button">
|
||||
<property name="text">
|
||||
@@ -1489,6 +1476,22 @@ N/A</string>
|
||||
</property>
|
||||
</widget>
|
||||
</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>
|
||||
</item>
|
||||
</layout>
|
||||
@@ -1538,6 +1541,9 @@ N/A</string>
|
||||
<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>
|
||||
@@ -1548,6 +1554,19 @@ N/A</string>
|
||||
</item>
|
||||
</layout>
|
||||
</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>
|
||||
</item>
|
||||
</layout>
|
||||
@@ -1579,515 +1598,42 @@ N/A</string>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Motor Driver Statuses</string>
|
||||
<string>Gripper Controls</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QGridLayout" name="gridLayout_9">
|
||||
<item row="0" column="0">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_6">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_20">
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Base</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QFormLayout" name="formLayout">
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_21">
|
||||
<property name="text">
|
||||
<string>Comms:</string>
|
||||
</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>
|
||||
</layout>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_9">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_32">
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<widget class="QPushButton" name="gripper_home_button">
|
||||
<property name="text">
|
||||
<string>Shoulder</string>
|
||||
<string>Home</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QFormLayout" name="formLayout_2">
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_33">
|
||||
<widget class="QPushButton" name="gripper_toggle_light_button">
|
||||
<property name="text">
|
||||
<string>Comms:</string>
|
||||
<string>Toggle Light</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</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>
|
||||
<spacer name="horizontalSpacer_17">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</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>
|
||||
</spacer>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
@@ -3191,157 +2737,6 @@ N/A</string>
|
||||
</property>
|
||||
</spacer>
|
||||
</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>
|
||||
<widget class="QWidget" name="widget_3" native="true">
|
||||
<property name="sizePolicy">
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -80,6 +80,8 @@ DEFAULT_GRIPPER_REGISTERS = [
|
||||
0, # Light off
|
||||
]
|
||||
|
||||
GRIPPER_UNIVERSAL_POSITION_MAX = 10000
|
||||
|
||||
# ##### Mining Defines #####
|
||||
MINING_MODBUS_REGISTERS = {
|
||||
"LIFT_SET_POSITIVE": 0,
|
||||
@@ -97,6 +99,7 @@ MINING_MODBUS_REGISTERS = {
|
||||
"MEASURED_WEIGHT": 11
|
||||
}
|
||||
|
||||
|
||||
MINING_POSITIONAL_THRESHOLD = 20
|
||||
|
||||
# ##### Science Defines #####
|
||||
@@ -316,10 +319,10 @@ class EffectorsControl(object):
|
||||
gripper_relative = self.gripper_control_message.gripper_position_relative
|
||||
|
||||
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:
|
||||
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)
|
||||
|
||||
|
||||
@@ -156,6 +156,7 @@
|
||||
{name: "/rover_control/tower/status/co2", compress: false, rate: 1.0},
|
||||
{name: "/rover_odometry/imu/data", compress: false, rate: 10.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}
|
||||
]
|
||||
</rosparam>
|
||||
|
||||
Reference in New Issue
Block a user