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