From e75ac4ae579d20b494552ce34b550ec5d4e7f81e Mon Sep 17 00:00:00 2001 From: Arash Negahdar Date: Wed, 29 Jan 2020 10:55:17 -0500 Subject: [PATCH] Tools: Allow multiple ArduPilot instances to be spawned --- Tools/autotest/sim_vehicle.py | 201 +++++++++++++++++++++++----------- 1 file changed, 139 insertions(+), 62 deletions(-) diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index d152486546..dd73e59955 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -404,25 +404,31 @@ def get_user_locations_path(): return user_locations_path -def find_new_spawn(loc, file_path): - (lat, lon, alt, heading) = loc.split(",") +def find_offsets(instances, file_path): + offsets = {} swarminit_filepath = os.path.join(autotest_dir, "swarminit.txt") - for path2 in [file_path, swarminit_filepath]: - if os.path.isfile(path2): - with open(path2, 'r') as swd: - next(swd) - for lines in swd: - if len(lines) == 0: + comment_regex = re.compile("\s*#.*") + for path in [file_path, swarminit_filepath]: + if os.path.isfile(path): + with open(path, 'r') as fd: + for line in fd: + line = re.sub(comment_regex, "", line) + line = line.rstrip("\n") + if len(line) == 0: + continue + (instance, offset) = line.split("=") + instance = (int)(instance) + if (instance not in offsets) and (instance in instances): + offsets[instance] = [ (float)(x) for x in offset.split(",") ] continue - (instance, offset) = lines.split("=") - if ((int)(instance) == (int)(cmd_opts.instance)): - (x, y, z, head) = offset.split(",") - g = mavextra.gps_offset((float)(lat), (float)(lon), (float)(x), (float)(y)) - loc = str(g[0])+","+str(g[1])+","+str(alt+z)+","+str(head) - return loc - g = mavextra.gps_newpos((float)(lat), (float)(lon), 90, 20*(int)(cmd_opts.instance)) - loc = str(g[0])+","+str(g[1])+","+str(alt)+","+str(heading) - return loc + if len(offsets) == len(instances): + return offsets + if len(offsets) == len(instances): + return offsets + for instance in instances: + if instance not in offsets: + offsets[instance] = [90.0, 20.0 * instance, 0.0, None] + return offsets def find_location_by_name(locname): @@ -442,14 +448,24 @@ def find_location_by_name(locname): continue (name, loc) = line.split("=") if name == locname: - if cmd_opts.swarm is not None: - loc = find_new_spawn(loc, cmd_opts.swarm) - return loc + return [ (float)(x) for x in loc.split(",") ] print("Failed to find location (%s)" % cmd_opts.location) sys.exit(1) +def find_spawns(loc, offsets): + lat, lon, alt, heading = loc + spawns = {} + for k in offsets: + (x, y, z, head) = offsets[k] + if head is None: + head = heading + g = mavextra.gps_offset(lat, lon, x, y) + spawns[k] = ",".join([str(g[0]), str(g[1]), str(alt+z), str(head)]) + return spawns + + def progress_cmd(what, cmd): """Print cmd in a way a user could cut-and-paste to get the same effect""" progress(what) @@ -476,7 +492,7 @@ def run_cmd_blocking(what, cmd, quiet=False, check=False, **kw): return ret -def run_in_terminal_window(name, cmd): +def run_in_terminal_window(name, cmd, **kw): """Execute the run_in_terminal_window.sh command for cmd""" global windowID @@ -486,7 +502,7 @@ def run_in_terminal_window(name, cmd): if under_macos() and os.environ.get('DISPLAY'): # on MacOS record the window IDs so we can close them later - out = subprocess.Popen(runme, stdout=subprocess.PIPE).communicate()[0] + out = subprocess.Popen(runme, stdout=subprocess.PIPE, **kw).communicate()[0] out = out.decode('utf-8') p = re.compile('tab 1 of window id (.*)') @@ -505,7 +521,7 @@ def run_in_terminal_window(name, cmd): else: progress("Cannot find %s process terminal" % name) else: - subprocess.Popen(runme) + subprocess.Popen(runme, **kw) tracker_uarta = None # blemish @@ -536,7 +552,7 @@ def start_antenna_tracker(opts): os.chdir(oldpwd) -def start_vehicle(binary, opts, stuff, loc=None): +def start_vehicle(binary, opts, stuff, spawns=None): """Run the ArduPilot binary""" cmd_name = opts.vehicle @@ -586,9 +602,6 @@ def start_vehicle(binary, opts, stuff, loc=None): cmd.append(binary) cmd.append("-S") - cmd.append("-I" + str(opts.instance)) - if loc is not None: - cmd.extend(["--home", loc]) if opts.wipe_eeprom: cmd.append("-w") cmd.extend(["--model", stuff["model"]]) @@ -622,7 +635,14 @@ def start_vehicle(binary, opts, stuff, loc=None): if opts.mcast: cmd.extend(["--uartA mcast:"]) - run_in_terminal_window(cmd_name, cmd) + old_dir = os.getcwd() + for i, i_dir in zip(instances, instance_dir): + c = ["-I" + str(i)] + if spawns is not None: + c.extend(["--home", spawns[i]]) + os.chdir(i_dir) + run_in_terminal_window(cmd_name, cmd + c) + os.chdir(old_dir) def start_mavproxy(opts, stuff): @@ -639,26 +659,6 @@ def start_mavproxy(opts, stuff): else: cmd.append("mavproxy.py") - if opts.hil: - cmd.extend(["--load-module", "HIL"]) - else: - if opts.mcast: - cmd.extend(["--master", "mcast:"]) - else: - cmd.extend(["--master", mavlink_port]) - if stuff["sitl-port"] and not opts.no_rcin: - cmd.extend(["--sitl", simout_port]) - - if not opts.no_extra_ports: - ports = [p + 10 * cmd_opts.instance for p in [14550, 14551]] - for port in ports: - if os.path.isfile("/ardupilot.vagrant"): - # We're running inside of a vagrant guest; forward our - # mavlink out to the containing host OS - cmd.extend(["--out", "10.0.2.2:" + str(port)]) - else: - cmd.extend(["--out", "127.0.0.1:" + str(port)]) - if opts.tracker: cmd.extend(["--load-module", "tracker"]) global tracker_uarta @@ -739,8 +739,36 @@ def start_mavproxy(opts, stuff): if old is not None: env['PYTHONPATH'] += os.path.pathsep + old - run_cmd_blocking("Run MavProxy", cmd, env=env) - progress("MAVProxy exited") + old_dir = os.getcwd() + for i, i_dir in zip(instances, instance_dir): + c = [] + + if not opts.no_extra_ports: + ports = [p + 10 * i for p in [14550, 14551]] + for port in ports: + if os.path.isfile("/ardupilot.vagrant"): + # We're running inside of a vagrant guest; forward our + # mavlink out to the containing host OS + c.extend(["--out", "10.0.2.2:" + str(port)]) + else: + c.extend(["--out", "127.0.0.1:" + str(port)]) + + if opts.hil: + c.extend(["--load-module", "HIL"]) + else: + if opts.mcast: + c.extend(["--master", "mcast:"]) + else: + c.extend(["--master", "tcp:127.0.0.1:" + str(5760 + 10 * i)]) + if stuff["sitl-port"] and not opts.no_rcin: + c.extend(["--sitl", "127.0.0.1:" + str(5501 + 10 * i)]) + + os.chdir(i_dir) + if i == instances[-1]: + run_cmd_blocking("Run MavProxy", cmd + c, env=env) + else: + run_in_terminal_window("Run MavProxy", cmd + c, env=env) + os.chdir(old_dir) vehicle_options_string = '|'.join(vinfo.options.keys()) @@ -838,6 +866,14 @@ group_sim.add_option("-I", "--instance", default=0, type='int', help="instance of simulator") +group_sim.add_option("-n", "--count", + type='int', + default=1, + help="vehicle count; if this is specified, -I is used as a base-value") +group_sim.add_option("-i", "--instances", + default=None, + type='string', + help="a space delimited list of instances to spawn; if specified, overrides -I and -n.") group_sim.add_option("-V", "--valgrind", action='store_true', default=False, @@ -1042,6 +1078,14 @@ if (cmd_opts.gdb or cmd_opts.gdb_stopped) and (cmd_opts.lldb or cmd_opts.lldb_st print("May not use lldb with gdb") sys.exit(1) +if cmd_opts.instance < 0: + print("May not specify a negative instance ID") + sys.exit(1) + +if cmd_opts.count < 1: + print("May not specify a count less than 1") + sys.exit(1) + if cmd_opts.strace and cmd_opts.valgrind: print("valgrind and strace almost certainly not a good idea") @@ -1088,10 +1132,6 @@ You could also try changing directory to e.g. the ArduCopter subdirectory if cmd_opts.frame is None: cmd_opts.frame = vinfo.options[cmd_opts.vehicle]["default_frame"] -# setup ports for this instance -mavlink_port = "tcp:127.0.0.1:" + str(5760 + 10 * cmd_opts.instance) -simout_port = "127.0.0.1:" + str(5501 + 10 * cmd_opts.instance) - frame_infos = vinfo.options_for_frame(cmd_opts.frame, cmd_opts.vehicle, cmd_opts) @@ -1101,6 +1141,18 @@ if not os.path.exists(vehicle_dir): print("vehicle directory (%s) does not exist" % (vehicle_dir,)) sys.exit(1) +if cmd_opts.instances is not None: + instances = set() + for i in cmd_opts.instances.split(' '): + i = (int)(i) + if i < 0: + print("May not specify a negative instance ID") + sys.exit(1) + instances.add(i) + instances = sorted(instances) # to list +else: + instances = range(cmd_opts.instance, cmd_opts.instance + cmd_opts.count) + if not cmd_opts.hil: if cmd_opts.instance == 0: kill_tasks() @@ -1109,7 +1161,7 @@ if cmd_opts.tracker: start_antenna_tracker(cmd_opts) if cmd_opts.custom_location: - location = cmd_opts.custom_location + location = [ (float)(x) for x in cmd_opts.custom_location.split(",") ] progress("Starting up at %s" % (location,)) elif cmd_opts.location is not None: location = find_location_by_name(cmd_opts.location) @@ -1117,15 +1169,38 @@ elif cmd_opts.location is not None: else: progress("Starting up at SITL location") location = None +if cmd_opts.swarm is not None: + offsets = find_offsets(instances, cmd_opts.swarm) +else: + offsets = { x: [0.0, 0.0, 0.0, None] for x in instances } +if location is not None: + spawns = find_spawns(location, offsets) +else: + spawns = None if cmd_opts.use_dir is not None: - new_dir = cmd_opts.use_dir + base_dir = os.path.realpath(cmd_opts.use_dir) try: - os.makedirs(os.path.realpath(new_dir)) + os.makedirs(base_dir) except OSError as exception: if exception.errno != errno.EEXIST: raise - os.chdir(new_dir) + os.chdir(base_dir) +else: + base_dir = os.getcwd() +instance_dir = [] +if len(instances) == 1: + instance_dir.append(base_dir) +else: + for i in instances: + i_dir = os.path.join(base_dir, str(i)) + try: + os.makedirs(i_dir) + except OSError as exception: + if exception.errno != errno.EEXIST: + raise + finally: + instance_dir.append(i_dir) if cmd_opts.hil: # (unlikely) @@ -1134,10 +1209,12 @@ if cmd_opts.hil: "jsb_sim/runsim.py"), "--speedup=" + str(cmd_opts.speedup) ] - if location is not None: - jsbsim_opts.extend(["--home", location]) + for i in instances: + c = [] + if spawns is not None: + c = ["--home", spawns[i]] + run_in_terminal_window("JSBSim", jsbsim_opts + c) - run_in_terminal_window("JSBSim", jsbsim_opts) else: if not cmd_opts.no_rebuild: # i.e. we should rebuild do_build(vehicle_dir, cmd_opts, frame_infos) @@ -1160,7 +1237,7 @@ else: start_vehicle(vehicle_binary, cmd_opts, frame_infos, - loc=location) + spawns=spawns) if cmd_opts.delay_start: progress("Sleeping for %f seconds" % (cmd_opts.delay_start,))