diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/LogitechControllerControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/LogitechControllerControlSender.py index 1b9b839..17cda1d 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/LogitechControllerControlSender.py +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/LogitechControllerControlSender.py @@ -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") diff --git a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py index 8cee877..bc16853 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py @@ -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 diff --git a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py index d9959b1..7047e4a 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py @@ -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)) \ No newline at end of file + return min(max_val, max(min_val, val)) diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py index 2755e98..4a75dd3 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py @@ -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): diff --git a/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py b/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py index 6a94bdc..3f186ad 100644 --- a/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py +++ b/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py @@ -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 diff --git a/software/ros_packages/ground_station/src/Resources/Settings/Mapping.py b/software/ros_packages/ground_station/src/Resources/Settings/Mapping.py deleted file mode 100644 index 6fe8115..0000000 --- a/software/ros_packages/ground_station/src/Resources/Settings/Mapping.py +++ /dev/null @@ -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 diff --git a/software/ros_packages/ground_station/src/Resources/Settings/MappingSettings.py b/software/ros_packages/ground_station/src/Resources/Settings/MappingSettings.py new file mode 100644 index 0000000..3c4bd93 --- /dev/null +++ b/software/ros_packages/ground_station/src/Resources/Settings/MappingSettings.py @@ -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 diff --git a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui index 627e6d2..25633b9 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui @@ -1390,7 +1390,7 @@ N/A - 0 + 4 @@ -2447,14 +2447,7 @@ N/A Settings - - - - Ubiquiti Radio Channel - - - - + Qt::Vertical @@ -2468,13 +2461,64 @@ N/A - - - Map Selection - - + + + + + + + + Map Selection + + + + + + + Ubiquiti Radio Channel + + + + + + + + + false + + + 0 + + + 11 + + + 0 + + + + + + + Apply + + + + + + + + + Map Zoom Level + + + + + + + - + Qt::Horizontal @@ -2487,42 +2531,6 @@ N/A - - - - - OSU Campus - - - - - - - - - - false - - - 0 - - - 11 - - - 0 - - - - - - - Apply - - - - - diff --git a/software/ros_packages/rover_arm/src/rover_arm.cpp b/software/ros_packages/rover_arm/src/rover_arm.cpp index 17ec2a9..5664caa 100644 --- a/software/ros_packages/rover_arm/src/rover_arm.cpp +++ b/software/ros_packages/rover_arm/src/rover_arm.cpp @@ -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; } } diff --git a/software/ros_packages/rover_control/src/effectors_control/effectors_control.py b/software/ros_packages/rover_control/src/effectors_control/effectors_control.py index 65b05a5..9e15232 100755 --- a/software/ros_packages/rover_control/src/effectors_control/effectors_control.py +++ b/software/ros_packages/rover_control/src/effectors_control/effectors_control.py @@ -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: diff --git a/software/ros_packages/rover_main/launch/rover/arm.launch b/software/ros_packages/rover_main/launch/rover/arm.launch index 6f81c28..d78f791 100644 --- a/software/ros_packages/rover_main/launch/rover/arm.launch +++ b/software/ros_packages/rover_main/launch/rover/arm.launch @@ -1,3 +1,3 @@ - +device.name