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]')