-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathchange_map.py
107 lines (77 loc) · 2.98 KB
/
change_map.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
#!/usr/bin/env python
# file to change the map of the simulation
import argparse
import os.path
from shutil import copyfile
import yaml
import xml.etree.ElementTree as ET
parser = argparse.ArgumentParser(description='Change the map of the simulation in f1tenth_gym')
parser.add_argument('map', help='Image of the map. The image has to be in the same directory as the corresponding YAML file.')
parser.add_argument('--dir', '-d', dest='dir_target', default='src/f1tenth_gym_ros/', const='src/f1tenth_gym_ros/', nargs='?', help='path to the target directory f1tenth_gym_ros')
args = parser.parse_args()
#print args
# check target directory
if not os.path.exists(args.dir_target):
print 'ERROR: directory {} not found'.format(args.dir_target)
exit(1)
dir_target1 = args.dir_target + "maps/"
dir_target2 = args.dir_target + "f1tenth_gym/maps/"
# check target directories
if not os.path.exists(dir_target1):
print 'ERROR: directory {} not found'.format(dir_target1)
exit(1)
if not os.path.exists(dir_target2):
print 'ERROR: directory {} not found'.format(dir_target2)
print 'Did you execute "build_docker.sh"?'
exit(1)
dirname = os.path.dirname(args.map)
dirname += '/'
#print 'dirname = {}'.format(dirname)
image_file = os.path.basename(args.map)
basename, extension = os.path.splitext(image_file)
#print 'basename = {}'.format(basename)
#print 'extension = {}'.format(extension)
yaml_file = basename + '.yaml'
#print 'image = {}'.format(image_file)
#print 'yaml = {}'.format(yaml_file)
global_image_file = dirname + image_file
global_yaml_file = dirname + yaml_file
# check existence of the image and YAML file
if not os.path.isfile(global_image_file):
print 'File {} not found'.format(global_image_file)
exit(1)
if not os.path.isfile(global_yaml_file):
print 'File {} not found'.format(global_yaml_file)
exit(1)
########################################
# copy files in target directories
print "Copy files...",
copyfile(global_image_file, dir_target1 + image_file)
copyfile(global_yaml_file, dir_target1 + yaml_file)
copyfile(global_image_file, dir_target2 + image_file)
copyfile(global_yaml_file, dir_target2 + yaml_file)
print " [DONE]"
########################################
# modify params.yaml
print "Modify params.yaml...",
params_file = args.dir_target + 'params.yaml'
params_stream = file(params_file,'r')
data = yaml.safe_load(params_stream)
params_stream.close()
data['map_path'] = '/f1tenth_gym/maps/' + yaml_file
data['map_img_ext'] = extension
params_stream = file(params_file,'w')
yaml.dump(data, params_stream)
print " [DONE]"
########################################
# modify gym_bridge.launch
print "Modify gym_bridge.launch...",
bridge_file = args.dir_target + 'launch/gym_bridge.launch'
#print "bridge_file = {}".format(bridge_file)
tree = ET.parse(bridge_file)
root = tree.getroot()
for arg in root.iter('arg'):
if 'map' == arg.get('name'):
arg.set('default','$(find f1tenth_gym_ros)/maps/' + yaml_file)
tree.write(bridge_file)
print " [DONE]"