-
Notifications
You must be signed in to change notification settings - Fork 1
/
reval.py
executable file
·101 lines (84 loc) · 5.34 KB
/
reval.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
import os
import shutil
import signal
import subprocess
import time
from turtle import bgcolor
from tqdm import tqdm
from utils.utils import bcolors, Loader, description, KeyboardInterrupt
from utils.arg_parse import command
if not os.path.exists('src/benchmark/log'):
os.makedirs('src/benchmark/log')
color = 'white'
ASCII = '.#'
def reval():
global loading
loading = False
battery_collision = subprocess.check_call("./husky_battery_bumper.sh '%s'", cwd="src/benchmark/service", shell=True)
set_config = subprocess.check_call("python set_config.py '%s'", cwd="src/benchmark/", shell=True)
for i in tqdm(range(5), desc="Set config method=random", colour=color, ascii=ASCII, bar_format='{l_bar}{bar:20}{r_bar}{bar:-20b}', leave=False):
time.sleep(1)
rosbag = subprocess.check_call("gnome-terminal -- ./ros_record.sh '%s'", cwd="src/benchmark/service", shell=True)
calculate_distance = subprocess.check_call("gnome-terminal -- python calculate_distance_traveled.py '%s'", cwd="src/benchmark", shell=True)
for i in tqdm(range(5), desc="Data logger", colour=color, ascii=ASCII, bar_format='{l_bar}{bar:20}{r_bar}{bar:-20b}', leave=False):
time.sleep(1)
print("")
reval.loader = Loader("Mission in progress...", bcolors.CGREEN + "" + bcolors.ENDC, 0.05).start()
loading = True
nav2d_goal = subprocess.check_call("python mission.py '%s'", cwd="src/benchmark/", shell=True)
reval.loader.stop()
loading = False
print(bcolors.CGREEN + "Mission Done!" + bcolors.ENDC)
for i in tqdm(range(10), desc="Finishing simulation", colour=color, ascii=ASCII, bar_format='{l_bar}{bar:20}{r_bar}{bar:-20b}', leave=False):
time.sleep(1)
kill_rosnode = subprocess.check_call("./kill_rosnode.sh &>/dev/null", cwd="src/benchmark/service", shell=True)
for i in tqdm(range(5), desc="Generating logs", colour=color, ascii=ASCII, bar_format='{l_bar}{bar:20}{r_bar}{bar:-20b}', leave=False):
time.sleep(1)
reval.loader = Loader("Evaluating logs...", bcolors.HEADER + "" + bcolors.ENDC, 0.05).start()
loading = True
eval = subprocess.check_call("./eval.sh '%s'", cwd="src/benchmark/service", shell=True)
reval.loader.stop()
loading = False
positinal_metrics = subprocess.check_call("python positional_error.py '%s'", cwd="src/benchmark/", shell=True)
evaluation_results = subprocess.check_call("python evaluation_results.py '%s'", cwd="src/benchmark/", shell=True)
# path = "src/benchmark/log"
# if os.path.isdir(path):
# shutil.rmtree(path)
if __name__== '__main__':
try:
signal.signal(signal.SIGINT, KeyboardInterrupt.signal_handler)
cursor_off = subprocess.check_call("tput civis", shell=True)
for i in tqdm(range(command.args.e), colour="green", desc="Epoch", bar_format='{l_bar}{bar:20}{r_bar}{bar:-20b}'):
if command.args.v == 'On' or command.args.v == 'on':
husky_gazebo = subprocess.check_call("./husky_gazebo.sh '%s'", cwd="src/benchmark/service", shell=True)
for i in tqdm(range(8), desc="Launching husky_gazebo", colour=color, ascii=ASCII, bar_format='{l_bar}{bar:20}{r_bar}{bar:-20b}', leave=False):
time.sleep(1)
husky_mb = subprocess.check_call("./husky_movebase.sh '%s'", cwd="src/benchmark/service", shell=True)
for i in tqdm(range(3), desc="Launching husky_MobeBase", colour=color, ascii=ASCII, bar_format='{l_bar}{bar:20}{r_bar}{bar:-20b}', leave=False):
time.sleep(1)
husky_rviz = subprocess.check_call("./husky_rviz.sh '%s'", cwd="src/benchmark/service", shell=True)
for i in tqdm(range(10), desc="Launching husky_rviz", colour=color, ascii=ASCII, bar_format='{l_bar}{bar:20}{r_bar}{bar:-20b}', leave=False):
time.sleep(1)
reval()
if command.args.v == 'Off' or command.args.v == 'off':
husky_gazebo = subprocess.check_call("./husky_gazebo_nogui.sh '%s'", cwd="src/benchmark/service", shell=True)
for i in tqdm(range(8), desc="Launching husky_gazebo", colour=color, ascii=ASCII, bar_format='{l_bar}{bar:20}{r_bar}{bar:-20b}', leave=False):
time.sleep(1)
husky_mb = subprocess.check_call("./husky_movebase.sh '%s'", cwd="src/benchmark/service", shell=True)
for i in tqdm(range(3), desc="Launching husky_MobeBase", colour=color, ascii=ASCII, bar_format='{l_bar}{bar:20}{r_bar}{bar:-20b}', leave=False):
time.sleep(1)
reval()
time.sleep(5)
cursor_on = subprocess.check_call("tput cvvis", shell=True)
except:
if loading == True:
reval.loader.stop()
print(bcolors.WARNING + "Killing rosnodes and roslaunch" + bcolors.ENDC)
kill_rosnode = subprocess.check_call("./kill_rosnode.sh '%s'", cwd="src/benchmark/service", shell=True)
for i in tqdm(range(5), desc="Closing everything", colour=color, ascii=ASCII, bar_format='{l_bar}{bar:20}{r_bar}{bar:-20b}'):
time.sleep(1)
print(bcolors.FAIL + "Mission Failed!" + bcolors.ENDC)
# path = "src/benchmark/log"
# if os.path.isdir(path):
# shutil.rmtree(path)
cursor_on = subprocess.check_call("tput cvvis", shell=True)