Merge branch 'master' of https://github.com/OSURoboticsClub/Rover_2017_2018

ground_station_git_reset.sh

@@ -0,0 +1,8 @@
+#!/bin/bash
+test -d ~/Maps_Backup || mkdir -p ~/Maps_Backup 
+cp -r software/ros_packages/ground_station/src/Resources/Maps/* ~/Maps_Backup
+git reset --hard
+git clean -fdx
+git pull
+test -d ~/software/ros_packages/ground_station/src/Resources/Maps || mkdir -p software/ros_packages/ground_station/src/Resources/Maps 
+cp -r ~/Maps_Backup/* software/ros_packages/ground_station/src/Resources/Maps/

rover_git_reset.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+
+git reset --hard
+git clean -fdx
+git pull

software/firmware/tower/tower.ino

@@ -2,6 +2,8 @@
 #include <ModbusRtu.h>
 #include <Adafruit_BNO055_t3.h>
 #include <ArduinoJson.h>
+#include <utility/imumaths.h>
+#include <EEPROM.h>
 
 /*
    Imu/data (Imu)
@@ -99,7 +101,7 @@ const char baud115200[] = "PUBX,41,1,3,3,115200,0";
 void setup() {
   // Debugging
   Serial.begin(9600);
-//  while (!Serial);
+ // while (!Serial);
 
   // Raw pin setup
   setup_hardware();
@@ -113,16 +115,7 @@ void setup() {
   GPS_IMU_STREAMING_PORT.begin(115200);
   GPS_IMU_STREAMING_PORT.transmitterEnable(HARDWARE::GPS_IMU_RS485_EN);
 
-  // IMU Setup
-  Serial.println("Setting up IMU");
-  if (!bno.begin()) {
-    /* There was a problem detecting the BNO055 ... check your connections */
-    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
-    while (1);
-  }
-  Serial.println("IMU Online. Setting to external crystal.");
-  bno.setExtCrystalUse(true);
-  Serial.println("IMU Configured.");
+  setup_imu();
 
   // GPS Setup
   GPS_SERIAL_PORT.begin(9600);
@@ -170,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();
@@ -186,18 +179,73 @@ void send_imu_stream_line(JsonObject &root) {
   imu_object["avy"] = angular_vel.y();
   imu_object["avz"] = angular_vel.z();
 
-  //
-  //  /* Display calibration status for each sensor. */
-  //  uint8_t system, gyro, accel, mag = 0;
-  //  bno.getCalibration(&system, &gyro, &accel, &mag);
-  //  Serial.print("CALIBRATION: Sys=");
-  //  Serial.print(system, DEC);
-  //  Serial.print(" Gyro=");
-  //  Serial.print(gyro, DEC);
-  //  Serial.print(" Accel=");
-  //  Serial.print(accel, DEC);
-  //  Serial.print(" Mag=");
-  //  Serial.print(mag, DEC);
+}
+
+void setup_imu(){
+
+    // IMU Setup
+    Serial.println("Setting up IMU");
+    if (!bno.begin()) {
+      /* There was a problem detecting the BNO055 ... check your connections */
+      Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+      while (1);
+    }
+    Serial.println("IMU Online. Setting to external crystal.");
+
+    int eeAddress = 0;
+    long bnoID;
+    bool foundCalib = false;
+
+    EEPROM.get(eeAddress, bnoID);
+
+    adafruit_bno055_offsets_t calibrationData;
+    sensor_t sensor;
+
+    /*
+    *  Look for the sensor's unique ID at the beginning oF EEPROM.
+    *  This isn't foolproof, but it's better than nothing.
+    */
+    bno.getSensor(&sensor);
+    eeAddress += sizeof(long);
+    EEPROM.get(eeAddress, calibrationData);
+    bno.setSensorOffsets(calibrationData);
+    bno.setAxisConfig(Adafruit_BNO055::REMAP_CONFIG_P6);
+    bno.setAxisSign(Adafruit_BNO055::REMAP_SIGN_P6);
+    bno.setMode(Adafruit_BNO055::OPERATION_MODE_IMUPLUS);
+    delay(300);
+
+    bno.setExtCrystalUse(true);
+    Serial.println("IMU Configured");
+    // while(1){
+    //     display_cal_status();
+    // }
+}
+
+void display_cal_status(void)
+{
+  /* Get the four calibration values (0..3) */
+  /* Any sensor data reporting 0 should be ignored, */
+  /* 3 means 'fully calibrated" */
+  uint8_t system, gyro, accel, mag;
+  system = gyro = accel = mag = 0;
+  bno.getCalibration(&system, &gyro, &accel, &mag);
+
+  /* The data should be ignored until the system calibration is > 0 */
+  Serial.print("\t");
+  if (!system)
+  {
+    Serial.print("! ");
+  }
+
+  /* Display the individual values */
+  Serial.print("Sys:");
+  Serial.print(system, DEC);
+  Serial.print(" G:");
+  Serial.print(gyro, DEC);
+  Serial.print(" A:");
+  Serial.print(accel, DEC);
+  Serial.print(" M:");
+  Serial.println(mag, DEC);
 }
 
 void process_gps_and_send_if_ready(JsonObject &root) {

software/reference/design_reference/UI Design/rover_icon.svg

@@ -0,0 +1,73 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+   xmlns:dc="http://purl.org/dc/elements/1.1/"
+   xmlns:cc="http://creativecommons.org/ns#"
+   xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+   xmlns:svg="http://www.w3.org/2000/svg"
+   xmlns="http://www.w3.org/2000/svg"
+   xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+   xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+   width="1000"
+   height="1000"
+   viewBox="0 0 1000 1000"
+   id="svg2"
+   version="1.1"
+   inkscape:version="0.91 r13725"
+   sodipodi:docname="rover_icon.svg"
+   inkscape:export-filename="/home/caperren/Github/Rover_2017_2018/software/ros_packages/ground_station/src/Resources/Images/rover.png"
+   inkscape:export-xdpi="400"
+   inkscape:export-ydpi="400">
+  <defs
+     id="defs4" />
+  <sodipodi:namedview
+     id="base"
+     pagecolor="#323232"
+     bordercolor="#666666"
+     borderopacity="1.0"
+     inkscape:pageopacity="0"
+     inkscape:pageshadow="2"
+     inkscape:zoom="0.35"
+     inkscape:cx="338.99045"
+     inkscape:cy="775.33545"
+     inkscape:document-units="px"
+     inkscape:current-layer="layer1"
+     showgrid="false"
+     inkscape:window-width="2556"
+     inkscape:window-height="1401"
+     inkscape:window-x="1080"
+     inkscape:window-y="18"
+     inkscape:window-maximized="0"
+     units="px" />
+  <metadata
+     id="metadata7">
+    <rdf:RDF>
+      <cc:Work
+         rdf:about="">
+        <dc:format>image/svg+xml</dc:format>
+        <dc:type
+           rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+        <dc:title />
+      </cc:Work>
+    </rdf:RDF>
+  </metadata>
+  <g
+     inkscape:label="Layer 1"
+     inkscape:groupmode="layer"
+     id="layer1"
+     transform="translate(0,-52.362161)">
+    <path
+       style="fill:#1974df;fill-opacity:1;stroke:#ffffff;stroke-width:26;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+       d="M 499.63597,74.698266 319.16703,356.39733 c 37.73561,-64.04828 106.4946,-103.40171 180.8327,-103.49752 74.34377,0.10172 143.10405,39.46639 180.8327,103.52539 L 499.63487,74.698266 Z"
+       id="rect4196"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cccccc" />
+    <circle
+       style="fill:#1974df;fill-opacity:1;stroke:#ffffff;stroke-width:30;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+       id="path4157-6"
+       cx="500"
+       cy="552.36218"
+       r="229.86818" />
+  </g>
+</svg>

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

@@ -26,8 +26,12 @@ from io import StringIO, BytesIO
 import os
 import time
 import PIL.ImageDraw
+import PIL.Image
+import PIL.ImageFont
 import signing
 import RoverMapHelper as MapHelper
+import cv2
+import numpy as np
 
 #####################################
 # Constants
@@ -36,7 +40,8 @@ _KEYS = []
 # Number of pixels in half the earth's circumference at zoom = 21
 _EARTHPIX = 268435456
 # Number of decimal places for rounding coordinates
-_DEGREE_PRECISION = 4
+_DEGREE_PRECISION = 6
+_PRECISION_FORMAT = '%.' + str(_DEGREE_PRECISION) + 'f'
 # Larget tile we can grab without paying
 _TILESIZE = 640
 # Fastest rate at which we can download tiles without paying
@@ -113,14 +118,14 @@ class GMapsStitcher(object):
         # Make the url string for polling
         # GET request header gets appended to the string
         urlbase = 'https://maps.googleapis.com/maps/api/staticmap?'
-        urlbase += 'center=%.4f,%.4f&zoom=%d&maptype=%s'
+        urlbase += 'center=' + _PRECISION_FORMAT + ',' + _PRECISION_FORMAT + '&zoom=%d&maptype=%s'
         urlbase += '&size=%dx%d&format=png&key=%s'
 
         # Fill the formatting
         specs = (self.helper.fast_round(latitude, _DEGREE_PRECISION),
                  self.helper.fast_round(longitude, _DEGREE_PRECISION),
                  self.zoom, self.maptype, _TILESIZE, _TILESIZE, _KEYS[0])
-        filename = 'Resources/Maps/' + ('%.4f_%.4f_%d_%s_%d_%d_%s' % specs)
+        filename = 'Resources/Maps/' + ((_PRECISION_FORMAT + '_' + _PRECISION_FORMAT + '_%d_%s_%d_%d_%s') % specs)
         filename += '.png'
 
         # Tile Image object
@@ -134,7 +139,13 @@ class GMapsStitcher(object):
             # make the url
             url = urlbase % specs
             url = signing.sign_url(url, _KEYS[1])
-            result = urllib2.urlopen(urllib2.Request(url)).read()
+            try:
+                result = urllib2.urlopen(urllib2.Request(url)).read()
+            except urllib2.HTTPError, e:
+                print "Error accessing url for reason:", e
+                print url
+                return
+
             tile_object = PIL.Image.open(BytesIO(result))
             if not os.path.exists('Resources/Maps'):
                 os.mkdir('Resources/Maps')
@@ -165,10 +176,10 @@ class GMapsStitcher(object):
         """
         # Magic Lines
         return math.degrees(math.pi / 2 - 2 * math.atan(math.exp(((lat_pixels +
-                            self.helper.pixels_to_degrees(
-                                (iterator - self.num_tiles /
-                                 2) * _TILESIZE, self.zoom)) -
-                                 _EARTHPIX) / _PIXRAD)))
+                                                                   self.helper.pixels_to_degrees(
+                                                                       (iterator - self.num_tiles /
+                                                                        2) * _TILESIZE, self.zoom)) -
+                                                                  _EARTHPIX) / _PIXRAD)))
 
     def fetch_tiles(self):
         """
@@ -188,14 +199,14 @@ class GMapsStitcher(object):
         # latitude to desired radius in meters
         if self.radius_meters is not None:
             self.num_tiles = (int(
-                              round(2 * self.helper.pixels_to_meters(
-                               self.latitude, self.zoom) /
-                               (_TILESIZE / 2. / self.radius_meters))))
+                round(2 * self.helper.pixels_to_meters(
+                    self.latitude, self.zoom) /
+                      (_TILESIZE / 2. / self.radius_meters))))
 
         lon_pixels = _EARTHPIX + self.longitude * math.radians(_PIXRAD)
 
         sin_lat = math.sin(math.radians(self.latitude))
-        lat_pixels = _EARTHPIX - _PIXRAD * math.log((1+sin_lat)/(1-sin_lat))/2
+        lat_pixels = _EARTHPIX - _PIXRAD * math.log((1 + sin_lat) / (1 - sin_lat)) / 2
         self.big_size = self.num_tiles * _TILESIZE
         big_image = self.helper.new_image(self.big_size, self.big_size)
 
@@ -231,7 +242,7 @@ class GMapsStitcher(object):
         new_value = self.left_x - diff
 
         if ((not new_value > 0) and
-           (new_value < self.big_image.size[0] - self.width)):
+                (new_value < self.big_image.size[0] - self.width)):
             return self.left_x
         else:
             return new_value
@@ -243,7 +254,7 @@ class GMapsStitcher(object):
         new_value = self.upper_y - diff
 
         if ((not new_value > 0) and
-           (new_value < self.big_image.size[1] - self.height)):
+                (new_value < self.big_image.size[1] - self.height)):
             return self.upper_y
         else:
             return new_value
@@ -266,8 +277,8 @@ class GMapsStitcher(object):
         Function to move the object/rover
         """
         x, y = self._get_cartesian(lat, lon)
-        self._constrain_x(self.center_x-x)
-        self._constrain_y(self.center_y-y)
+        self._constrain_x(self.center_x - x)
+        self._constrain_y(self.center_y - y)
         self.update()
 
     def _get_cartesian(self, lat, lon):
@@ -309,9 +320,9 @@ class GMapsStitcher(object):
         x, y = self._get_cartesian(lat, lon)
         draw = PIL.ImageDraw.Draw(self.big_image)
         if shape is "ellipsis":
-            draw.ellipsis((x-size, y-size, x+size, y+size), fill)
+            draw.ellipsis((x - size, y - size, x + size, y + size), fill)
         else:
-            draw.rectangle([x-size, y-size, x+size, y+size], fill)
+            draw.rectangle([x - size, y - size, x + size, y + size], fill)
         self.update()
 
     def center_display(self, lat, lon):
@@ -322,8 +333,8 @@ class GMapsStitcher(object):
         self.center_x = x
         self.center_y = y
 
-        self.left_x = (self.center_x - (self.width/2))
-        self.upper_y = (self.center_y - (self.height/2))
+        self.left_x = (self.center_x - (self.width / 2))
+        self.upper_y = (self.center_y - (self.height / 2))
         self.update()
 
     # def update_rover_map_location(self, lat, lon):
@@ -348,7 +359,9 @@ class OverlayImage(object):
         self.width = width
         self.height = height
         self.big_image = None
+        self.big_image_copy = None
         self.display_image = None
+        self.display_image_copy = None
         self.indicator = None
         self.helper = MapHelper.MapHelper()
 
@@ -356,12 +369,19 @@ class OverlayImage(object):
         self.center_x = x
         self.center_y = y
 
-        self.left_x = (self.center_x - (self.width/2))
-        self.upper_y = (self.center_y - (self.height/2))
+        self.left_x = (self.center_x - (self.width / 2))
+        self.upper_y = (self.center_y - (self.height / 2))
 
         self.generate_image_files()
         self.write_once = True
 
+        # Text Drawing Variables
+        self.font = cv2.FONT_HERSHEY_TRIPLEX
+        self.font_thickness = 1
+        self.font_baseline = 0
+
+        self.nav_coordinates_text_image = None
+
     def generate_image_files(self):
         """
         Creates big_image and display image sizes
@@ -370,9 +390,15 @@ class OverlayImage(object):
         """
         self.big_image = self.helper.new_image(self.big_width, self.big_height,
                                                True)
+
+        self.big_image_copy = self.big_image.copy()
+
         self.display_image = self.helper.new_image(self.width, self.height,
                                                    True)
-        self.generate_dot_and_hat()
+
+        self.display_image_copy = self.display_image.copy()
+
+        self.load_rover_icon()
         self.indicator.save("location.png")
 
     def _get_cartesian(self, lat, lon):
@@ -406,43 +432,49 @@ class OverlayImage(object):
 
     def update_new_location(self, latitude, longitude,
                             compass, navigation_list, landmark_list):
+        self.big_image = self.big_image_copy.copy()
+        self.display_image = self.display_image_copy.copy()
+
         size = 5
         draw = PIL.ImageDraw.Draw(self.big_image)
+
         for element in navigation_list:
-            x, y = self._get_cartesian(float(element[2]), float(element[1]))
-            draw.ellipse((x-size, y-size, x+size, y+size), fill="red")
-        # for element in landmark_list:
-        #     x, y = self._get_cartesian(element[1], element[2])
-        #     draw.ellipsis((x-size, y-size, x+size, y+size), fill="blue")
-        self._draw_rover(longitude, latitude, compass)
-        self.update()
+            x, y = self._get_cartesian(float(element[1]), float(element[2]))
+            draw.text((x + 10, y - 5), str(element[0]))
+            draw.ellipse((x - size, y - size, x + size, y + size), fill=(element[3].red(), element[3].green(), element[3].blue()))
+
+        for element in landmark_list:
+            x, y = self._get_cartesian(element[1], element[2])
+            draw.text((x + 10, y - 5), str(element[0]))
+            draw.ellipse((x - size, y - size, x + size, y + size), fill=(element[3].red(), element[3].green(), element[3].blue()))
+
+        self._draw_rover(latitude, longitude, compass)
+        self.update(latitude, longitude)
 
         return self.display_image
 
-    def generate_dot_and_hat(self):
-        self.indicator = self.helper.new_image(100, 100, True)
-        draw = PIL.ImageDraw.Draw(self.indicator)
-        draw.ellipse((50-12, 50-12, 50+12, 50+12), fill="red")
-        draw.line((25, 40, 50, 12), fill="red", width=7)
-        draw.line((50, 12, 75, 40), fill="red", width=7)
+    def load_rover_icon(self):
+        self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((40, 40))
 
     def _draw_rover(self, lat, lon, angle=0):
         x, y = self._get_cartesian(lat, lon)
-        # print x,y
-        # Center of the circle on the indicator is (12.5, 37.5)
-        x = x - 50
-        y = y - 50
+
+        x -= 25 # Half the height of the icon
+        y -= 25
+
         rotated = self.indicator.copy()
-        rotated = rotated.rotate(angle, expand=True)
-        rotated.save("rotated.png")
+        rotated = rotated.rotate(-angle, resample=PIL.Image.BICUBIC)
+        # rotated.save("rotated.png")
         self.big_image.paste(rotated, (x, y), rotated)
         if self.write_once:
-            self.display_image.save("Something.png")
+            # self.display_image.save("Something.png")
             self.write_once = False
 
-    def update(self):
+    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)
 
     def connect_signals_and_slots(self):
         pass
-

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

@@ -5,14 +5,22 @@
 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
 
 import rospy
+from tf import transformations
+from scipy.interpolate import interp1d
+import math
+from sensor_msgs.msg import Imu
 
 # Custom Imports
 import RoverMap
+from Resources.Settings import Mapping as MappingSettings
 from sensor_msgs.msg import NavSatFix
 
 #####################################
@@ -21,6 +29,12 @@ 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"
+
+
+MAP_WIDGET_WIDTH = float(1280)
+MAP_WIDGET_HEIGHT = float(720)
+MAP_WIDGET_RATIO = MAP_WIDGET_WIDTH / MAP_WIDGET_HEIGHT
 
 
 class RoverMapCoordinator(QtCore.QThread):
@@ -32,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
 
@@ -47,43 +61,65 @@ class RoverMapCoordinator(QtCore.QThread):
 
         self.google_maps_object = None
         self.map_image = None
+        self.map_image_copy = None
         self.overlay_image = None
         self.overlay_image_object = None
 
-        self.map_pixmap = None
+        self.map_pixmap = QtGui.QPixmap.fromImage(ImageQt(Image.open("Resources/Images/maps_loading.png").resize((1280, 720), Image.BICUBIC)))
         self.last_map_pixmap_cache_key = None
 
-        self.longitude = None
-        self.latitude = None
+        self.longitude = 0
+        self.latitude = 0
+        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)
+
+        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
+
+        self.x_drag_end = -1
+        self.y_drag_end = -1
 
     def run(self):
         self.logger.debug("Starting Map Coordinator Thread")
-
+        self.pixmap_ready_signal.emit()  # This gets us the loading map
         while self.run_thread_flag:
             if self.setup_map_flag:
                 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)
+            self.msleep(10)
 
         self.logger.debug("Stopping Map Coordinator Thread")
 
-    # def _setup_map_threads(self):
-    #     self.google_maps_object = RoverMap.GMapsStitcher(1280,
-    #                                                      720,
-    #                                                      44.567161,
-    #                                                      -123.278432,
-    #                                                      18,
-    #                                                      'satellite',
-    #                                                      None, 20)
-
     def _map_setup(self):
         self.google_maps_object = RoverMap.GMapsStitcher(1280,
                                                          720,
                                                          44.5675721667,
                                                          -123.2750535,
-                                                         20,  # FIXME: Used to be 18
+                                                         18,  # FIXME: Used to be 18
                                                          'satellite',
                                                          None, 20)
         self.overlay_image_object = (
@@ -97,30 +133,65 @@ class RoverMapCoordinator(QtCore.QThread):
     def _get_map_image(self):
         while self.map_image is None:
             self.map_image = self.google_maps_object.display_image
-        # self.overlay_image_object.update_new_location(44.567161,
-        #                                               -123.278432,
-        #                                               .7,
-        #                                               [],
-        #                                               [])
+
+            if self.map_image:
+                self.map_image_copy = self.map_image.copy()
+
         self.update_overlay()
+
+        self.map_image = self.map_image_copy.copy()
         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 - 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_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))
+
+            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):
+
+        if latitude == 0 and longitude == 0:
+            location_text = "LAT:    NO FIX\nLON:    NO FIX"
+        else:
+            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
 
@@ -137,31 +208,79 @@ class RoverMapCoordinator(QtCore.QThread):
         temp_list = []
         count = UI_element.rowCount()
         for row in range(0, count):
-            num = UI_element.item(row, 0).text()
+            name = UI_element.item(row, 0).text()
             lat = float(UI_element.item(row, 1).text())
             lng = float(UI_element.item(row, 2).text())
-            temp_tuple = (num, lat, lng)
+            color = UI_element.item(row, 3).background().color()
+            temp_tuple = (name, lat, lng, color)
             temp_list.append(temp_tuple)
         return temp_list
 
     def update_overlay(self):
-        if self.latitude and self.longitude:
-            if not numpy.isnan(self.latitude) and not numpy.isnan(self.longitude):
+        if not numpy.isnan(self.latitude) and not numpy.isnan(self.longitude):
+            latitude = float(self.latitude)
+            longitude = float(self.longitude)
 
-                longitude = self.latitude
-                latitude = self.longitude
-
-                navigation_list = self._get_table_elements(self.navigation_label)
-                # landmark_list = self._get_table_elements(self.landmark_label)
-                landmark_list = []
-                self.overlay_image = self.overlay_image_object.update_new_location(
-                                                              latitude,
-                                                              longitude,
-                                                              70,
-                                                              navigation_list,
-                                                              landmark_list)
-                # self.overlay_image.save("something.png")
+            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")
 
     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) + MappingSettings.DECLINATION_OFFSET) % 360
+
+    def __mouse_released_event(self, _):
+        self.x_drag_start = -1
+        self.y_drag_start = -1
+
+        self.x_drag_end += self.x_drag
+        self.y_drag_end += self.y_drag
+
+        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):
+        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))

software/ros_packages/ground_station/src/Framework/MiscSystems/SSHConsoleCore.py

@@ -0,0 +1,47 @@
+# coding=utf-8
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets
+import logging
+from time import time
+import paramiko
+
+#####################################
+# Global Variables
+#####################################
+ACCESS_POINT_IP = "192.168.1.20"  # The channel only has to be set on the access point. The staion will adjust.
+ACCESS_POINT_USER = "ubnt"
+ACCESS_POINT_PASSWORD = "rover4lyfe^"  # We don't care about this password, don't freak out. Wifi is open anyways...
+
+
+#####################################
+# UbiquitiRadioSettings Class Definition
+#####################################
+class SSHConsole(QtCore.QThread):
+    def __init__(self, shared_objects):
+        super(SSHConsole, self).__init__()
+
+        # ########## Reference to class init variables ##########
+        self.shared_objects = shared_objects
+        self.left_screen = self.shared_objects["screens"]["left_screen"]
+
+        self.ubiquiti_channel_spin_box = self.left_screen.ssh_console_widget  # type: QtWidgets.QSpinBox
+        self.ubiquiti_channel_apply_button = self.left_screen.ubiquiti_channel_apply_button  # type: QtWidgets.QPushButton
+
+        # ########## Get the settings instance ##########
+        self.settings = QtCore.QSettings()
+
+        # ########## Get the Pick And Plate instance of the logger ##########
+        self.logger = logging.getLogger("groundstation")
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+
+        # ########## Class Variables ##########
+
+        self.connect_signals_and_slots()
+
+    def connect_signals_and_slots(self):
+        pass

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

@@ -9,6 +9,12 @@ 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
+from Resources.Settings import Mapping as MappingSettings
 
 #####################################
 # Global Variables
@@ -17,6 +23,8 @@ THREAD_HERTZ = 20
 
 ROTATION_SPEED_MODIFIER = 2.5
 
+IMU_DATA_TOPIC = "/rover_odometry/imu/data"
+
 
 #####################################
 # Controller Class Definition
@@ -58,15 +66,30 @@ class SpeedAndHeadingIndication(QtCore.QThread):
 
         self.shown_heading = (self.current_heading + (1.5 * ROTATION_SPEED_MODIFIER)) % 360
         self.current_heading_shown_rotation_angle = 0
-        self.last_current_heading_shown = 0
+        self.last_current_heading_shown = -1000
         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 +100,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) + MappingSettings.DECLINATION_OFFSET) % 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 +149,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)

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

@@ -2,6 +2,10 @@ from PyQt5 import QtCore, QtWidgets, QtGui
 import logging
 import rospy
 
+from sensor_msgs.msg import NavSatFix
+
+GPS_POSITION_TOPIC = "/rover_odometry/fix"
+
 
 class WaypointsCoordinator(QtCore.QThread):
     new_manual_waypoint_entry = QtCore.pyqtSignal(str, float, float, int)
@@ -20,12 +24,9 @@ class WaypointsCoordinator(QtCore.QThread):
                                  navigation_waypoints_table_widget)
         self.landmark_label = self.left_screen.landmark_waypoints_table_widget
 
-        self.name_edit_label = (self.left_screen.
-                                manual_waypoint_landmark_name_line_edit)
-        self.latitude_label = (self.left_screen.
-                               manual_waypoint_decimal_lattitude_spin_box)
-        self.longitude_label = (self.left_screen.
-                                manual_waypoint_decimal_longitude_spin_box)
+        self.name_edit_label = self.left_screen.manual_waypoint_landmark_name_line_edit
+        self.latitude_label = self.left_screen.manual_waypoint_decimal_lattitude_spin_box
+        self.longitude_label = self.left_screen.manual_waypoint_decimal_longitude_spin_box
 
         self.latitude_degree_label = self.left_screen.manual_waypoint_degrees_lattitude_spin_box
 
@@ -43,11 +44,18 @@ class WaypointsCoordinator(QtCore.QThread):
 
         self.longitude_card_label = self.left_screen.manual_waypoint_cardinal_longitude_combo_box
 
+        # Color Labels and Buttons
+        self.nav_color_label = self.left_screen.manual_waypoint_navigation_color_label
+        self.nav_color_set_button = self.left_screen.manual_waypoint_navigation_color_set_button
+
+        self.landmark_color_label = self.left_screen.manual_waypoint_landmark_color_label
+        self.landmark_color_set_button = self.left_screen.manual_waypoint_landmark_color_set_button
+
         # Nav Table Buttons
         self.nav_set_button_label = (self.left_screen.
                                      navigation_waypoints_set_button)
         self.nav_add_manual_button_label = (
-                     self.left_screen.navigation_waypoints_add_manual_button)
+            self.left_screen.navigation_waypoints_add_manual_button)
         self.nav_add_gps_button_label = (self.left_screen.
                                          navigation_waypoints_add_gps_button)
         self.nav_delete_button_label = (self.left_screen.
@@ -58,7 +66,7 @@ class WaypointsCoordinator(QtCore.QThread):
                                       landmark_waypoints_set_button)
 
         self.land_add_manual_button_label = (
-                       self.left_screen.landmark_waypoints_add_manual_button)
+            self.left_screen.landmark_waypoints_add_manual_button)
 
         self.land_add_gps_button_label = (self.left_screen.
                                           landmark_waypoints_add_gps_button)
@@ -70,6 +78,23 @@ class WaypointsCoordinator(QtCore.QThread):
 
         self.logger = logging.getLogger("groundstation")
 
+        self.gps_position_subscriber = rospy.Subscriber(GPS_POSITION_TOPIC, NavSatFix,
+                                                        self.gps_position_updated_callback)
+
+        self.longitude = None
+        self.latitude = None
+
+        self.nav_color = QtCore.Qt.yellow
+        self.landmark_color = QtCore.Qt.blue
+
+        self.nav_color_dialog = QtWidgets.QColorDialog()
+        self.nav_color_dialog.setWindowFlags(
+            QtCore.Qt.WindowStaysOnTopHint | QtCore.Qt.X11BypassWindowManagerHint | QtCore.Qt.WindowTitleHint | QtCore.Qt.WindowCloseButtonHint)
+
+        self.landmark_color_dialog = QtWidgets.QColorDialog()
+        self.landmark_color_dialog.setWindowFlags(
+            QtCore.Qt.WindowStaysOnTopHint | QtCore.Qt.X11BypassWindowManagerHint | QtCore.Qt.WindowTitleHint | QtCore.Qt.WindowCloseButtonHint)
+
     def run(self):
         while self.run_thread_flag:
             self.msleep(3)
@@ -93,13 +118,22 @@ class WaypointsCoordinator(QtCore.QThread):
         self.navigation_label.cellClicked.connect(self._on_nav_clicked)
         self.landmark_label.cellClicked.connect(self._on_land_clicked)
 
-    def _add_to_table(self, name, lat, lng, dist, table):
+        self.nav_color_set_button.clicked.connect(self.nav_color_dialog.show)
+        self.landmark_color_set_button.clicked.connect(self.landmark_color_dialog.show)
+
+        self.nav_color_dialog.currentColorChanged.connect(self.__on_new_nav_color_selected)
+        self.landmark_color_dialog.currentColorChanged.connect(self.__on_new_landmark_color_selected)
+
+    def _add_to_table(self, name, lat, lng, color, table):
+        color_table_item = QtWidgets.QTableWidgetItem()
+        color_table_item.setBackground(color)
+
         count = table.rowCount()
         table.insertRow(count)
         table.setItem(count, 0, QtWidgets.QTableWidgetItem(name))
         table.setItem(count, 1, QtWidgets.QTableWidgetItem(lat))
         table.setItem(count, 2, QtWidgets.QTableWidgetItem(lng))
-        table.setItem(count, 3, QtWidgets.QTableWidgetItem(dist))
+        table.setItem(count, 3, color_table_item)
 
     def _clear_inputs(self):
         self.name_edit_label.clear()
@@ -107,50 +141,54 @@ class WaypointsCoordinator(QtCore.QThread):
         self.longitude_label.clear()
 
     def _is_empty_inputs(self):
-        if self.name_edit_label.text().isEmpty():
+        if not self.name_edit_label.text():
             return True
-        if self.latitude_label.text().isEmpty():
+        if not self.latitude_label.text():
             return True
-        if self.longitude_label.text().isEmpty():
+        if not self.longitude_label.text():
             return True
         return False
 
     def _nav_add_gps(self):
-        # request GPS data
-        name = self.navigation_label.rowCount()
-        lat = 44.567200
-        lng = -123.27860
-        distance = 200
-        self._add_to_table(str(name+1), str(lat),
-                           str(lng), str(distance),
-                           self.navigation_label)
-        self._clear_inputs()
+        if self.longitude and self.latitude:
+            name = self.navigation_label.rowCount()
+            self._add_to_table(str(name + 1), str(self.latitude),
+                               str(self.longitude), self.nav_color,
+                               self.navigation_label)
+            self._clear_inputs()
 
     def _nav_save(self):
         if not self._is_empty_inputs():
-            lat = self.latitude_label.getText()
-            lng = self.longitude_label.getText()
+            lat = self.latitude_label.text()
+            lng = self.longitude_label.text()
             self.navigation_label.setItem(
                 self.navigation_table_cur_click,
                 1,
                 QtWidgets.QTableWidgetItem(lat))
             self.navigation_label.setItem(
-                self.navigation_label,
+                self.navigation_table_cur_click,
                 2,
                 QtWidgets.QTableWidgetItem(lng))
+
+            color_table_item = QtWidgets.QTableWidgetItem()
+            color_table_item.setBackground(self.nav_color)
+
+            self.navigation_label.setItem(
+                self.navigation_table_cur_click,
+                3,
+                color_table_item)
             self._clear_inputs()
 
     def _nav_add_manual(self):
         # request GPS data
         if not self._is_empty_inputs():
             name = self.navigation_label.rowCount()
-            lat = self.latitude_label.getText()
-            lng = self.longitude_label.getText()
-            distance = 200
-            self._add_to_table(str(name+1), lat,
-                               lng, str(distance),
+            lat = self.latitude_label.text()
+            lng = self.longitude_label.text()
+            self._add_to_table(str(name + 1), lat,
+                               lng, self.nav_color,
                                self.navigation_label)
-            self._clear_inputs
+            self._clear_inputs()
 
     def _nav_del(self):
         if self.navigation_table_cur_click is not None:
@@ -160,27 +198,22 @@ class WaypointsCoordinator(QtCore.QThread):
                 self.navigation_label.setItem(x,
                                               0,
                                               QtWidgets.
-                                              QTableWidgetItem(str(x+1)))
+                                              QTableWidgetItem(str(x + 1)))
             self._clear_inputs()
 
     def _land_add_gps(self):
-        name = self.name_edit_label.getText()
-        lat = 44.19223
-        lng = -123.12394
-        distance = 200
-        self._add_to_table(name, str(lat),
-                           str(lng), str(distance),
-                           self.landmark_label)
-        self._clear_inputs()
+        if self.longitude and self.latitude:
+            name = self.name_edit_label.text()
+            self._add_to_table(name, str(self.latitude), str(self.longitude), self.landmark_color, self.landmark_label)
+            self._clear_inputs()
 
     def _land_add_manual(self):
         if not self._is_empty_inputs():
-            name = self.name_edit_label.getText()
-            lat = self.latitude_label.getText()
-            lng = self.longitude_label.getText()
-            distance = 200
+            name = self.name_edit_label.text()
+            lat = self.latitude_label.text()
+            lng = self.longitude_label.text()
             self._add_to_table(name, lat,
-                               lng, str(distance),
+                               lng, self.landmark_color,
                                self.landmark_label)
             self._clear_inputs()
 
@@ -192,14 +225,14 @@ class WaypointsCoordinator(QtCore.QThread):
                 self.navigation_label.setItem(x,
                                               0,
                                               QtWidgets.
-                                              QTableWidgetItem(str(x+1)))
+                                              QTableWidgetItem(str(x + 1)))
             self._clear_inputs()
 
     def _land_save(self):
         if not self._is_empty_inputs():
-            name = self.name_edit_label.getText()
-            lat = self.latitude_label.getText()
-            lng = self.longitude_label.getText()
+            name = self.name_edit_label.text()
+            lat = self.latitude_label.text()
+            lng = self.longitude_label.text()
             self.landmark_label.setItem(self.landmark_table_cur_click, 0,
                                         QtWidgets.QTableWidgetItem(name))
 
@@ -209,6 +242,12 @@ class WaypointsCoordinator(QtCore.QThread):
             self.landmark_label.setItem(self.landmark_table_cur_click, 2,
                                         QtWidgets.QTableWidgetItem(lng))
 
+            color_table_item = QtWidgets.QTableWidgetItem()
+            color_table_item.setBackground(self.landmark_color)
+
+            self.landmark_label.setItem(self.landmark_table_cur_click, 3,
+                                        color_table_item)
+
             self._clear_inputs()
 
     def setup_signals(self, start_signal,
@@ -228,7 +267,7 @@ class WaypointsCoordinator(QtCore.QThread):
 
         lat_d = float(abs(int(lat)))
         lat_m = float(int((abs(lat) - lat_d) * 60))
-        lat_s = ((abs(lat) - lat_d - (lat_m/60.0)) * 3600.)
+        lat_s = ((abs(lat) - lat_d - (lat_m / 60.0)) * 3600.)
         if lat > 0.:
             self.latitude_card_label.setCurrentText("N")
         else:
@@ -239,7 +278,7 @@ class WaypointsCoordinator(QtCore.QThread):
 
         lng_d = float(abs(int(lng)))
         lng_m = float(int((abs(lng) - lng_d) * 60))
-        lng_s = ((abs(lng) - lng_d - (lng_m/60.0)) * 3600.)
+        lng_s = ((abs(lng) - lng_d - (lng_m / 60.0)) * 3600.)
         if lng > 0.:
             self.longitude_card_label.setCurrentText("E")
         else:
@@ -267,3 +306,19 @@ class WaypointsCoordinator(QtCore.QThread):
             float(self.landmark_label.item(row, 2).text()),
             1
         )
+
+    def __on_new_nav_color_selected(self, color):
+        self.nav_color_label.setStyleSheet(
+            "background-color: rgb(%s, %s, %s)" % (color.red(), color.green(), color.blue()))
+
+        self.nav_color = color
+
+    def __on_new_landmark_color_selected(self, color):
+        self.landmark_color_label.setStyleSheet(
+            "background-color: rgb(%s, %s, %s)" % (color.red(), color.green(), color.blue()))
+
+        self.landmark_color = color
+
+    def gps_position_updated_callback(self, data):
+        self.latitude = data.latitude
+        self.longitude = data.longitude

software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py

@@ -7,13 +7,14 @@ from PyQt5 import QtWidgets, QtCore, QtGui, uic
 from std_msgs.msg import Empty
 import PIL.Image
 from PIL.ImageQt import ImageQt
+import time
 
 from std_msgs.msg import UInt16
+
 # import Timer
 
 REQUEST_UPDATE_TOPIC = "/rover_status/update_requested"
 
-
 CAMERA_TOPIC_NAME = "/rover_status/camera_status"
 BOGIE_TOPIC_NAME = "/rover_status/bogie_status"
 FRSKY_TOPIC_NAME = "/rover_status/frsky_status"
@@ -25,8 +26,14 @@ CO2_TOPIC_NAME = "/rover_control/tower/status/co2"
 
 COLOR_GREEN = "background-color: darkgreen; border: 1px solid black;"
 COLOR_ORANGE = "background-color: orange; border: 1px solid black;"
+COLOR_YELLOW = "background-color: rgb(204,204,0); border: 1px solid black; color: black;"
 COLOR_RED = "background-color: darkred; border: 1px solid black;"
 
+GPS_BEST_CASE_ACCURACY = 3
+
+LOW_BATTERY_DIALOG_TIMEOUT = 120
+CRITICAL_BATTERY_DIALOG_TIMEOUT = 30
+
 
 class SensorCore(QtCore.QThread):
     # ########## create signals for slots ##########
@@ -62,6 +69,9 @@ class SensorCore(QtCore.QThread):
     battery_voltage_update_ready__signal = QtCore.pyqtSignal(str)
     battery_voltage_stylesheet_change_ready__signal = QtCore.pyqtSignal(str)
 
+    show_battery_low__signal = QtCore.pyqtSignal()
+    show_battery_critical__signal = QtCore.pyqtSignal()
+
     def __init__(self, shared_objects):
         super(SensorCore, self).__init__()
 
@@ -106,7 +116,7 @@ class SensorCore(QtCore.QThread):
         self.co2_status = rospy.Subscriber(CO2_TOPIC_NAME, UInt16, self.__co2_callback)
 
         self.camera_msg = CameraStatuses()
-        self.bogie_msg = None # BogieStatuses()
+        self.bogie_msg = None  # BogieStatuses()
         self.FrSky_msg = FrSkyStatus()
         self.GPS_msg = GPSInfo()
         self.jetson_msg = JetsonInfo()
@@ -120,6 +130,28 @@ class SensorCore(QtCore.QThread):
         self.osurc_logo_pixmap = QtGui.QPixmap.fromImage(ImageQt(self.osurc_logo_pil))
         self.osurc_logo_label.setPixmap(self.osurc_logo_pixmap)  # Init should be in main thread, should be fine
 
+        self.low_battery_warning_dialog = QtWidgets.QMessageBox()
+        self.low_battery_warning_dialog.setIcon(QtWidgets.QMessageBox.Warning)
+        self.low_battery_warning_dialog.setText("\n\n\n\nRover battery low!\nReturn and charge soon to avoid battery damage!\n\n\n\n")
+        self.low_battery_warning_dialog.setWindowTitle("Low Battery")
+        self.low_battery_warning_dialog.setStandardButtons(QtWidgets.QMessageBox.Ok)
+        self.low_battery_warning_dialog.setWindowFlags(
+            QtCore.Qt.WindowStaysOnTopHint | QtCore.Qt.X11BypassWindowManagerHint | QtCore.Qt.WindowTitleHint | QtCore.Qt.WindowCloseButtonHint)
+        self.low_battery_warning_dialog.setStyleSheet(COLOR_YELLOW)
+
+        self.critical_battery_warning_dialog = QtWidgets.QMessageBox()
+        self.critical_battery_warning_dialog.setIcon(QtWidgets.QMessageBox.Critical)
+        self.critical_battery_warning_dialog.setText(
+            "\n\n\n\nRover battery critical!\nPower down immediately or battery damage will occur!\n\n\n\n")
+        self.critical_battery_warning_dialog.setWindowTitle("Critical Battery")
+        self.critical_battery_warning_dialog.setStandardButtons(QtWidgets.QMessageBox.Ok)
+        self.critical_battery_warning_dialog.setWindowFlags(
+            QtCore.Qt.WindowStaysOnTopHint | QtCore.Qt.X11BypassWindowManagerHint | QtCore.Qt.WindowTitleHint | QtCore.Qt.WindowCloseButtonHint)
+        self.critical_battery_warning_dialog.setStyleSheet(COLOR_RED)
+
+        self.low_battery_warning_last_shown = 0
+        self.critical_battery_warning_last_shown = 0
+
     def __camera_callback(self, data):
         self.camera_msg.camera_zed = data.camera_zed
         self.camera_msg.camera_undercarriage = data.camera_undercarriage
@@ -215,7 +247,8 @@ class SensorCore(QtCore.QThread):
             self.gps_heading_valid_update_ready__signal.emit("GPS Heading Valid\nFalse")
 
         self.gps_num_satellites_update_ready__signal.emit("GPS Satellites\n%s" % data.num_satellites)
-        self.gps_accuracy_update_ready__signal.emit("GPS Accuracy\n%2.2f m" % data.horizontal_dilution)
+        self.gps_accuracy_update_ready__signal.emit(
+            "GPS Accuracy\n%2.2f m" % (data.horizontal_dilution * GPS_BEST_CASE_ACCURACY))
 
     def __misc_callback(self, data):
         self.misc_msg.arm_connection_status = data.arm_connection_status
@@ -227,11 +260,22 @@ class SensorCore(QtCore.QThread):
     def __battery_callback(self, data):
         voltage = data.battery_voltage / 1000.0
 
-        if voltage >= 20:
+        if voltage >= 21:
             self.battery_voltage_stylesheet_change_ready__signal.emit(COLOR_GREEN)
+        elif voltage >= 19:
+            self.battery_voltage_stylesheet_change_ready__signal.emit(COLOR_YELLOW)
+
+            if (time.time() - self.low_battery_warning_last_shown) > LOW_BATTERY_DIALOG_TIMEOUT:
+                self.show_battery_low__signal.emit()
+                self.low_battery_warning_last_shown = time.time()
+
         else:
             self.battery_voltage_stylesheet_change_ready__signal.emit(COLOR_RED)
 
+            if (time.time() - self.critical_battery_warning_last_shown) > CRITICAL_BATTERY_DIALOG_TIMEOUT:
+                self.show_battery_critical__signal.emit()
+                self.critical_battery_warning_last_shown = time.time()
+
         self.battery_voltage_update_ready__signal.emit("Battery Voltage\n" + str(voltage) + " V")
 
     def __co2_callback(self, data):
@@ -247,7 +291,7 @@ class SensorCore(QtCore.QThread):
 
     def run(self):
         while self.run_thread_flag:
-            self.update_requester.publish(Empty())
+            # self.update_requester.publish(Empty())
             self.__display_time()
             self.msleep(1000)
 
@@ -277,6 +321,9 @@ class SensorCore(QtCore.QThread):
         self.battery_voltage_update_ready__signal.connect(self.battery.setText)
         self.battery_voltage_stylesheet_change_ready__signal.connect(self.battery.setStyleSheet)
 
+        self.show_battery_low__signal.connect(self.low_battery_warning_dialog.show)
+        self.show_battery_critical__signal.connect(self.critical_battery_warning_dialog.show)
+
     def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
         start_signal.connect(self.start)
         signals_and_slots_signal.connect(self.connect_signals_and_slots)

software/ros_packages/ground_station/src/Resources/Settings/Mapping.py

@@ -0,0 +1,23 @@
+# 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

software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui

@@ -1392,224 +1392,13 @@ N/A</string>
         <property name="currentIndex">
          <number>0</number>
         </property>
-        <widget class="QWidget" name="tab_3">
+        <widget class="QWidget" name="tab">
          <attribute name="title">
-          <string>Science Readouts</string>
+          <string>SSH Console</string>
          </attribute>
-         <layout class="QHBoxLayout" name="horizontalLayout_4">
-          <item>
-           <layout class="QVBoxLayout" name="verticalLayout_6">
-            <item>
-             <widget class="QLabel" name="label_15">
-              <property name="font">
-               <font>
-                <pointsize>12</pointsize>
-                <weight>75</weight>
-                <bold>true</bold>
-               </font>
-              </property>
-              <property name="text">
-               <string>Soil Probe</string>
-              </property>
-              <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-              </property>
-             </widget>
-            </item>
-            <item>
-             <layout class="QGridLayout" name="gridLayout_9">
-              <item row="0" column="0">
-               <widget class="QLabel" name="label_20">
-                <property name="text">
-                 <string>Soil Temperature:</string>
-                </property>
-                <property name="alignment">
-                 <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-                </property>
-               </widget>
-              </item>
-              <item row="1" column="1">
-               <widget class="QLabel" name="label_23">
-                <property name="text">
-                 <string>N/A</string>
-                </property>
-               </widget>
-              </item>
-              <item row="1" column="0">
-               <widget class="QLabel" name="label_22">
-                <property name="text">
-                 <string>Soil PH:</string>
-                </property>
-                <property name="alignment">
-                 <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-                </property>
-               </widget>
-              </item>
-              <item row="0" column="1">
-               <widget class="QLabel" name="label_21">
-                <property name="text">
-                 <string>N/A</string>
-                </property>
-               </widget>
-              </item>
-              <item row="0" column="2">
-               <spacer name="horizontalSpacer_3">
-                <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>
-            </item>
-            <item>
-             <spacer name="horizontalSpacer_6">
-              <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>
-             <layout class="QHBoxLayout" name="horizontalLayout_5">
-              <item>
-               <widget class="QPushButton" name="pushButton_11">
-                <property name="text">
-                 <string>Read Soil Probe</string>
-                </property>
-               </widget>
-              </item>
-              <item>
-               <spacer name="horizontalSpacer_5">
-                <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>
-            </item>
-            <item>
-             <spacer name="verticalSpacer_3">
-              <property name="orientation">
-               <enum>Qt::Vertical</enum>
-              </property>
-              <property name="sizeHint" stdset="0">
-               <size>
-                <width>20</width>
-                <height>40</height>
-               </size>
-              </property>
-             </spacer>
-            </item>
-           </layout>
-          </item>
-          <item>
-           <widget class="Line" name="line">
-            <property name="orientation">
-             <enum>Qt::Vertical</enum>
-            </property>
-           </widget>
-          </item>
-          <item>
-           <layout class="QVBoxLayout" name="verticalLayout_7">
-            <item>
-             <widget class="QLabel" name="label_19">
-              <property name="font">
-               <font>
-                <pointsize>12</pointsize>
-                <weight>75</weight>
-                <bold>true</bold>
-               </font>
-              </property>
-              <property name="text">
-               <string>Dust Sensor</string>
-              </property>
-              <property name="alignment">
-               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-              </property>
-             </widget>
-            </item>
-            <item>
-             <layout class="QGridLayout" name="gridLayout_8">
-              <item row="0" column="0">
-               <widget class="QLabel" name="label_12">
-                <property name="text">
-                 <string>Small Dust Present:</string>
-                </property>
-                <property name="alignment">
-                 <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-                </property>
-               </widget>
-              </item>
-              <item row="1" column="0">
-               <widget class="QLabel" name="label_14">
-                <property name="text">
-                 <string>Large Dust Present:</string>
-                </property>
-                <property name="alignment">
-                 <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-                </property>
-               </widget>
-              </item>
-              <item row="0" column="1">
-               <widget class="QLabel" name="label_11">
-                <property name="text">
-                 <string>False</string>
-                </property>
-               </widget>
-              </item>
-              <item row="1" column="1">
-               <widget class="QLabel" name="label_13">
-                <property name="text">
-                 <string>False</string>
-                </property>
-               </widget>
-              </item>
-              <item row="0" column="2">
-               <spacer name="horizontalSpacer_2">
-                <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>
-            </item>
-            <item>
-             <spacer name="verticalSpacer_4">
-              <property name="orientation">
-               <enum>Qt::Vertical</enum>
-              </property>
-              <property name="sizeHint" stdset="0">
-               <size>
-                <width>20</width>
-                <height>40</height>
-               </size>
-              </property>
-             </spacer>
-            </item>
-           </layout>
+         <layout class="QGridLayout" name="gridLayout_4">
+          <item row="0" column="0">
+           <widget class="QWidget" name="ssh_console_widget" native="true"/>
           </item>
          </layout>
         </widget>
@@ -2108,6 +1897,125 @@ N/A</string>
                </item>
               </layout>
              </item>
+             <item>
+              <layout class="QHBoxLayout" name="horizontalLayout_4">
+               <property name="spacing">
+                <number>6</number>
+               </property>
+               <item>
+                <widget class="QLabel" name="label_11">
+                 <property name="font">
+                  <font>
+                   <weight>75</weight>
+                   <bold>true</bold>
+                  </font>
+                 </property>
+                 <property name="text">
+                  <string>Color</string>
+                 </property>
+                </widget>
+               </item>
+               <item>
+                <widget class="Line" name="line">
+                 <property name="orientation">
+                  <enum>Qt::Vertical</enum>
+                 </property>
+                </widget>
+               </item>
+               <item>
+                <widget class="QLabel" name="label_4">
+                 <property name="text">
+                  <string>Navigation</string>
+                 </property>
+                </widget>
+               </item>
+               <item>
+                <widget class="QLabel" name="manual_waypoint_navigation_color_label">
+                 <property name="minimumSize">
+                  <size>
+                   <width>25</width>
+                   <height>25</height>
+                  </size>
+                 </property>
+                 <property name="maximumSize">
+                  <size>
+                   <width>25</width>
+                   <height>25</height>
+                  </size>
+                 </property>
+                 <property name="styleSheet">
+                  <string notr="true">background-color:yellow;</string>
+                 </property>
+                 <property name="text">
+                  <string/>
+                 </property>
+                </widget>
+               </item>
+               <item>
+                <widget class="QPushButton" name="manual_waypoint_navigation_color_set_button">
+                 <property name="text">
+                  <string>Set</string>
+                 </property>
+                </widget>
+               </item>
+               <item>
+                <widget class="Line" name="line_7">
+                 <property name="orientation">
+                  <enum>Qt::Vertical</enum>
+                 </property>
+                </widget>
+               </item>
+               <item>
+                <widget class="QLabel" name="label_12">
+                 <property name="text">
+                  <string>Landmark</string>
+                 </property>
+                </widget>
+               </item>
+               <item>
+                <widget class="QLabel" name="manual_waypoint_landmark_color_label">
+                 <property name="minimumSize">
+                  <size>
+                   <width>25</width>
+                   <height>25</height>
+                  </size>
+                 </property>
+                 <property name="maximumSize">
+                  <size>
+                   <width>25</width>
+                   <height>25</height>
+                  </size>
+                 </property>
+                 <property name="styleSheet">
+                  <string notr="true">background-color:blue;</string>
+                 </property>
+                 <property name="text">
+                  <string/>
+                 </property>
+                </widget>
+               </item>
+               <item>
+                <widget class="QPushButton" name="manual_waypoint_landmark_color_set_button">
+                 <property name="text">
+                  <string>Set</string>
+                 </property>
+                </widget>
+               </item>
+               <item>
+                <spacer name="horizontalSpacer_2">
+                 <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>
+             </item>
              <item>
               <widget class="Line" name="line_2">
                <property name="orientation">
@@ -2279,22 +2187,26 @@ N/A</string>
              </item>
              <item>
               <widget class="QTableWidget" name="navigation_waypoints_table_widget">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+                 <horstretch>0</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="minimumSize">
+                <size>
+                 <width>0</width>
+                 <height>0</height>
+                </size>
+               </property>
+               <property name="maximumSize">
+                <size>
+                 <width>16777215</width>
+                 <height>9999999</height>
+                </size>
+               </property>
                <property name="styleSheet">
-                <string notr="true">QTableView{
-	selection-background-color: #DE8D47;
-}
-
-QTableView QHeaderView{
-	background-color: #201F1D;
-}
-
-QTableView QHeaderView::section{
-	background-color: #201F1D;
-}
-
-QTableView QTableCornerButton::section{
-	background-color: #201F1D;
-}</string>
+                <string notr="true"/>
                </property>
                <property name="horizontalScrollBarPolicy">
                 <enum>Qt::ScrollBarAlwaysOff</enum>
@@ -2332,16 +2244,6 @@ QTableView QTableCornerButton::section{
                <attribute name="verticalHeaderDefaultSectionSize">
                 <number>30</number>
                </attribute>
-               <row>
-                <property name="text">
-                 <string>1</string>
-                </property>
-               </row>
-               <row>
-                <property name="text">
-                 <string>2</string>
-                </property>
-               </row>
                <column>
                 <property name="text">
                  <string>#</string>
@@ -2359,49 +2261,9 @@ QTableView QTableCornerButton::section{
                </column>
                <column>
                 <property name="text">
-                 <string>Dist</string>
+                 <string>Color</string>
                 </property>
                </column>
-               <item row="0" column="0">
-                <property name="text">
-                 <string>1</string>
-                </property>
-               </item>
-               <item row="0" column="1">
-                <property name="text">
-                 <string>32.1665</string>
-                </property>
-               </item>
-               <item row="0" column="2">
-                <property name="text">
-                 <string>-112.1115</string>
-                </property>
-               </item>
-               <item row="0" column="3">
-                <property name="text">
-                 <string>2.4 KM</string>
-                </property>
-               </item>
-               <item row="1" column="0">
-                <property name="text">
-                 <string>2</string>
-                </property>
-               </item>
-               <item row="1" column="1">
-                <property name="text">
-                 <string>33.1124</string>
-                </property>
-               </item>
-               <item row="1" column="2">
-                <property name="text">
-                 <string>-111.4334</string>
-                </property>
-               </item>
-               <item row="1" column="3">
-                <property name="text">
-                 <string>5.7 KM</string>
-                </property>
-               </item>
               </widget>
              </item>
              <item>
@@ -2409,6 +2271,9 @@ QTableView QTableCornerButton::section{
                <property name="spacing">
                 <number>2</number>
                </property>
+               <property name="bottomMargin">
+                <number>4</number>
+               </property>
                <item>
                 <widget class="QPushButton" name="navigation_waypoints_set_button">
                  <property name="sizePolicy">
@@ -2423,11 +2288,13 @@ QTableView QTableCornerButton::section{
                    <height>35</height>
                   </size>
                  </property>
+                 <property name="font">
+                  <font>
+                   <pointsize>8</pointsize>
+                  </font>
+                 </property>
                  <property name="styleSheet">
-                  <string notr="true">QPushButton{
-	color: #201F1D;
-	background-color: #868685;
-}</string>
+                  <string notr="true"/>
                  </property>
                  <property name="text">
                   <string>Set</string>
@@ -2448,11 +2315,13 @@ QTableView QTableCornerButton::section{
                    <height>35</height>
                   </size>
                  </property>
+                 <property name="font">
+                  <font>
+                   <pointsize>8</pointsize>
+                  </font>
+                 </property>
                  <property name="styleSheet">
-                  <string notr="true">QPushButton{
-	color: #201F1D;
-	background-color: #868685;
-}</string>
+                  <string notr="true"/>
                  </property>
                  <property name="text">
                   <string>Add
@@ -2474,11 +2343,13 @@ Manual</string>
                    <height>35</height>
                   </size>
                  </property>
+                 <property name="font">
+                  <font>
+                   <pointsize>8</pointsize>
+                  </font>
+                 </property>
                  <property name="styleSheet">
-                  <string notr="true">QPushButton{
-	color: #201F1D;
-	background-color: #868685;
-}</string>
+                  <string notr="true"/>
                  </property>
                  <property name="text">
                   <string>Add
@@ -2500,11 +2371,13 @@ GPS</string>
                    <height>35</height>
                   </size>
                  </property>
+                 <property name="font">
+                  <font>
+                   <pointsize>8</pointsize>
+                  </font>
+                 </property>
                  <property name="styleSheet">
-                  <string notr="true">QPushButton{
-	color: #201F1D;
-	background-color: #868685;
-}</string>
+                  <string notr="true"/>
                  </property>
                  <property name="text">
                   <string>Delete</string>
@@ -2575,22 +2448,26 @@ GPS</string>
              </item>
              <item>
               <widget class="QTableWidget" name="landmark_waypoints_table_widget">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+                 <horstretch>0</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="minimumSize">
+                <size>
+                 <width>0</width>
+                 <height>0</height>
+                </size>
+               </property>
+               <property name="maximumSize">
+                <size>
+                 <width>16777215</width>
+                 <height>9999999</height>
+                </size>
+               </property>
                <property name="styleSheet">
-                <string notr="true">QTableView{
-	selection-background-color: #DE8D47;
-}
-
-QTableView QHeaderView{
-	background-color: #201F1D;
-}
-
-QTableView QHeaderView::section{
-	background-color: #201F1D;
-}
-
-QTableView QTableCornerButton::section{
-	background-color: #201F1D;
-}</string>
+                <string notr="true"/>
                </property>
                <property name="horizontalScrollBarPolicy">
                 <enum>Qt::ScrollBarAlwaysOff</enum>
@@ -2631,16 +2508,6 @@ QTableView QTableCornerButton::section{
                <attribute name="verticalHeaderDefaultSectionSize">
                 <number>30</number>
                </attribute>
-               <row>
-                <property name="text">
-                 <string>1</string>
-                </property>
-               </row>
-               <row>
-                <property name="text">
-                 <string>2</string>
-                </property>
-               </row>
                <column>
                 <property name="text">
                  <string>Name</string>
@@ -2658,49 +2525,9 @@ QTableView QTableCornerButton::section{
                </column>
                <column>
                 <property name="text">
-                 <string>Dist</string>
+                 <string>Color</string>
                 </property>
                </column>
-               <item row="0" column="0">
-                <property name="text">
-                 <string>Astronaut</string>
-                </property>
-               </item>
-               <item row="0" column="1">
-                <property name="text">
-                 <string>32.1665</string>
-                </property>
-               </item>
-               <item row="0" column="2">
-                <property name="text">
-                 <string>-112.1115</string>
-                </property>
-               </item>
-               <item row="0" column="3">
-                <property name="text">
-                 <string>2.4 KM</string>
-                </property>
-               </item>
-               <item row="1" column="0">
-                <property name="text">
-                 <string>Water</string>
-                </property>
-               </item>
-               <item row="1" column="1">
-                <property name="text">
-                 <string>33.1124</string>
-                </property>
-               </item>
-               <item row="1" column="2">
-                <property name="text">
-                 <string>-111.4334</string>
-                </property>
-               </item>
-               <item row="1" column="3">
-                <property name="text">
-                 <string>5.7 KM</string>
-                </property>
-               </item>
               </widget>
              </item>
              <item>
@@ -2708,6 +2535,9 @@ QTableView QTableCornerButton::section{
                <property name="spacing">
                 <number>2</number>
                </property>
+               <property name="bottomMargin">
+                <number>4</number>
+               </property>
                <item>
                 <widget class="QPushButton" name="landmark_waypoints_set_button">
                  <property name="sizePolicy">
@@ -2722,11 +2552,13 @@ QTableView QTableCornerButton::section{
                    <height>35</height>
                   </size>
                  </property>
+                 <property name="font">
+                  <font>
+                   <pointsize>8</pointsize>
+                  </font>
+                 </property>
                  <property name="styleSheet">
-                  <string notr="true">QPushButton{
-	color: #201F1D;
-	background-color: #868685;
-}</string>
+                  <string notr="true"/>
                  </property>
                  <property name="text">
                   <string>Set</string>
@@ -2747,11 +2579,13 @@ QTableView QTableCornerButton::section{
                    <height>35</height>
                   </size>
                  </property>
+                 <property name="font">
+                  <font>
+                   <pointsize>8</pointsize>
+                  </font>
+                 </property>
                  <property name="styleSheet">
-                  <string notr="true">QPushButton{
-	color: #201F1D;
-	background-color: #868685;
-}</string>
+                  <string notr="true"/>
                  </property>
                  <property name="text">
                   <string>Add
@@ -2773,11 +2607,13 @@ Manual</string>
                    <height>35</height>
                   </size>
                  </property>
+                 <property name="font">
+                  <font>
+                   <pointsize>8</pointsize>
+                  </font>
+                 </property>
                  <property name="styleSheet">
-                  <string notr="true">QPushButton{
-	color: #201F1D;
-	background-color: #868685;
-}</string>
+                  <string notr="true"/>
                  </property>
                  <property name="text">
                   <string>Add
@@ -2799,11 +2635,13 @@ GPS</string>
                    <height>35</height>
                   </size>
                  </property>
+                 <property name="font">
+                  <font>
+                   <pointsize>8</pointsize>
+                  </font>
+                 </property>
                  <property name="styleSheet">
-                  <string notr="true">QPushButton{
-	color: #201F1D;
-	background-color: #868685;
-}</string>
+                  <string notr="true"/>
                  </property>
                  <property name="text">
                   <string>Delete</string>

software/ros_packages/rover_camera/src/rover_camera.cpp

@@ -26,6 +26,8 @@ public:
         node_handle->param("device_path", capture_device_path, std::string("/dev/video0"));
         node_handle->param("fps", fps, 30);
 
+        node_handle->param("upside_down", upside_down, false);
+
         node_handle->param("large_image_width", large_image_width, 1280);
         node_handle->param("large_image_height", large_image_height, 720);
         node_handle->param("medium_image_width", medium_image_width, 640);
@@ -97,30 +99,22 @@ public:
             }
 
             if(!image_large.empty()){
-                bool large_ready_to_broadcast = false;
-                bool medium_ready_to_broadcast = false;
-                bool small_ready_to_broadcast = false;
+                if(upside_down){
+                    cv::flip(image_large, image_large, -1);
+                }
 
                 if(broadcast_large_image){
                     large_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_large).toImageMsg();
-                    large_ready_to_broadcast = true;
-                }
-
-                if(broadcast_medium_image){
+                    large_image_publisher.publish(large_image_message);
+                }else if(broadcast_medium_image){
                     cv::resize(image_large, image_medium, cv::Size(medium_image_width, medium_image_height));
                     medium_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_medium).toImageMsg();
-                    medium_ready_to_broadcast = true;
-                }
-
-                if(broadcast_small_image){
+                    medium_image_publisher.publish(medium_image_message);
+                }else if(broadcast_small_image){
                     cv::resize(image_large, image_small, cv::Size(small_image_width, small_image_height));
                     small_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_small).toImageMsg();
-                    small_ready_to_broadcast = true;
+                    small_image_publisher.publish(small_image_message);
                 }
-
-                if(large_ready_to_broadcast){ large_image_publisher.publish(large_image_message); };
-                if(medium_ready_to_broadcast){ medium_image_publisher.publish(medium_image_message); };
-                if(small_ready_to_broadcast){ small_image_publisher.publish(small_image_message); };
             }
 
             ros::spinOnce();
@@ -152,6 +146,8 @@ private:
 
     ros::Rate *loop_rate;
 
+    bool upside_down;
+
     int large_image_width;
     int large_image_height;
     int medium_image_width;

software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py

@@ -83,7 +83,7 @@ class DriveCoordinator(object):
             try:
                 self.process_drive_commands()
             except Exception, error:
-                print "Error occurred:", error
+                print "COORDINATOR: Error occurred:", error
 
             time_diff = time() - start_time
 

software/ros_packages/rover_control/src/iris_controller/iris_controller.py

@@ -118,11 +118,9 @@ class IrisController(object):
                 self.broadcast_drive_if_current_mode()
                 self.broadcast_arm_if_current_mode()
                 self.broadcast_iris_status()
-                self.iris_last_seen_time = time()
 
             except Exception, error:
-                print "Error occurred:", error
-                return
+                print "IRIS: Error occurred:", error
 
             if (time() - self.iris_last_seen_time) > IRIS_LAST_SEEN_TIMEOUT:
                 print "Iris not seen for", IRIS_LAST_SEEN_TIMEOUT, "seconds. Exiting."
@@ -135,6 +133,7 @@ class IrisController(object):
     def read_registers(self):
         try:
             self.registers = self.iris.read_registers(0, len(MODBUS_REGISTERS))
+            self.iris_last_seen_time = time()
         except Exception, error:
             self.iris_connected = False
 

software/ros_packages/rover_main/launch/rover/cameras.launch

@@ -3,6 +3,7 @@
         <!-- Start Undercarriage Camera -->
         <node name="undercarriage" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 1" respawn="true" output="screen">
             <param name="device_path" value="/dev/rover/camera_undercarriage"/>
+            <param name="upside_down" value="true"/>
         </node>
 
         <!--&lt;!&ndash; Start Main Navigation Camera &ndash;&gt;-->
@@ -17,7 +18,7 @@
 
         <!--&lt;!&ndash; Start End Effector Camera &ndash;&gt;-->
         <node name="end_effector" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 4" respawn="true" output="screen" >
-            <param name="is_rtsp_camera" value="True" />
+            <param name="is_rtsp_camera" value="true" />
             <param name="device_path" value="rtsp://192.168.1.11" />
         </node>
     </group>

software/ros_packages/rover_main/launch/rover/control.launch

@@ -9,10 +9,8 @@
             <param name="port" value="/dev/rover/ttyREAR"/>
             <param name="drive_control_topic" value="drive_control/rear"/>
             <param name="drive_control_status_topic" value="drive_status/rear"/>
-            <param name="first_motor_id" value="1"/>
-            <param name="second_motor_id" value="2"/>
-            <param name="invert_first_motor" value="True"/>
-            <param name="invert_second_motor" value="True"/>
+            <param name="first_motor_id" value="2"/>
+            <param name="second_motor_id" value="1"/>
         </node>
 
         <node name="left_bogie" pkg="rover_control" type="drive_control.py" respawn="true" output="screen">

software/ros_packages/rover_main/launch/rover/odometry2.launch

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

software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch

@@ -153,7 +153,8 @@
             <rosparam param="topics">
                 [
                 {name: "/rover_status/battery_status", compress: false, rate: 1.0},
-                {name: "/rover_control/tower/status/co2", compress: false, rate: 1.0}
+                {name: "/rover_control/tower/status/co2", compress: false, rate: 1.0},
+                {name: "/rover_odometry/imu/data", compress: false, rate: 10.0},
                 ]
             </rosparam>
         </node>

software/ros_packages/rover_odometry/src/odometry.py

@@ -28,6 +28,8 @@ DEFAULT_IMU_TOPIC = "imu/data"
 
 DEFAULT_HERTZ = 100
 
+ODOM_LAST_SEEN_TIMEOUT = 1  # seconds
+
 
 #####################################
 # DriveControl Class Definition
@@ -50,6 +52,8 @@ class Odometry(object):
         self.sentence_publisher = rospy.Publisher(self.gps_sentence_topic, Sentence, queue_size=1)
         self.imu_data_publisher = rospy.Publisher(self.imu_data_topic, Imu, queue_size=1)
 
+        self.odom_last_seen_time = time()
+
         self.run()
 
     def run(self):
@@ -58,9 +62,14 @@ class Odometry(object):
 
             try:
                 self.process_messages()
+                self.odom_last_seen_time = time()
 
             except Exception, error:
-                print "Error occurred:", error
+                pass
+
+            if (time() - self.odom_last_seen_time) > ODOM_LAST_SEEN_TIMEOUT:
+                print "Odometry not seen for", ODOM_LAST_SEEN_TIMEOUT, "seconds. Exiting."
+                return  # Exit so respawn can take over
 
             time_diff = time() - start_time