From c80d37373b4e8b3f49b1fc31d544939d9d4f7d17 Mon Sep 17 00:00:00 2001 From: Alex Davies Date: Thu, 14 Nov 2024 11:46:00 -0400 Subject: [PATCH] Better support for multiple docker images, more clear ros topic and robot names --- guiTools/spiri_sdk_guitools/launcher.py | 17 ++- guiTools/spiri_sdk_guitools/sim_drone.py | 32 +++-- robots/spiri-mu/core/docker-compose.yaml | 2 +- sim_drone.py | 167 ----------------------- 4 files changed, 35 insertions(+), 183 deletions(-) delete mode 100644 sim_drone.py diff --git a/guiTools/spiri_sdk_guitools/launcher.py b/guiTools/spiri_sdk_guitools/launcher.py index 6b38d79..918f2cb 100644 --- a/guiTools/spiri_sdk_guitools/launcher.py +++ b/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 diff --git a/guiTools/spiri_sdk_guitools/sim_drone.py b/guiTools/spiri_sdk_guitools/sim_drone.py index 23584dd..1827421 100644 --- a/guiTools/spiri_sdk_guitools/sim_drone.py +++ b/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", diff --git a/robots/spiri-mu/core/docker-compose.yaml b/robots/spiri-mu/core/docker-compose.yaml index ca0348d..2ff1dbb 100644 --- a/robots/spiri-mu/core/docker-compose.yaml +++ b/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 diff --git a/sim_drone.py b/sim_drone.py deleted file mode 100644 index 45f8945..0000000 --- a/sim_drone.py +++ /dev/null @@ -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="{time} {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)