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

Got point and shoot control working. Needs zooming. Backup scale node added. Lots of random things, super tired.

Browse Source
This commit is contained in:
Corwin Perren
2018-08-11 07:12:06 -07:00
parent 12ed32ef54
commit 03cdd2e3a6
27 changed files with 768 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
software/ros_packages/__init__.py Normal file
View File

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

@@ -63,6 +63,14 @@ FAULT_TO_STRING = {
15: "HOST COMM ERROR"
}
GRIPPER_MODES = {
0: "No Change",
1: "Normal",
2: "Pinch",
3: "Wide ",
4: "Scissor"
}
#####################################
# Controller Class Definition
@@ -106,6 +114,7 @@ class ArmIndication(QtCore.QObject):
thumb_current_updated__signal = QtCore.pyqtSignal(int)
middlefinger_current_updated__signal = QtCore.pyqtSignal(int)
gripper_reported_mode_updated__signal = QtCore.pyqtSignal(str)
gripper_reported_setpoint_updated__signal = QtCore.pyqtSignal(int)
def __init__(self, shared_objects):
@@ -155,6 +164,7 @@ class ArmIndication(QtCore.QObject):
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
self.gripper_reported_mode_label = self.right_screen.gripper_reported_mode_label # type:QtWidgets.QLabel
self.gripper_reported_setpoint_lcd_number = self.right_screen.gripper_reported_setpoint_lcd_number # type: QtWidgets.QLCDNumber
# ########## Get the settings instance ##########
@@ -209,6 +219,7 @@ class ArmIndication(QtCore.QObject):
self.thumb_current_updated__signal.connect(self.thumb_current_lcd_number.display)
self.middlefinger_current_updated__signal.connect(self.middlefinger_current_lcd_number.display)
self.gripper_reported_mode_updated__signal.connect(self.gripper_reported_mode_label.setText)
self.gripper_reported_setpoint_updated__signal.connect(self.gripper_reported_setpoint_lcd_number.display)
def on_arm_status_update_received__callback(self, data):
@@ -254,6 +265,7 @@ class ArmIndication(QtCore.QObject):
self.thumb_current_updated__signal.emit(data.thumb_current)
self.middlefinger_current_updated__signal.emit(data.middlefinger_current)
self.gripper_reported_mode_updated__signal.emit(GRIPPER_MODES[data.current_mode])
self.gripper_reported_setpoint_updated__signal.emit(data.current_finger_position)
@staticmethod

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

@@ -226,7 +226,7 @@ class XBOXControllerControlSender(QtCore.QThread):
self.last_right_bumper_state = 0
self.last_back_button_state = 0
self.gripper_control_mode = 0
self.gripper_control_mode = 1
self.gripper_control_mode_just_changed = False
self.send_new_gripper_mode = False
@@ -257,7 +257,7 @@ class XBOXControllerControlSender(QtCore.QThread):
"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],
"ABS_MOVE": self.PINCH_MODE_ABSOLUTE_SET_POSITION
# "ABS_MOVE": self.PINCH_MODE_ABSOLUTE_SET_POSITION
}
}
@@ -314,15 +314,14 @@ class XBOXControllerControlSender(QtCore.QThread):
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.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.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
@@ -390,7 +389,7 @@ class XBOXControllerControlSender(QtCore.QThread):
should_publish_arm = True
should_publish_gripper = True
arm_control_message.wrist_roll = ((right_x_axis / THUMB_STICK_MAX) * WRIST_ROLL_SCALAR) * right_trigger_ratio
arm_control_message.wrist_roll = ((left_x_axis / THUMB_STICK_MAX) * BASE_SCALAR) * right_trigger_ratio
arm_control_message.wrist_pitch = (-(left_y_axis / THUMB_STICK_MAX) * WRIST_PITCH_SCALAR) * right_trigger_ratio
gripper_control_message.gripper_position_relative = (-(right_y_axis / THUMB_STICK_MAX) * GRIPPER_MOVEMENT_SCALAR) * right_trigger_ratio
@@ -408,7 +407,7 @@ class XBOXControllerControlSender(QtCore.QThread):
if self.last_back_button_state == 0 and back_state == 1:
gripper_control_message.should_home = True
gripper_control_message.gripper_mode = 1
gripper_control_message.gripper_mode = self.gripper_control_mode + 1
self.gripper_control_publisher.publish(gripper_control_message)
self.last_back_button_state = 1
elif self.last_back_button_state == 1 and back_state == 0:

38
software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py
View File

@@ -7,8 +7,9 @@ from PyQt5 import QtCore, QtWidgets
import logging
import rospy
from rover_control.msg import MiningStatusMessage, MiningControlMessage
from rover_control.msg import MiningStatusMessage, MiningControlMessage, CameraControlMessage
from rover_science.msg import SoilSensorStatusMessage
from std_msgs.msg import Float64
#####################################
# Global Variables
@@ -16,8 +17,12 @@ from rover_science.msg import SoilSensorStatusMessage
MINING_STATUS_TOPIC = "/rover_control/mining/status"
MINING_CONTROL_TOPIC = "/rover_control/mining/control"
SCALE_MEASUREMENT_TOPIC = "/rover_control/scale/measurement"
SOIL_PROBE_TOPIC = "/rover_science/soil_probe/data"
CAMERA_CONTROL_TOPIC = "/rover_control/camera/control"
TRAVEL_POSITION_LIFT = 110
TRAVEL_POSITION_TILT = 1023
@@ -71,6 +76,17 @@ class Mining(QtCore.QObject):
self.science_real_dielectric_lcd_number = self.left_screen.science_real_dielectric_lcd_number # type:QtWidgets.QLCDNumber
self.science_imaginary_dielectric_lcd_number = self.left_screen.science_imaginary_dielectric_lcd_number # type:QtWidgets.QLCDNumber
self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton
self.cam_network_output_button = self.left_screen.cam_network_output_button # type:QtWidgets.QPushButton
self.cam_zoom_in_button = self.left_screen.cam_zoom_in_button # type:QtWidgets.QPushButton
self.cam_zoom_out_button = self.left_screen.cam_zoom_out_button # type:QtWidgets.QPushButton
self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton
self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton
self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton
self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton
# ########## Get the settings instance ##########
self.settings = QtCore.QSettings()
@@ -85,8 +101,10 @@ class Mining(QtCore.QObject):
self.mining_status_message_received__callback)
self.soil_probe_subscriber = rospy.Subscriber(SOIL_PROBE_TOPIC, SoilSensorStatusMessage, self.on_soil_probe_message_received__callback)
self.scale_measurement_subscriber = rospy.Subscriber(SCALE_MEASUREMENT_TOPIC, Float64, self.on_scale_measurement_received__callback)
self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
self.camera_control_publisher = rospy.Publisher(CAMERA_CONTROL_TOPIC, CameraControlMessage, queue_size=1)
self.connect_signals_and_slots()
@@ -111,6 +129,9 @@ class Mining(QtCore.QObject):
self.real_dielectric_update_ready__signal.connect(self.science_real_dielectric_lcd_number.display)
self.imaginary_dielectric_update_ready__signal.connect(self.science_imaginary_dielectric_lcd_number.display)
self.cam_lcd_output_button.clicked.connect(self.on_cam_lcd_button_clicked__slot)
self.cam_network_output_button.clicked.connect(self.on_cam_network_button_clicked__slot)
def on_mining_set_cal_factor_clicked__slot(self):
message = MiningControlMessage()
@@ -162,11 +183,21 @@ class Mining(QtCore.QObject):
self.mining_control_publisher.publish(message)
def on_cam_lcd_button_clicked__slot(self):
message = CameraControlMessage()
message.camera_mode = 1
self.camera_control_publisher.publish(message)
def on_cam_network_button_clicked__slot(self):
message = CameraControlMessage()
message.camera_mode = 2
self.camera_control_publisher.publish(message)
def mining_status_message_received__callback(self, status):
status = status # type:MiningStatusMessage
self.tilt_position_update_ready__signal.emit(status.tilt_position)
self.lift_position_update_ready__signal.emit(status.lift_position)
self.weight_measurement_update_ready__signal.emit(status.measured_weight)
# self.weight_measurement_update_ready__signal.emit(status.measured_weight)
def on_soil_probe_message_received__callback(self, data):
self.temp_update_ready__signal.emit(data.temp_c)
@@ -175,3 +206,6 @@ class Mining(QtCore.QObject):
self.electrical_conductivity_update_ready__signal.emit(data.soil_electrical_conductivity)
self.real_dielectric_update_ready__signal.emit(data.real_dielectric_permittivity)
self.imaginary_dielectric_update_ready__signal.emit(data.imaginary_dielectric_permittivity)
def on_scale_measurement_received__callback(self, data):
self.weight_measurement_update_ready__signal.emit(data.data * 1000)

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

@@ -26,7 +26,7 @@ POSITIONAL_TOLERANCE = 0.02
# Order is [base, shoulder, elbow, roll, wrist_pitch, wrist_roll]
ARM_STOW_PROCEDURE = [
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0],
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0], # Out in front of rover
[0.0, -0.035, -0.28, -0.25, 0.25, 0.0],
[0.0, -0.035, -0.5, -0.25, 0.25, 0.0],
[0.0, -0.25, -0.5, -0.25, 0.25, -0.25]
@@ -39,6 +39,18 @@ ARM_UNSTOW_PROCEDURE = [
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0]
]
APPROACH_O2 = [
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0], # Out in front of rover
[0.0, 0.0485472094896, -0.273685193022, -0.00102834607876, -0.17200773156, 0], # Correct positioning out to left side of rover
[-0.436250296246, 0.0485472094896, -0.273685193022, -0.00102834607876, -0.17200773156, 0] # Directly positioned, ready to grip
]
APPROACH_BEACON = [
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0], # Out in front of rover
[0.0, 0.0361371382336, -0.325145836608, -0.00731261537597, -0.129662333807, 0.0569339269566], # Correct positioning out to right side of rover
[0.458505849045, 0.0361371382336, -0.325145633259, -0.00731200471916, -0.131140162948, 0.0920117742056]
]
#####################################
# UbiquitiRadioSettings Class Definition
@@ -59,6 +71,11 @@ 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_approach_o2_button = self.left_screen.arm_controls_approach_o2_button # type:QtWidgets.QPushButton
self.arm_controls_depart_o2_button = self.left_screen.arm_controls_depart_o2_button # type:QtWidgets.QPushButton
self.arm_controls_approach_beacon_button = self.left_screen.arm_controls_approach_beacon_button # type:QtWidgets.QPushButton
self.arm_controls_depart_beacon_button = self.left_screen.arm_controls_depart_beacon_button # type:QtWidgets.QPushButton
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
@@ -94,6 +111,11 @@ class MiscArm(QtCore.QThread):
self.should_stow_arm = False
self.should_unstow_arm = False
self.should_approach_o2 = False
self.should_depart_o2 = False
self.should_approach_beacon = False
self.should_depart_beacon = False
def run(self):
self.logger.debug("Starting MiscArm Thread")
@@ -107,6 +129,22 @@ class MiscArm(QtCore.QThread):
self.unstow_rover_arm()
self.should_unstow_arm = False
elif self.should_approach_o2:
self.approach_o2()
self.should_approach_o2 = False
elif self.should_depart_o2:
self.depart_o2()
self.should_depart_o2 = False
elif self.should_approach_beacon:
self.approach_beacon()
self.should_approach_beacon = False
elif self.should_depart_beacon:
self.depart_beacon()
self.should_depart_beacon = False
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff), 0))
@@ -121,6 +159,22 @@ class MiscArm(QtCore.QThread):
for movement in ARM_UNSTOW_PROCEDURE:
self.process_absolute_move_command(movement)
def approach_o2(self):
for movement in APPROACH_O2:
self.process_absolute_move_command(movement)
def depart_o2(self):
for movement in reversed(APPROACH_O2):
self.process_absolute_move_command(movement)
def approach_beacon(self):
for movement in APPROACH_BEACON:
self.process_absolute_move_command(movement)
def depart_beacon(self):
for movement in reversed(APPROACH_BEACON):
self.process_absolute_move_command(movement)
def process_absolute_move_command(self, movement):
message = ArmControlMessage()
@@ -134,6 +188,7 @@ class MiscArm(QtCore.QThread):
self.arm_absolute_control_publisher.publish(message)
self.wait_for_targets_reached(movement)
self.msleep(250)
def wait_for_targets_reached(self, movement):
base_set = movement[0]
@@ -176,6 +231,9 @@ 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.arm_controls_approach_o2_button.clicked.connect(self.on_approach_o2_button_pressed__slot)
self.arm_controls_approach_beacon_button.clicked.connect(self.on_approach_beacon_button_pressed__slot)
self.gripper_home_button.clicked.connect(self.on_gripper_home_pressed)
self.gripper_toggle_light_button.clicked.connect(self.on_gripper_toggle_light_pressed)
@@ -203,9 +261,21 @@ class MiscArm(QtCore.QThread):
def on_unstow_arm_button_pressed__slot(self):
self.should_unstow_arm = True
def on_approach_o2_button_pressed__slot(self):
self.should_approach_o2 = True
def on_depart_o2_button_pressed__slot(self):
self.should_depart_o2 = True
def on_approach_beacon_button_pressed__slot(self):
self.should_approach_beacon = True
def on_depart_beacon_button_pressed__slot(self):
self.should_depart_beacon = True
def on_gripper_home_pressed(self):
message = GripperControlMessage()
message.gripper_mode = 1
message.gripper_mode = 2
message.gripper_position_absolute = -1
message.should_home = True
@@ -213,7 +283,7 @@ class MiscArm(QtCore.QThread):
def on_gripper_toggle_light_pressed(self):
message = GripperControlMessage()
message.gripper_mode = 1
message.gripper_mode = 2
message.toggle_light = True
message.gripper_position_absolute = -1

2
software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py
View File

@@ -98,7 +98,7 @@ class RDF(QtCore.QThread):
valid_range = []
for n in range(0, len(xf)):
if (xf[n] > 0.5) and (xf[n] < 5.0):
if (xf[n] > 0.5) and (xf[n] <= 5.0):
valid_range.append(n)
yf = numpy.take(yf, valid_range)

12
software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py
View File

@@ -34,6 +34,9 @@ class SpeedAndHeadingIndication(QtCore.QThread):
heading_text_update_ready__signal = QtCore.pyqtSignal(str)
new_speed_update_ready__signal = QtCore.pyqtSignal(str)
pitch_update_ready__signal = QtCore.pyqtSignal(float)
roll_update_ready__signal = QtCore.pyqtSignal(float)
def __init__(self, shared_objects):
super(SpeedAndHeadingIndication, self).__init__()
@@ -46,6 +49,9 @@ class SpeedAndHeadingIndication(QtCore.QThread):
self.next_goal_label = self.right_screen.next_goal_label # type: QtWidgets.QLabel
self.current_speed_label = self.right_screen.current_speed_label # type: QtWidgets.QLabel
self.imu_pitch_lcd_number = self.right_screen.imu_pitch_lcd_number # type: QtWidgets.QLCDNumber
self.imu_roll_lcd_number = self.right_screen.imu_roll_lcd_number # type: QtWidgets.QLCDNumber
# ########## Get the settings instance ##########
self.settings = QtCore.QSettings()
@@ -110,6 +116,9 @@ class SpeedAndHeadingIndication(QtCore.QThread):
self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat)
self.current_heading = (self.euler_interpolator(self.yaw) + MappingSettings.DECLINATION_OFFSET) % 360
self.pitch_update_ready__signal.emit(self.pitch)
self.roll_update_ready__signal.emit(self.roll)
def rotate_compass_if_needed(self):
self.current_heading_shown_rotation_angle = int(self.current_heading)
@@ -161,6 +170,9 @@ class SpeedAndHeadingIndication(QtCore.QThread):
self.heading_compass_label.mousePressEvent = self.__on_heading_clicked__slot
self.pitch_update_ready__signal.connect(self.imu_pitch_lcd_number.display)
self.roll_update_ready__signal.connect(self.imu_roll_lcd_number.display)
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)

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

@@ -381,7 +381,7 @@ class RoverVideoCoordinator(QtCore.QThread):
self.low_res_button_text_update_ready__signal.emit("DISABLED")
self.low_res_button_stylesheet_update_ready__signal.emit(COLOR_GREEN)
else:
self.in_low_res_mode = False
self.in_low_res_mode = True
self.set_max_resolutions_flag = True
self.low_res_button_text_update_ready__signal.emit("ENABLED")

2
software/ros_packages/ground_station/src/Resources/Settings/MappingSettings.py
View File

@@ -63,4 +63,4 @@ LAST_SELECTION = "Graf Hall"
LAST_ZOOM_LEVEL = MAPPING_LOCATIONS[LAST_SELECTION]["default_zoom"]
# ##### This is the offset from magnetic north to true north
DECLINATION_OFFSET = 15
DECLINATION_OFFSET = 0 # We set this to 0 so we can use a phone to calibrate

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

@@ -251,6 +251,48 @@
<property name="spacing">
<number>0</number>
</property>
<item row="10" column="1">
<widget class="QLabel" name="co2_levels_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>9999999</width>
<height>9999999</height>
</size>
</property>
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property>
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="text">
<string>CO2 Levels
--- ppm</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="5" column="3">
<widget class="QLabel" name="emmc">
<property name="font">
@@ -1322,48 +1364,6 @@ N/A</string>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="co2_levels_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>9999999</width>
<height>9999999</height>
</size>
</property>
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property>
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="text">
<string>CO2 Levels
--- ppm</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
</layout>
</item>
</layout>
@@ -1390,8 +1390,145 @@ N/A</string>
</size>
</property>
<property name="currentIndex">
<number>1</number>
<number>0</number>
</property>
<widget class="QWidget" name="tab_6">
<attribute name="title">
<string>Science Cam</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_20">
<item row="0" column="0">
<layout class="QVBoxLayout" name="verticalLayout_26">
<item>
<widget class="QLabel" name="label_72">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Video Output</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_21">
<item>
<widget class="QPushButton" name="cam_lcd_output_button">
<property name="text">
<string>LCD</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="cam_network_output_button">
<property name="text">
<string>Network</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="0" column="1">
<spacer name="horizontalSpacer_20">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>422</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="0">
<layout class="QVBoxLayout" name="verticalLayout_27">
<item>
<widget class="QLabel" name="label_73">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Controls</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout">
<item row="0" column="0">
<widget class="QPushButton" name="cam_zoom_in_button">
<property name="text">
<string>Zoom In</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QPushButton" name="cam_zoom_out_button">
<property name="text">
<string>Zoom Out</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QPushButton" name="cam_full_zoom_in_button">
<property name="text">
<string>Full Zoom In</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QPushButton" name="cam_full_zoom_out_button">
<property name="text">
<string>Full Zoom Out</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QPushButton" name="cam_focus_button">
<property name="text">
<string>Focus</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QPushButton" name="cam_shoot_button">
<property name="text">
<string>Shoot</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="2" column="0">
<spacer name="verticalSpacer_8">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>331</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="tab_5">
<attribute name="title">
<string>RDF</string>
@@ -1575,7 +1712,7 @@ N/A</string>
</font>
</property>
<property name="text">
<string>Preset Movements</string>
<string>Special Movements</string>
</property>
</widget>
</item>
@@ -1702,6 +1839,83 @@ N/A</string>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_25">
<item>
<widget class="QLabel" name="label_69">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Task Movements</string>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_19">
<item row="0" column="0">
<widget class="QPushButton" name="arm_controls_approach_o2_button">
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>Approach O2 Tank</string>
</property>
</widget>
</item>
<item row="5" column="0">
<spacer name="verticalSpacer_7">
<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>
<item row="1" column="0">
<widget class="QPushButton" name="arm_controls_depart_o2_button">
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>Depart O2 Tank</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QPushButton" name="arm_controls_approach_beacon_button">
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>Approach Beacon</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QPushButton" name="arm_controls_depart_beacon_button">
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>Depart Beacon</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item>
<spacer name="horizontalSpacer_15">
<property name="orientation">

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

@@ -1604,7 +1604,7 @@ Position</string>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 1px solid black;</string>
<string notr="true">border: 1px solid black;</string>
</property>
<property name="text">
<string>Normal</string>
@@ -1634,7 +1634,7 @@ Position</string>
</font>
</property>
<property name="styleSheet">
<string notr="true">border: 1px solid black;</string>
<string notr="true">border: 1px solid black; background-color:darkgreen;</string>
</property>
<property name="text">
<string>Pinch</string>
@@ -1810,6 +1810,49 @@ Position</string>
</property>
</spacer>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_6">
<property name="rightMargin">
<number>6</number>
</property>
<item>
<widget class="QLabel" name="label_102">
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Reported Mode</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="gripper_reported_mode_label">
<property name="minimumSize">
<size>
<width>0</width>
<height>50</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">border: 1px solid grey;</string>
</property>
<property name="text">
<string>-------</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_68">
<item>
@@ -1824,6 +1867,9 @@ Position</string>
<property name="text">
<string>Reported Setpoint</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
@@ -2511,16 +2557,6 @@ Position</string>
<property name="spacing">
<number>0</number>
</property>
<item>
<widget class="QDial" name="dial_3">
<property name="minimum">
<number>-100</number>
</property>
<property name="maximum">
<number>100</number>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label">
<property name="font">
@@ -2537,6 +2573,16 @@ Position</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="imu_pitch_lcd_number">
<property name="minimumSize">
<size>
<width>0</width>
<height>50</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -2545,23 +2591,7 @@ Position</string>
<number>0</number>
</property>
<item>
<widget class="QDial" name="dial_4">
<property name="minimum">
<number>-100</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="invertedAppearance">
<bool>false</bool>
</property>
<property name="wrapping">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_15">
<widget class="QLabel" name="imu_pitch_lcd_number_2">
<property name="font">
<font>
<weight>75</weight>
@@ -2576,6 +2606,16 @@ Position</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="imu_roll_lcd_number">
<property name="minimumSize">
<size>
<width>0</width>
<height>50</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
</layout>

1
software/ros_packages/rover_control/CMakeLists.txt
View File

@@ -59,6 +59,7 @@ find_package(catkin REQUIRED COMPONENTS
MiningStatusMessage.msg
GripperControlMessage.msg
GripperStatusMessage.msg
CameraControlMessage.msg
)
## Generate services in the 'srv' folder

0
software/ros_packages/rover_control/__init__.py Normal file
View File

8
software/ros_packages/rover_control/msg/CameraControlMessage.msg Normal file
View File

@@ -0,0 +1,8 @@
uint8 camera_mode
bool zoom_in
bool zoom_out
bool full_zoom_in
bool full_zoom_out
bool focus
bool shoot

0
software/ros_packages/rover_control/src/__init__.py Normal file
View File

0
software/ros_packages/rover_control/src/effectors_control/__init__.py Normal file
View File

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

@@ -12,7 +12,7 @@ import minimalmodbus
# from std_msgs.msg import UInt8, UInt16
# Custom Imports
from rover_control.msg import MiningControlMessage, MiningStatusMessage, GripperControlMessage, GripperStatusMessage
from rover_control.msg import MiningControlMessage, MiningStatusMessage, GripperControlMessage, GripperStatusMessage, CameraControlMessage
#####################################
# Global Variables
@@ -45,6 +45,8 @@ GRIPPER_STATUS_PUBLISHER_TOPIC = "gripper/status"
MINING_CONTROL_SUBSCRIBER_TOPIC = "mining/control"
MINING_STATUS_PUBLISHER_TOPIC = "mining/status"
CAMERA_CONTROL_SUBSCRIBER_TOPIC = "camera/control"
# ##### Gripper Defines #####
GRIPPER_MODBUS_REGISTERS = {
"MODE": 0,
@@ -94,9 +96,17 @@ MINING_MODBUS_REGISTERS = {
"TARE": 7,
"CAL_FACTOR": 8,
"LIFT_POSITION": 9,
"TILT_POSITION": 10,
"MEASURED_WEIGHT": 11
"CHANGE_VIEW_MODE": 9,
"ZOOM_IN": 10,
"ZOOM_OUT": 11,
"FULL_ZOOM_IN": 12,
"FULL_ZOOM_OUT": 13,
"FOCUS": 14,
"SHOOT": 15,
"CURRENT_POSITION_LIFT": 16,
"CURRENT_POSITION_TILT": 17,
"MEASURED_WEIGHT": 18
}
@@ -139,6 +149,9 @@ class EffectorsControl(object):
self.mining_status_publisher_topic = rospy.get_param("~mining_status_publisher_topic",
MINING_STATUS_PUBLISHER_TOPIC)
self.camera_control_subscriber_topic = rospy.get_param("~camera_control_subscriber_topic",
CAMERA_CONTROL_SUBSCRIBER_TOPIC)
self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
self.gripper_node = None # type:minimalmodbus.Instrument
@@ -153,14 +166,14 @@ class EffectorsControl(object):
# ##### Subscribers #####
self.gripper_control_subscriber = rospy.Subscriber(self.gripper_control_subscriber_topic, GripperControlMessage, self.gripper_control_message_received__callback)
self.mining_control_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningControlMessage,
self.mining_control_message_received__callback)
self.mining_control_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningControlMessage, self.mining_control_message_received__callback)
self.camera_control_subscriber = rospy.Subscriber(self.camera_control_subscriber_topic, CameraControlMessage, self.camera_control_message_received__callback)
# ##### Publishers #####
self.gripper_status_publisher = rospy.Publisher(self.gripper_status_publisher_topic, GripperStatusMessage, queue_size=1)
self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage,
queue_size=1)
self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage, queue_size=1)
# ##### Misc #####
self.modbus_nodes_seen_time = time()
@@ -175,6 +188,9 @@ class EffectorsControl(object):
self.gripper_control_message = None
self.new_gripper_control_message = False
self.camera_control_message = None # type: CameraControlMessage
self.new_camera_control_message = False
self.failed_gripper_modbus_count = 0
self.failed_mining_modbus_count = 0
@@ -210,7 +226,8 @@ class EffectorsControl(object):
try:
self.run_mining()
self.failed_mining_modbus_count = 0
except:
except Exception, e:
print e
self.failed_mining_modbus_count += 1
if self.failed_mining_modbus_count == FAILED_MINING_MODBUS_LIMIMT:
@@ -224,6 +241,7 @@ class EffectorsControl(object):
def run_mining(self):
self.process_mining_control_message()
self.send_mining_status_message()
self.process_camera_control_message()
def connect_to_nodes(self):
self.gripper_node = minimalmodbus.Instrument(self.port, int(self.gripper_node_id))
@@ -268,13 +286,39 @@ class EffectorsControl(object):
self.modbus_nodes_seen_time = time()
self.new_mining_control_message = False
def process_camera_control_message(self):
if self.new_camera_control_message:
print self.camera_control_message
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_ABSOLUTE"]] = 1024
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_POSITIVE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_NEGATIVE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_ABSOLUTE"]] = 1024
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_POSITIVE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_NEGATIVE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["MEASURE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["CHANGE_VIEW_MODE"]] = self.camera_control_message.camera_mode
self.mining_registers[MINING_MODBUS_REGISTERS["ZOOM_IN"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["ZOOM_OUT"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["FULL_ZOOM_IN"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["FULL_ZOOM_OUT"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["FOCUS"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["SHOOT"]] = 0
self.mining_node.write_registers(0, self.mining_registers)
self.modbus_nodes_seen_time = time()
self.new_camera_control_message = False
def send_mining_status_message(self):
if self.mining_node_present:
self.mining_registers = self.mining_node.read_registers(0, len(MINING_MODBUS_REGISTERS))
message = MiningStatusMessage()
message.lift_position = self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_POSITION"]]
message.tilt_position = self.mining_registers[MINING_MODBUS_REGISTERS["TILT_POSITION"]]
message.lift_position = self.mining_registers[MINING_MODBUS_REGISTERS["CURRENT_POSITION_LIFT"]]
message.tilt_position = self.mining_registers[MINING_MODBUS_REGISTERS["CURRENT_POSITION_TILT"]]
message.measured_weight = self.mining_registers[MINING_MODBUS_REGISTERS["MEASURED_WEIGHT"]]
self.mining_status_publisher.publish(message)
@@ -296,16 +340,17 @@ class EffectorsControl(object):
homing_complete = False
while not homing_complete:
registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
# print registers
self.gripper_registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
self.send_gripper_status_message()
if registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION_OUTPUT"]] == 0:
if self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION_OUTPUT"]] == 0:
homing_complete = True
self.gripper_registers = None
else:
if self.gripper_control_message.toggle_light:
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] = not self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]]
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] = 0 if self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] else 1
self.gripper_control_message.toggle_light = False
gripper_absolute = self.gripper_control_message.gripper_position_absolute
gripper_relative = self.gripper_control_message.gripper_position_relative
@@ -320,7 +365,8 @@ class EffectorsControl(object):
self.gripper_node.write_registers(0, DEFAULT_GRIPPER_REGISTERS)
self.new_gripper_control_message = False
self.gripper_control_message = None
self.new_gripper_control_message = False
def send_gripper_status_message(self):
registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
@@ -348,6 +394,10 @@ class EffectorsControl(object):
self.mining_control_message = control_message
self.new_mining_control_message = True
def camera_control_message_received__callback(self, control_message):
self.camera_control_message = control_message
self.new_camera_control_message = True
if __name__ == "__main__":
EffectorsControl()

12
software/ros_packages/rover_control/src/effectors_control/effectors_tester.py
View File

@@ -2,20 +2,18 @@
import rospy
import time
from rover_control.msg import GripperControlMessage
from rover_control.msg import CameraControlMessage
TOPIC = "/gripper/control"
TOPIC = "/rover_control/camera/control"
rospy.init_node("effectors_tester")
publisher = rospy.Publisher(TOPIC, GripperControlMessage, queue_size=1)
publisher = rospy.Publisher(TOPIC, CameraControlMessage, queue_size=1)
time.sleep(2)
message = GripperControlMessage()
message.gripper_mode = 1
message.gripper_position = 0
message.should_home = 0
message = CameraControlMessage()
message.camera_mode = 2
publisher.publish(message)

58
software/ros_packages/rover_control/src/effectors_control/emergency_scale.py Normal file
View File

@@ -0,0 +1,58 @@
#!/usr/bin/env python
#####################################
# Imports
#####################################
# Python native imports
import rospy
from time import time, sleep
import serial
from std_msgs.msg import Float64
#####################################
# Global Variables
#####################################
NODE_NAME = "effectors_control"
# ##### Communication Defines #####
DEFAULT_PORT = "/dev/rover/ttyScale"
# DEFAULT_PORT = "/dev/ttyUSB3"
DEFAULT_BAUD = 115200
DEFAULT_TOPIC = "scale/measurement"
CAL_FACTOR = 89500
#####################################
# DriveControl Class Definition
#####################################
class EmergencyScale(object):
def __init__(self):
rospy.init_node(NODE_NAME)
self.scale_serial = serial.Serial(port=DEFAULT_PORT, baudrate=DEFAULT_BAUD, timeout=200)
self.publisher = rospy.Publisher(DEFAULT_TOPIC, Float64, queue_size=1)
self.run()
def run(self):
sleep(.2)
self.scale_serial.write(str(CAL_FACTOR))
sleep(.2)
while not rospy.is_shutdown():
value = self.scale_serial.readline()
try:
message = Float64()
message.data = float(value)
self.publisher.publish(message)
print
except:
print "No data"
if __name__ == "__main__":
EmergencyScale()

3
software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch
View File

@@ -24,7 +24,8 @@
{name: "/cameras/main_navigation/camera_control", compress: false, rate: 5.0},
{name: "/cameras/end_effector/camera_control", compress: false, rate: 5.0},
{name: "/rover_status/update_requested", compress: false, rate: 5.0},
{name: "/rover_arm/control/absolute", compress: true, rate: 5.0}]
{name: "/rover_arm/control/absolute", compress: true, rate: 5.0},
{name: "/rover_control/camera/control", compress: true, rate: 5.0}]
</rosparam>
</node>
</group>

7
software/ros_packages/rover_main/launch/rover/control.launch
View File

@@ -35,8 +35,11 @@
<node name="chassis_pan_tilt" pkg="rover_control" type="chassis_pan_tilt_control.py" respawn="true" output="screen"/>
<!--<node name="effectors" pkg="rover_control" type="effectors_control.py" respawn="true" output="screen">-->
<node name="effectors" pkg="rover_control" type="effectors_control.py" respawn="true" output="screen">
<!--<param name="port" value="/dev/rover/ttyARM"/>-->
<!--</node>-->
</node>
<node name="scale" pkg="rover_control" type="emergency_scale.py" respawn="true" output="screen">
</node>
</group>
</launch>

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

@@ -158,7 +158,7 @@
{name: "/rover_control/mining/status", compress: false, rate: 5.0},
{name: "/rover_control/gripper/status", compress: false, rate: 5.0},
{name: "/rover_science/rdf/data", compress: false, rate: 50.0},
{name: "/rover_science/soil_probe/data", compress: false, rate: 50.0}
{name: "/rover_control/scale/measurement", compress: false, rate: 20.0}
]
</rosparam>
</node>

6
software/ros_packages/rover_status/src/system_statuses_node.py
View File

@@ -143,7 +143,11 @@ class SystemStatuses:
self.GPS_msg.num_satellites = int(self.Nmea_Message.num_sats)
self.GPS_msg.horizontal_dilution = float(self.Nmea_Message.horizontal_dil)
if self.Nmea_Message.sentence_type == 'VTG':
self.GPS_msg.kmph = float(self.Nmea_Message.spd_over_grnd_kmph)
try:
self.GPS_msg.kmph = float(self.Nmea_Message.spd_over_grnd_kmph)
except:
pass
if self.Nmea_Message.true_track is not None:
self.GPS_msg.gps_heading = float(self.Nmea_Message.true_track)
else: