Working multi-drone simulation

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

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

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)