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

Gripper control changed a bit, needs more work. Readouts for soil probe on gui, finished soil/rdf node.

Browse Source
This commit is contained in:
Corwin Perren
2018-08-08 05:13:04 -07:00
parent 05b4c85ad2
commit 97cd60325d
20 changed files with 702 additions and 361 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
software/firmware/tower/tower.ino
View File

@@ -131,6 +131,7 @@ void loop() {
poll_modbus();
set_leds();
send_imu_stream_line(root);
add_cal_status(root);
process_gps_and_send_if_ready(root);
process_white_led_command();
get_co2_data();
@@ -221,31 +222,17 @@ void setup_imu(){
// }
}
void display_cal_status(void)
void add_cal_status(JsonObject &root)
{
/* Get the four calibration values (0..3) */
/* Any sensor data reporting 0 should be ignored, */
/* 3 means 'fully calibrated" */
JsonObject& imu_cal_object = root.createNestedObject("imu_cal");
uint8_t system, gyro, accel, mag;
system = gyro = accel = mag = 0;
bno.getCalibration(&system, &gyro, &accel, &mag);
/* The data should be ignored until the system calibration is > 0 */
Serial.print("\t");
if (!system)
{
Serial.print("! ");
}
/* Display the individual values */
Serial.print("Sys:");
Serial.print(system, DEC);
Serial.print(" G:");
Serial.print(gyro, DEC);
Serial.print(" A:");
Serial.print(accel, DEC);
Serial.print(" M:");
Serial.println(mag, DEC);
imu_cal_object["gyro"] = gyro;
imu_cal_object["accel"] = accel;
imu_cal_object["mag"] = mag;
}
void process_gps_and_send_if_ready(JsonObject &root) {

1
software/ground_station_setup.sh
View File

@@ -14,6 +14,7 @@ folders_to_link=(
rover_camera
rover_status
rover_arm
rover_science
)
# Print heading

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

@@ -106,6 +106,8 @@ class ArmIndication(QtCore.QObject):
thumb_current_updated__signal = QtCore.pyqtSignal(int)
middlefinger_current_updated__signal = QtCore.pyqtSignal(int)
gripper_reported_setpoint_updated__signal = QtCore.pyqtSignal(int)
def __init__(self, shared_objects):
super(ArmIndication, self).__init__()
@@ -153,6 +155,8 @@ 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_setpoint_lcd_number = self.right_screen.gripper_reported_setpoint_lcd_number # type: QtWidgets.QLCDNumber
# ########## Get the settings instance ##########
self.settings = QtCore.QSettings()
@@ -205,6 +209,8 @@ 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_setpoint_updated__signal.connect(self.gripper_reported_setpoint_lcd_number.display)
def on_arm_status_update_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))
@@ -248,6 +254,8 @@ 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_setpoint_updated__signal.emit(data.current_finger_position)
@staticmethod
def process_faults_to_string(faults):
fault_output = ""

22
software/ros_packages/ground_station/src/Framework/InputSystems/LogitechControllerControlSender.py
View File

@@ -138,7 +138,7 @@ class LogitechJoystick(QtCore.QThread):
events = self.gamepad.read()
for event in events:
print event.code, event.state
# print event.code, event.state
if event.code in self.raw_mapping_to_class_mapping:
self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state
@@ -164,7 +164,7 @@ class LogitechControllerControlSender(QtCore.QThread):
self.shared_objects = shared_objects
self.video_coordinator = self.shared_objects["threaded_classes"]["Video Coordinator"]
self.right_screen = self.shared_objects["screens"]["right_screen"]
self.speed_limit_progress_bar = self.right_screen.speed_limit_progress_bar # type: QtWidgets.QProgressBar
self.rover_speed_limit_slider = self.right_screen.rover_speed_limit_slider # type: QtWidgets.QSlider
self.left_drive_progress_bar = self.right_screen.left_drive_progress_bar # type: QtWidgets.QProgressBar
self.right_drive_progress_bar = self.right_screen.right_drive_progress_bar # type: QtWidgets.QProgressBar
@@ -219,11 +219,11 @@ class LogitechControllerControlSender(QtCore.QThread):
self.logger.debug("Stopping Joystick Thread")
def connect_signals_and_slots(self):
self.set_speed_limit__signal.connect(self.speed_limit_progress_bar.setValue)
self.set_left_drive_output__signal.connect(self.left_drive_progress_bar.setValue)
self.set_right_drive_output__signal.connect(self.right_drive_progress_bar.setValue)
self.video_coordinator.pan_tilt_selection_changed__signal.connect(self.on_pan_tilt_selection_changed__slot)
self.rover_speed_limit_slider.valueChanged.connect(self.on_speed_limit_slider_value_changed__slot)
def check_and_set_pause_state(self):
thumb_pressed = self.joystick.controller_states["start"]
@@ -238,18 +238,15 @@ class LogitechControllerControlSender(QtCore.QThread):
self.publish_pan_tilt_control_commands()
def publish_drive_command(self):
throttle_axis = 1
# throttle_axis = max((255 - self.joystick.controller_states["throttle_axis"]) / 255.0, THROTTLE_MIN)
if self.drive_paused:
drive_message = DriveCommandMessage()
else:
drive_message = self.get_drive_message(throttle_axis)
drive_message = self.get_drive_message(self.speed_limit)
left_output = abs(drive_message.drive_twist.linear.x - drive_message.drive_twist.angular.z)
right_output = abs(drive_message.drive_twist.linear.x + drive_message.drive_twist.angular.z)
self.set_speed_limit__signal.emit(throttle_axis * 100)
self.set_speed_limit__signal.emit(self.speed_limit * 100)
self.set_left_drive_output__signal.emit(left_output * 100)
self.set_right_drive_output__signal.emit(right_output * 100)
@@ -303,14 +300,14 @@ class LogitechControllerControlSender(QtCore.QThread):
pan_tilt_message.relative_tilt_adjustment = -(hat_y * CHASSIS_PAN_TILT_Y_AXIS_SCALAR)
self.chassis_pan_tilt_command_publisher.publish(pan_tilt_message)
def get_drive_message(self, throttle_axis):
def get_drive_message(self, speed_limit):
drive_message = DriveCommandMessage()
left_y_axis = self.joystick.controller_states["left_y_axis"] if abs(self.joystick.controller_states["left_y_axis"]) > STICK_DEADBAND else 0
right_y_axis = self.joystick.controller_states["right_y_axis"] if abs(self.joystick.controller_states["right_y_axis"]) > STICK_DEADBAND else 0
left_y_axis = throttle_axis * (-(left_y_axis - STICK_OFFSET) / STICK_MAX)
right_y_axis = throttle_axis * (-(right_y_axis - STICK_OFFSET) / STICK_MAX)
left_y_axis = speed_limit * (-(left_y_axis - STICK_OFFSET) / STICK_MAX)
right_y_axis = speed_limit * (-(right_y_axis - STICK_OFFSET) / STICK_MAX)
drive_message.drive_twist.linear.x = (left_y_axis + right_y_axis) / 2.0
drive_message.drive_twist.angular.z = (right_y_axis - left_y_axis) / 2.0
@@ -320,6 +317,9 @@ class LogitechControllerControlSender(QtCore.QThread):
def on_pan_tilt_selection_changed__slot(self, selection):
self.current_pan_tilt_selection = selection
def on_speed_limit_slider_value_changed__slot(self, value):
self.speed_limit = value / 100.0
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)

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

@@ -29,7 +29,7 @@ ROLL_SCALAR = 0.003
WRIST_PITCH_SCALAR = 0.003
WRIST_ROLL_SCALAR = 0.006
GRIPPER_MOVEMENT_SCALAR = 300
GRIPPER_MOVEMENT_SCALAR = 1500
LEFT_X_AXIS_DEADZONE = 1500
LEFT_Y_AXIS_DEADZONE = 1500
@@ -180,6 +180,8 @@ class XBOXControllerControlSender(QtCore.QThread):
"SCISSOR": 4
}
PINCH_MODE_ABSOLUTE_SET_POSITION = 57740
def __init__(self, shared_objects):
super(XBOXControllerControlSender, self).__init__()
@@ -222,6 +224,7 @@ class XBOXControllerControlSender(QtCore.QThread):
self.last_xbox_button_state = 0
self.last_left_bumper_state = 0
self.last_right_bumper_state = 0
self.last_back_button_state = 0
self.gripper_control_mode = 0
self.gripper_control_mode_just_changed = False
@@ -253,7 +256,8 @@ class XBOXControllerControlSender(QtCore.QThread):
"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]
self.gripper_mode_normal_stylesheet_update_ready__signal],
"ABS_MOVE": self.PINCH_MODE_ABSOLUTE_SET_POSITION
}
}
@@ -266,6 +270,7 @@ class XBOXControllerControlSender(QtCore.QThread):
self.change_control_state_if_needed()
if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"):
self.send_gripper_home_on_back_press()
self.process_and_send_arm_control()
elif self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("MINING"):
self.send_mining_commands()
@@ -331,6 +336,12 @@ class XBOXControllerControlSender(QtCore.QThread):
for signal in signal_map["UNSET"]:
signal.emit(COLOR_NONE)
if "ABS_MOVE" in signal_map:
gripper_control_message = GripperControlMessage()
gripper_control_message.gripper_mode = self.GRIPPER_CONTROL_MODES["SCISSOR"]
gripper_control_message.gripper_position_absolute = signal_map["ABS_MOVE"]
self.gripper_control_publisher.publish(gripper_control_message)
else:
self.send_new_gripper_mode = True
self.gripper_control_mode_just_changed = False
@@ -340,12 +351,17 @@ class XBOXControllerControlSender(QtCore.QThread):
arm_control_message = ArmControlMessage()
gripper_control_message = GripperControlMessage()
gripper_control_message.gripper_position_absolute = -1
gripper_control_message.gripper_mode = self.gripper_control_mode + 1
should_publish_arm = False
should_publish_gripper = True if self.send_new_gripper_mode else False
if self.send_new_gripper_mode:
gripper_control_message.gripper_position_absolute = 0
else:
gripper_control_message.gripper_position_absolute = -1
gripper_control_message.gripper_mode = self.gripper_control_mode + 1
left_trigger = self.controller.controller_states["left_trigger"]
right_trigger = self.controller.controller_states["right_trigger"]
@@ -386,6 +402,18 @@ class XBOXControllerControlSender(QtCore.QThread):
self.gripper_control_publisher.publish(gripper_control_message)
self.send_new_gripper_mode = False
def send_gripper_home_on_back_press(self):
gripper_control_message = GripperControlMessage()
back_state = self.controller.controller_states["back_button"]
if self.last_back_button_state == 0 and back_state == 1:
gripper_control_message.should_home = True
gripper_control_message.gripper_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:
self.last_back_button_state = 0
def send_mining_commands(self):
left_y_axis = self.controller.controller_states["left_y_axis"] if abs(

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

@@ -8,6 +8,7 @@ import logging
import rospy
from rover_control.msg import MiningStatusMessage, MiningControlMessage
from rover_science.msg import SoilSensorStatusMessage
#####################################
# Global Variables
@@ -15,6 +16,8 @@ from rover_control.msg import MiningStatusMessage, MiningControlMessage
MINING_STATUS_TOPIC = "/rover_control/mining/status"
MINING_CONTROL_TOPIC = "/rover_control/mining/control"
SOIL_PROBE_TOPIC = "/rover_science/soil_probe/data"
TRAVEL_POSITION_LIFT = 110
TRAVEL_POSITION_TILT = 1023
@@ -35,6 +38,13 @@ class Mining(QtCore.QObject):
weight_measurement_update_ready__signal = QtCore.pyqtSignal(int)
temp_update_ready__signal = QtCore.pyqtSignal(float)
moisture_update_ready__signal = QtCore.pyqtSignal(float)
loss_tangent_update_ready__signal = QtCore.pyqtSignal(float)
electrical_conductivity_update_ready__signal = QtCore.pyqtSignal(float)
real_dielectric_update_ready__signal = QtCore.pyqtSignal(float)
imaginary_dielectric_update_ready__signal = QtCore.pyqtSignal(float)
def __init__(self, shared_objects):
super(Mining, self).__init__()
@@ -54,6 +64,13 @@ class Mining(QtCore.QObject):
self.mining_transport_move_button = self.left_screen.mining_transport_move_button # type:QtWidgets.QPushButton
self.mining_scoop_move_button = self.left_screen.mining_scoop_move_button # type:QtWidgets.QPushButton
self.science_temp_lcd_number = self.left_screen.science_temp_lcd_number # type:QtWidgets.QLCDNumber
self.science_moisture_lcd_number = self.left_screen.science_moisture_lcd_number # type:QtWidgets.QLCDNumber
self.science_loss_tangent_lcd_number = self.left_screen.science_loss_tangent_lcd_number # type:QtWidgets.QLCDNumber
self.science_electrical_conductivity_lcd_number = self.left_screen.science_electrical_conductivity_lcd_number # type:QtWidgets.QLCDNumber
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
# ########## Get the settings instance ##########
self.settings = QtCore.QSettings()
@@ -67,6 +84,8 @@ class Mining(QtCore.QObject):
self.mining_status_subscriber = rospy.Subscriber(MINING_STATUS_TOPIC, MiningStatusMessage,
self.mining_status_message_received__callback)
self.soil_probe_subscriber = rospy.Subscriber(SOIL_PROBE_TOPIC, SoilSensorStatusMessage, self.on_soil_probe_message_received__callback)
self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
self.connect_signals_and_slots()
@@ -85,6 +104,13 @@ class Mining(QtCore.QObject):
self.weight_measurement_update_ready__signal.connect(self.mining_qlcdnumber.display)
self.temp_update_ready__signal.connect(self.science_temp_lcd_number.display)
self.moisture_update_ready__signal.connect(self.science_moisture_lcd_number.display)
self.loss_tangent_update_ready__signal.connect(self.science_loss_tangent_lcd_number.display)
self.electrical_conductivity_update_ready__signal.connect(self.science_electrical_conductivity_lcd_number.display)
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)
def on_mining_set_cal_factor_clicked__slot(self):
message = MiningControlMessage()
@@ -141,3 +167,11 @@ class Mining(QtCore.QObject):
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)
def on_soil_probe_message_received__callback(self, data):
self.temp_update_ready__signal.emit(data.temp_c)
self.moisture_update_ready__signal.emit(data.moisture)
self.loss_tangent_update_ready__signal.emit(data.loss_tangent)
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)

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

@@ -13,7 +13,7 @@ from std_msgs.msg import Float64MultiArray
#####################################
# Global Variables
#####################################
RDF_DATA_TOPIC = "/rdf/data"
RDF_DATA_TOPIC = "/rover_science/rdf/data"
THREAD_HERTZ = 5

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

@@ -1390,7 +1390,7 @@ N/A</string>
</size>
</property>
<property name="currentIndex">
<number>1</number>
<number>2</number>
</property>
<widget class="QWidget" name="tab_5">
<attribute name="title">
@@ -1652,7 +1652,7 @@ N/A</string>
</widget>
<widget class="QWidget" name="tab_3">
<attribute name="title">
<string>Mining</string>
<string>Mining / Science</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_8">
<item row="5" column="0">
@@ -1721,7 +1721,7 @@ N/A</string>
<widget class="QLabel" name="label_13">
<property name="font">
<font>
<pointsize>20</pointsize>
<pointsize>16</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
@@ -1851,13 +1851,13 @@ N/A</string>
<property name="minimumSize">
<size>
<width>300</width>
<height>150</height>
<height>75</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>300</width>
<height>150</height>
<height>75</height>
</size>
</property>
<property name="font">
@@ -1905,7 +1905,7 @@ N/A</string>
</item>
</layout>
</item>
<item row="8" column="0">
<item row="10" column="0">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
@@ -1976,6 +1976,216 @@ N/A</string>
</item>
</layout>
</item>
<item row="8" column="0">
<widget class="Line" name="line_9">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="9" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_16">
<item>
<layout class="QVBoxLayout" name="verticalLayout_10">
<item>
<widget class="QLabel" name="label_20">
<property name="font">
<font>
<pointsize>16</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Science Probe Data</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_17">
<item row="0" column="0">
<layout class="QVBoxLayout" name="verticalLayout_6">
<item>
<widget class="QLabel" name="label_21">
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Temp °C</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="science_temp_lcd_number">
<property name="minimumSize">
<size>
<width>0</width>
<height>35</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
<item row="0" column="1">
<layout class="QVBoxLayout" name="verticalLayout_11">
<item>
<widget class="QLabel" name="moisture_label">
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Moisture %</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="science_moisture_lcd_number">
<property name="minimumSize">
<size>
<width>0</width>
<height>35</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
<item row="0" column="2">
<layout class="QVBoxLayout" name="verticalLayout_12">
<item>
<widget class="QLabel" name="tangent_label">
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Loss Tangent</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="science_loss_tangent_lcd_number">
<property name="minimumSize">
<size>
<width>0</width>
<height>35</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
<item row="1" column="0">
<layout class="QVBoxLayout" name="verticalLayout_13">
<item>
<widget class="QLabel" name="conductivity_label">
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Soil
Electrical
Conductivity</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="science_electrical_conductivity_lcd_number">
<property name="minimumSize">
<size>
<width>0</width>
<height>35</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
<item row="1" column="1">
<layout class="QVBoxLayout" name="verticalLayout_16">
<item>
<widget class="QLabel" name="real_dielectric_label">
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Real
Dielectric
Permittivity</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="science_real_dielectric_lcd_number">
<property name="minimumSize">
<size>
<width>0</width>
<height>35</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
<item row="1" column="2">
<layout class="QVBoxLayout" name="verticalLayout_18">
<item>
<widget class="QLabel" name="imaginary_label">
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Imaginary
Dielectric
Permittivity</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="science_imaginary_dielectric_lcd_number">
<property name="minimumSize">
<size>
<width>0</width>
<height>35</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
</layout>
</item>
</layout>
</item>
</layout>
</widget>
<widget class="QWidget" name="tab">

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

@@ -1551,7 +1551,7 @@ Position</string>
</font>
</property>
<property name="text">
<string>Mode Readouts</string>
<string>Mode Readouts / Setpoint</string>
</property>
</widget>
</item>
@@ -1801,6 +1801,34 @@ Position</string>
</property>
</spacer>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_68">
<item>
<widget class="QLabel" name="label_101">
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Reported Setpoint</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="gripper_reported_setpoint_lcd_number">
<property name="minimumSize">
<size>
<width>0</width>
<height>50</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
</layout>
@@ -2201,8 +2229,23 @@ Position</string>
</item>
</layout>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_15">
<item>
<spacer name="verticalSpacer_3">
<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>
<widget class="QLabel" name="label_3">
<property name="font">
@@ -2221,46 +2264,20 @@ Position</string>
</widget>
</item>
<item>
<widget class="QProgressBar" name="speed_limit_progress_bar">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>133</width>
<height>16777215</height>
</size>
</property>
<widget class="QSlider" name="rover_speed_limit_slider">
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>50</number>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
<property name="textVisible">
<bool>true</bool>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="invertedAppearance">
<bool>false</bool>
</property>
<property name="textDirection">
<enum>QProgressBar::TopToBottom</enum>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer_2">
<property name="orientation">

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

@@ -20,8 +20,8 @@ from rover_control.msg import MiningControlMessage, MiningStatusMessage, Gripper
NODE_NAME = "effectors_control"
# ##### Communication Defines #####
# DEFAULT_PORT = "/dev/rover/ttyEffectors"
DEFAULT_PORT = "/dev/ttyUSB1"
DEFAULT_PORT = "/dev/rover/ttyEffectors"
# DEFAULT_PORT = "/dev/ttyUSB0"
DEFAULT_BAUD = 115200
GRIPPER_NODE_ID = 1
@@ -227,7 +227,7 @@ class EffectorsControl(object):
self.process_gripper_control_message()
self.send_gripper_status_message()
except Exception, error:
print error
pass #print ""error
def connect_to_nodes(self):
self.gripper_node = minimalmodbus.Instrument(self.port, int(self.gripper_node_id))
@@ -309,20 +309,35 @@ class EffectorsControl(object):
def process_gripper_control_message(self):
if self.new_gripper_control_message:
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["MODE"]] = self.gripper_control_message.gripper_mode
if self.gripper_control_message.should_home:
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["HOME"]] = 1
self.gripper_node.write_registers(0, self.gripper_registers)
self.gripper_node.write_registers(0, DEFAULT_GRIPPER_REGISTERS)
homing_complete = False
while not homing_complete:
registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
# print registers
if registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION_OUTPUT"]] == 0:
homing_complete = True
self.gripper_registers = registers
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["HOME"]] = self.gripper_control_message.should_home
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["MODE"]] = self.gripper_control_message.gripper_mode
gripper_absolute = self.gripper_control_message.gripper_position_absolute
gripper_relative = self.gripper_control_message.gripper_position_relative
if -1 < gripper_absolute < UINT16_MAX:
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(gripper_absolute, 0), GRIPPER_UNIVERSAL_POSITION_MAX)
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(gripper_absolute, 0), UINT16_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), GRIPPER_UNIVERSAL_POSITION_MAX)
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(new_position, 0), UINT16_MAX)
self.gripper_node.write_registers(0, self.gripper_registers)
@@ -332,6 +347,7 @@ class EffectorsControl(object):
def send_gripper_status_message(self):
registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
# print registers
message = GripperStatusMessage()
message.pinch_position_raw = registers[GRIPPER_MODBUS_REGISTERS["POSITION_PINCH"]]

4
software/ros_packages/rover_main/launch/rover/science.launch
View File

@@ -1,3 +1,5 @@
<launch>
<node name="rover_science" pkg="rover_science" type="rover_science.py" respawn="true" output="screen"/>
<group ns="rover_science">
<node name="rover_science" pkg="rover_science" type="rover_science_node.py" respawn="true" output="screen"/>
</group>
</launch>

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

@@ -157,7 +157,8 @@
{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}
{name: "/rover_science/rdf/data", compress: false, rate: 50.0},
{name: "/rover_science/soil_probe/data", compress: false, rate: 50.0}
]
</rosparam>
</node>

5
software/ros_packages/rover_odometry/src/odometry.py
View File

@@ -83,6 +83,7 @@ class Odometry(object):
gps = temp.get('gps', None)
imu = temp.get('imu', None)
imu_cal = temp.get('imu_cal', None)
if gps:
# ###### THIS IS HERE TO DEAL WITH UBLOX GPS #####
@@ -98,6 +99,9 @@ class Odometry(object):
# print imu
self.broadcast_imu(imu)
# if imu_cal:
# print imu_cal
@staticmethod
def chksum_nmea(sentence):
# String slicing: Grabs all the characters
@@ -148,5 +152,6 @@ class Odometry(object):
self.imu_data_publisher.publish(message)
if __name__ == "__main__":
Odometry()

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

@@ -56,6 +56,7 @@ find_package(catkin REQUIRED COMPONENTS
add_message_files(
FILES
SoilSensorStatusMessage.msg
)
## Generate services in the 'srv' folder

2
software/ros_packages/rover_science/msg/RDFStatusMessage.msg
View File

@@ -1,2 +0,0 @@
uint16 raw_value
double time

7
software/ros_packages/rover_science/msg/SoilSensorStatusMessage.msg Normal file
View File

@@ -0,0 +1,7 @@
float32 temp_c
float32 moisture
float32 loss_tangent
float32 soil_electrical_conductivity
float32 real_dielectric_permittivity
float32 imaginary_dielectric_permittivity

1
software/ros_packages/rover_science/package.xml
View File

@@ -57,6 +57,7 @@
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>std_msgs</exec_depend>

239
software/ros_packages/rover_science/src/rover_science.py
View File

@@ -1,239 +0,0 @@
#!/usr/bin/env python
#####################################
# Imports
#####################################
# Python native imports
import rospy
from time import time, sleep
import serial.rs485
import minimalmodbus
import numpy
# Custom Imports
from rover_control.msg import TowerPanTiltControlMessage
from std_msgs.msg import Float64MultiArray
#####################################
# Global Variables
#####################################
NODE_NAME = "science_node"
DEFAULT_PORT = "/dev/rover/ttyRDF_SoilProbe"
DEFAULT_BAUD = 9600
DEFAULT_INVERT = False
DEFAULT_RDF_PUBLISHER_TOPIC = "rdf/data"
PAN_TILT_NODE_ID = 1
COMMUNICATIONS_TIMEOUT = 0.1 # Seconds
RX_DELAY = 0.01
TX_DELAY = 0.01
DEFAULT_HERTZ = 20
SOIL_PROBE_ADDRESS = "mar"
PAN_TILT_MODBUS_REGISTERS = {
"CENTER_ALL": 0,
"PAN_ADJUST_POSITIVE": 1,
"PAN_ADJUST_NEGATIVE": 2,
"TILT_ADJUST_POSITIVE": 3,
"TILT_ADJUST_NEGATIVE": 4
}
SOIL_PROBE_COMMANDS = {
"GET_ADDRESS": "AD=",
"DESCRIPTION": "DS=",
"PROBE_ENABLED": "PE=",
"TAKE_READING": "TR",
"TRANSMIT_READING": "T0",
"QUERY": "?"
}
NODE_LAST_SEEN_TIMEOUT = 2 # seconds
#####################################
# DriveControl Class Definition
#####################################
class RoverScience(object):
def __init__(self):
rospy.init_node(NODE_NAME)
self.port = rospy.get_param("~port", DEFAULT_PORT)
self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
self.pan_tilt_node_id = rospy.get_param("~pan_tilt_node_id", PAN_TILT_NODE_ID)
self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
self.pan_tilt_node = None
self.tower_node = None
self.connect_to_pan_tilt_and_tower()
self.rdf_publisher = rospy.Publisher(DEFAULT_RDF_PUBLISHER_TOPIC, Float64MultiArray, queue_size=1)
self.pan_tilt_control_message = None
self.new_pan_tilt_control_message = False
self.modbus_nodes_seen_time = time()
self.data = numpy.array([])
self.times = numpy.array([])
self.max_data_len = 200
self.count = 0
self.start_time = time()
self.run()
def __setup_minimalmodbus_for_485(self):
self.pan_tilt_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT) # type: serial.Serial
self.pan_tilt_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
delay_before_rx=RX_DELAY,
delay_before_tx=TX_DELAY)
def run(self):
# self.send_startup_centering_command()
# self.pan_tilt_node.serial.timeout(500)
out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["PROBE_ENABLED"] + "1")
self.pan_tilt_node.serial.write(out)
sleep(0.1)
while not rospy.is_shutdown():
out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["TAKE_READING"])
self.pan_tilt_node.serial.write(out)
sleep(0.1)
out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["TRANSMIT_READING"])
self.pan_tilt_node.serial.write(out)
start_time = time()
char = ""
response = ""
while char != '\n' and (time() - start_time) < 2:
# print time() - start_time, (time() - start_time) > 2
if self.pan_tilt_node.serial.inWaiting():
char = self.pan_tilt_node.serial.read()
response += char
# print char
if response:
print response
else:
print "timeout"
# print "No response"
# start_time = time()
# try:
# registers = self.pan_tilt_node.read_registers(0, 1)
# self.rdf_publisher.publish(Float64MultiArray(data=[registers[0], time()]))
# self.modbus_nodes_seen_time = time()
# except Exception, Error:
# print Error
#
# if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
# print "Science not seen for", NODE_LAST_SEEN_TIMEOUT, "seconds. Exiting."
# return # Exit so respawn can take over
# time_diff = time() - start_time
# def run(self):
# # self.send_startup_centering_command()
# while not rospy.is_shutdown():
# registers = self.pan_tilt_node.read_registers(0, 1)
# self.data = numpy.append(self.data, registers[0])
# self.times = numpy.append(self.times, time())
#
# if len(self.data) > self.max_data_len:
# numpy.delete(self.data, 0)
# numpy.delete(self.times, 0)
#
# # for item in self.smoothListTriangle(self.data):
# # print item
#
# print fq.freq_from_fft(self.data, 1/40.0)
# # self.data = self.smoothListGaussian(self.data)
# #
# # w = numpy.fft.fft(self.data)
# #
# # # for item in w:
# # # print abs(item)
# # #
# # # print
# # # print
# # numpy.delete(w, 0)
# # freqs = numpy.fft.fftfreq(len(w), 1/40.0)
# # # # print len(freqs), len(w)
# # #
# # # # print freqs
# # # # print(freqs.min(), freqs.max())
# # # # # (-0.5, 0.499975)
# # # #
# # # # # Find the peak in the coefficients
# # idx = numpy.argmax(numpy.abs(w))
# # freq = freqs[idx]
# # freq_in_hertz = abs(freq * 40)
# # print(freq_in_hertz)
def smoothListTriangle(self, list, strippedXs=False, degree=5):
weight = []
window = degree * 2 - 1
smoothed = [0.0] * (len(list) - window)
for x in range(1, 2 * degree): weight.append(degree - abs(degree - x))
w = numpy.array(weight)
for i in range(len(smoothed)):
smoothed[i] = sum(numpy.array(list[i:i + window]) * w) / float(sum(w))
return smoothed
def smoothListGaussian(self, list, strippedXs=False, degree=5):
window = degree * 2 - 1
weight = numpy.array([1.0] * window)
weightGauss = []
for i in range(window):
i = i - degree + 1
frac = i / float(window)
gauss = 1 / (numpy.exp((4 * (frac)) ** 2))
weightGauss.append(gauss)
weight = numpy.array(weightGauss) * weight
smoothed = [0.0] * (len(list) - window)
for i in range(len(smoothed)):
smoothed[i] = sum(numpy.array(list[i:i + window]) * weight) / sum(weight)
return smoothed
def connect_to_pan_tilt_and_tower(self):
self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.pan_tilt_node_id))
self.__setup_minimalmodbus_for_485()
if __name__ == "__main__":
RoverScience()

265
software/ros_packages/rover_science/src/rover_science_node.py Executable file
View File

@@ -0,0 +1,265 @@
#!/usr/bin/env python
#####################################
# Imports
#####################################
# Python native imports
import rospy
from time import time, sleep
import serial.rs485
import minimalmodbus
import numpy
# Custom Imports
from rover_science.msg import SoilSensorStatusMessage
from std_msgs.msg import Float64MultiArray
#####################################
# Global Variables
#####################################
NODE_NAME = "science_node"
DEFAULT_PORT = "/dev/rover/ttyRDF_SoilProbe"
DEFAULT_RDF_BAUD = 115200
DEFAULT_SOIL_BAUD = 9600
DEFAULT_RDF_PUBLISHER_TOPIC = "rdf/data"
DEFAULT_SOIL_PROBE_PUBLISHER_TOPIC = "soil_probe/data"
RDF_NODE_ID = 1
COMMUNICATIONS_TIMEOUT = 0.1 # Seconds
RX_DELAY = 0.01
TX_DELAY = 0.01
DEFAULT_HERTZ = 20
FAILED_RDF_LIMIT = 20
SOIL_PROBE_READ_TIMEOUT = 8
SOIL_PROBE_EXIT_TIMEOUT = 20
SOIL_PROBE_ADDRESS = "mar"
PAN_TILT_MODBUS_REGISTERS = {
"CENTER_ALL": 0,
"PAN_ADJUST_POSITIVE": 1,
"PAN_ADJUST_NEGATIVE": 2,
"TILT_ADJUST_POSITIVE": 3,
"TILT_ADJUST_NEGATIVE": 4
}
SOIL_PROBE_COMMANDS = {
"GET_ADDRESS": "AD=",
"DESCRIPTION": "DS=",
"PROBE_ENABLED": "PE=",
"TAKE_READING": "TR",
"TRANSMIT_READING": "T3",
"QUERY": "?"
}
TRANSMIT_SET_3_INDICES = {
"TEMP C": 0,
"Moisture": 2,
"Loss Tangent": 3,
"Soil Electrical Conductivity (tc)": 4,
"Real Dielectric Permittivity (tc)": 6,
"Imag Dielectric Permittivity (tc)": 8,
}
NODE_LAST_SEEN_TIMEOUT = 2 # seconds
#####################################
# DriveControl Class Definition
#####################################
class RoverScience(object):
INSTRUMENTS = [
"RDF",
"SOIL"
]
def __init__(self):
rospy.init_node(NODE_NAME)
self.port = rospy.get_param("~port", DEFAULT_PORT)
self.baud = rospy.get_param("~baud", DEFAULT_RDF_BAUD)
self.science_node_id = rospy.get_param("~pan_tilt_node_id", RDF_NODE_ID)
self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
self.rdf_node = None
self.soil_node = None
self.connect_to_rdf()
self.rdf_publisher = rospy.Publisher(DEFAULT_RDF_PUBLISHER_TOPIC, Float64MultiArray, queue_size=1)
self.soil_probe_publisher = rospy.Publisher(DEFAULT_SOIL_PROBE_PUBLISHER_TOPIC, SoilSensorStatusMessage, queue_size=1)
self.modbus_nodes_seen_time = time()
self.failed_rdf_modbus_count = 0
self.soil_probe_timeout_cumulative = 0
self.which_instrument = self.INSTRUMENTS.index("RDF")
self.probe_response_line = ""
self.run()
def __setup_minimalmodbus_for_485(self):
self.rdf_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT) # type: serial.Serial
self.rdf_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
delay_before_rx=RX_DELAY,
delay_before_tx=TX_DELAY)
def run(self):
while not rospy.is_shutdown():
if self.which_instrument == self.INSTRUMENTS.index("RDF"):
try:
registers = self.rdf_node.read_registers(0, 1)
self.rdf_publisher.publish(Float64MultiArray(data=[registers[0], time()]))
self.failed_rdf_modbus_count = 0
except Exception, e:
# print e
self.failed_rdf_modbus_count += 1
if self.failed_rdf_modbus_count == FAILED_RDF_LIMIT:
print "RDF not present. Trying soil sensor"
self.which_instrument = self.INSTRUMENTS.index("SOIL")
elif self.which_instrument == self.INSTRUMENTS.index("SOIL"):
if not self.soil_node:
self.switch_node_to_soil_probe()
self.broadcast_soil_sensor_data()
if self.soil_probe_timeout_cumulative > SOIL_PROBE_EXIT_TIMEOUT:
print "No science devices present. Exiting..."
return
def switch_node_to_soil_probe(self):
del self.rdf_node
self.soil_node = serial.rs485.RS485(self.port, baudrate=DEFAULT_SOIL_BAUD, timeout=COMMUNICATIONS_TIMEOUT) # type: serial.Serial
self.soil_node.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0, delay_before_rx=RX_DELAY, delay_before_tx=TX_DELAY)
out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["PROBE_ENABLED"] + "1")
self.soil_node.write(out)
sleep(0.1)
def broadcast_soil_sensor_data(self):
self.request_soil_reading()
self.request_reading_results()
self.get_probe_response_line()
self.process_probe_response_and_send()
def process_probe_response_and_send(self):
if self.probe_response_line != "":
try:
self.probe_response_line = self.probe_response_line.replace(SOIL_PROBE_ADDRESS, "")
split_results = self.probe_response_line.split(",")
temp_c = split_results[TRANSMIT_SET_3_INDICES["TEMP C"]]
moisture = split_results[TRANSMIT_SET_3_INDICES["Moisture"]]
loss_tangent = split_results[TRANSMIT_SET_3_INDICES["Loss Tangent"]]
sec_tc = split_results[TRANSMIT_SET_3_INDICES["Soil Electrical Conductivity (tc)"]]
rdp_tc = split_results[TRANSMIT_SET_3_INDICES["Real Dielectric Permittivity (tc)"]]
idp_tc = split_results[TRANSMIT_SET_3_INDICES["Imag Dielectric Permittivity (tc)"]]
message = SoilSensorStatusMessage()
message.temp_c = float(temp_c)
message.moisture = float(moisture)
message.loss_tangent = float(loss_tangent)
message.soil_electrical_conductivity = float(sec_tc)
message.real_dielectric_permittivity = float(rdp_tc)
message.imaginary_dielectric_permittivity = float(idp_tc)
self.soil_probe_publisher.publish(message)
except:
print "Soil probe line corrupted. Trying again..."
def get_probe_response_line(self):
start_time = time()
char = ""
self.probe_response_line = ""
while char != '\n' and (time() - start_time) < 2:
if self.soil_node.inWaiting():
char = self.soil_node.read()
self.probe_response_line += char
if self.probe_response_line:
self.soil_probe_timeout_cumulative = 0
# print self.probe_response_line
else:
# print "timeout"
self.soil_probe_timeout_cumulative += 2
def request_soil_reading(self):
out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["TAKE_READING"])
self.soil_node.write(out)
sleep(0.1)
def request_reading_results(self):
out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["TRANSMIT_READING"])
self.soil_node.write(out)
def smoothListTriangle(self, list, strippedXs=False, degree=5):
weight = []
window = degree * 2 - 1
smoothed = [0.0] * (len(list) - window)
for x in range(1, 2 * degree): weight.append(degree - abs(degree - x))
w = numpy.array(weight)
for i in range(len(smoothed)):
smoothed[i] = sum(numpy.array(list[i:i + window]) * w) / float(sum(w))
return smoothed
def smoothListGaussian(self, list, strippedXs=False, degree=5):
window = degree * 2 - 1
weight = numpy.array([1.0] * window)
weightGauss = []
for i in range(window):
i = i - degree + 1
frac = i / float(window)
gauss = 1 / (numpy.exp((4 * (frac)) ** 2))
weightGauss.append(gauss)
weight = numpy.array(weightGauss) * weight
smoothed = [0.0] * (len(list) - window)
for i in range(len(smoothed)):
smoothed[i] = sum(numpy.array(list[i:i + window]) * weight) / sum(weight)
return smoothed
def connect_to_rdf(self):
self.rdf_node = minimalmodbus.Instrument(self.port, int(self.science_node_id))
self.__setup_minimalmodbus_for_485()
if __name__ == "__main__":
RoverScience()

1
software/rover_setup.sh
View File

@@ -11,7 +11,6 @@ folders_to_link=(
rover_control
rover_arm
rover_status
zed_wrapper
nimbro_topic_transport
rover_main
rover_odometry