Better support for multiple docker images, more clear ros topic and

robot names

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"
+                )
+                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

@@ -73,7 +73,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,6 +87,8 @@ 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("-","_")
 
     async def ui_containers(self, element):
         docker_elements = {}
@@ -129,8 +131,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 topic[0].startswith(f"/{self.robot_name}/"):
+                            ui.label(topic[0])
                     await asyncio.sleep(10)
 
     async def ui(self, element):
@@ -159,7 +163,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 +175,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 +187,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,20 +210,23 @@ 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,
             ):
-                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:
                     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",
@@ -234,8 +240,8 @@ class Robot:
                         stderr=subprocess.PIPE,
                     )
 
-    @staticmethod
-    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 +258,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 +275,13 @@ class Robot:
         ros2_gz_create_proc.kill()
         return
 
-    @staticmethod
-    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",

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

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)