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 ##
# =============================================================================
# ROBOT 1
with open("data/sim02/simRobot1.yml", 'r') as file:
robot1 = yaml.full_load(file)
r1 = camis.CamisDrivingModel(robot1)
env1.computeVecCostMap(r1)
env7 = copy.deepcopy(env1)
r1.showCAMIS()
# ROBOT 2
with open("data/sim02/simRobot2.yml", 'r') as file:
robot2 = yaml.full_load(file)
r2 = camis.CamisDrivingModel(robot2)
env2.computeVecCostMap(r2)
env8 = copy.deepcopy(env2)
r2.showCAMIS()
# ROBOT 3
with open("data/sim02/simRobot3.yml", 'r') as file:
robot3 = yaml.full_load(file)
env1 = camis.AnisotropicMap(DEM, demRes, planRes, (0, 0)) env1.smoothMap(2.0) sdThreshold = 11.0 slopeThreshold = 25.0 env1.computeObstacles(sdThreshold, slopeThreshold) env2 = copy.deepcopy(env1) env3 = copy.deepcopy(env1) env4 = copy.deepcopy(env1) env1.show3dDEM() # ============================================================================= ## DIFFERENT CUSTOM CAMIS CREATION ## # ============================================================================= # ROBOT 1 r1 = camis.CamisDrivingModel(25.0, .6, .6, 9.8 * 100) env1.computeVecCostMap(r1) r1.showCAMIS() # ROBOT 2 r2 = camis.CamisDrivingModel(25.0, .7, .7, 9.8 * 100) env2.computeVecCostMap(r2) r2.showCAMIS() # ROBOT 3 r3 = camis.CamisDrivingModel(25.0, .8, .8, 9.8 * 100) env3.computeVecCostMap(r3) r3.showCAMIS() # ROBOT 4 r4 = camis.CamisDrivingModel(25.0, .6, .9, 9.8 * 100)
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
OTHER DEALINGS IN THE SOFTWARE.
Authors: J. Ricardo Sánchez Ibáñez, Carlos J. Pérez del Pulgar
Affiliation: Department of Systems Engineering and Automation
Space Robotics Lab (www.uma.es/space-robotics)
"""
# -*- coding: utf-8 -*-
import numpy as np
import matplotlib.pyplot as plt
from context import camis
import copy
import yaml
import matplotlib.gridspec as gridspec
try:
from scipy import signal
except:
raise ImportError('ERROR: scipy module could not be imported')
with open("spikeCAMIS.yml", 'r') as file:
robot1 = yaml.full_load(file)
r1 = camis.CamisDrivingModel(robot1)
r1.showBraking()
r1.showDirCosts()
r1.showAnisotropy()
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)
with open("data/sim01/cuadriga_aniso_02.yml", 'r') as file:
cuadriga_data = yaml.full_load(file)
aniso_02 = camis.CamisDrivingModel(cuadriga_data)
aniso_02.showDirCosts()
with open("data/sim01/cuadriga_iso_02.yml", 'r') as file:
cuadriga_data = yaml.full_load(file)
iso_02 = camis.CamisDrivingModel(cuadriga_data)
with open("data/sim01/cuadriga_aniso_03.yml", 'r') as file:
cuadriga_data = yaml.full_load(file)
"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)
env.computeVecCostMap(aniso_02)
env.show3dDEM()
def showPoints(ax):
ax.annotate(
'A',
xy=(posA[0], posA[1]),
xytext=(-4, 0), # 3 points vertical offset