Update README.md

Reviewed-on: #11

.copier/answers.docs.yml

@@ -0,0 +1,5 @@
+# Changes here will be overwritten by Copier; NEVER EDIT MANUALLY
+_commit: v1.0.2
+_src_path: https://git.spirirobotics.com/Spiri/template-docs.git
+author_name: Spiri Robotics
+project_name: spiri-sdk

.github/workflows/build-docs.yaml

@@ -0,0 +1,52 @@
+name: Build Docs
+
+on:
+  push:
+
+env:
+  REGISTRY: git.spirirobotics.com
+  IMAGE_NAME: ${{ github.repository }}
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    container: sphinxdoc/sphinx-latexpdf
+
+    steps:
+    - name: Install sphinx-rtd-theme
+      run: pip install sphinx-rtd-theme myst-parser
+    - name: Install node so that custom actions work.
+      run : apt-get update && apt-get --yes install nodejs git
+
+    - name: Checkout Repository
+      uses: actions/checkout@v4
+
+    - name: Build Docs
+      run: make html latexpdf
+      working-directory: docs  # assuming your documentation is in a 'docs' folder
+
+    - name: Save PDF Artifacts
+      run: mv docs/build/latex/*.pdf ${{ github.workspace }}/docs.pdf
+
+    - name: Compress HTML
+      run: tar -czvf docs_html.tar.gz -C docs/build/html .
+
+    - name: Upload Docs
+      uses: actions/upload-artifact@v3
+      with:
+        name: docs_html.tar.gz
+        path: docs_html.tar.gz
+
+    - name: Upload PDF
+      uses: actions/upload-artifact@v3
+      with:
+        name: docs.pdf
+        path: docs.pdf
+
+    - name: Deploy
+      uses: s0/git-publish-subdir-action@develop
+      env:
+        REPO: http://git-spirirobotics-com-app:3000/Spiri/spiri-sdk.git/
+        BRANCH: gh-pages
+        FOLDER: ./docs/build/html
+        GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}

.gitignore

@@ -1,3 +1,3 @@
 docs/build/
 .vscode
-*.pyc
+*.pyc

README.md

@@ -72,28 +72,6 @@ sudo nvidia-ctk runtime configure --runtime=docker --cdi.enabled
 sudo systemctl restart docker
 ```
 
-##### Rootless Mode
-
-To configure the container runtime for Docker running in [Rootless mode](https://docs.docker.com/engine/security/rootless/), follow these steps:
-
-1. Configure the container runtime by using the nvidia-ctk command:
-
-   ```bash
-   nvidia-ctk runtime configure --runtime=docker --config=$HOME/.config/docker/daemon.json
-   ```
-
-2. Restart the Rootless Docker daemon
-
-   ```bash
-   systemctl --user restart docker
-   ```
-
-3. Configure /etc/nvidia-container-runtime/config.toml by using the sudo nvidia-ctk command:
-
-   ```bash
-   sudo nvidia-ctk config --set nvidia-container-cli.no-cgroups --in-place
-   ```
-
 ### Sample Workload
 
 After you install and configure the toolkit and install an NVIDIA GPU Driver, you can verify your installation by running a sample workload.

docs/Makefile

@@ -0,0 +1,20 @@
+# Minimal makefile for Sphinx documentation
+#
+
+# You can set these variables from the command line, and also
+# from the environment for the first two.
+SPHINXOPTS    ?=
+SPHINXBUILD   ?= sphinx-build
+SOURCEDIR     = source
+BUILDDIR      = build
+
+# Put it first so that "make" without argument is like "make help".
+help:
+	@$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
+
+.PHONY: help Makefile
+
+# Catch-all target: route all unknown targets to Sphinx using the new
+# "make mode" option.  $(O) is meant as a shortcut for $(SPHINXOPTS).
+%: Makefile
+	@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)

docs/README.md

@@ -0,0 +1,11 @@
+If you have a correctly configured sphinx environment you can build this project
+using `make html latexpdf`.
+
+You can also use nektos/act to build this project in the same way our build does.
+```bash
+cd ../ #Make sure you're in the project root, you should have a hidden folder
+# named ./.github/workflows available.
+act --artifact-server-path ./doc-build
+```
+
+Your compiled doc project will now be in the ./doc-build folder.

docs/make.bat

@@ -0,0 +1,35 @@
+@ECHO OFF
+
+pushd %~dp0
+
+REM Command file for Sphinx documentation
+
+if "%SPHINXBUILD%" == "" (
+	set SPHINXBUILD=sphinx-build
+)
+set SOURCEDIR=source
+set BUILDDIR=build
+
+%SPHINXBUILD% >NUL 2>NUL
+if errorlevel 9009 (
+	echo.
+	echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
+	echo.installed, then set the SPHINXBUILD environment variable to point
+	echo.to the full path of the 'sphinx-build' executable. Alternatively you
+	echo.may add the Sphinx directory to PATH.
+	echo.
+	echo.If you don't have Sphinx installed, grab it from
+	echo.https://www.sphinx-doc.org/
+	exit /b 1
+)
+
+if "%1" == "" goto help
+
+%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
+goto end
+
+:help
+%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
+
+:end
+popd

docs/source/_static/custom.css

@@ -0,0 +1,34 @@
+.wy-side-nav-search {
+    background: #FFFFFF !important;
+}
+
+
+.wy-nav-side {
+    background-color: #FFFFFF !important;
+}
+
+
+/* Add borders and box-shadow */
+.wy-side-nav {
+    border: 1px solid #899CA3 !important;
+}
+
+.logo {
+    width: 100px !important;
+}
+
+.wy-menu-vertical a {
+    color: #899CA3 !important; /* Change to your desired color */
+}
+
+.document-title {
+    color: #000 !important; 
+    font-size: 24px !important;
+    text-transform: uppercase !important;
+}
+
+.icon-home {
+    font-weight: bold !important;
+    text-transform: uppercase !important;
+    color: #000 !important; 
+}

docs/source/conf.py

@@ -0,0 +1,70 @@
+# Configuration file for the Sphinx documentation builder.
+#
+# For the full list of built-in configuration values, see the documentation:
+# https://www.sphinx-doc.org/en/master/usage/configuration.html
+
+# -- Project information -----------------------------------------------------
+# https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information
+import sphinx_rtd_theme
+
+project = "spiri-sdk"
+copyright = "2024, Spiri Robotics"
+author = "Spiri Robotics"
+
+html_logo = "logos/SPIRI_STLockup_Mixed_RGB.png"  # For HTML output
+html_logo_width = '200px' 
+latex_logo = "logos/SPIRI_STLockup_Mixed_RGB.png"
+latex_logo_width = '5cm'
+
+
+# -- General configuration ---------------------------------------------------
+# https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration
+
+extensions = [
+    "myst_parser",
+    "sphinx.ext.duration",
+    "sphinx.ext.doctest",
+    "sphinx.ext.autodoc",
+    "sphinx.ext.autosummary",
+    "sphinx.ext.intersphinx",
+    "sphinx.ext.todo",
+]
+
+
+numfig = True
+
+todo_include_todos = True
+todo_emit_warnings = True
+todo_link_only = True
+
+
+templates_path = ["_templates"]
+exclude_patterns = []
+
+# -- Options for HTML output -------------------------------------------------
+# https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output
+
+html_theme = "sphinx_rtd_theme"
+html_theme_path = [sphinx_rtd_theme.get_html_theme_path()]
+
+html_static_path = ['_static']
+
+html_css_files = [
+    'custom.css',
+]
+
+
+html_theme_options = {
+    'collapse_navigation': True,
+    'sticky_navigation': True,
+    'navigation_depth': 4, #could be set to -1 if we want unlimited depth
+    'includehidden': True,
+    'titles_only': False
+}
+
+latex_engine = "xelatex"
+# Configure LaTeX options for PDF generation
+latex_show_urls = 'footnote'
+
+# Enable syntax highlighting for PDFs
+pygments_style = 'sphinx'

docs/source/index.rst

@@ -0,0 +1,21 @@
+.. spiri-sdk documentation master file, created by
+   sphinx-quickstart on Wed Feb 14 11:51:45 2024.
+   You can adapt this file completely to your liking, but it should at least
+   contain the root `toctree` directive.
+
+Welcome to spiri-sdk's documentation!
+============================================
+
+.. toctree::
+   :maxdepth: 2
+   :caption: Contents:
+
+.. include:: ../../README.md
+   :parser: myst_parser.sphinx_
+
+Indices and tables
+==================
+
+* :ref:`genindex`
+* :ref:`modindex`
+* :ref:`search`

guiTools/spiri_sdk_guitools/launcher.py

@@ -68,9 +68,22 @@ async def main():
                 newRobotParams = defaultdict(binding.BindableProperty)
                 ui.number(value=1, label="SysID", min=1, max=254,
                           ).bind_value(newRobotParams, 'sysid')
-                ui.textarea(value="/robots/spiri-mu/core/docker-compose.yaml", label="Compose files (comma or newline seperated)").bind_value(newRobotParams, 'compose_files')
+                default_robot_compose = (
+                        "/robots/spiri-mu/core/docker-compose.yaml\n"
+                        "#/robots/spiri-mu/virtual_camera/docker-compose.yaml --build"
+                )
+                ui.label("Compose files").classes("text-xl")
+                ui.codemirror(value=default_robot_compose, language="bash", theme="basicDark").bind_value(newRobotParams, 'compose_files')
                 async def new_robot():
-                    robot = Robot(**newRobotParams)
+                    compose_files = []
+                    #Split on comma or newline, and remove comments
+                    for line in newRobotParams['compose_files'].split('\n'):
+                        line = line.split('#')[0].strip()
+                        if line:
+                            compose_files.append(line)
+                    current_robot = newRobotParams.copy()
+                    current_robot['compose_files'] = compose_files
+                    robot = Robot(**current_robot)
                     asyncio.tasks.create_task(robot.ui(robots_widget))
 
                     newRobotParams['sysid'] += 1

guiTools/spiri_sdk_guitools/sim_drone.py

@@ -6,6 +6,7 @@ import os
 import sh
 import subprocess
 from nicegui import ui, run, app
+import yaml
 
 import docker
 import aiodocker
@@ -73,7 +74,7 @@ async def container_logs(container, element):
 
 
 class Robot:
-    robot_type = "spiri-mu"
+    robot_type = "spiri_mu"
 
     def __init__(self, sysid: int, compose_files: List[Path] | str):
         if sysid > 255 or sysid < 0:
@@ -87,40 +88,56 @@ class Robot:
         self.processes = []
         self.video_button = None
         robots.add(self)
+        #Ros doesn't like dashes in node names
+        self.robot_name = f"{self.robot_type}_{self.sysid}".replace("-","_")
+        self.world_name = "citadel_hill"
 
     async def ui_containers(self, element):
         docker_elements = {}
         container_status = {}
         with element:
             while True:
-                # Poll for data that changes
+                with logger.catch():
-                for container in self.containers():
+                    # Poll for data that changes
-                    try:
+                    for container in self.containers():
-                        health = container.attrs["State"]["Health"]["Status"]
+                        try:
-                    except KeyError:
+                            health = container.attrs["State"]["Health"]["Status"]
-                        health = "Unknown"
+                        except KeyError:
+                            health = "Unknown"
 
-                    container_status[container] = (
+                        container_status[container] = (
-                        f"{container.name} {container.status} {health}"
+                            f"{container.name} {container.status} {health}"
-                    )
+                        )
-                    if container not in docker_elements:
+                        if container not in docker_elements:
-                        docker_elements[container] = ui.element().classes("w-full")
+                            docker_elements[container] = ui.element().classes("w-full")
-                        with docker_elements[container]:
+                            with docker_elements[container]:
-                            ui.label().bind_text(container_status, container).classes(
+                                ui.label().bind_text(container_status, container).classes(
-                                "text-lg"
+                                    "text-2xl"
-                            )
+                                )
-                            logelement = (
+                                #Show the command the container is running
-                                ui.expansion("Logs")
+                                # ui.label(container.attrs["Config"]["Cmd"])
-                                .style("margin: 10px;")
+                                cmd_widget = ui.codemirror(" ".join(container.attrs["Config"]["Cmd"]), language="bash",theme="basicDark").classes('h-auto max-h-32')
-                                .classes("w-full outline outline-1")
+                                cmd_widget.enabled = False
-                            )
+
-                            asyncio.create_task(container_logs(container, logelement))
+                                with ui.expansion("Env Variables").classes("w-full outline outline-1").style("margin: 10px;"):
-                # Check for containers that have been removed
+                                    env_widget = ui.codemirror("\n".join(container.attrs["Config"]["Env"]), language="bash",theme="basicDark")
-                removed = set(docker_elements.keys()) - set(self.containers())
+                                    env_widget.enabled = False
-                for container in removed:
+
-                    self.robot_ui.remove(docker_elements[container])
+                                logelement = (
-                    docker_elements.pop(container)
+                                    ui.expansion("Logs")
-                await asyncio.sleep(1)
+                                    .style("margin: 10px;")
+                                    .classes("w-full outline outline-1")
+                                )
+                                asyncio.create_task(container_logs(container, logelement))
+                                with ui.expansion("Full details").classes("w-full outline outline-1").style("margin: 10px;"):
+                                    details_widget = ui.codemirror(yaml.dump(container.attrs), language="yaml",theme="basicDark")
+                                    details_widget.enabled = False
+                    # Check for containers that have been removed
+                    removed = set(docker_elements.keys()) - set(self.containers())
+                    for container in removed:
+                        self.robot_ui.remove(docker_elements[container])
+                        docker_elements.pop(container)
+                    await asyncio.sleep(1)
 
     async def ui_ros(self, element):
         with element:
@@ -129,8 +146,10 @@ class Robot:
             with scroll_area:
                 while True:
                     scroll_area.clear()
+                    #Filter for topics that start with self.robot_name
                     for topic in node_dummy.get_topic_names_and_types():
-                        ui.label(topic)
+                        if self.robot_name in topic[0]:
+                            ui.label(topic[0])
                     await asyncio.sleep(10)
 
     async def ui(self, element):
@@ -143,7 +162,7 @@ class Robot:
                 ui.label(f"""Sysid: {self.sysid}""")
                 ui.button("Start", on_click=self.async_start).classes("m-2")
                 ui.button("Stop", on_click=self.async_stop).classes("m-2")
-                self.video_button = EnableStreamingButton(sysid=self.sysid).classes(
+                self.video_button = EnableStreamingButton(robot_name=self.robot_name).classes(
                     "m-2"
                 )
 
@@ -159,7 +178,6 @@ class Robot:
                 with ui.tab_panels(tabs, value=tab_containers):
                     tab = ui.tab_panel(tab_containers).classes("w-full")
                     asyncio.create_task(self.ui_containers(tab))
-                with ui.tab_panels(tabs, value=tab_ros):
                     tab = ui.tab_panel(tab_ros).classes("w-full")
                     asyncio.create_task(self.ui_ros(tab))
 
@@ -172,7 +190,7 @@ class Robot:
         if isinstance(self.video_button, EnableStreamingButton):
             self.video_button.stop_video()
         # Delete gazebo model
-        self.delete_gz_model(sysid=self.sysid)
+        self.delete_gz_model()
         # Signal all processes to stop
         for process in self.processes:
             process.terminate()
@@ -184,7 +202,7 @@ class Robot:
 
     def containers(self):
         return docker_client.containers.list(
-            all=True, filters={"name": f"robot-sim-{self.robot_type}-{self.sysid}"}
+            all=True, filters={"name": f"robot-sim-{self.robot_name}"}
         )
 
     async def async_start(self):
@@ -207,35 +225,47 @@ class Robot:
                 SITL_PORT=str(int(env["SITL_PORT"]) + 10 * instance),
                 INSTANCE=str(instance),
                 DRONE_SYS_ID=str(self.sysid),
+                ROBOT_NAME=self.robot_name,
+                WORLD_NAME="citadel_hill",
             ):
-                self.spawn_gz_model(self.sysid)
+                self.spawn_gz_model()
                 logger.info("Starting drone stack, this may take some time")
                 for compose_file in self.compose_files:
+                    arguments = compose_file.split(" ")
+                    arguments = [arg.strip() for arg in arguments]
+                    compose_file = arguments[0]
+                    arguments = arguments[1:]
+
                     if not isinstance(compose_file, Path):
                         compose_file = Path(compose_file)
                     if not compose_file.exists():
                         raise FileNotFoundError(f"File {compose_file} does not exist")
+                    #Get the folder the compose file is in
+                    compose_folder = compose_file.parent
                     args = [
                         "docker-compose",
                         "--profile",
                         "uav-sim",
                         "-p",
-                        f"robot-sim-{self.robot_type}-{sysid}",
+                        f"robot-sim-{self.robot_name}-{compose_folder.name}",
                         "-f",
                         compose_file.as_posix(),
                         "up",
+                        *arguments,
                     ]
                     command = " ".join(args)
 
                     logger.info(f"Starting drone stack with command: {command}")
                     docker_stack = subprocess.Popen(
                         args,
-                        stdout=subprocess.PIPE,
+                        # stdout=subprocess.PIPE,
-                        stderr=subprocess.PIPE,
+                        # stderr=subprocess.PIPE,
                     )
+                    logger.info(f"Started drone stack with PID: {docker_stack.pid}")
 
-    @staticmethod
+    @logger.catch
-    def spawn_gz_model(sysid):
+    def spawn_gz_model(self):
+        sysid = self.sysid
         logger.info("")
         env = os.environ
         GSTREAMER_UDP_PORT = env["GSTREAMER_UDP_PORT"]
@@ -252,7 +282,7 @@ class Robot:
         ]
         # This path is breaking if this drone_model folder doesnt exist!
         # TODO: fix this model path for minimal code maintenance
-        ROS2_CMD = f"ros2 run ros_gz_sim create -world {WORLD_NAME} -file /ardupilot_gazebo/models/{DRONE_MODEL}/model.sdf -name spiri-{sysid} -x {sysid - 1}  -y 0 -z 0.195"
+        ROS2_CMD = f"ros2 run ros_gz_sim create -world {WORLD_NAME} -file /ardupilot_gazebo/models/{DRONE_MODEL}/model.sdf -name {self.robot_name} -x {sysid - 1}  -y 0 -z 0.195"
 
         xacro_proc = subprocess.Popen(
             XACRO_CMD,
@@ -269,13 +299,14 @@ class Robot:
         ros2_gz_create_proc.kill()
         return
 
-    @staticmethod
+    @logger.catch
-    def delete_gz_model(sysid):
+    def delete_gz_model(self):
+        sysid = self.sysid
         env = os.environ
         WORLD_NAME = env["WORLD_NAME"]
         # http://osrf-distributions.s3.amazonaws.com/gazebo/api/7.1.0/classgazebo_1_1physics_1_1Entity.html
         ENTITY_TYPE_MODEL = 0x00000002
-        REQUEST_ARG = f"name: 'spiri-{sysid}' type: {ENTITY_TYPE_MODEL}"
+        REQUEST_ARG = f"name: '{self.robot_name}' type: {ENTITY_TYPE_MODEL}"
         GZ_SERVICE_CMD = [
             "gz",
             "service",

guiTools/spiri_sdk_guitools/video_button.py

@@ -4,13 +4,13 @@ from loguru import logger
 import os
 
 GZ_TOPIC_INFO = ["gz", "topic", "-i", "-t"]
-ENABLE_STREAMING_TOPIC = "/world/{world_name}/model/spiri-{sysid}/link/pitch_link/sensor/camera/image/enable_streaming"
+ENABLE_STREAMING_TOPIC = "/world/{world_name}/model/{robot_name}/link/pitch_link/sensor/camera/image/enable_streaming"
 
 
 class EnableStreamingButton(ui.element):
-    def __init__(self, sysid, state: bool = False) -> None:
+    def __init__(self, robot_name, state: bool = False) -> None:
         super().__init__()
-        self.sysid = sysid
+        self.robot_name = robot_name
         self._state = state
         self.button = None
         with self.classes():
@@ -24,7 +24,7 @@ class EnableStreamingButton(ui.element):
     async def on_click(self) -> None:
         spinner = ui.spinner(size="lg")
         # So we don't block UI
-        result = await run.cpu_bound(self.enable_streaming, self.sysid, not self._state)
+        result = await run.cpu_bound(self.enable_streaming, self.robot_name, not self._state)
         if result:
             ui.notify("Success", type="positive]")
             self.set_state(state=not self._state)
@@ -43,17 +43,17 @@ class EnableStreamingButton(ui.element):
 
     def stop_video(self):
         if self._state != False:
-            self.enable_streaming(sysid=self.sysid, is_streaming=False)
+            self.enable_streaming(robot_name=self.robot_name, is_streaming=False)
             self.set_state(state=False)
 
     @staticmethod
-    def enable_streaming(sysid: int, is_streaming: bool) -> bool:
+    def enable_streaming(robot_name: str, is_streaming: bool) -> bool:
 
         world_name = os.environ["WORLD_NAME"]
         # Check if this topic has any subscribers i.e. model is up
         gz_topic_list_proc = subprocess.Popen(
             GZ_TOPIC_INFO
-            + [ENABLE_STREAMING_TOPIC.format(world_name=world_name, sysid=sysid)],
+            + [ENABLE_STREAMING_TOPIC.format(world_name=world_name, robot_name=robot_name)],
             stdout=subprocess.PIPE,
             stderr=subprocess.PIPE,
             text=True,
@@ -72,7 +72,7 @@ class EnableStreamingButton(ui.element):
             "gz",
             "topic",
             "-t",
-            ENABLE_STREAMING_TOPIC.format(world_name=world_name, sysid=sysid),
+            ENABLE_STREAMING_TOPIC.format(world_name=world_name, robot_name=robot_name),
             "-m",
             "gz.msgs.Boolean",
             "-p",

robots/spiri-mu/core/docker-compose.yaml

@@ -32,7 +32,7 @@ services:
     env_file:
       - .env
     image: git.spirirobotics.com/spiri/services-ros2-mavros:main
-    command: ros2 launch mavros apm.launch fcu_url:="udp://0.0.0.0:$MAVROS2_PORT@:14555" namespace:="spiri$DRONE_SYS_ID" tgt_system:="$DRONE_SYS_ID"
+    command: ros2 launch mavros apm.launch fcu_url:="udp://0.0.0.0:$MAVROS2_PORT@:14555" namespace:="$ROBOT_NAME" tgt_system:="$DRONE_SYS_ID"
     ipc: host
     network_mode: host
     restart: always

robots/spiri-mu/virtual_camera/Dockerfile

@@ -0,0 +1,8 @@
+FROM git.spirirobotics.com/spiri/services-ros2-mavros:main
+
+RUN apt-get update
+RUN apt-get --yes install ros-${ROS_DISTRO}-ros-gz-bridge \
+    ros-${ROS_DISTRO}-ros-gz-image \
+    ros-${ROS_DISTRO}-compressed-image-transport \
+    ros-${ROS_DISTRO}-rmw-cyclonedds-cpp
+

robots/spiri-mu/virtual_camera/docker-compose.yaml

@@ -0,0 +1,13 @@
+
+services:
+  front-gimbal:
+    ipc: host
+    network_mode: host
+    # image: git.spirirobotics.com/spiri/services-ros2-mavros:main
+    #Build the iamge, give it a name, don't try to pull the image
+    build: ./
+    image: spirisdk-virtual_camera
+    pull_policy: never
+    environment:
+      - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+    command: ros2 run ros_gz_image image_bridge /world/${WORLD_NAME}/model/${ROBOT_NAME}/link/pitch_link/sensor/camera/image

sim_drone.py

@@ -1,167 +0,0 @@
-#!/bin/env python3
-import typer
-import os
-import sys
-import contextlib
-from dotenv import load_dotenv
-from typing import List
-from loguru import logger
-import sh
-import atexit
-
-load_dotenv()
-logger.remove()
-logger.add(
-    sys.stdout, format="<green>{time}</green> <level>{level}</level> {extra} {message}"
-)
-
-# px4Path = os.environ.get("SPIRI_SIM_PX4_PATH", "/opt/spiri-sdk/PX4-Autopilot/")
-# logger.info(f"SPIRI_SIM_PX4_PATH={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, instance):
-        self.name = name
-        self.instance = instance
-
-    def __call__(self, message):
-        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
-            if message.startswith("INFO"):
-                message = message.lstrip("INFO")
-                logger.info(message)
-            elif message.startswith("WARN"):
-                message = message.lstrip("WARN")
-                logger.warning(message)
-            elif message.startswith("ERROR"):
-                message = message.lstrip("ERROR")
-                logger.error(message)
-            elif message.startswith("DEBUG"):
-                message = message.lstrip("DEBUG")
-                logger.debug(message)
-            else:
-                logger.info(message)
-
-
-@contextlib.contextmanager
-def modified_environ(*remove, **update):
-    """
-    Temporarily updates the ``os.environ`` dictionary in-place.
-
-    The ``os.environ`` dictionary is updated in-place so that the modification
-    is sure to work in all situations.
-
-    :param remove: Environment variables to remove.
-    :param update: Dictionary of environment variables and values to add/update.
-    """
-    env = os.environ
-    update = update or {}
-    remove = remove or []
-
-    # List of environment variables being updated or removed.
-    stomped = (set(update.keys()) | set(remove)) & set(env.keys())
-    # Environment variables and values to restore on exit.
-    update_after = {k: env[k] for k in stomped}
-    # Environment variables and values to remove on exit.
-    remove_after = frozenset(k for k in update if k not in env)
-
-    try:
-        env.update(update)
-        [env.pop(k, None) for k in remove]
-        yield
-    finally:
-        env.update(update_after)
-        [env.pop(k) for k in remove_after]
-
-
-# @app.command()
-def start(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.
-    """
-    if sys_id < 1 or sys_id > 254:
-        logger.error("sys_id must be between 1 and 254")
-        raise typer.Exit(code=1)
-    with logger.contextualize(syd_id=sys_id):
-        env = os.environ
-        with modified_environ(
-            SERIAL0_PORT=str(int(env["SERIAL0_PORT"]) + 10 * instance),
-            MAVROS2_PORT=str(int(env["MAVROS2_PORT"]) + 10 * instance),
-            MAVROS1_PORT=str(int(env["MAVROS1_PORT"]) + 10 * instance),
-            FDM_PORT_IN=str(int(env["FDM_PORT_IN"]) + 10 * instance),
-            SITL_PORT=str(int(env["SITL_PORT"]) + 10 * instance),
-            INSTANCE=str(instance),
-            DRONE_SYS_ID=str(sys_id),
-        ):
-            logger.info("Starting drone stack, this may take some time")
-            docker_stack = sh.docker.compose(
-                "--profile",
-                "uav-sim",
-                "-p",
-                f"robot-sim-{sys_id}",
-                "up",
-                _out=outputLogger("docker_stack", sys_id),
-                _err=sys.stderr,
-                _bg=True,
-            )
-            processes.append(docker_stack)
-
-
-@app.command()
-def start_group():
-    env = os.environ
-    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(instance=i, sys_id=i + 1)
-        # if i == 0:
-        #     wait_for_gazebo()
-
-
-def start_ros_master():
-    docker_stack = sh.docker.compose(
-        "--profile",
-        "ros-master",
-        "up",
-        _out=outputLogger("docker_stack", "ros-master"),
-        _err=sys.stderr,
-        _bg=True,
-    )
-    processes.append(docker_stack)
-
-
-def cleanup():
-    # Wait for all subprocesses to exit
-    logger.info("Waiting for commands to exit")
-    try:
-        if processes:
-            print(processes)
-            for waitable in processes:
-                waitable.kill()
-                waitable.wait()
-    except Exception as e:
-        print(e)
-
-
-atexit.register(cleanup)
-
-if __name__ == "__main__":
-    try:
-        app()
-    except KeyboardInterrupt:
-        logger.info("KeyboardInterrupt caught, exiting...")
-        cleanup()
-        sys.exit(0)