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_change_time = time()
self.last_camera_toggle_time = time() self.last_camera_toggle_time = time()
self.speed_limit = 0.5
def run(self): def run(self):
self.logger.debug("Starting Joystick Thread") 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 ReWritten by Chris Pham
Copyright OSURC, orginal code from GooMPy by Alec Singer and Simon D. Levy 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 # Custom Imports
import RoverMap import RoverMap
from Resources.Settings import Mapping as MappingSettings from Resources.Settings import MappingSettings
from sensor_msgs.msg import NavSatFix from sensor_msgs.msg import NavSatFix
##################################### #####################################
@@ -31,7 +31,6 @@ from sensor_msgs.msg import NavSatFix
GPS_POSITION_TOPIC = "/rover_odometry/fix" GPS_POSITION_TOPIC = "/rover_odometry/fix"
IMU_DATA_TOPIC = "/rover_odometry/imu/data" IMU_DATA_TOPIC = "/rover_odometry/imu/data"
MAP_WIDGET_WIDTH = float(1280) MAP_WIDGET_WIDTH = float(1280)
MAP_WIDGET_HEIGHT = float(720) MAP_WIDGET_HEIGHT = float(720)
MAP_WIDGET_RATIO = MAP_WIDGET_WIDTH / MAP_WIDGET_HEIGHT MAP_WIDGET_RATIO = MAP_WIDGET_WIDTH / MAP_WIDGET_HEIGHT
@@ -41,6 +40,8 @@ class RoverMapCoordinator(QtCore.QThread):
pixmap_ready_signal = QtCore.pyqtSignal() pixmap_ready_signal = QtCore.pyqtSignal()
change_waypoint_signal = QtCore.pyqtSignal() change_waypoint_signal = QtCore.pyqtSignal()
zoom_level_enabled_state_update__signal = QtCore.pyqtSignal(int)
def __init__(self, shared_objects): def __init__(self, shared_objects):
super(RoverMapCoordinator, self).__init__() super(RoverMapCoordinator, self).__init__()
@@ -49,12 +50,15 @@ class RoverMapCoordinator(QtCore.QThread):
self.mapping_label = self.left_screen.mapping_label # type:QtWidgets.QLabel self.mapping_label = self.left_screen.mapping_label # type:QtWidgets.QLabel
self.navigation_label = self.left_screen.navigation_waypoints_table_widget self.navigation_label = self.left_screen.navigation_waypoints_table_widget
self.landmark_label = self.left_screen.landmark_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.setings = QtCore.QSettings()
self.logger = logging.getLogger("groundstation") 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.run_thread_flag = True
self.setup_map_flag = True self.setup_map_flag = True
@@ -65,7 +69,10 @@ class RoverMapCoordinator(QtCore.QThread):
self.overlay_image = None self.overlay_image = None
self.overlay_image_object = 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.last_map_pixmap_cache_key = None
self.longitude = 0 self.longitude = 0
@@ -97,9 +104,19 @@ class RoverMapCoordinator(QtCore.QThread):
self.x_drag_end = -1 self.x_drag_end = -1
self.y_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): def run(self):
self.logger.debug("Starting Map Coordinator Thread") self.logger.debug("Starting Map Coordinator Thread")
self.pixmap_ready_signal.emit() # This gets us the loading map self.pixmap_ready_signal.emit() # This gets us the loading map
# self.precache_all_maps()
while self.run_thread_flag: while self.run_thread_flag:
if self.setup_map_flag: if self.setup_map_flag:
self._map_setup() self._map_setup()
@@ -114,16 +131,67 @@ class RoverMapCoordinator(QtCore.QThread):
self.logger.debug("Stopping Map Coordinator Thread") self.logger.debug("Stopping Map Coordinator Thread")
def _map_setup(self): def setup_mapping_locations(self):
self.google_maps_object = RoverMap.GMapsStitcher(1280, 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, 720,
44.5675721667, lat,
-123.2750535, lon,
18, # FIXME: Used to be 18 zoom,
'satellite', 'satellite',
None, 20) 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,
self.map_selection_latitude,
self.map_selection_longitude,
self.map_selection_zoom,
'satellite',
None, 20)
self.overlay_image_object = None
self.overlay_image_object = ( 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.northwest,
self.google_maps_object.southeast, self.google_maps_object.southeast,
self.google_maps_object.big_image.size[0], 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_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_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_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)) 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.wheelEvent = self.__mouse_wheel_event
self.mapping_label.mouseMoveEvent = self.__mouse_move_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): def on_kill_threads_requested_slot(self):
self.run_thread_flag = False self.run_thread_flag = False
@@ -229,8 +301,6 @@ class RoverMapCoordinator(QtCore.QThread):
self.last_heading, self.last_heading,
navigation_list, navigation_list,
landmark_list) landmark_list)
# self.last_heading = (self.last_heading + 5) % 360
# self.overlay_image.save("something.png")
def gps_position_updated_callback(self, data): def gps_position_updated_callback(self, data):
self.latitude = data.latitude self.latitude = data.latitude
@@ -260,9 +330,7 @@ class RoverMapCoordinator(QtCore.QThread):
self.x_drag = 0 self.x_drag = 0
self.x_drag = 0 self.x_drag = 0
def __mouse_wheel_event(self, event): def __mouse_wheel_event(self, event):
# print "wheel:", event.angleDelta()
self.zoom_subtraction += event.angleDelta().y() / 12 self.zoom_subtraction += event.angleDelta().y() / 12
def __mouse_move_event(self, event): def __mouse_move_event(self, event):
@@ -281,6 +349,27 @@ class RoverMapCoordinator(QtCore.QThread):
self.x_drag_start = event.pos().x() self.x_drag_start = event.pos().x()
self.y_drag_start = event.pos().y() 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 @staticmethod
def constrain(val, min_val, max_val): 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_pitch = movement[4]
message.wrist_roll = movement[5] message.wrist_roll = movement[5]
print message
self.arm_absolute_control_publisher.publish(message) self.arm_absolute_control_publisher.publish(message)
self.wait_for_targets_reached(movement) self.wait_for_targets_reached(movement)
@@ -227,27 +226,27 @@ class MiscArm(QtCore.QThread):
wrist_roll_set = movement[5] - (wrist_pitch_set / 2.0) wrist_roll_set = movement[5] - (wrist_pitch_set / 2.0)
while abs(self.base_position - base_set) > POSITIONAL_TOLERANCE: 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) self.msleep(10)
while abs(self.shoulder_position - shoulder_set) > POSITIONAL_TOLERANCE: 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) self.msleep(10)
while abs(self.elbow_position - elbow_set) > POSITIONAL_TOLERANCE: 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) self.msleep(10)
while abs(self.roll_position - roll_set) > POSITIONAL_TOLERANCE: 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) self.msleep(10)
while abs(self.wrist_pitch_position - wrist_pitch_set) > POSITIONAL_TOLERANCE: 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) self.msleep(10)
while abs(self.wrist_roll_position - wrist_roll_set) > POSITIONAL_TOLERANCE: 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) self.msleep(10)
def connect_signals_and_slots(self): def connect_signals_and_slots(self):

View File

@@ -14,7 +14,7 @@ from tf import transformations
from scipy.interpolate import interp1d from scipy.interpolate import interp1d
import math import math
from sensor_msgs.msg import Imu from sensor_msgs.msg import Imu
from Resources.Settings import Mapping as MappingSettings from Resources.Settings import MappingSettings as MappingSettings
##################################### #####################################
# Global Variables # 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> </size>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>0</number> <number>4</number>
</property> </property>
<widget class="QWidget" name="tab_5"> <widget class="QWidget" name="tab_5">
<attribute name="title"> <attribute name="title">
@@ -2447,14 +2447,7 @@ N/A</string>
<string>Settings</string> <string>Settings</string>
</attribute> </attribute>
<layout class="QGridLayout" name="gridLayout_6"> <layout class="QGridLayout" name="gridLayout_6">
<item row="1" column="0"> <item row="4" column="1">
<widget class="QLabel" name="label">
<property name="text">
<string>Ubiquiti Radio Channel</string>
</property>
</widget>
</item>
<item row="2" column="1">
<spacer name="verticalSpacer_2"> <spacer name="verticalSpacer_2">
<property name="orientation"> <property name="orientation">
<enum>Qt::Vertical</enum> <enum>Qt::Vertical</enum>
@@ -2468,35 +2461,25 @@ N/A</string>
</spacer> </spacer>
</item> </item>
<item row="0" column="0"> <item row="0" column="0">
<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"> <widget class="QLabel" name="label_6">
<property name="text"> <property name="text">
<string>Map Selection</string> <string>Map Selection</string>
</property> </property>
</widget> </widget>
</item> </item>
<item row="1" column="2"> <item row="3" column="0">
<spacer name="horizontalSpacer"> <widget class="QLabel" name="label">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="1">
<widget class="QComboBox" name="comboBox">
<item>
<property name="text"> <property name="text">
<string>OSU Campus</string> <string>Ubiquiti Radio Channel</string>
</property> </property>
</item>
</widget> </widget>
</item> </item>
<item row="1" column="1"> <item row="3" column="1">
<layout class="QHBoxLayout" name="horizontalLayout_3"> <layout class="QHBoxLayout" name="horizontalLayout_3">
<item> <item>
<widget class="QSpinBox" name="ubiquiti_channel_spin_box"> <widget class="QSpinBox" name="ubiquiti_channel_spin_box">
@@ -2523,6 +2506,31 @@ N/A</string>
</item> </item>
</layout> </layout>
</item> </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="3" column="2">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout> </layout>
</widget> </widget>
</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, 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_pitch_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART);
smSetParameter(arm_bus_handle, wrist_roll_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; should_reset = false;
} }
} }

View File

@@ -20,14 +20,15 @@ from rover_control.msg import MiningControlMessage, MiningStatusMessage
NODE_NAME = "effectors_control" NODE_NAME = "effectors_control"
# ##### Communication Defines ##### # ##### Communication Defines #####
DEFAULT_PORT = "/dev/rover/ttyEffectors" # DEFAULT_PORT = "/dev/rover/ttyEffectors"
DEFAULT_PORT = "/dev/ttyUSB0"
DEFAULT_BAUD = 115200 DEFAULT_BAUD = 115200
GRIPPER_NODE_ID = 1 GRIPPER_NODE_ID = 1
MINING_NODE_ID = 2 MINING_NODE_ID = 2
SCIENCE_NODE_ID = 3 SCIENCE_NODE_ID = 3
GRIPPER_TIMEOUT = 0.15 GRIPPER_TIMEOUT = 0.5
MINING_TIMEOUT = 0.3 MINING_TIMEOUT = 0.3
SCIENCE_TIMEOUT = 0.15 SCIENCE_TIMEOUT = 0.15
@@ -150,25 +151,30 @@ class EffectorsControl(object):
# self.initialize_mining_system() # self.initialize_mining_system()
while not rospy.is_shutdown(): while not rospy.is_shutdown():
try: # try:
self.process_mining_control_message() # self.process_mining_control_message()
except IOError, e: # except IOError, e:
print e # # print e
if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT: # if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
print "Lost connection to mining system. Exiting for reconnect." # print "Lost connection to mining system. Exiting for reconnect."
return # return
except Exception, e: # except Exception, e:
print 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: try:
self.send_mining_status_message() print self.gripper_node.read_register(0)
except IOError, e: except Exception, error:
print e print error
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
def connect_to_nodes(self): def connect_to_nodes(self):
self.gripper_node = minimalmodbus.Instrument(self.port, int(self.gripper_node_id)) self.gripper_node = minimalmodbus.Instrument(self.port, int(self.gripper_node_id))
@@ -196,30 +202,6 @@ class EffectorsControl(object):
except: except:
self.science_node_present = False 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): def process_mining_control_message(self):
if self.new_mining_control_message and self.mining_node_present: 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"/> <node name="rover_arm" pkg="rover_arm" type="rover_arm" respawn="true" output="screen"/>
</launch> </launch>