From 50c823c66cda2f55bfa716fa2b960dca81fa8233 Mon Sep 17 00:00:00 2001 From: Alex Davies Date: Mon, 3 Jun 2024 15:27:24 -0300 Subject: [PATCH] Working multi-drone simulation --- README.md | 16 ++++ .../simulated-drone/docker-compose.yaml | 16 ++-- skel/Desktop/simulated-drone/sim_drone.py | 96 +++++++++++++++---- 3 files changed, 98 insertions(+), 30 deletions(-) diff --git a/README.md b/README.md index 4ae9b2f..2740dd6 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,22 @@ distrobox enter spiri-sdk-desktop-main cd /opt/spiri-sdk/PX4-Autopilot/ make px4_sitl gazebo-classic #Start the simulator ``` + +# Usage + +Each drone has it's own ros master, you can specify a ros master by adding a sys_id to your port + +To launch the drone, run the command `sim_drone.py start-group 2`. For more detailed information user +the `--help` command. You must run the command from inside a folder with a compliant dockerfile. + +If using the VM there should be one on your desktop, if not you can run `cp -r /opt/spiri-sdk/user-home-skeleton/ ~/` +and all relevent development resources will be added to your home folder. + +```bash +ROS_MASTER_URI=http://localhost:11311 rostopic list #Drone with sys_id 1 +ROS_MASTER_URI=http://localhost:11312 rostopic list #Drone with sys_id 2 +``` + # Building ```bash diff --git a/skel/Desktop/simulated-drone/docker-compose.yaml b/skel/Desktop/simulated-drone/docker-compose.yaml index ff4303a..b13e3e1 100644 --- a/skel/Desktop/simulated-drone/docker-compose.yaml +++ b/skel/Desktop/simulated-drone/docker-compose.yaml @@ -3,12 +3,10 @@ version: "3.8" services: mavros: - #This service bridges our mavlink-based robot-coprosessor into ROS - #In this example it connects to a simulated coprocessor. image: git.spirirobotics.com/spiri/services-ros1-mavros:master - command: roslaunch mavros px4.launch fcu_url:="udp://:14555@mavproxy:14550" + command: roslaunch mavros px4.launch fcu_url:="udp://:14555@host.docker.internal:${MAVLINK_SIM_PORT:-14550}" environment: - - "ROS_MASTER_URI=http://ros-master:11311" + - "ROS_MASTER_URI=http://ros-master:${ROS_MASTER_PORT:-11311}" depends_on: ros-master: condition: service_healthy @@ -16,26 +14,24 @@ services: deploy: resources: limits: - # cpus: '0.01' memory: 200M ulimits: nofile: soft: 1024 hard: 524288 + ros-master: image: git.spirirobotics.com/spiri/services-ros1-core:main - command: stdbuf -o L roscore + command: stdbuf -o L roscore --port ${ROS_MASTER_PORT:-11311} environment: - - "ROS_MASTER_URI=http://ros-master:11311" + - "ROS_MASTER_URI=http://ros-master:${ROS_MASTER_PORT:-11311}" restart: always ports: - - "0.0.0.0:11311:11311" + - "127.0.0.1:${ROS_MASTER_PORT:-11311}:${ROS_MASTER_PORT:-11311}" deploy: resources: limits: memory: 1G - # Madness, setting a low ulimit here fixes memory leaks - # https://answers.ros.org/question/336963/rosout-high-memory-usage/ ulimits: nofile: soft: 1024 diff --git a/skel/Desktop/simulated-drone/sim_drone.py b/skel/Desktop/simulated-drone/sim_drone.py index 9d8b235..5fa40d8 100755 --- a/skel/Desktop/simulated-drone/sim_drone.py +++ b/skel/Desktop/simulated-drone/sim_drone.py @@ -1,5 +1,6 @@ #!/bin/env python3 import typer, os, sys, contextlib +import time from loguru import logger import sh import atexit @@ -17,17 +18,21 @@ px4 = sh.Command(px4Path) app = typer.Typer() +# This is a list of processes that we need to .kill and .wait for on exit +processes = [] + class outputLogger: """ Logs command output to loguru """ - def __init__(self, name): + def __init__(self, name, instance): self.name = name + self.instance = instance def __call__(self, message): - with logger.contextualize(cmd=self.name): + with logger.contextualize(cmd=self.name, instance=self.instance): if message.endswith("\n"): message = message[:-1] # ToDo, this doesn't work because the output is coloured @@ -44,7 +49,6 @@ class outputLogger: logger.info(message) - @contextlib.contextmanager def modified_environ(*remove, **update): """ @@ -76,30 +80,82 @@ def modified_environ(*remove, **update): [env.pop(k) for k in remove_after] +def wait_for_gazebo(timeout=60, interval=1): + start_time = time.time() + while time.time() - start_time < timeout: + try: + # Run the 'gz topic list' command + topics = sh.gz("topic", "-l").strip() + if topics: + logger.info("Gazebo Ignition is running.") + return True + except sh.ErrorReturnCode: + # If the command fails, Gazebo might not be running yet + pass + logger.info( + f"Gazebo Ignition is not running. Retrying in {interval} seconds..." + ) + time.sleep(interval) + + logger.error("Timeout reached. Gazebo Ignition is not running.") + return False + + @app.command() def start(sys_id: int = 1): """Starts the simulated drone with a given sys_id, each drone must have it's own unique ID. """ - with modified_environ( + with logger.contextualize(syd_id=sys_id): + with modified_environ( PX4_SIM_MODEL=os.environ.get("PX4_SIM_MODEL") or "gz_x500", - PX4_GZ_MODEL_POSE=os.environ.get("PX4_GZ_MODEL_POSE") or f"0,{sys_id-1}" + PX4_GZ_MODEL_POSE=os.environ.get("PX4_GZ_MODEL_POSE") or f"0,{(sys_id-1)}", + PX4_INSTANCE="intance_id", + ROS_MASTER_PORT=str(11310 + sys_id), + MAVLINK_SIM_PORT=str(14549 + sys_id), ): - instance_id = sys_id - 1 # PX4 will add 1 to the instance ID to get sysID - px4Instance = px4(i=instance_id, _out=outputLogger("px4"), _bg=True) - atexit.register(lambda: px4Instance.kill()) - logger.info("Starting drone stack, this may take some time") - # docker_stack = sh.docker.compose( - # "--project-name", f"simulated_robot_{sys_id}", - # "up", - # _out=outputLogger("docker_stack"), - # _err=sys.stderr, - # _bg=True - # ) - # atexit.register(lambda: docker_stack.kill()) - px4Instance.wait() - # docker_stack.wait() + instance_id = sys_id - 1 # PX4 will add 1 to the instance ID to get sysID + px4Instance = px4( + "-d", i=instance_id, _out=outputLogger("px4", instance_id), _bg=True + ) + processes.append(px4Instance) + logger.info("Starting drone stack, this may take some time") + docker_stack = sh.docker.compose( + "--project-name", + f"simulated_robot_{sys_id}", + "up", + _out=outputLogger("docker_stack", sys_id), + _err=sys.stderr, + _bg=True, + ) + processes.append(docker_stack) +@app.command() +def start_group(count: int): + """Start a group of robots""" + for i in range(count): + logger.info(f"start robot {i}") + start(sys_id=i + 1) + if i == 0: + wait_for_gazebo() + + +def cleanup(): + # Wait for all subprocesses to exit + logger.info("Waiting for commands to exit") + print(processes) + for waitable in processes: + waitable.kill() + waitable.wait() + + +atexit.register(cleanup) + if __name__ == "__main__": - app() + try: + app() + except KeyboardInterrupt: + logger.info("KeyboardInterrupt caught, exiting...") + cleanup() + sys.exit(0)