From 6ff6068759a0397960822eafec1a70319d7fef23 Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Sat, 28 Jul 2018 15:11:32 -0700 Subject: [PATCH] Choppy panning and zooming working! Not the nicest, but it works. --- software/firmware/tower/tower.ino | 2 +- .../src/Framework/MapSystems/RoverMap.py | 17 +-- .../MapSystems/RoverMapCoordinator.py | 101 ++++++++++++++++-- .../SpeedAndHeadingIndication.py | 3 +- .../{MappingLocations.py => Mapping.py} | 5 +- .../rover_main/launch/rover/odometry2.launch | 2 +- 6 files changed, 101 insertions(+), 29 deletions(-) rename software/ros_packages/ground_station/src/Resources/Settings/{MappingLocations.py => Mapping.py} (88%) diff --git a/software/firmware/tower/tower.ino b/software/firmware/tower/tower.ino index b1ecf62..1f4410d 100644 --- a/software/firmware/tower/tower.ino +++ b/software/firmware/tower/tower.ino @@ -163,7 +163,7 @@ void send_imu_stream_line(JsonObject &root) { JsonObject& imu_object = root.createNestedObject("imu"); quat = bno.getQuat(); - linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); + linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER); angular_vel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); imu_object["ox"] = quat.x(); 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 53c0951..a42fe1f 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py @@ -447,26 +447,15 @@ class OverlayImage(object): draw.ellipse((x - size, y - size, x + size, y + size), fill="blue") self._draw_rover(latitude, longitude, compass) - self._draw_coordinate_text(latitude, longitude) + # self._draw_coordinate_text(latitude, longitude) self.update(latitude, longitude) return self.display_image def load_rover_icon(self): - self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((60, 60)) + self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((40, 40)) - def _draw_coordinate_text(self, latitude, longitude): - location_text = "LAT: %+014.9f\nLON: %+014.9f" % (latitude, longitude) - font = PIL.ImageFont.truetype("UbuntuMono-R", size=20) - - new_image = PIL.Image.new('RGBA', (200, 45), "black") - - draw = PIL.ImageDraw.Draw(new_image) - - draw.multiline_text((5, 0), location_text, font=font) - - self.display_image.paste(new_image, (0, 0)) def _draw_rover(self, lat, lon, angle=0): x, y = self._get_cartesian(lat, lon) @@ -486,7 +475,7 @@ class OverlayImage(object): # self.left_x -= 50 # self.upper_y -= 50 self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y)) - self._draw_coordinate_text(latitude, longitude) + # self._draw_coordinate_text(latitude, longitude) def connect_signals_and_slots(self): pass 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 ce1ccff..a38386f 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py @@ -5,6 +5,9 @@ from PyQt5 import QtCore, QtWidgets, QtGui from PIL.ImageQt import ImageQt from PIL import Image +import PIL.ImageDraw +import PIL.Image +import PIL.ImageFont import numpy import logging @@ -17,6 +20,7 @@ from sensor_msgs.msg import Imu # Custom Imports import RoverMap +from Resources.Settings import Mapping as MappingSettings from sensor_msgs.msg import NavSatFix ##################################### @@ -28,6 +32,11 @@ 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 + + class RoverMapCoordinator(QtCore.QThread): pixmap_ready_signal = QtCore.pyqtSignal() change_waypoint_signal = QtCore.pyqtSignal() @@ -37,7 +46,7 @@ class RoverMapCoordinator(QtCore.QThread): self.shared_objects = shared_objects self.left_screen = self.shared_objects["screens"]["left_screen"] - self.mapping_label = self.left_screen.mapping_label + 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 @@ -74,6 +83,17 @@ class RoverMapCoordinator(QtCore.QThread): self.imu_data_subscriber = rospy.Subscriber(IMU_DATA_TOPIC, Imu, self.on_imu_data_received) + self.zoom_pan_x_location = 0 + self.zoom_pan_y_location = 0 + + self.zoom_subtraction = 0 + + self.x_drag = 0 + self.y_drag = 0 + + self.x_drag_start = -1 + self.y_drag_start = -1 + def run(self): self.logger.debug("Starting Map Coordinator Thread") self.pixmap_ready_signal.emit() # This gets us the loading map @@ -87,7 +107,7 @@ class RoverMapCoordinator(QtCore.QThread): self.new_imu_data = False self._get_map_image() - self.msleep(30) + self.msleep(10) self.logger.debug("Stopping Map Coordinator Thread") @@ -96,7 +116,7 @@ class RoverMapCoordinator(QtCore.QThread): 720, 44.5675721667, -123.2750535, - 19, # FIXME: Used to be 18 + 18, # FIXME: Used to be 18 'satellite', None, 20) self.overlay_image_object = ( @@ -120,21 +140,51 @@ class RoverMapCoordinator(QtCore.QThread): self.map_image.paste(self.overlay_image_object.display_image, (0, 0), self.overlay_image_object.display_image) - # self.map_image = Image.alpha_composite( - # self.google_maps_object.display_image, - # self.overlay_image_object.display_image) - # get overlay here + + # ##### Zoom testing! ##### + if self.zoom_subtraction: + self.zoom_subtraction = self.constrain(self.zoom_subtraction, 0, 520) + + crop_x_start = ((self.zoom_subtraction * MAP_WIDGET_RATIO) / 2) - self.x_drag + crop_y_start = (self.zoom_subtraction / 2) - self.y_drag + crop_x_end = (MAP_WIDGET_WIDTH - ((self.zoom_subtraction * MAP_WIDGET_RATIO) / 2)) - self.x_drag + crop_y_end = (MAP_WIDGET_HEIGHT - (self.zoom_subtraction / 2)) - self.y_drag + crop_box = (int(crop_x_start), int(crop_y_start), int(crop_x_end), int(crop_y_end)) + + self.map_image = self.map_image.crop(crop_box) + self.map_image = self.map_image.resize((1280, 720), resample=PIL.Image.BICUBIC) + + # ##### Draw coordinates ##### + self._draw_coordinate_text(self.latitude, self.longitude) + qim = ImageQt(self.map_image) self.map_pixmap = QtGui.QPixmap.fromImage(qim) - if self.map_pixmap.cacheKey() != self.last_map_pixmap_cache_key: - self.last_map_pixmap_cache_key = self.map_pixmap.cacheKey() - self.pixmap_ready_signal.emit() + # if self.map_pixmap.cacheKey() != self.last_map_pixmap_cache_key: + # self.last_map_pixmap_cache_key = self.map_pixmap.cacheKey() + self.pixmap_ready_signal.emit() + + def _draw_coordinate_text(self, latitude, longitude): + location_text = "LAT: %+014.9f\nLON: %+014.9f" % (latitude, longitude) + + font = PIL.ImageFont.truetype("UbuntuMono-R", size=20) + + new_image = PIL.Image.new('RGBA', (200, 45), "black") + + draw = PIL.ImageDraw.Draw(new_image) + + draw.multiline_text((5, 0), location_text, font=font) + + self.map_image.paste(new_image, (0, 0)) def connect_signals_and_slots(self): self.pixmap_ready_signal.connect(self.pixmap_ready__slot) self.change_waypoint_signal.connect(self.update_overlay) + self.mapping_label.mouseReleaseEvent = self.__mouse_released_event + self.mapping_label.wheelEvent = self.__mouse_wheel_event + self.mapping_label.mouseMoveEvent = self.__mouse_move_event + def on_kill_threads_requested_slot(self): self.run_thread_flag = False @@ -191,4 +241,33 @@ class RoverMapCoordinator(QtCore.QThread): self.imu_data.orientation.w, ) self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat) - self.last_heading = self.euler_interpolator(self.yaw) % 360 + self.last_heading = (self.euler_interpolator(self.yaw) + MappingSettings.DECLINATION_OFFSET) % 360 + + def __mouse_released_event(self, event): + self.x_drag_start = -1 + self.y_drag_start = -1 + + + def __mouse_wheel_event(self, event): + # print "wheel:", event.angleDelta() + self.zoom_subtraction += event.angleDelta().y() / 12 + + def __mouse_move_event(self, event): + if self.x_drag_start != -1 and self.x_drag_start != -1: + buttons = event.buttons() + + if buttons == QtCore.Qt.LeftButton: + x_pos = event.pos().x() + y_pos = event.pos().y() + dx = x_pos - self.x_drag_start + dy = y_pos - self.y_drag_start + + self.x_drag = dx + self.y_drag = dy + else: + self.x_drag_start = event.pos().x() + self.y_drag_start = event.pos().y() + + @staticmethod + def constrain(val, min_val, max_val): + return min(max_val, max(min_val, val)) \ No newline at end of file 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 a01861a..eb5c45a 100644 --- a/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py +++ b/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py @@ -14,6 +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 ##################################### # Global Variables @@ -107,7 +108,7 @@ class SpeedAndHeadingIndication(QtCore.QThread): self.imu_data.orientation.w, ) self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat) - self.current_heading = self.euler_interpolator(self.yaw) % 360 + self.current_heading = (self.euler_interpolator(self.yaw) + MappingSettings.DECLINATION_OFFSET) % 360 def rotate_compass_if_needed(self): diff --git a/software/ros_packages/ground_station/src/Resources/Settings/MappingLocations.py b/software/ros_packages/ground_station/src/Resources/Settings/Mapping.py similarity index 88% rename from software/ros_packages/ground_station/src/Resources/Settings/MappingLocations.py rename to software/ros_packages/ground_station/src/Resources/Settings/Mapping.py index 8d52243..6fe8115 100644 --- a/software/ros_packages/ground_station/src/Resources/Settings/MappingLocations.py +++ b/software/ros_packages/ground_station/src/Resources/Settings/Mapping.py @@ -17,4 +17,7 @@ MAPPING_LOCATIONS = { "valid_zoom_options": [15, 16, 17, 18, 19, 20], "pre_cache_map_zoom_levels": [18, 19, 20] } -} \ No newline at end of file +} + +# ##### This is the offset from magnetic north to true north +DECLINATION_OFFSET = 15 diff --git a/software/ros_packages/rover_main/launch/rover/odometry2.launch b/software/ros_packages/rover_main/launch/rover/odometry2.launch index 024e8f2..dbe51b3 100644 --- a/software/ros_packages/rover_main/launch/rover/odometry2.launch +++ b/software/ros_packages/rover_main/launch/rover/odometry2.launch @@ -32,7 +32,7 @@ - +