Got farther with mapping, heading indication, and firmware with cal for IMU. Need to figure out which direction IMU is in for ROS to work right.

software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py

@@ -453,7 +453,7 @@ class OverlayImage(object):
         return self.display_image
 
     def load_rover_icon(self):
-        self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((50, 50))
+        self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((60, 60))
 
     def _draw_coordinate_text(self, latitude, longitude):
         location_text = "LAT: %+014.9f\nLON: %+014.9f" % (latitude, longitude)
@@ -475,7 +475,7 @@ class OverlayImage(object):
         y -= 25
 
         rotated = self.indicator.copy()
-        rotated = rotated.rotate(angle, resample=PIL.Image.BICUBIC)
+        rotated = rotated.rotate(-angle, resample=PIL.Image.BICUBIC)
         # rotated.save("rotated.png")
         self.big_image.paste(rotated, (x, y), rotated)
         if self.write_once:
@@ -483,7 +483,8 @@ class OverlayImage(object):
             self.write_once = False
 
     def update(self, latitude, longitude):
-
+        # 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)
 

software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py

@@ -10,6 +10,10 @@ import numpy
 import logging
 
 import rospy
+from tf import transformations
+from scipy.interpolate import interp1d
+import math
+from sensor_msgs.msg import Imu
 
 # Custom Imports
 import RoverMap
@@ -21,6 +25,7 @@ from sensor_msgs.msg import NavSatFix
 # put some stuff here later so you can remember
 
 GPS_POSITION_TOPIC = "/rover_odometry/fix"
+IMU_DATA_TOPIC = "/rover_odometry/imu/data"
 
 
 class RoverMapCoordinator(QtCore.QThread):
@@ -58,6 +63,17 @@ class RoverMapCoordinator(QtCore.QThread):
         self.latitude = None
         self.last_heading = 0
 
+        self.imu_data = None
+        self.new_imu_data = False
+
+        self.yaw = None
+        self.pitch = None
+        self.roll = None
+
+        self.euler_interpolator = interp1d([math.pi, -math.pi], [-180, 180])
+
+        self.imu_data_subscriber = rospy.Subscriber(IMU_DATA_TOPIC, Imu, self.on_imu_data_received)
+
     def run(self):
         self.logger.debug("Starting Map Coordinator Thread")
         self.pixmap_ready_signal.emit()  # This gets us the loading map
@@ -66,6 +82,10 @@ class RoverMapCoordinator(QtCore.QThread):
                 self._map_setup()
                 self.setup_map_flag = False
             else:
+                if self.new_imu_data:
+                    self.calculate_euler_from_imu()
+                    self.new_imu_data = False
+
                 self._get_map_image()
             self.msleep(30)
 
@@ -158,3 +178,17 @@ class RoverMapCoordinator(QtCore.QThread):
     def gps_position_updated_callback(self, data):
         self.latitude = data.latitude
         self.longitude = data.longitude
+
+    def on_imu_data_received(self, data):
+        self.imu_data = data
+        self.new_imu_data = True
+
+    def calculate_euler_from_imu(self):
+        quat = (
+            self.imu_data.orientation.x,
+            self.imu_data.orientation.y,
+            self.imu_data.orientation.z,
+            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

software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py

@@ -9,6 +9,11 @@ from time import time
 import PIL.Image
 from PIL.ImageQt import ImageQt
 from random import random
+import rospy
+from tf import transformations
+from scipy.interpolate import interp1d
+import math
+from sensor_msgs.msg import Imu
 
 #####################################
 # Global Variables
@@ -17,6 +22,8 @@ THREAD_HERTZ = 20
 
 ROTATION_SPEED_MODIFIER = 2.5
 
+IMU_DATA_TOPIC = "/rover_odometry/imu/data"
+
 
 #####################################
 # Controller Class Definition
@@ -61,12 +68,27 @@ class SpeedAndHeadingIndication(QtCore.QThread):
         self.last_current_heading_shown = 0
         self.rotation_direction = 1
 
+        self.imu_data = None
+        self.new_imu_data = False
+
+        self.yaw = None
+        self.pitch = None
+        self.roll = None
+
+        self.euler_interpolator = interp1d([math.pi, -math.pi], [-180, 180])
+
+        self.imu_data_subscriber = rospy.Subscriber(IMU_DATA_TOPIC, Imu, self.on_imu_data_received)
+
     def run(self):
         self.on_heading_changed__slot(self.current_heading)
 
         while self.run_thread_flag:
             start_time = time()
 
+            if self.new_imu_data:
+                self.calculate_euler_from_imu()
+                self.new_imu_data = False
+
             if self.current_heading_changed:
                 self.update_heading_movement()
                 self.current_heading_changed = False
@@ -77,27 +99,27 @@ class SpeedAndHeadingIndication(QtCore.QThread):
 
             self.msleep(max(int(self.wait_time - time_diff), 0))
 
+    def calculate_euler_from_imu(self):
+        quat = (
+            self.imu_data.orientation.x,
+            self.imu_data.orientation.y,
+            self.imu_data.orientation.z,
+            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
+
     def rotate_compass_if_needed(self):
-        heading_difference = abs(int(self.shown_heading) - self.current_heading)
-        should_update = False
 
-        if heading_difference > ROTATION_SPEED_MODIFIER:
-            self.shown_heading += self.rotation_direction * ROTATION_SPEED_MODIFIER
-            self.shown_heading %= 360
-            should_update = True
-        elif heading_difference != 0:
-            self.shown_heading = self.current_heading
-            should_update = True
+        self.current_heading_shown_rotation_angle = int(self.current_heading)
 
-        if should_update:
-            self.current_heading_shown_rotation_angle = int(self.shown_heading)
+        if self.current_heading_shown_rotation_angle != self.last_current_heading_shown:
+            new_compass_image = self.main_compass_image.rotate(self.current_heading_shown_rotation_angle, resample=PIL.Image.BICUBIC)
+            self.last_current_heading_shown = self.current_heading_shown_rotation_angle
 
-            if self.current_heading_shown_rotation_angle != self.last_current_heading_shown:
-                new_compass_image = self.main_compass_image.rotate(self.current_heading_shown_rotation_angle, resample=PIL.Image.BICUBIC)
-                self.last_current_heading_shown = self.current_heading_shown_rotation_angle
-
-                self.compass_pixmap = QtGui.QPixmap.fromImage(ImageQt(new_compass_image))
-                self.show_compass_image__signal.emit()
+            self.compass_pixmap = QtGui.QPixmap.fromImage(ImageQt(new_compass_image))
+            self.show_compass_image__signal.emit()
+            self.heading_text_update_ready__signal.emit(str(self.current_heading_shown_rotation_angle) + "°")
 
     def update_heading_movement(self):
         current_minus_shown = (self.current_heading - self.shown_heading) % 360
@@ -126,6 +148,10 @@ class SpeedAndHeadingIndication(QtCore.QThread):
     def on_new_compass_image_ready__slot(self):
         self.heading_compass_label.setPixmap(self.compass_pixmap)
 
+    def on_imu_data_received(self, data):
+        self.imu_data = data
+        self.new_imu_data = True
+
     def connect_signals_and_slots(self):
         self.show_compass_image__signal.connect(self.on_new_compass_image_ready__slot)
         self.heading_text_update_ready__signal.connect(self.heading_text_label.setText)