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

Choppy panning and zooming working! Not the nicest, but it works.

Browse Source
This commit is contained in:
Corwin Perren
2018-07-28 15:11:32 -07:00
parent 9d3969f9e4
commit 6ff6068759
6 changed files with 101 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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();

17
software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py
View File

@@ -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

101
software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py
View File

@@ -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))

3
software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py
View File

@@ -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):

3
software/ros_packages/ground_station/src/Resources/Settings/MappingLocations.py → software/ros_packages/ground_station/src/Resources/Settings/Mapping.py
View File

@@ -18,3 +18,6 @@ MAPPING_LOCATIONS = {
"pre_cache_map_zoom_levels": [18, 19, 20]
}
}
# ##### This is the offset from magnetic north to true north
DECLINATION_OFFSET = 15

2
software/ros_packages/rover_main/launch/rover/odometry2.launch
View File

@@ -32,7 +32,7 @@
<!--<param name="base_link_frame" value="base_link"/>-->
<!--<param name="world_frame" value="odom"/>-->
<!--<param name="odom0" value="odometry/gps"/>-->
<!--<param name="odom0" value="fix"/>-->
<!--<param name="imu0" value="imu/data"/>-->
<!--<rosparam param="odom0_config">[true, true, false,-->