diff --git a/sim_drone.py b/sim_drone.py index 289b90f..c9600fe 100644 --- a/sim_drone.py +++ b/sim_drone.py @@ -85,7 +85,7 @@ def modified_environ(*remove, **update): [env.pop(k) for k in remove_after] -#@app.command() +# @app.command() def start(tracker_instance: int = 0, sys_id: int = 1): """Starts the simulated drone with a given sys_id, each drone must have it's own unique ID. @@ -96,14 +96,14 @@ def start(tracker_instance: int = 0, sys_id: int = 1): with logger.contextualize(syd_id=sys_id): env = os.environ with modified_environ( - SERIAL0_PORT =str( int(env['SERIAL0_PORT']) + 10 * tracker_instance), - MAVROS2_PORT=str( int(env['MAVROS2_PORT']) + 10 * tracker_instance), - MAVROS1_PORT=str( int(env['MAVROS1_PORT']) + 10 * tracker_instance), - FDM_PORT_IN=str( int(env['FDM_PORT_IN']) + 10 * tracker_instance), - SITL_PORT=str( int(env['SITL_PORT']) + 10 * tracker_instance), + SERIAL0_PORT=str(int(env["SERIAL0_PORT"]) + 10 * tracker_instance), + MAVROS2_PORT=str(int(env["MAVROS2_PORT"]) + 10 * tracker_instance), + MAVROS1_PORT=str(int(env["MAVROS1_PORT"]) + 10 * tracker_instance), + FDM_PORT_IN=str(int(env["FDM_PORT_IN"]) + 10 * tracker_instance), + SITL_PORT=str(int(env["SITL_PORT"]) + 10 * tracker_instance), TRACKER_INSTANCE=str(tracker_instance), DRONE_SYS_ID=str(sys_id), - ): + ): logger.info("Starting drone stack, this may take some time") docker_stack = sh.docker.compose( "--profile", @@ -119,15 +119,16 @@ def start(tracker_instance: int = 0, sys_id: int = 1): @app.command() def start_group(): env = os.environ - sim_drone_count = int(env['SIM_DRONE_COUNT']) + sim_drone_count = int(env["SIM_DRONE_COUNT"]) start_ros_master() """Start a group of robots""" for i in range(sim_drone_count): logger.info(f"start robot {i}") - start(tracker_instance=i,sys_id=i + 1) + start(tracker_instance=i, sys_id=i + 1) # if i == 0: # wait_for_gazebo() + def start_ros_master(): docker_stack = sh.docker.compose( "--profile", @@ -139,6 +140,7 @@ def start_ros_master(): ) processes.append(docker_stack) + def cleanup(): # Wait for all subprocesses to exit logger.info("Waiting for commands to exit") @@ -148,7 +150,9 @@ def cleanup(): for waitable in processes: waitable.kill() waitable.wait() - except Exception as e: print(e) + except Exception as e: + print(e) + atexit.register(cleanup)