update_codes_for_paper

Publication/src/Agents/AgentMyopic.py

@@ -82,7 +82,8 @@ def run(self) -> None:
             self.mu_truth_data[i, :] = mu_truth.flatten()
 
             if self.debug:
-                self.ap.plot_agent()
+                # self.ap.plot_agent()
+                self.ap.plot_agent4paper()
 
             # p1: parallel move AUV to the first location
             wp_next = self.myopic.get_next_waypoint()

Publication/src/Agents/AgentRRTStar.py

@@ -87,7 +87,8 @@ def run(self) -> None:
             self.mu_truth_data[i, :] = mu_truth.flatten()
 
             if self.debug:
-                self.ap.plot_agent()
+                # self.ap.plot_agent()
+                self.ap.plot_agent4paper()
 
             # s0, get the current waypoint
             wp_now = self.planner.get_current_waypoint()

Publication/src/Config.py

@@ -56,7 +56,7 @@ def __init__(self) -> None:
 
         """ Default simulation parameter seteup. """
         self.__num_steps = 120  # number of steps.
-        self.__num_replicates = 1  # number of replicates
+        self.__num_replicates = 100  # number of replicates
         self.__num_cores = 1  # number of cores to use
 
     @staticmethod

Publication/src/Visualiser/AgentPlotMyopic.py

@@ -6,6 +6,9 @@
 Date: 2023-09-03
 """
 from Config import Config
+from Field import Field
+from WGS import WGS
+from usr_func.is_list_empty import is_list_empty
 import os
 import matplotlib.pyplot as plt
 from matplotlib.patches import Ellipse
@@ -17,7 +20,7 @@
 from datetime import datetime
 plt.rcParams["font.family"] = "Times New Roman"
 plt.rcParams["font.size"] = 20
-from Field import Field
+
 field = Field()
 
 
@@ -34,13 +37,20 @@ def __init__(self, agent, figpath) -> None:
         self.grid = self.field.get_grid()
         self.xgrid = self.grid[:, 0]
         self.ygrid = self.grid[:, 1]
+        self.lat_grid, self.lon_grid = WGS.xy2latlon(self.grid[:, 0], self.grid[:, 1])
         self.config = Config()
         self.plg_border = self.config.get_polygon_border()
         self.plg_obs = self.config.get_polygon_obstacle()
+        self.plg_border_wgs = self.config.get_wgs_polygon_border()
+        self.plg_obs_wgs = self.config.get_wgs_polygon_obstacle()
+        self.plg_border_wgs_shapely = Polygon(self.plg_border_wgs)
+        self.plg_obs_wgs_shapely = Polygon(self.plg_obs_wgs)
         self.ylim, self.xlim = self.field.get_border_limits()
+        lat, lon = WGS.xy2latlon(self.ylim, self.xlim)
+        self.xlim_wgs = np.array([lon[0], lon[1]])
+        self.ylim_wgs = np.array([lat[0], lat[1]])
 
-        self.c = Config()
-        self.loc_start = self.c.get_loc_start()
+        self.loc_start = self.config.get_loc_start()
 
     def plot_agent(self):
         # s0: get updated field
@@ -131,6 +141,144 @@ def plot_waypoints():
         # plt.show()
         plt.close("all")
 
+    def plot_agent4paper(self):
+        # s0: get updated field
+        mu = self.grf.get_mu()
+
+        Sigma = self.grf.get_covariance_matrix()
+        threshold = self.grf.get_threshold()
+        self.cnt = self.agent.counter
+
+        traj_past = np.array(self.agent.trajectory)
+        if len(traj_past) == 0:
+            traj_past_wgs = np.array([[], []]).T
+        else:
+            lat, lon = WGS.xy2latlon(traj_past[:, 0], traj_past[:, 1])
+            traj_past_wgs = np.array([lat, lon]).T
+
+        # s1: get updated waypoints
+        wp_curr = self.myopic.get_previous_waypoint()
+        lat, lon = WGS.xy2latlon(wp_curr[0], wp_curr[1])
+        wp_curr_wgs = np.array([lat, lon])
+        wp_next = self.myopic.get_next_waypoint()
+        lat, lon = WGS.xy2latlon(wp_next[0], wp_next[1])
+        wp_next_wgs = np.array([lat, lon])
+
+        # s2: get cost valley and trees.
+        cost_valley = self.cv.get_cost_field()
+
+        fig = plt.figure(figsize=(36, 10))
+        gs = GridSpec(nrows=1, ncols=3)
+
+        """ mu, sigma, cost """
+
+        def plot_waypoints():
+            ax = plt.gca()
+            ax.plot(self.plg_border_wgs[:, 1], self.plg_border_wgs[:, 0], 'k-.')
+            ax.plot(self.plg_obs_wgs[:, 1], self.plg_obs_wgs[:, 0], 'k-.')
+            ax.plot(traj_past_wgs[:, 1], traj_past_wgs[:, 0], 'k.-', label="Trajectory", linewidth=3, markersize=20)
+            ax.plot(wp_curr_wgs[1], wp_curr_wgs[0], 'r.', markersize=20, label="Current waypoint")
+            ax.plot(wp_next_wgs[1], wp_next_wgs[0], 'y.', markersize=20, label="Next waypoint")
+            ax.set_xlabel("Longitude")
+            ax.set_ylabel("Latitude")
+            plt.legend(loc='lower right')
+
+        """ plot mean"""
+        ax = fig.add_subplot(gs[0])
+        str_timestamp = datetime.fromtimestamp(self.agent.auv.ctd.timestamp).strftime("%H:%M")
+        self.plotf_vector_wgs(self.lat_grid, self.lon_grid, mu, title="Updated salinity field at " + str_timestamp,
+                          cmap=get_cmap("BrBG", 10), vmin=10, vmax=33,
+                          cbar_title="Salinity", stepsize=1.5, threshold=threshold)
+        plot_waypoints()
+
+        """ plot var """
+        ax = fig.add_subplot(gs[1])
+        # im = ax.scatter(self.ygrid, self.xgrid, c=np.sqrt(np.diag(Sigma)), s=200,
+        #                 cmap=get_cmap("RdBu", 10), vmin=0, vmax=2)
+        # plt.title("Conditional uncertainty field")
+        # plt.colorbar(im)
+        self.plotf_vector_wgs(self.lat_grid, self.lon_grid, np.sqrt(np.diag(Sigma)),
+                          title="Updated uncertainty field at "+str_timestamp,
+                          cmap=get_cmap("RdBu", 10), vmin=0, vmax=.8, stepsize=.05, cbar_title="STD")
+        plot_waypoints()
+
+        """ plot cost valley. """
+        ax = fig.add_subplot(gs[2])
+        self.plotf_vector_wgs(self.lat_grid, self.lon_grid, cost_valley, title="Updated cost valley at " + str_timestamp,
+                          cmap=get_cmap("GnBu", 10), vmin=0, vmax=2.1, stepsize=.2, cbar_title="Cost")
+        plot_waypoints()
+
+        ax.set_xlim([self.xlim_wgs[0], self.xlim_wgs[1]])
+        ax.set_ylim([self.ylim_wgs[0], self.ylim_wgs[1]])
+
+        plt.savefig(self.figpath + "P_{:03d}.png".format(self.cnt))
+        # plt.show()
+        plt.close("all")
+
+    def plotf_vector_wgs(self, lat, lon, values, title=None, alpha=None, cmap=get_cmap("BrBG", 10),
+                     cbar_title='test', colorbar=True, vmin=None, vmax=None, ticks=None,
+                     stepsize=None, threshold=None, polygon_border=None,
+                     polygon_obstacle=None, xlabel=None, ylabel=None):
+        """ Note for triangulation:
+        - Maybe sometimes it cannot triangulate based on one axis, but changing to another axis might work.
+        - So then the final output needs to be carefully treated so that it has the correct visualisation.
+        - Also note, the floating point number can cause issues as well.
+        - Triangulation uses a different axis than lat lon after its done.
+        """
+        """ To show threshold as a red line, then vmin, vmax, stepsize, threshold needs to have values. """
+        triangulated = tri.Triangulation(lon, lat)
+        lat_triangulated = lat[triangulated.triangles].mean(axis=1)
+        lon_triangulated = lon[triangulated.triangles].mean(axis=1)
+
+        ind_mask = []
+        for i in range(len(lat_triangulated)):
+            ind_mask.append(self.is_masked_wgs(lat_triangulated[i], lon_triangulated[i]))
+        triangulated.set_mask(ind_mask)
+        refiner = tri.UniformTriRefiner(triangulated)
+        triangulated_refined, value_refined = refiner.refine_field(values.flatten(), subdiv=3)
+
+        ax = plt.gca()
+        if np.any([vmin, vmax]):
+            levels = np.arange(vmin, vmax, stepsize)
+        else:
+            levels = None
+        if np.any(levels):
+            linewidths = np.ones_like(levels) * .3
+            colors = len(levels) * ['black']
+            if threshold:
+                dist = np.abs(threshold - levels)
+                ind = np.where(dist == np.amin(dist))[0]
+                linewidths[ind[0]] = 4
+                colors[ind[0]] = 'red'
+            contourplot = ax.tricontourf(triangulated_refined, value_refined, levels=levels, cmap=cmap, alpha=alpha)
+            ax.tricontour(triangulated_refined, value_refined, levels=levels, linewidths=linewidths, colors=colors,
+                          alpha=alpha)
+        else:
+            contourplot = ax.tricontourf(triangulated_refined, value_refined, cmap=cmap, alpha=alpha)
+            ax.tricontour(triangulated_refined, value_refined, vmin=vmin, vmax=vmax, alpha=alpha)
+
+        if colorbar:
+            # fig = plt.gcf()
+            # cax = fig.add_axes([0.85, .1, 0.03, 0.25])  # left, bottom, width, height, in percentage for left and bottom
+            # cbar = fig.colorbar(contourplot, cax=cax, ticks=ticks)
+
+            cbar = plt.colorbar(contourplot, ax=ax, ticks=ticks, orientation='vertical')
+            cbar.ax.set_title(cbar_title)
+            # cbar.ax.set_ylabel(cbar_title, rotation=270, labelpad=40)
+        ax.set_title(title)
+
+        if polygon_border is not None:
+            ax.plot(polygon_border[:, 1], polygon_border[:, 0], 'r-.')
+        if polygon_obstacle is not None:
+            plg = plt.Polygon(np.fliplr(polygon_obstacle), facecolor='w', edgecolor='r', fill=True,
+                              linestyle='-.')
+            plt.gca().add_patch(plg)
+
+        ax.set_xlabel(xlabel)
+        ax.set_ylabel(ylabel)
+
+        return ax
+
     def plotf_vector(self, xplot, yplot, values, title=None, alpha=None, cmap=get_cmap("BrBG", 10),
                      cbar_title='test', colorbar=True, vmin=None, vmax=None, ticks=None,
                      stepsize=None, threshold=None, polygon_border=None,
@@ -260,6 +408,13 @@ def is_masked(xgrid, ygrid) -> bool:
             masked = True
         return masked
 
+    def is_masked_wgs(self, lat, lon) -> bool:
+        p = Point(lat, lon)
+        masked = False
+        if not self.plg_border_wgs_shapely.contains(p) or self.plg_obs_wgs_shapely.contains(p):
+            masked = True
+        return masked
+
     # @staticmethod
     # def is_masked(x, y) -> bool:
     #     """