Map now shows coordinates and tiny rover in middle. Going to try to debug IRIS issues.

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
@@ -165,10 +170,10 @@ class GMapsStitcher(object):
         """
         # Magic Lines
         return math.degrees(math.pi / 2 - 2 * math.atan(math.exp(((lat_pixels +
-                            self.helper.pixels_to_degrees(
+                                                                   self.helper.pixels_to_degrees(
-                                (iterator - self.num_tiles /
+                                                                       (iterator - self.num_tiles /
-                                 2) * _TILESIZE, self.zoom)) -
+                                                                        2) * _TILESIZE, self.zoom)) -
-                                 _EARTHPIX) / _PIXRAD)))
+                                                                  _EARTHPIX) / _PIXRAD)))
 
     def fetch_tiles(self):
         """
@@ -188,14 +193,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(
+                round(2 * self.helper.pixels_to_meters(
-                               self.latitude, self.zoom) /
+                    self.latitude, self.zoom) /
-                               (_TILESIZE / 2. / self.radius_meters))))
+                      (_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 +236,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 +248,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 +271,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_x(self.center_x - x)
-        self._constrain_y(self.center_y-y)
+        self._constrain_y(self.center_y - y)
         self.update()
 
     def _get_cartesian(self, lat, lon):
@@ -309,9 +314,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 +327,8 @@ class GMapsStitcher(object):
         self.center_x = x
         self.center_y = y
 
-        self.left_x = (self.center_x - (self.width/2))
+        self.left_x = (self.center_x - (self.width / 2))
-        self.upper_y = (self.center_y - (self.height/2))
+        self.upper_y = (self.center_y - (self.height / 2))
         self.update()
 
     # def update_rover_map_location(self, lat, lon):
@@ -348,7 +353,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 +363,19 @@ class OverlayImage(object):
         self.center_x = x
         self.center_y = y
 
-        self.left_x = (self.center_x - (self.width/2))
+        self.left_x = (self.center_x - (self.width / 2))
-        self.upper_y = (self.center_y - (self.height/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,8 +384,14 @@ 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.display_image_copy = self.display_image.copy()
+
         self.generate_dot_and_hat()
         self.indicator.save("location.png")
 
@@ -406,43 +426,63 @@ 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")
+            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._draw_rover(latitude, longitude, compass)
-        self.update()
+        self._draw_coordinate_text(latitude, longitude)
+        self.update(latitude, longitude)
 
         return self.display_image
 
     def generate_dot_and_hat(self):
-        self.indicator = self.helper.new_image(100, 100, True)
+        self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((50, 50))
-        draw = PIL.ImageDraw.Draw(self.indicator)
+        # self.indicator = self.helper.new_image(100, 100, True)
-        draw.ellipse((50-12, 50-12, 50+12, 50+12), fill="red")
+        # draw = PIL.ImageDraw.Draw(self.indicator)
-        draw.line((25, 40, 50, 12), fill="red", width=7)
+        # draw.ellipse((50 - 12, 50 - 12, 50 + 12, 50 + 12), fill="red")
-        draw.line((50, 12, 75, 40), fill="red", width=7)
+        # draw.line((25, 40, 50, 12), fill="red", width=7)
+        # draw.line((50, 12, 75, 40), fill="red", width=7)
+
+    def _draw_coordinate_text(self, latitude, longitude):
+        location_text = "LAT: %+014.9f\nLON: %+014.9f" % (latitude, longitude)
+        # location_text = "LAT: " + str(latitude) + "\nLON: " + str(longitude)
+
+        font = PIL.ImageFont.truetype("UbuntuMono-R", size=20)
+
+        new_image = PIL.Image.new('RGBA', (200, 45), "black")
+
+        draw = PIL.ImageDraw.Draw(new_image)
+
+        draw.multiline_text((5, 0), location_text, font=font)
+
+        self.display_image.paste(new_image, (0, 0))
 
     def _draw_rover(self, lat, lon, angle=0):
         x, y = self._get_cartesian(lat, lon)
-        # print x,y
+
-        # Center of the circle on the indicator is (12.5, 37.5)
+        x -= 25
-        x = x - 50
+        y -= 25
-        y = y - 50
+
         rotated = self.indicator.copy()
-        rotated = rotated.rotate(angle, expand=True)
+        rotated = rotated.rotate(angle, resample=PIL.Image.BICUBIC)
-        rotated.save("rotated.png")
+        # 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.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

@@ -47,18 +47,20 @@ 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.last_heading = 0
 
     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()
@@ -83,7 +85,7 @@ class RoverMapCoordinator(QtCore.QThread):
                                                          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,12 +99,17 @@ class RoverMapCoordinator(QtCore.QThread):
     def _get_map_image(self):
         while self.map_image is None:
             self.map_image = self.google_maps_object.display_image
+
+            if self.map_image:
+                self.map_image_copy = self.map_image.copy()
         # self.overlay_image_object.update_new_location(44.567161,
         #                                               -123.278432,
         #                                               .7,
         #                                               [],
         #                                               [])
         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)
@@ -147,9 +154,8 @@ class RoverMapCoordinator(QtCore.QThread):
     def update_overlay(self):
         if self.latitude and self.longitude:
             if not numpy.isnan(self.latitude) and not numpy.isnan(self.longitude):
-
+                latitude = float(self.latitude)
-                longitude = self.latitude
+                longitude = float(self.longitude)
-                latitude = self.longitude
 
                 navigation_list = self._get_table_elements(self.navigation_label)
                 # landmark_list = self._get_table_elements(self.landmark_label)
@@ -157,9 +163,10 @@ class RoverMapCoordinator(QtCore.QThread):
                 self.overlay_image = self.overlay_image_object.update_new_location(
                                                               latitude,
                                                               longitude,
-                                                              70,
+                                                              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):

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

@@ -1392,226 +1392,10 @@ 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>
-          </item>
-         </layout>
         </widget>
         <widget class="QWidget" name="tab_2">
          <attribute name="title">

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

@@ -121,7 +121,7 @@ class IrisController(object):
                 self.iris_last_seen_time = time()
 
             except Exception, error:
-                print "Error occurred:", error
+                print "IRIS: Error occurred:", error
                 return
 
             if (time() - self.iris_last_seen_time) > IRIS_LAST_SEEN_TIMEOUT: