From a752ed603eb81ce378c97c29022c9fa4be753c6a Mon Sep 17 00:00:00 2001 From: Burak Ozter Date: Thu, 7 Nov 2024 13:09:37 -0400 Subject: [PATCH] enable/disable video stream, spawn/delete gazebo entity --- .gitignore | 3 +- guiTools/Dockerfile | 4 +- guiTools/gz_entrypoint.sh | 6 + guiTools/spiri_sdk_guitools/launcher.py | 2 +- guiTools/spiri_sdk_guitools/sim_drone.py | 139 ++++++++++++++++---- guiTools/spiri_sdk_guitools/video_button.py | 75 +++++++++++ 6 files changed, 201 insertions(+), 28 deletions(-) create mode 100644 guiTools/gz_entrypoint.sh create mode 100644 guiTools/spiri_sdk_guitools/video_button.py diff --git a/.gitignore b/.gitignore index 0d22318..3b8d2ba 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ docs/build/ -.vscode \ No newline at end of file +.vscode +*.pyc \ No newline at end of file diff --git a/guiTools/Dockerfile b/guiTools/Dockerfile index a27c41a..63306ea 100644 --- a/guiTools/Dockerfile +++ b/guiTools/Dockerfile @@ -21,8 +21,8 @@ COPY --from=git.spirirobotics.com/spiri/gazebo-resources:main /worlds /ardupilot ENV GZ_SIM_SYSTEM_PLUGIN_PATH=/ardupilot_gazebo/plugins ENV GZ_SIM_RESOURCE_PATH=/ardupilot_gazebo/models:/ardupilot_gazebo/worlds -COPY ./spawn_drones.sh /spawn_drones.sh -RUN chmod +x /spawn_drones.sh +COPY ./gz_entrypoint.sh /gz_entrypoint.sh +RUN chmod +x /gz_entrypoint.sh WORKDIR /app diff --git a/guiTools/gz_entrypoint.sh b/guiTools/gz_entrypoint.sh new file mode 100644 index 0000000..bd7a93f --- /dev/null +++ b/guiTools/gz_entrypoint.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e #PR #6 +source /opt/ros/$ROS_DISTRO/setup.bash +gz sim -v -r $WORLD_FILE_NAME + +exec "$@" \ No newline at end of file diff --git a/guiTools/spiri_sdk_guitools/launcher.py b/guiTools/spiri_sdk_guitools/launcher.py index 3f87e6a..6b38d79 100644 --- a/guiTools/spiri_sdk_guitools/launcher.py +++ b/guiTools/spiri_sdk_guitools/launcher.py @@ -11,7 +11,7 @@ applications = { "Terminal": ["qterminal"], "rqt": ["rqt"], "rviz2": ["rviz2"], - "Gazebo": ["/spawn_drones.sh"], + "Gazebo": ["/gz_entrypoint.sh"], "Gazebo Standalone": "gz sim -v4".split(), # Add more applications here if needed } diff --git a/guiTools/spiri_sdk_guitools/sim_drone.py b/guiTools/spiri_sdk_guitools/sim_drone.py index 8bc0a03..05f3841 100644 --- a/guiTools/spiri_sdk_guitools/sim_drone.py +++ b/guiTools/spiri_sdk_guitools/sim_drone.py @@ -10,6 +10,8 @@ from nicegui import ui, run, app import docker import aiodocker import asyncio +from spiri_sdk_guitools.video_button import EnableStreamingButton + docker_client = docker.from_env() from rclpy.executors import ExternalShutdownException @@ -17,6 +19,7 @@ from rclpy.node import Node import rclpy import threading + def ros_main() -> None: rclpy.init() try: @@ -24,8 +27,10 @@ def ros_main() -> None: except ExternalShutdownException: pass + app.on_startup(lambda: threading.Thread(target=ros_main).start()) + @contextlib.contextmanager def modified_environ(*remove, **update): """ @@ -56,8 +61,10 @@ def modified_environ(*remove, **update): env.update(update_after) [env.pop(k) for k in remove_after] + robots = set() + async def container_logs(container, element): adocker = aiodocker.Docker() with element: @@ -68,14 +75,18 @@ async def container_logs(container, element): ui.label(line) # ui.html(conv.convert(bytes(log,'utf-8').decode('utf-8', 'xmlcharrefreplace'), full=False)) + class Robot: robot_type = "spiri-mu" - def __init__(self, sysid: int, compose_files: List[Path] | str ): + + def __init__(self, sysid: int, compose_files: List[Path] | str): if sysid > 255 or sysid < 0: raise ValueError("sysid must be between 0 and 255") self.sysid = int(sysid) if isinstance(compose_files, str): - compose_files = [ Path(file) for file in compose_files.replace("\n",",").split(",") ] + compose_files = [ + Path(file) for file in compose_files.replace("\n", ",").split(",") + ] self.compose_files = compose_files self.processes = [] robots.add(self) @@ -85,21 +96,29 @@ class Robot: container_status = {} with element: while True: - #Poll for data that changes + # Poll for data that changes for container in self.containers(): try: - health = container.attrs['State']['Health']['Status'] + health = container.attrs["State"]["Health"]["Status"] except KeyError: health = "Unknown" - container_status[container] = f"{container.name} {container.status} {health}" + container_status[container] = ( + f"{container.name} {container.status} {health}" + ) if container not in docker_elements: docker_elements[container] = ui.element().classes("w-full") with docker_elements[container]: - ui.label().bind_text(container_status, container).classes("text-lg") - logelement = ui.expansion("Logs").style('margin: 10px;').classes('w-full outline outline-1') + ui.label().bind_text(container_status, container).classes( + "text-lg" + ) + logelement = ( + ui.expansion("Logs") + .style("margin: 10px;") + .classes("w-full outline outline-1") + ) asyncio.create_task(container_logs(container, logelement)) - #Check for containers that have been removed + # 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]) @@ -127,14 +146,17 @@ class Robot: ui.label(f"""Sysid: {self.sysid}""") ui.button("Start", on_click=self.start).classes("m-2") ui.button("Stop", on_click=self.async_stop).classes("m-2") + EnableStreamingButton(sysid=self.sysid).classes("m-2") + async def delete_robot(): await self.async_stop() robots.remove(self) element.remove(self.robot_ui) + ui.button("Delete", on_click=delete_robot).classes("m-2") with ui.tabs() as tabs: - tab_containers = ui.tab('Containers') - tab_ros = ui.tab('ROS Topics') + tab_containers = ui.tab("Containers") + tab_ros = ui.tab("ROS Topics") with ui.tab_panels(tabs, value=tab_containers): tab = ui.tab_panel(tab_containers).classes("w-full") asyncio.create_task(self.ui_containers(tab)) @@ -145,9 +167,10 @@ class Robot: async def async_stop(self): return await run.io_bound(self.stop) - def stop(self): - #Signal all processes to stop + # Delete gazebo model + self.delete_gz_model(sysid=self.sysid) + # Signal all processes to stop for process in self.processes: process.terminate() process.kill() @@ -157,13 +180,15 @@ class Robot: container.remove(force=True) def containers(self): - return docker_client.containers.list(all=True, filters={"name": f"robot-sim-{self.robot_type}-{self.sysid}"}) + return docker_client.containers.list( + all=True, filters={"name": f"robot-sim-{self.robot_type}-{self.sysid}"} + ) def start(self): """Starts the simulated drone with a given sysid, each drone must have it's own unique ID. """ - instance = self.sysid-1 + instance = self.sysid - 1 sysid = self.sysid with logger.contextualize(syd_id=sysid): env = os.environ @@ -172,10 +197,12 @@ class Robot: 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), + GSTREAMER_UDP_PORT=str(int(env["GSTREAMER_UDP_PORT"]) + 10 * instance), SITL_PORT=str(int(env["SITL_PORT"]) + 10 * instance), INSTANCE=str(instance), DRONE_SYS_ID=str(self.sysid), ): + self.spawn_gz_model(self.sysid) logger.info("Starting drone stack, this may take some time") for compose_file in self.compose_files: if not isinstance(compose_file, Path): @@ -183,15 +210,15 @@ class Robot: if not compose_file.exists(): raise FileNotFoundError(f"File {compose_file} does not exist") args = [ - "docker-compose", - "--profile", - "uav-sim", - "-p", - f"robot-sim-{self.robot_type}-{sysid}", - "-f", - compose_file.as_posix(), - "up", - ] + "docker-compose", + "--profile", + "uav-sim", + "-p", + f"robot-sim-{self.robot_type}-{sysid}", + "-f", + compose_file.as_posix(), + "up", + ] command = " ".join(args) logger.info(f"Starting drone stack with command: {command}") @@ -199,4 +226,68 @@ class Robot: args, stdout=subprocess.PIPE, stderr=subprocess.PIPE, - ) + ) + + @staticmethod + def spawn_gz_model(sysid): + logger.info("") + env = os.environ + GSTREAMER_UDP_PORT = env["GSTREAMER_UDP_PORT"] + FDM_PORT_IN = env["FDM_PORT_IN"] + WORLD_NAME = env["WORLD_NAME"] + DRONE_MODEL = env["DRONE_MODEL"] + XACRO_CMD = [ + "xacro", + f"gstreamer_udp_port:={GSTREAMER_UDP_PORT}", + f"fdm_port_in:={FDM_PORT_IN}", + "model.xacro.sdf", + "-o", + "model.sdf", + ] + # 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" + + xacro_proc = subprocess.Popen( + XACRO_CMD, + cwd=f"/ardupilot_gazebo/models/{DRONE_MODEL}", + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + ros2_gz_create_proc = subprocess.Popen( + ROS2_CMD.split(), + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + out, err = ros2_gz_create_proc.communicate(timeout=15) + ros2_gz_create_proc.kill() + return + + @staticmethod + def delete_gz_model(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}" + GZ_SERVICE_CMD = [ + "gz", + "service", + "-s", + f"/world/{WORLD_NAME}/remove", + "--reqtype", + "gz.msgs.Entity", + "--reptype", + "gz.msgs.Boolean", + "--timeout", + "5000", + "--req", + REQUEST_ARG, + ] + remove_entity_proc = subprocess.Popen( + GZ_SERVICE_CMD, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + out, err = remove_entity_proc.communicate(timeout=15) + remove_entity_proc.kill() diff --git a/guiTools/spiri_sdk_guitools/video_button.py b/guiTools/spiri_sdk_guitools/video_button.py new file mode 100644 index 0000000..07205a0 --- /dev/null +++ b/guiTools/spiri_sdk_guitools/video_button.py @@ -0,0 +1,75 @@ +from nicegui import run, ui +import subprocess +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" + + +class EnableStreamingButton(ui.element): + def __init__(self, sysid, state: bool = False) -> None: + super().__init__() + self.sysid = sysid + self._state = state + self.button = None + with self.classes(): + with ui.row(): + self.button = ui.button(on_click=self.on_click) + self.button._text = ( + f'{"Disable" if self._state else "Enable"}' + " Video" + ) + self.button.props(f'color={"red" if self._state else "green"}') + + 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) + if result: + ui.notify("Success", type="positive]") + self._state = not self._state + self.update() + else: + self._state = False + self.update() + ui.notify("No Video Streaming Available..", type="negative") + spinner.delete() + + def update(self) -> None: + self.button._text = f'{"Disable" if self._state else "Enable"}' + " Video" + self.button.props(f'color={"red" if self._state else "green"}') + + @staticmethod + def enable_streaming(sysid: int, 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)], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + ) + # Check exceptions.. timeout, error etc. + + output, error = gz_topic_list_proc.communicate(timeout=15) + if gz_topic_list_proc.returncode != 0: + logger.error(error) + return False + if "No subscribers".casefold() in output.casefold(): + logger.error("No Subscribers on enable_streaming topic.") + return False + + gz_topic_pub = [ + "gz", + "topic", + "-t", + ENABLE_STREAMING_TOPIC.format(world_name=world_name, sysid=sysid), + "-m", + "gz.msgs.Boolean", + "-p", + f"data:{is_streaming}", + ] + subprocess.run((gz_topic_pub)) + return True