convMatrix = convMatrix.astype(float) DEM = DEM * (1 + (50 - XX) / 50 * 0.2) DEM = signal.convolve2d(DEM, convMatrix/convMatrix.sum(), \ mode='same', boundary='symm') goal = np.asarray([10, 50]) #start = np.asarray([5,5]) start = np.asarray([45, 35]) # DEM resolution demRes = 1.0 # Planning resolution planRes = 1.0 # We create a new environment (offset is zero,zero) and 4 copies env1 = camis.AnisotropicMap(DEM, demRes, planRes, (0, 0)) env2 = copy.deepcopy(env1) env3 = copy.deepcopy(env1) env4 = copy.deepcopy(env1) env5 = copy.deepcopy(env1) env6 = copy.deepcopy(env1) env1.show3dDEM() fig, axes = plt.subplots(constrained_layout=True) env1.showMap('elevation', fig, axes) axes.set_xlabel('X-axis (m)') axes.set_ylabel('Y-axis (m)') # ============================================================================= ## DIFFERENT CUSTOM CAMIS CREATION ##
# ============================================================================= # A # B # C # ============================================================================= ## LOADING DEM # ============================================================================= hiRes_elevationMap = np.loadtxt(\ open("data/terrainData/UMATerrainCuesta_10cmDEM.csv",\ "rb"), delimiter=" ", skiprows=0) hiRes = 0.1 offset = np.loadtxt(open("data/terrainData/UMATerrainCuesta_10cmOffset.csv",\ "rb"), delimiter=" ", skiprows=0) env = camis.AnisotropicMap(hiRes_elevationMap, hiRes, 0.4,\ offset) posA = np.asarray([10, 36]) #Very good posB = np.asarray([30, 60]) posC = np.asarray([30, 30]) print('TEST_DEMO: DEM is loaded') # ============================================================================= ## LOADING DIFFERENT CAMIS # ============================================================================= with open("data/sim01/cuadriga_aniso_01.yml", 'r') as file: cuadriga_data = yaml.full_load(file) aniso_01 = camis.CamisDrivingModel(cuadriga_data) aniso_01.showDirCosts() with open("data/sim01/cuadriga_iso_01.yml", 'r') as file: cuadriga_data = yaml.full_load(file) iso_01 = camis.CamisDrivingModel(cuadriga_data)
from scipy.optimize import curve_fit # ============================================================================= ## LOADING DEM # ============================================================================= hiRes_elevationMap = np.loadtxt(\ open("data/terrainData/UMATerrainCuesta_10cmDEM.csv",\ "rb"), delimiter=" ", skiprows=0) hiRes = 0.1 offset = np.loadtxt(open("data/terrainData/UMATerrainCuesta_10cmOffset.csv",\ "rb"), delimiter=" ", skiprows=0) offset = offset + [350 * 0.1, 160 * 0.1] env = camis.AnisotropicMap(hiRes_elevationMap[160:960,350:600], hiRes, 0.4,\ offset) posA = np.asarray([5, 70]) posB = np.asarray([20, 55]) posC = np.asarray([5, 40]) posD = np.asarray([20, 25]) posE = np.asarray([5, 10]) print('TEST_DEMO: DEM is loaded') with open("data/sim01/cuadriga_aniso_01.yml", 'r') as file: cuadriga_data = yaml.full_load(file) aniso_01 = camis.CamisDrivingModel(cuadriga_data) with open("data/sim01/cuadriga_aniso_02.yml", 'r') as file: cuadriga_data = yaml.full_load(file) aniso_02 = camis.CamisDrivingModel(cuadriga_data)
# ============================================================================= hiRes_elevationMap = np.loadtxt(\ open("data/terrainData/UMATerrainCuesta_10cmDEM.csv",\ "rb"), delimiter=" ", skiprows=0) hiRes = 0.1 offset = np.loadtxt(open("data/terrainData/UMATerrainCuesta_10cmOffset.csv",\ "rb"), delimiter=" ", skiprows=0) hexRes = 1.0 / np.sqrt(6) localDEM = hiRes_elevationMap[260:510, 350:600] offset = offset + [350 * 0.1, 260 * 0.1] occupancy_radius = 0.5 tracking_error = 0.5 env = camis.AnisotropicMap(hiRes_elevationMap[260:510,350:600], hiRes, hexRes,\ offset, occupancy_radius, tracking_error) #offset = offset + [350*0.1, 650*0.1] #env = camis.AnisotropicMap(hiRes_elevationMap[650:1000,350:600], hiRes, hexRes,\ # offset) def computeAllPlannings(anisoMapList): anisoMapList[0].executeBiPlanning(posB, posA) anisoMapList[1].executeBiPlanning(posC, posB) anisoMapList[2].executeBiPlanning(posD, posC) anisoMapList[3].executeBiPlanning(posA, posD) anisoMapList[4].executeBiPlanning(posD, posA) anisoMapList[5].executeBiPlanning(posC, posD) anisoMapList[6].executeBiPlanning(posB, posC) anisoMapList[7].executeBiPlanning(posA, posB)
cbar = fig.colorbar(cc, ax=ax1,shrink=0.6, location = 'top') cbar.set_label('Elevation [deg]') ax1.set_xlabel('X [m]') ax1.set_ylabel('Y [m]') ax1.set_aspect('equal') hiRes = 1.0 hexRes = 0.5 offset = (0,0) occupancy_radius = 0.5 tracking_error = 0.5 #posA = np.asarray([20,45]) #posB = np.asarray([30,5]) posA = np.asarray([27,5]) posB = np.asarray([20,45]) env = camis.AnisotropicMap(z, hiRes, hexRes,\ offset, occupancy_radius, tracking_error) plt.style.use('default') plt.rcParams["font.family"] = "Constantia" plt.rcParams['mathtext.fontset'] = 'cm' plt.rcParams['mathtext.rm'] = 'serif' fig, ax1 = plt.subplots(figsize=(3.2, 3.9),nrows = 1, ncols = 1, constrained_layout=True) cc = ax1.scatter(env.hexXmap, env.hexYmap, c = 180/np.pi*env.hexSlopeMap, cmap="nipy_spectral",s=16.0, vmin = 0.0, vmax = 25.0, rasterized=True) cbar = fig.colorbar(cc, ax=ax1,shrink=0.6, location = 'top', \ ticks=[0,5,10,15,20,25]) #cc.set_clim(0,50.0) cbar.set_label('Steepness α [deg]') ax1.set_xlim([0,48.0]) ax1.set_ylim([0,48.0]) ax1.set_xlabel('X [m]') ax1.set_ylabel('Y [m]')