This commit is contained in:
Dylan Thrush
2018-08-07 00:28:35 -07:00
11 changed files with 270 additions and 147 deletions

View File

@@ -201,6 +201,8 @@ class LogitechControllerControlSender(QtCore.QThread):
self.last_camera_change_time = time()
self.last_camera_toggle_time = time()
self.speed_limit = 0.5
def run(self):
self.logger.debug("Starting Joystick Thread")

View File

@@ -1,5 +1,5 @@
'''
Mapping.py: Objected Orientated Google Maps for Python
MappingSettings.py: Objected Orientated Google Maps for Python
ReWritten by Chris Pham
Copyright OSURC, orginal code from GooMPy by Alec Singer and Simon D. Levy

View File

@@ -20,7 +20,7 @@ from sensor_msgs.msg import Imu
# Custom Imports
import RoverMap
from Resources.Settings import Mapping as MappingSettings
from Resources.Settings import MappingSettings
from sensor_msgs.msg import NavSatFix
#####################################
@@ -31,7 +31,6 @@ from sensor_msgs.msg import NavSatFix
GPS_POSITION_TOPIC = "/rover_odometry/fix"
IMU_DATA_TOPIC = "/rover_odometry/imu/data"
MAP_WIDGET_WIDTH = float(1280)
MAP_WIDGET_HEIGHT = float(720)
MAP_WIDGET_RATIO = MAP_WIDGET_WIDTH / MAP_WIDGET_HEIGHT
@@ -41,6 +40,8 @@ class RoverMapCoordinator(QtCore.QThread):
pixmap_ready_signal = QtCore.pyqtSignal()
change_waypoint_signal = QtCore.pyqtSignal()
zoom_level_enabled_state_update__signal = QtCore.pyqtSignal(int)
def __init__(self, shared_objects):
super(RoverMapCoordinator, self).__init__()
@@ -49,12 +50,15 @@ class RoverMapCoordinator(QtCore.QThread):
self.mapping_label = self.left_screen.mapping_label # type:QtWidgets.QLabel
self.navigation_label = self.left_screen.navigation_waypoints_table_widget
self.landmark_label = self.left_screen.landmark_waypoints_table_widget
self.map_selection_combo_box = self.left_screen.map_selection_combo_box # type: QtWidgets.QComboBox
self.map_zoom_level_combo_box = self.left_screen.map_zoom_level_combo_box # type: QtWidgets.QComboBox
self.setings = QtCore.QSettings()
self.logger = logging.getLogger("groundstation")
self.gps_position_subscriber = rospy.Subscriber(GPS_POSITION_TOPIC, NavSatFix, self.gps_position_updated_callback)
self.gps_position_subscriber = rospy.Subscriber(GPS_POSITION_TOPIC, NavSatFix,
self.gps_position_updated_callback)
self.run_thread_flag = True
self.setup_map_flag = True
@@ -65,7 +69,10 @@ class RoverMapCoordinator(QtCore.QThread):
self.overlay_image = None
self.overlay_image_object = None
self.map_pixmap = QtGui.QPixmap.fromImage(ImageQt(Image.open("Resources/Images/maps_loading.png").resize((1280, 720), Image.BICUBIC)))
self.loading_image_pixmap = QtGui.QPixmap.fromImage(
ImageQt(Image.open("Resources/Images/maps_loading.png").resize((1280, 720), Image.BICUBIC)))
self.map_pixmap = self.loading_image_pixmap
self.last_map_pixmap_cache_key = None
self.longitude = 0
@@ -97,9 +104,19 @@ class RoverMapCoordinator(QtCore.QThread):
self.x_drag_end = -1
self.y_drag_end = -1
self.map_selection_name = ""
self.map_selection_latitude = 0
self.map_selection_longitude = 0
self.map_selection_zoom = 0
self.setup_mapping_locations()
def run(self):
self.logger.debug("Starting Map Coordinator Thread")
self.pixmap_ready_signal.emit() # This gets us the loading map
# self.precache_all_maps()
while self.run_thread_flag:
if self.setup_map_flag:
self._map_setup()
@@ -114,16 +131,67 @@ class RoverMapCoordinator(QtCore.QThread):
self.logger.debug("Stopping Map Coordinator Thread")
def setup_mapping_locations(self):
locations = [location for location in MappingSettings.MAPPING_LOCATIONS]
self.map_selection_combo_box.addItems(locations)
self.update_settings_from_selection()
self.map_selection_combo_box.setCurrentText(self.map_selection_name)
self.update_zoom_combo_box_items(
MappingSettings.MAPPING_LOCATIONS[MappingSettings.LAST_SELECTION]["valid_zoom_options"],
MappingSettings.LAST_ZOOM_LEVEL)
def update_settings_from_selection(self):
self.map_selection_latitude = MappingSettings.MAPPING_LOCATIONS[MappingSettings.LAST_SELECTION]["latitude"]
self.map_selection_longitude = MappingSettings.MAPPING_LOCATIONS[MappingSettings.LAST_SELECTION]["longitude"]
self.map_selection_zoom = MappingSettings.LAST_ZOOM_LEVEL
self.map_selection_name = MappingSettings.LAST_SELECTION
def precache_all_maps(self):
print "Caching all map options!!!"
for map in MappingSettings.MAPPING_LOCATIONS:
lat = MappingSettings.MAPPING_LOCATIONS[map]["latitude"]
lon = MappingSettings.MAPPING_LOCATIONS[map]["longitude"]
for zoom in MappingSettings.MAPPING_LOCATIONS[map]["valid_zoom_options"]:
print "Caching map: %s at zoom %d" % (map, zoom)
try:
RoverMap.GMapsStitcher(1280,
720,
lat,
lon,
zoom,
'satellite',
None, 20)
except:
print "Could not cache map: %s at zoom %d" % (map, zoom)
print "Finished caching map: %s at zoom %d" % (map, zoom)
print "Map cache complete!"
def _map_setup(self):
self.update_zoom_combo_box_items(
MappingSettings.MAPPING_LOCATIONS[MappingSettings.LAST_SELECTION]["valid_zoom_options"],
MappingSettings.LAST_ZOOM_LEVEL)
self.map_image = None
self.map_pixmap = self.loading_image_pixmap
self.pixmap_ready__slot()
self.google_maps_object = None
self.google_maps_object = RoverMap.GMapsStitcher(1280,
720,
44.5675721667,
-123.2750535,
18, # FIXME: Used to be 18
self.map_selection_latitude,
self.map_selection_longitude,
self.map_selection_zoom,
'satellite',
None, 20)
self.overlay_image_object = None
self.overlay_image_object = (
RoverMap.OverlayImage(44.5675721667, -123.2750535,
RoverMap.OverlayImage(self.map_selection_latitude, self.map_selection_longitude,
self.google_maps_object.northwest,
self.google_maps_object.southeast,
self.google_maps_object.big_image.size[0],
@@ -150,7 +218,8 @@ class RoverMapCoordinator(QtCore.QThread):
crop_x_start = ((self.zoom_subtraction * MAP_WIDGET_RATIO) / 2) - self.x_drag - self.x_drag_end
crop_y_start = (self.zoom_subtraction / 2) - self.y_drag - self.y_drag_end
crop_x_end = (MAP_WIDGET_WIDTH - ((self.zoom_subtraction * MAP_WIDGET_RATIO) / 2)) - self.x_drag - self.x_drag_end
crop_x_end = (MAP_WIDGET_WIDTH - (
(self.zoom_subtraction * MAP_WIDGET_RATIO) / 2)) - self.x_drag - self.x_drag_end
crop_y_end = (MAP_WIDGET_HEIGHT - (self.zoom_subtraction / 2)) - self.y_drag - self.y_drag_end
crop_box = (int(crop_x_start), int(crop_y_start), int(crop_x_end), int(crop_y_end))
@@ -192,6 +261,9 @@ class RoverMapCoordinator(QtCore.QThread):
self.mapping_label.wheelEvent = self.__mouse_wheel_event
self.mapping_label.mouseMoveEvent = self.__mouse_move_event
self.map_selection_combo_box.currentTextChanged.connect(self.map_selection_changed__slot)
self.map_zoom_level_combo_box.currentTextChanged.connect(self.zoom_selection_changed__slot)
def on_kill_threads_requested_slot(self):
self.run_thread_flag = False
@@ -224,13 +296,11 @@ class RoverMapCoordinator(QtCore.QThread):
navigation_list = self._get_table_elements(self.navigation_label)
landmark_list = self._get_table_elements(self.landmark_label)
self.overlay_image = self.overlay_image_object.update_new_location(
latitude,
longitude,
self.last_heading,
navigation_list,
landmark_list)
# self.last_heading = (self.last_heading + 5) % 360
# self.overlay_image.save("something.png")
latitude,
longitude,
self.last_heading,
navigation_list,
landmark_list)
def gps_position_updated_callback(self, data):
self.latitude = data.latitude
@@ -260,9 +330,7 @@ class RoverMapCoordinator(QtCore.QThread):
self.x_drag = 0
self.x_drag = 0
def __mouse_wheel_event(self, event):
# print "wheel:", event.angleDelta()
self.zoom_subtraction += event.angleDelta().y() / 12
def __mouse_move_event(self, event):
@@ -281,6 +349,27 @@ class RoverMapCoordinator(QtCore.QThread):
self.x_drag_start = event.pos().x()
self.y_drag_start = event.pos().y()
def update_zoom_combo_box_items(self, zooms, selection):
self.map_zoom_level_combo_box.clear()
self.map_zoom_level_combo_box.addItems([str(item) for item in zooms])
self.map_zoom_level_combo_box.setCurrentText(str(selection))
def zoom_selection_changed__slot(self, zoom):
if zoom:
MappingSettings.LAST_ZOOM_LEVEL = int(zoom)
self.map_selection_zoom = int(zoom)
self.setup_map_flag = True
def map_selection_changed__slot(self, selection):
MappingSettings.LAST_SELECTION = selection
MappingSettings.LAST_ZOOM_LEVEL = MappingSettings.MAPPING_LOCATIONS[selection]["default_zoom"]
self.update_settings_from_selection()
self.setup_map_flag = True
self.map_selection_name = selection
@staticmethod
def constrain(val, min_val, max_val):
return min(max_val, max(min_val, val))
return min(max_val, max(min_val, val))

View File

@@ -213,7 +213,6 @@ class MiscArm(QtCore.QThread):
message.wrist_pitch = movement[4]
message.wrist_roll = movement[5]
print message
self.arm_absolute_control_publisher.publish(message)
self.wait_for_targets_reached(movement)
@@ -227,27 +226,27 @@ class MiscArm(QtCore.QThread):
wrist_roll_set = movement[5] - (wrist_pitch_set / 2.0)
while abs(self.base_position - base_set) > POSITIONAL_TOLERANCE:
self.logger.debug("Waiting for base| %f\t%f" % (self.base_position, base_set))
# self.logger.debug("Waiting for base| %f\t%f" % (self.base_position, base_set))
self.msleep(10)
while abs(self.shoulder_position - shoulder_set) > POSITIONAL_TOLERANCE:
self.logger.debug("Waiting for shoulder| %f\t%f" % (self.shoulder_position, shoulder_set))
# self.logger.debug("Waiting for shoulder| %f\t%f" % (self.shoulder_position, shoulder_set))
self.msleep(10)
while abs(self.elbow_position - elbow_set) > POSITIONAL_TOLERANCE:
self.logger.debug("Waiting for elbow| %f\t%f" % (self.elbow_position, elbow_set))
# self.logger.debug("Waiting for elbow| %f\t%f" % (self.elbow_position, elbow_set))
self.msleep(10)
while abs(self.roll_position - roll_set) > POSITIONAL_TOLERANCE:
self.logger.debug("Waiting for roll| %f\t%f" % (self.roll_position, roll_set))
# self.logger.debug("Waiting for roll| %f\t%f" % (self.roll_position, roll_set))
self.msleep(10)
while abs(self.wrist_pitch_position - wrist_pitch_set) > POSITIONAL_TOLERANCE:
self.logger.debug("Waiting for wrist_pitch| %f\t%f" % (self.wrist_pitch_position, wrist_pitch_set))
# self.logger.debug("Waiting for wrist_pitch| %f\t%f" % (self.wrist_pitch_position, wrist_pitch_set))
self.msleep(10)
while abs(self.wrist_roll_position - wrist_roll_set) > POSITIONAL_TOLERANCE:
self.logger.debug("Waiting for wrist_roll| %f\t%f" % (self.wrist_roll_position, wrist_roll_set))
# self.logger.debug("Waiting for wrist_roll| %f\t%f" % (self.wrist_roll_position, wrist_roll_set))
self.msleep(10)
def connect_signals_and_slots(self):

View File

@@ -14,7 +14,7 @@ from tf import transformations
from scipy.interpolate import interp1d
import math
from sensor_msgs.msg import Imu
from Resources.Settings import Mapping as MappingSettings
from Resources.Settings import MappingSettings as MappingSettings
#####################################
# Global Variables

View File

@@ -1,23 +0,0 @@
# Note that the lat and lon positions below correspond to the center point of the maps you want to download
# Proper zoom level selection determines total viewable area
MAPPING_LOCATIONS = {
"Graf Hall": {
"latitude": 44.5675721667,
"longitude": -123.2750535,
"default_zoom": 18,
"valid_zoom_options": [15, 16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"Crystal Lake": {
"latitude": 44.547155,
"longitude": -123.251438,
"default_zoom": 18,
"valid_zoom_options": [15, 16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
}
}
# ##### This is the offset from magnetic north to true north
DECLINATION_OFFSET = 15

View File

@@ -0,0 +1,66 @@
# Note that the lat and lon positions below correspond to the center point of the maps you want to download
# Proper zoom level selection determines total viewable area
MAPPING_LOCATIONS = {
"Graf Hall": {
"latitude": 44.5675721667,
"longitude": -123.2750535,
"default_zoom": 18,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"Crystal Lake": {
"latitude": 44.547155,
"longitude": -123.251438,
"default_zoom": 18,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"Bend's \"The Pit\"": {
"latitude": 43.9941317,
"longitude": -121.4150066,
"default_zoom": 17,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"McMullen": {
"latitude": 51.470326,
"longitude": -112.773995,
"default_zoom": 17,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"Compound": {
"latitude": 51.470941,
"longitude": -112.752322,
"default_zoom": 17,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"1st Avenue": {
"latitude": 51.453744,
"longitude": -112.715879,
"default_zoom": 17,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
"Rosedale": {
"latitude": 51.421368,
"longitude": -112.641666,
"default_zoom": 17,
"valid_zoom_options": [16, 17, 18, 19, 20],
"pre_cache_map_zoom_levels": [18, 19, 20]
},
}
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

View File

@@ -1390,7 +1390,7 @@ N/A</string>
</size>
</property>
<property name="currentIndex">
<number>0</number>
<number>4</number>
</property>
<widget class="QWidget" name="tab_5">
<attribute name="title">
@@ -2447,14 +2447,7 @@ N/A</string>
<string>Settings</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_6">
<item row="1" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>Ubiquiti Radio Channel</string>
</property>
</widget>
</item>
<item row="2" column="1">
<item row="4" column="1">
<spacer name="verticalSpacer_2">
<property name="orientation">
<enum>Qt::Vertical</enum>
@@ -2468,13 +2461,64 @@ N/A</string>
</spacer>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Map Selection</string>
</property>
</widget>
<layout class="QFormLayout" name="formLayout_7">
<item row="1" column="1">
<widget class="QComboBox" name="map_selection_combo_box"/>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Map Selection</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>Ubiquiti Radio Channel</string>
</property>
</widget>
</item>
<item row="3" column="1">
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<widget class="QSpinBox" name="ubiquiti_channel_spin_box">
<property name="readOnly">
<bool>false</bool>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>11</number>
</property>
<property name="value">
<number>0</number>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="ubiquiti_channel_apply_button">
<property name="text">
<string>Apply</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_30">
<property name="text">
<string>Map Zoom Level</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QComboBox" name="map_zoom_level_combo_box"/>
</item>
</layout>
</item>
<item row="1" column="2">
<item row="3" column="2">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@@ -2487,42 +2531,6 @@ N/A</string>
</property>
</spacer>
</item>
<item row="0" column="1">
<widget class="QComboBox" name="comboBox">
<item>
<property name="text">
<string>OSU Campus</string>
</property>
</item>
</widget>
</item>
<item row="1" column="1">
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<widget class="QSpinBox" name="ubiquiti_channel_spin_box">
<property name="readOnly">
<bool>false</bool>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>11</number>
</property>
<property name="value">
<number>0</number>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="ubiquiti_channel_apply_button">
<property name="text">
<string>Apply</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</widget>

View File

@@ -149,7 +149,7 @@ public:
smSetParameter(arm_bus_handle, roll_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART);
smSetParameter(arm_bus_handle, wrist_pitch_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART);
smSetParameter(arm_bus_handle, wrist_roll_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART);
// arm_successfully_connected = false;
arm_successfully_connected = false;
should_reset = false;
}
}

View File

@@ -20,14 +20,15 @@ from rover_control.msg import MiningControlMessage, MiningStatusMessage
NODE_NAME = "effectors_control"
# ##### Communication Defines #####
DEFAULT_PORT = "/dev/rover/ttyEffectors"
# DEFAULT_PORT = "/dev/rover/ttyEffectors"
DEFAULT_PORT = "/dev/ttyUSB0"
DEFAULT_BAUD = 115200
GRIPPER_NODE_ID = 1
MINING_NODE_ID = 2
SCIENCE_NODE_ID = 3
GRIPPER_TIMEOUT = 0.15
GRIPPER_TIMEOUT = 0.5
MINING_TIMEOUT = 0.3
SCIENCE_TIMEOUT = 0.15
@@ -150,25 +151,30 @@ class EffectorsControl(object):
# self.initialize_mining_system()
while not rospy.is_shutdown():
try:
self.process_mining_control_message()
except IOError, e:
print e
if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
print "Lost connection to mining system. Exiting for reconnect."
return
except Exception, e:
print e
# try:
# self.process_mining_control_message()
# except IOError, e:
# # print e
# if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
# print "Lost connection to mining system. Exiting for reconnect."
# return
# except Exception, e:
# pass
#
# try:
# self.send_mining_status_message()
# except IOError, e:
# print e
# if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
# print "Lost connection to mining system. Exiting for reconnect."
# return
# except Exception, e:
# pass
try:
self.send_mining_status_message()
except IOError, e:
print e
if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
print "Lost connection to mining system. Exiting for reconnect."
return
except Exception, e:
print e
print self.gripper_node.read_register(0)
except Exception, error:
print error
def connect_to_nodes(self):
self.gripper_node = minimalmodbus.Instrument(self.port, int(self.gripper_node_id))
@@ -196,30 +202,6 @@ class EffectorsControl(object):
except:
self.science_node_present = False
def initialize_mining_system(self):
if self.mining_node_present:
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET"]] = 1023
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET"]] = 350
self.mining_registers[MINING_MODBUS_REGISTERS["CAL_FACTOR"]] = 114
while abs(self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_POSITION"]] - self.mining_registers[
MINING_MODBUS_REGISTERS["LIFT_SET"]]) > MINING_POSITIONAL_THRESHOLD or \
abs(self.mining_registers[MINING_MODBUS_REGISTERS["TILT_POSITION"]] - self.mining_registers[
MINING_MODBUS_REGISTERS["TILT_SET"]]) > MINING_POSITIONAL_THRESHOLD:
try:
self.mining_node.write_registers(0, self.mining_registers)
self.mining_registers = self.mining_node.read_registers(0, 7)
except Exception, e:
print "Had trouble communicating:", e
try:
self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = 1
self.mining_node.write_registers(0, self.mining_registers)
self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = 0
except:
print "Had trouble communicating: no tare: ", e
def process_mining_control_message(self):
if self.new_mining_control_message and self.mining_node_present:

View File

@@ -1,3 +1,3 @@
<launch>
<launch>device.name
<node name="rover_arm" pkg="rover_arm" type="rover_arm" respawn="true" output="screen"/>
</launch>