add offline map for 2D plot

src/sailing_robot/scripts/debugging_2D_plot_matplot

@@ -1,14 +1,15 @@
 #!/usr/bin/env python
 
+import os
 from sailing_robot.navigation import Navigation
 import rospy, math, time, collections
 import matplotlib.pyplot as plt
 import matplotlib.animation as animation
+import matplotlib.image as mpimg
 
 from std_msgs.msg import String, Float32, Float64
 from sensor_msgs.msg import NavSatFix
 import numpy as np
-from matplotlib.transforms import blended_transform_factory
 
 import smopy
 
@@ -29,6 +30,7 @@ class Debugging_2D_matplot():
         self.rate = rospy.Rate(rospy.get_param("config/rate"))
         self.wp_radius = rospy.get_param('wp/acceptRadius')
 
+        # Get waypoints
         if rospy.has_param('wp/list'):
             wp_list = rospy.get_param('wp/list')
         elif rospy.has_param('wp/tasks'):
@@ -86,6 +88,45 @@ class Debugging_2D_matplot():
         self.wind_boat = msg.data
         self.wind_north = np.radians(self.heading + self.wind_boat)
 
+    def get_bg_image(self):
+
+        self.has_bg_img = False
+        self.image = None
+        side_dist = None 
+        image_origin = None
+
+        my_dir = os.path.dirname(__file__)
+        image_dir = os.path.abspath(os.path.join(my_dir, '../../../utilities/map_bg_images'))
+
+        # select the correct image if any
+        for filename in os.listdir(image_dir):
+            if not filename.endswith(".png"):
+                continue
+
+            data_filename = filename[:-4].split("_")
+            current_img_origin = self.nav.latlon_to_utm(float(data_filename[0]), float(data_filename[1]))
+
+            dist_to_origin = ((self.origin[0] - current_img_origin[0])**2 +
+                              (self.origin[1] - current_img_origin[1])**2)**0.5
+
+            if dist_to_origin < 1000:
+                image_origin = current_img_origin 
+                side_dist = float(data_filename[2])
+                self.image = mpimg.imread(os.path.join(image_dir,filename))
+                self.has_bg_img = True
+                break
+
+        if not self.has_bg_img:
+            return
+
+        minx = image_origin[0] - side_dist - self.origin[0]
+        maxx = image_origin[0] + side_dist - self.origin[0]
+        miny = image_origin[1] - side_dist - self.origin[1]
+        maxy = image_origin[1] + side_dist - self.origin[1]
+
+        image_size = (minx, maxx, miny, maxy)
+        self.image_show = self.ax.imshow(self.image, extent=image_size)
+
 
     def init_plot(self):
 
@@ -103,13 +144,13 @@ class Debugging_2D_matplot():
         plt.plot([], [], c=C[7], label="Goal heading")
         plt.plot([], [], c=C[1], label="Wind direction")
 
-        # display  Waypoints
+        # display Waypoints
         self.wpfig = plt.scatter(self.wp_array[0], self.wp_array[1], c=C[3])
 
         plt.tight_layout()
         self.ax = plt.subplot(111)
 
-        plt.legend()
+        self.get_bg_image()
 
 
     def get_arrow(self, angle, color, reverse=False):
@@ -123,14 +164,6 @@ class Debugging_2D_matplot():
         else:
             style = '->'
 
-        # sign = -(reverse-0.5)*2
-        # tform = blended_transform_factory(self.ax.transAxes, self.ax.transAxes)
-        # arrow = plt.arrow(0.9, 0.1,
-        #                   sign*0.03*np.sin(angle)/scale_dx, sign*0.03*np.cos(angle)/scale_dy,
-        #                   head_width=0.005, 
-        #                   head_length=0.005,  fc=color, ec=color, 
-        #                   transform=tform, label='Heading')
-
         arrow_ori = (0.88, 0.12)
         arrow_target = (arrow_ori[0] + 0.05*np.sin(angle)/scale_dx, arrow_ori[1] + 0.05*np.cos(angle)/scale_dy)
         arrow = self.ax.annotate("",
@@ -150,32 +183,35 @@ class Debugging_2D_matplot():
             lat, lon = np.array(self.position_history).T
             self.boatline.set_data(lat,lon)
 
-        self.wind_arrow = self.get_arrow(self.wind_north, C[1], reverse=True)
+        wind_arrow = self.get_arrow(self.wind_north, C[1], reverse=True)
 
-        self.heading_arrow = self.get_arrow(np.radians(self.heading), C[0])
+        heading_arrow = self.get_arrow(np.radians(self.heading), C[0])
 
-        self.goal_heading_arrow = self.get_arrow(np.radians(self.goal_heading), C[7])
+        goal_heading_arrow = self.get_arrow(np.radians(self.goal_heading), C[7])
 
         arrow_col = C[0]
         if self.sailing_state != 'normal':
             arrow_col = C[1]
         arrow_dx = 0.1*np.sin(np.radians(self.heading))
         arrow_dy = 0.1*np.cos(np.radians(self.heading))
-        self.boat_arrow = plt.arrow(self.position[0] - arrow_dx, self.position[1] - arrow_dy,
+        boat_arrow = plt.arrow(self.position[0] - arrow_dx, self.position[1] - arrow_dy,
                                     arrow_dx, arrow_dy,
                                     head_width=0.5, 
                                     head_length=1.,  
                                     fc=arrow_col, 
                                     ec=arrow_col)
 
         self.update_window(i)
+        if self.has_bg_img:
+            return self.image_show, self.boatline, wind_arrow, boat_arrow, self.wpfig, goal_heading_arrow, heading_arrow, plt.legend()
+        else:
+            return self.boatline, wind_arrow, boat_arrow, self.wpfig, goal_heading_arrow, heading_arrow, plt.legend()
 
-        return self.boatline, self.wind_arrow, self.boat_arrow, self.wpfig, self.goal_heading_arrow, self.heading_arrow
 
     def update_window(self, i):
         # update window only every second
         if not i%10 == 0:
-            return
+            return 
 
         # rounding of the window size in m 
         rounding = 10.0
@@ -205,37 +241,14 @@ class Debugging_2D_matplot():
         maxx = + dist * scale_dx
         miny = - dist * scale_dy
         maxy = + dist * scale_dy 
-        self.ax.axis([minx, maxx, miny, maxy])
 
         if self.window == [minx, maxx, miny, maxy]:
-            return
+            return 
 
         self.window = [minx, maxx, miny, maxy]
-
-
-        SO_corner = self.nav.utm_to_latlon(self.origin[0] + minx, self.origin[1] + miny)
-        NE_corner = self.nav.utm_to_latlon(self.origin[0] + maxx, self.origin[1] + maxy)
-
-        image_map = smopy.Map((float(SO_corner.lat),
-                              float(SO_corner.lon),
-                              float(NE_corner.lat),
-                              float(NE_corner.lon)), )
-
-        mapminx, mapminy = image_map.to_pixels((float(SO_corner.lat),
-                                                float(SO_corner.lon)), )
-
-        mapmaxx, mapmaxy = image_map.to_pixels((float(NE_corner.lat),
-                                                float(NE_corner.lon)), )
-
-        mapminx = int(mapminx)
-        mapminy = int(mapminy)
-        mapmaxx = int(mapmaxx)
-        mapmaxy = int(mapmaxy)
-        image = image_map.to_numpy()[mapmaxy:mapminy, mapminx:mapmaxx, :]
-
-        self.ax.imshow(image, extent=(minx, maxx, miny, maxy))
-
+        self.ax.axis(self.window)
 
+        return 
 
 
 

utilities/gen_map_image.py

@@ -0,0 +1,67 @@
+#!/usr/bin/python2
+
+import sys
+import os
+import smopy
+import yaml
+
+from PIL import Image
+import numpy as np
+from sailing_robot.navigation import Navigation
+
+
+my_dir = os.path.dirname(__file__)
+image_dir = os.path.abspath(os.path.join(my_dir, 'map_bg_images'))
+
+# Load yaml file given in argument
+input_file = sys.argv[1]
+with open(input_file, 'r') as f:
+    yaml_data = yaml.safe_load(f)
+
+
+waypoints = np.array(yaml_data['wp/table'].values())
+
+origin = [waypoints[:,0].mean(), waypoints[:,1].mean()]
+
+side_dist=250 # half of the size of the square in m
+
+location = os.path.basename(input_file).split("_")[0]
+
+output_file_name = os.path.join(image_dir,
+                                str(origin[0]) + '_' +
+                                str(origin[1]) + "_" + 
+                                str(side_dist) + "_" + 
+                                location + ".png")
+
+nav = Navigation()
+origin_utm = nav.latlon_to_utm(origin[0], origin[1])
+
+minx = - side_dist
+maxx = + side_dist
+miny = - side_dist
+maxy = + side_dist
+
+SO_corner = nav.utm_to_latlon(origin_utm[0] + minx, origin_utm[1] + miny)
+NE_corner = nav.utm_to_latlon(origin_utm[0] + maxx, origin_utm[1] + maxy)
+
+image_map = smopy.Map((float(SO_corner.lat),
+                       float(SO_corner.lon),
+                       float(NE_corner.lat),
+                       float(NE_corner.lon)), z=18, maxtiles=302, margin=0)
+
+mapminx, mapminy = image_map.to_pixels((float(SO_corner.lat),
+                                        float(SO_corner.lon)), )
+
+mapmaxx, mapmaxy = image_map.to_pixels((float(NE_corner.lat),
+                                        float(NE_corner.lon)), )
+
+mapminx = int(mapminx)
+mapminy = int(mapminy)
+mapmaxx = int(mapmaxx)
+mapmaxy = int(mapmaxy)
+image = image_map.to_numpy()[mapmaxy:mapminy, mapminx:mapmaxx, :]
+
+im = Image.fromarray(image)
+im.save(output_file_name)
+
+