From 90676bace142f698d3afa7d1ea2a90b3c8be01e5 Mon Sep 17 00:00:00 2001 From: Yaolin Ge Date: Mon, 11 Sep 2023 20:24:03 +0200 Subject: [PATCH] update myopic planner so to sync the timestamp --- .gitignore | 8 + Publication/src/Config.py | 6 +- Publication/src/Planner/Myopic2D/Myopic2D.py | 105 ++++--- Publication/src/Simulators/EDA_SIM.py | 275 ++++++++++++++----- Publication/src/Simulators/Simulator.py | 2 +- Publication/src/tests/test_agent_myopic.py | 8 +- Publication/src/tests/test_myopic2d.py | 20 +- Publication/src/tests/test_simulator.py | 5 +- 8 files changed, 304 insertions(+), 125 deletions(-) diff --git a/.gitignore b/.gitignore index c727fef6f..3f4adedae 100755 --- a/.gitignore +++ b/.gitignore @@ -10,6 +10,10 @@ *RRT_Random_Locations.npy **RRT_Random_Locations.npy +*/npy +**/npy +*npy +**npy _site .sass-cache @@ -24,3 +28,7 @@ vendor **.npz *.npy **.npy + +*.joblib +**.joblib + diff --git a/Publication/src/Config.py b/Publication/src/Config.py index 759d33590..c7d01a3d4 100644 --- a/Publication/src/Config.py +++ b/Publication/src/Config.py @@ -55,9 +55,9 @@ def __init__(self) -> None: self.__budget_mode = False """ Default simulation parameter seteup. """ - self.__num_steps = 120 # number of steps. - self.__num_replicates = 100 # number of replicates - self.__num_cores = 10 # number of cores to use + self.__num_steps = 15 # number of steps. + self.__num_replicates = 3 # number of replicates + self.__num_cores = 3 # number of cores to use @staticmethod def wgs2xy(value: np.ndarray) -> np.ndarray: diff --git a/Publication/src/Planner/Myopic2D/Myopic2D.py b/Publication/src/Planner/Myopic2D/Myopic2D.py index e526ffd58..a8bdde101 100755 --- a/Publication/src/Planner/Myopic2D/Myopic2D.py +++ b/Publication/src/Planner/Myopic2D/Myopic2D.py @@ -8,9 +8,9 @@ - Next waypoint: contains the next waypoint, and the AUV should go to next waypoint once it arrives at the current one. """ from CostValley.CostValley import CostValley -from Field import Field from Config import Config from usr_func.is_list_empty import is_list_empty +from shapely.geometry import Point, LineString import numpy as np import os @@ -28,10 +28,18 @@ def __init__(self, weight_eibv: float = 1., weight_ivr: float = 1.) -> None: # s0: set up default environment self.__cost_valley = CostValley(weight_eibv=weight_eibv, weight_ivr=weight_ivr) self.__grf = self.__cost_valley.get_grf_model() - wp_distance = self.__config.get_waypoint_distance() - self.__field = Field(neighbour_distance=wp_distance) - - # s1: set up trackers + self.__waypoint_distance = self.__config.get_waypoint_distance() + self.__candidates_angle = np.linspace(0, 2 * np.pi, 7) + + # s1: add polygon border and polygon obstacles. + self.__polygon_border = self.__config.get_polygon_border() + self.__polygon_border_shapely = self.__config.get_polygon_border_shapely() + self.__line_border_shapely = self.__config.get_line_border_shapely() + self.__polygon_obstacle = self.__config.get_polygon_obstacle() + self.__polygon_obstacle_shapely = self.__config.get_polygon_obstacle_shapely() + self.__line_obstacle_shapely = self.__config.get_line_obstacle_shapely() + + # s2: set up trackers self.__wp_curr = self.__config.get_loc_start() self.__wp_prev = self.__wp_curr self.__wp_next = self.__wp_curr @@ -56,31 +64,36 @@ def update_next_waypoint(self, ctd_data: np.ndarray = None) -> np.ndarray: self.__cost_valley.update_cost_valley(self.__wp_curr) # s1: find candidate locations - id_smooth, id_neighbours = self.get_candidates_indices() + # id_smooth, id_neighbours = self.get_candidates_indices() + wp_smooth, wp_neighbours = self.get_candidates_waypoints() - if not is_list_empty(id_smooth): + if not is_list_empty(wp_smooth): # get cost associated with those valid candidate locations. costs = [] - self.__loc_cand = self.__field.get_location_from_ind(id_smooth) + self.__loc_cand = wp_smooth for loc in self.__loc_cand: costs.append(self.__cost_valley.get_cost_at_location(loc)) - id_next = id_smooth[np.argmin(costs)] + wp_next = wp_smooth[np.argmin(costs)] else: - rng_ind = np.random.randint(0, len(id_neighbours)) - id_next = id_neighbours[rng_ind] + angles = np.linspace(0, 2 * np.pi, 61) + for angle in angles: + wp_next = self.__wp_curr + self.__waypoint_distance * np.array([np.sin(angle), np.cos(angle)]) + if self.is_location_legal(wp_next) and self.is_path_legal(self.__wp_curr, wp_next): + break - self.__wp_next = self.__field.get_location_from_ind(id_next) + self.__wp_next = wp_next self.__wp_prev = self.__wp_curr self.__wp_curr = self.__wp_next self.__trajectory.append([self.__wp_curr[0], self.__wp_curr[1]]) return self.__wp_next - def get_candidates_indices(self) -> tuple: + def get_candidates_waypoints(self) -> tuple: """ Filter sharp turn, bottom up and dive down behaviours. It computes the dot product between two vectors. One is from the previous waypoint to the current waypoint. The other is from the current waypoint to the potential next waypoint. + Example: >>> wp_prev = np.array([x1, y1]) >>> wp_curr = np.array([x2, y2]) @@ -92,39 +105,59 @@ def get_candidates_indices(self) -> tuple: >>> append(wp) Returns: - id_smooth: filtered candidate indices in the waypointgraph. - id_neighbours: all the neighbours at the current location. + wp_smooth: filtered candidate waypoints. + wp_neighbours: all the neighbours at the current location. """ # s1: get vec from previous wp to current wp. vec1 = self.get_vector_between_two_waypoints(self.__wp_prev, self.__wp_curr) - # s2: get all neighbours. - id_neighbours = self.__field.get_neighbour_indices(self.__field.get_ind_from_location(self.__wp_curr)) - - # s3: smooth neighbour locations. - id_smooth = [] - if self.__directional_penalty: - for iid in id_neighbours: - wp_n = self.__field.get_location_from_ind(iid) - vec2 = self.get_vector_between_two_waypoints(self.__wp_curr, wp_n) - if vec1.T @ vec2 >= 0: - id_smooth.append(iid) - else: - [id_smooth.append(iid) for iid in id_neighbours] - return id_smooth, id_neighbours + # s2: get all neighbour waypoints + wp_neighbours = [] + wp_smooth = [] + for angle in self.__candidates_angle: + wp_temp = self.__wp_curr + self.__waypoint_distance * np.array([np.sin(angle), np.cos(angle)]) + # s3: filter out illegal locations + if self.is_location_legal(wp_temp) and self.is_path_legal(self.__wp_curr, wp_temp): + wp_neighbours.append(wp_temp) + vec2 = self.get_vector_between_two_waypoints(self.__wp_curr, wp_temp) + if self.__directional_penalty: + if vec1.T @ vec2 >= 0: + wp_smooth.append(wp_temp) + else: + wp_smooth.append(wp_temp) + return wp_smooth, wp_neighbours + + def is_location_legal(self, loc: np.ndarray) -> bool: + """ + Check if the location is legal. + Args: + loc: np.array([x, y]) - def get_field(self) -> 'Field': + Returns: + True if legal, False if illegal. """ - Return the field supporting the myopic2d path planner. + point = Point(loc[0], loc[1]) + if self.__polygon_border_shapely.contains(point) and \ + not self.__polygon_obstacle_shapely.contains(point): + return True + else: + return False - Example: - >>> field = myopic2d.get_field() - >>> field.get_neighbour_indices(0) + def is_path_legal(self, loc_start: np.ndarray, loc_end: np.ndarray) -> bool: + """ + Check if the path is legal. + Args: + loc_start: np.array([x1, y1]) + loc_end: np.array([x2, y2]) Returns: - Field: field object supporting the myopic2d path planner. + True if legal, False if illegal. """ - return self.__field + line = LineString([loc_start, loc_end]) + if self.__line_border_shapely.intersects(line) or self.__line_obstacle_shapely.intersects(line): + return False + else: + return True def get_previous_waypoint(self) -> np.ndarray: """ Previous waypoint. """ diff --git a/Publication/src/Simulators/EDA_SIM.py b/Publication/src/Simulators/EDA_SIM.py index 2492cc522..288fdf789 100644 --- a/Publication/src/Simulators/EDA_SIM.py +++ b/Publication/src/Simulators/EDA_SIM.py @@ -15,7 +15,8 @@ import os from tqdm import tqdm from matplotlib.gridspec import GridSpec -from joblib import Parallel, delayed +from joblib import Parallel, delayed, dump, load +from concurrent.futures import ThreadPoolExecutor import seaborn as sns import pandas as pd from scipy.stats import gaussian_kde @@ -29,7 +30,7 @@ plt.rcParams["font.family"] = "Times New Roman" plt.rcParams["font.size"] = 20 -filepath = "./npy/temporal/" +folderpath = "./npy/temporal/" figpath = os.getcwd() + ("/../../../../OneDrive - NTNU/MASCOT_PhD/" "Projects/GOOGLE/Docs/fig/Sim_2DNidelva/Simulator/temporal") @@ -55,7 +56,7 @@ def __init__(self) -> None: self.string_myopic = "SimulatorMyopic2D" self.string_rrt = "SimulatorRRTStar" - replicates = os.listdir(filepath) + replicates = os.listdir(folderpath) self.num_replicates = 0 for rep in replicates: if rep.startswith("R_"): @@ -76,89 +77,192 @@ def __init__(self) -> None: self.lon_ticks = np.round(np.arange(self.lon_min, self.lon_max, 0.02), 2) self.lat_ticks = np.round(np.arange(self.lat_min, self.lat_max, 0.005), 2) - # self.plot_trajectory() - self.dataset = self.load_data() + self.load_data() + self.plot_ground_truth() + + self.trajectory + + def load_data(self) -> None: + t0 = time() + fpath = os.getcwd() + "/../simulation_result/temporal/" + self.trajectory = load(fpath + "trajectory.joblib") + self.ibv = load(fpath + "ibv.joblib") + self.rmse = load(fpath + "rmse.joblib") + self.vr = load(fpath + "vr.joblib") + self.mu = load(fpath + "mu.joblib") + self.cov = load(fpath + "cov.joblib") + self.sigma = load(fpath + "sigma.joblib") + self.truth = load(fpath + "truth.joblib") + print("Loading data takes {:.2f} seconds.".format(time() - t0)) + + def plot_ground_truth(self) -> None: + """ + Plot the ground truth for one replicate to check. + """ + num_replicate = 0 + fpath = figpath + "/R_{:03d}/".format(num_replicate) + checkfolder(fpath) + for i in range(1, self.num_steps): + plt.figure(figsize=(20, 5)) + gs = GridSpec(nrows=1, ncols=3) + ax = plt.subplot(gs[0, 0]) + plt.scatter(self.grid_wgs[:, 1], self.grid_wgs[:, 0], c=self.truth["myopic"]["eibv"][num_replicate, i, :], + cmap=get_cmap("BrBG", 10), vmin=10, vmax=30, alpha=.5) + plt.colorbar() + traj = self.trajectory["myopic"]["eibv"][num_replicate, :i, :] + lat, lon = WGS.xy2latlon(traj[:, 0], traj[:, 1]) + plt.plot(lon, lat, 'k.-') + plt.plot(self.polygon_border_wgs[:, 1], self.polygon_border_wgs[:, 0], 'r-.') + plg = plt.Polygon(np.fliplr(self.polygon_obstacle_wgs), facecolor='w', edgecolor='r', fill=True, + linestyle='-.') + plt.gca().add_patch(plg) + plt.xlabel("Longitude") + plt.xticks(self.lon_ticks) + plt.xlim([self.lon_min, self.lon_max]) + plt.ylim([self.lat_min, self.lat_max]) + plt.title("Myopic EIBV") + + ax = plt.subplot(gs[0, 1]) + plt.scatter(self.grid_wgs[:, 1], self.grid_wgs[:, 0], c=self.truth["rrt"]["eibv"][num_replicate, i, :], + cmap=get_cmap("BrBG", 10), vmin=10, vmax=30, alpha=.5) + plt.colorbar() + traj = self.trajectory["rrt"]["eibv"][num_replicate, :i, :] + lat, lon = WGS.xy2latlon(traj[:, 0], traj[:, 1]) + plt.plot(lon, lat, 'k.-') + plt.plot(self.polygon_border_wgs[:, 1], self.polygon_border_wgs[:, 0], 'r-.') + plg = plt.Polygon(np.fliplr(self.polygon_obstacle_wgs), facecolor='w', edgecolor='r', fill=True, + linestyle='-.') + plt.gca().add_patch(plg) + plt.xlabel("Longitude") + plt.xticks(self.lon_ticks) + plt.xlim([self.lon_min, self.lon_max]) + plt.ylim([self.lat_min, self.lat_max]) + plt.title("RRT EIBV") + + ax = plt.subplot(gs[0, 2]) + plt.scatter(self.grid_wgs[:, 1], self.grid_wgs[:, 0], c=self.truth["myopic"]["eibv"][num_replicate, i, :] - + self.truth["rrt"]["eibv"][num_replicate, i, :], cmap=get_cmap("RdBu", 10), vmin=-1, vmax=1) + plt.colorbar() + plt.plot(self.polygon_border_wgs[:, 1], self.polygon_border_wgs[:, 0], 'r-.') + plt.xlabel("Longitude") + plt.xticks(self.lon_ticks) + plt.xlim([self.lon_min, self.lon_max]) + plt.ylim([self.lat_min, self.lat_max]) + plt.title("Difference") + + plt.tight_layout() + plt.savefig(fpath + "P_{:03d}.png".format(i)) + plt.close("all") + + def load_raw_data_from_replicate_files(self) -> 'dict': + """ + Load raw data from the replicate files. Needed after running the simulation study. + """ + def load_single_file_data(file) -> 'dict': + """ + Load data from a single file. + """ + single_file_dataset = {} + single_file_dataset[file] = {} + for item in self.cv: + filepath = folderpath + file + "/" + item + "/" + single_file_dataset[file][item] = {} + for planner in self.planners: + if planner == "myopic": + data = np.load(filepath + "myopic.npz") + else: + data = np.load(filepath + "rrtstar.npz") + single_file_dataset[file][item][planner] = {} + for metric in self.metrics: + single_file_dataset[file][item][planner][metric] = data[metric] + return single_file_dataset + + self.cv = ['eibv', 'ivr', 'equal'] + self.planners = ['myopic', 'rrt'] + self.metrics = ['traj', 'ibv', 'rmse', 'vr', 'mu', 'cov', 'sigma', 'truth'] - def load_data(self) -> 'dict': t0 = time() - folderpath = os.getcwd() + "/npy/temporal/" files = os.listdir(folderpath) - cv = ["eibv", "ivr", "equal"] - planners = ['myopic', 'rrt'] - metrics = ['traj', 'ibv', 'rmse', 'vr', 'mu', 'cov', 'sigma', 'truth'] dataset = {} # s0, initialize the dataset dictionary. for i in range(self.num_replicates): num_replicate = "R_{:03d}".format(i) dataset[num_replicate] = {} - for item in cv: + for item in self.cv: dataset[num_replicate][item] = {} - for planner in planners: + for planner in self.planners: dataset[num_replicate][item][planner] = {} - for metric in metrics: + for metric in self.metrics: + # print("num_replicate: ", num_replicate, " | item: ", item, " | planner: ", planner, " | metric: ", metric) dataset[num_replicate][item][planner][metric] = None - # s1, load actual data. - for file in files: - if file.startswith("R_"): - for item in cv: - filepath = folderpath + file + "/" + item.upper() + "/" - print("Loading data from: ", filepath) - for planner in planners: - if planner == "myopic": - data = np.load(filepath + "myopic.npz") - else: - data = np.load(filepath + "rrtstar.npz") - - for metric in metrics: - dataset[file][item][planner][metric] = data[metric] - - # s2, make it array. - # for item in cv: - # for planner in planners: - # for metric in metrics: - # dataset[item][planner][metric] = np.array(dataset[item][planner][metric]) + with ThreadPoolExecutor(max_workers=16) as executor: + futures = [executor.submit(load_single_file_data, file) for file in files if file.startswith("R_")] + for future in tqdm(futures): + data = future.result() + file = list(data.keys())[0] + dataset[file] = data[file] print("Loading data takes {:.2f} seconds.".format(time() - t0)) return dataset - def load_sim_data(self, string: str = "/sigma_10/nugget_025/SimulatorRRTStar") -> tuple: + def organize_data_to_dict(self) -> None: """ - Reorganize the data from the simulation study. + Organize the data to a dictionary. Only once, no need for future use. """ - traj = np.empty([self.num_replicates, 3, self.num_steps, 2]) - mu = np.empty([self.num_replicates, 3, self.num_steps, self.Ngrid]) - sigma = np.empty([self.num_replicates, 3, self.num_steps, self.Ngrid]) - truth = np.empty([self.num_replicates, 3, self.num_steps, self.Ngrid]) + self.trajectory = {} + self.ibv = {} + self.rmse = {} + self.vr = {} + self.mu = {} + self.cov = {} + self.sigma = {} + self.truth = {} + t0 = time() + for planner in self.planners: + self.trajectory[planner] = {} + self.ibv[planner] = {} + self.rmse[planner] = {} + self.vr[planner] = {} + self.mu[planner] = {} + self.cov[planner] = {} + self.sigma[planner] = {} + self.truth[planner] = {} + for item in self.cv: + self.trajectory[planner][item] = np.empty([self.num_replicates, self.num_steps, 2]) + self.ibv[planner][item] = np.empty([self.num_replicates, self.num_steps]) + self.rmse[planner][item] = np.empty([self.num_replicates, self.num_steps]) + self.vr[planner][item] = np.empty([self.num_replicates, self.num_steps]) + self.mu[planner][item] = np.empty([self.num_replicates, self.num_steps, self.Ngrid]) + self.cov[planner][item] = np.empty([self.num_replicates, self.num_steps // 15, self.Ngrid, self.Ngrid]) + self.sigma[planner][item] = np.empty([self.num_replicates, self.num_steps, self.Ngrid]) + self.truth[planner][item] = np.empty([self.num_replicates, self.num_steps, self.Ngrid]) for i in range(self.num_replicates): - rep = "R_{:03d}".format(i) - datapath = filepath + rep + string - data_eibv_dominant = np.load(datapath + "eibv.npz") - data_ivr_dominant = np.load(datapath + "ivr.npz") - data_equal = np.load(datapath + "eq.npz") - - # s0, extract trajectory - traj[i, 0, :, :] = data_eibv_dominant["traj"] - traj[i, 1, :, :] = data_ivr_dominant["traj"] - traj[i, 2, :, :] = data_equal["traj"] - - # s1, extract mu - mu[i, 0, :, :] = data_eibv_dominant["mu_data"] - mu[i, 1, :, :] = data_ivr_dominant["mu_data"] - mu[i, 2, :, :] = data_equal["mu_data"] - - # s2, extract sigma - sigma[i, 0, :, :] = data_eibv_dominant["sigma_data"] - sigma[i, 1, :, :] = data_ivr_dominant["sigma_data"] - sigma[i, 2, :, :] = data_equal["sigma_data"] - - # s3, extract truth - truth[i, 0, :, :] = data_eibv_dominant["mu_truth_data"] - truth[i, 1, :, :] = data_ivr_dominant["mu_truth_data"] - truth[i, 2, :, :] = data_equal["mu_truth_data"] - return traj, mu, sigma, truth + num_replicate = "R_{:03d}".format(i) + for planner in self.planners: + for item in self.cv: + self.trajectory[planner][item][i, :, :] = self.dataset[num_replicate][item][planner]['traj'] + self.ibv[planner][item][i, :] = self.dataset[num_replicate][item][planner]['ibv'] + self.rmse[planner][item][i, :] = self.dataset[num_replicate][item][planner]['rmse'] + self.vr[planner][item][i, :] = self.dataset[num_replicate][item][planner]['vr'] + self.mu[planner][item][i, :, :] = self.dataset[num_replicate][item][planner]['mu'] + self.cov[planner][item][i, :, :, :] = self.dataset[num_replicate][item][planner]['cov'] + self.sigma[planner][item][i, :, :] = self.dataset[num_replicate][item][planner]['sigma'] + self.truth[planner][item][i, :, :] = self.dataset[num_replicate][item][planner]['truth'] + + fpath = os.getcwd() + "/../simulation_result/temporal/" + dump(self.trajectory, fpath + "trajectory.joblib") + dump(self.ibv, fpath + "ibv.joblib") + dump(self.rmse, fpath + "rmse.joblib") + dump(self.vr, fpath + "vr.joblib") + dump(self.mu, fpath + "mu.joblib") + dump(self.cov, fpath + "cov.joblib") + dump(self.sigma, fpath + "sigma.joblib") + dump(self.truth, fpath + "truth.joblib") + t1 = time() + print("Reorganizing data takes {:.2f} seconds.".format(t1 - t0)) def plot_trajectory_temporal(self) -> None: @@ -194,7 +298,7 @@ def make_subplot(traj, value, num_step, cmap=get_cmap("BrBG", 10), for i in range(len(planners)): for j in range(len(cv)): - + print("planner: ", planners[i], " cv: ", cv[j], " field: ", field) traj = self.dataset["R_{:03d}".format(num_replicate)][cv[j]][planners[i]]["traj"] data_field = self.dataset["R_{:03d}".format(num_replicate)][cv[j]][planners[i]][field] @@ -215,6 +319,43 @@ def make_subplot(traj, value, num_step, cmap=get_cmap("BrBG", 10), self.dataset print("he") + def load_sim_data(self, string: str = "/sigma_10/nugget_025/SimulatorRRTStar") -> tuple: + """ + Reorganize the data from the simulation study. + """ + traj = np.empty([self.num_replicates, 3, self.num_steps, 2]) + mu = np.empty([self.num_replicates, 3, self.num_steps, self.Ngrid]) + sigma = np.empty([self.num_replicates, 3, self.num_steps, self.Ngrid]) + truth = np.empty([self.num_replicates, 3, self.num_steps, self.Ngrid]) + + for i in range(self.num_replicates): + rep = "R_{:03d}".format(i) + datapath = folderpath + rep + string + data_eibv_dominant = np.load(datapath + "eibv.npz") + data_ivr_dominant = np.load(datapath + "ivr.npz") + data_equal = np.load(datapath + "eq.npz") + + # s0, extract trajectory + traj[i, 0, :, :] = data_eibv_dominant["traj"] + traj[i, 1, :, :] = data_ivr_dominant["traj"] + traj[i, 2, :, :] = data_equal["traj"] + + # s1, extract mu + mu[i, 0, :, :] = data_eibv_dominant["mu_data"] + mu[i, 1, :, :] = data_ivr_dominant["mu_data"] + mu[i, 2, :, :] = data_equal["mu_data"] + + # s2, extract sigma + sigma[i, 0, :, :] = data_eibv_dominant["sigma_data"] + sigma[i, 1, :, :] = data_ivr_dominant["sigma_data"] + sigma[i, 2, :, :] = data_equal["sigma_data"] + + # s3, extract truth + truth[i, 0, :, :] = data_eibv_dominant["mu_truth_data"] + truth[i, 1, :, :] = data_ivr_dominant["mu_truth_data"] + truth[i, 2, :, :] = data_equal["mu_truth_data"] + return traj, mu, sigma, truth + def plot_trajectory_static_truth(self) -> None: def make_subplot(traj, num_step, j: int = 0): for k in range(traj.shape[0]): @@ -276,7 +417,7 @@ def load_metricdata4simulator(self, string) -> tuple: for i in range(self.num_replicates): rep = "R_{:03d}".format(i) - datapath = filepath + rep + string + datapath = folderpath + rep + string r_traj = np.load(datapath + "traj.npy").reshape(1, 3, self.num_steps, 2) r_ibv = np.load(datapath + "ibv.npy").reshape(1, 3, self.num_steps) r_vr = np.load(datapath + "vr.npy").reshape(1, 3, self.num_steps) diff --git a/Publication/src/Simulators/Simulator.py b/Publication/src/Simulators/Simulator.py index d9f84af14..8fe3be70c 100755 --- a/Publication/src/Simulators/Simulator.py +++ b/Publication/src/Simulators/Simulator.py @@ -34,7 +34,7 @@ def __init__(self, weight_eibv: float = 1., weight_ivr: float = 1., self.__agent_rrtstar = AgentRRTStar(weight_eibv=weight_eibv, weight_ivr=weight_ivr, random_seed=self.__random_seed, debug=self.__debug, name=self.__name) - self.__datapath = os.getcwd() + "/npy/temporal/R_{:03d}/".format(replicate_id) + self.__name + "/" + self.__datapath = os.getcwd() + "/npy/temporal/Synced/R_{:03d}/".format(replicate_id) + self.__name + "/" checkfolder(self.__datapath) def run_myopic(self) -> None: diff --git a/Publication/src/tests/test_agent_myopic.py b/Publication/src/tests/test_agent_myopic.py index fc8c676cc..0c737b850 100644 --- a/Publication/src/tests/test_agent_myopic.py +++ b/Publication/src/tests/test_agent_myopic.py @@ -15,14 +15,14 @@ def setUp(self) -> None: seed = 0 debug = True self.agent1 = Agent(weight_eibv=2., weight_ivr=.0, random_seed=seed, debug=debug, name="EIBV") - self.agent2 = Agent(weight_eibv=.0, weight_ivr=2., random_seed=seed, debug=debug, name="IVR") - self.agent3 = Agent(weight_eibv=1., weight_ivr=1., random_seed=seed, debug=debug, name="Equal") + # self.agent2 = Agent(weight_eibv=.0, weight_ivr=2., random_seed=seed, debug=debug, name="IVR") + # self.agent3 = Agent(weight_eibv=1., weight_ivr=1., random_seed=seed, debug=debug, name="Equal") def test_run(self) -> None: num_steps = 10 self.agent1.run(num_steps) - self.agent2.run(num_steps) - self.agent3.run(num_steps) + # self.agent2.run(num_steps) + # self.agent3.run(num_steps) import matplotlib.pyplot as plt from matplotlib.pyplot import get_cmap diff --git a/Publication/src/tests/test_myopic2d.py b/Publication/src/tests/test_myopic2d.py index 22892bdb4..449b86b12 100644 --- a/Publication/src/tests/test_myopic2d.py +++ b/Publication/src/tests/test_myopic2d.py @@ -20,13 +20,13 @@ def setUp(self) -> None: self.c = Config() self.myopic = Myopic2D(weight_eibv=1., weight_ivr=1.) self.cv = self.myopic.getCostValley() - self.field = self.myopic.get_field() + self.field = Field() self.polygon_border = self.c.get_polygon_border() def test_get_next_waypoint(self) -> None: figpath = "/Users/yaolin/Downloads/fig/" - id_s, id_n = self.myopic.get_candidates_indices() + wp_s, wp_n = self.myopic.get_candidates_waypoints() wp_curr = self.myopic.get_current_waypoint() wp_prev = self.myopic.get_previous_waypoint() wp_next = self.myopic.get_next_waypoint() @@ -38,11 +38,9 @@ def test_get_next_waypoint(self) -> None: cmap=get_cmap("BrBG", 10), vmin=.0, vmax=2., s=250) plt.colorbar() - for iid in id_n: - wp = self.field.get_location_from_ind(iid) + for wp in wp_n: plt.plot(wp[1], wp[0], 'b.', label="Neighbour locations", markersize=35) - for iid in id_s: - wp = self.field.get_location_from_ind(iid) + for wp in wp_s: plt.plot(wp[1], wp[0], 'y.', label="Candidate locations", markersize=30) plt.plot(wp_next[1], wp_next[0], 'r.', label="Next waypoint", markersize=25) @@ -56,10 +54,10 @@ def test_get_next_waypoint(self) -> None: plt.close("all") # plt.show() - for i in range(30): + for i in range(120): print(i) ctd = np.array([[i * 600 + 1623450000, wp_curr[0], wp_curr[1], 25 + np.random.rand()]]) - id_s, id_n = self.myopic.get_candidates_indices() + wp_s, wp_n = self.myopic.get_candidates_waypoints() wp_next = self.myopic.update_next_waypoint(ctd) wp_curr = self.myopic.get_current_waypoint() wp_prev = self.myopic.get_previous_waypoint() @@ -70,11 +68,9 @@ def test_get_next_waypoint(self) -> None: cmap=get_cmap("BrBG", 10), vmin=.0, vmax=2., s=250) plt.colorbar() - for iid in id_n: - wp = self.field.get_location_from_ind(iid) + for wp in wp_n: plt.plot(wp[1], wp[0], 'b.', label="Neighbour locations", markersize=35) - for iid in id_s: - wp = self.field.get_location_from_ind(iid) + for wp in wp_s: plt.plot(wp[1], wp[0], 'y.', label="Candidate locations", markersize=30) plt.plot(wp_next[1], wp_next[0], 'r.', label="Next waypoint", markersize=25) diff --git a/Publication/src/tests/test_simulator.py b/Publication/src/tests/test_simulator.py index 048ad1897..48c718666 100644 --- a/Publication/src/tests/test_simulator.py +++ b/Publication/src/tests/test_simulator.py @@ -14,12 +14,13 @@ class TestSimulator(TestCase): def setUp(self) -> None: seed = 0 - debug = False + debug = True weight_eibv = 2. weight_ivr = 0. self.simulator = Simulator(weight_eibv=weight_eibv, weight_ivr=weight_ivr, random_seed=seed, replicate_id=0, debug=debug) def test_run(self) -> None: - self.simulator.run() + self.simulator.run_myopic() + self.simulator.run_rrt() pass