From a752ed603eb81ce378c97c29022c9fa4be753c6a Mon Sep 17 00:00:00 2001 From: Burak Ozter Date: Thu, 7 Nov 2024 13:09:37 -0400 Subject: [PATCH 1/4] 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 From c1484b58cecaa06591f0f53fa73dbb82e289d714 Mon Sep 17 00:00:00 2001 From: Burak Ozter Date: Thu, 7 Nov 2024 13:12:19 -0400 Subject: [PATCH 2/4] Alex D: fix rclpy.spin no node error --- guiTools/spiri_sdk_guitools/sim_drone.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/guiTools/spiri_sdk_guitools/sim_drone.py b/guiTools/spiri_sdk_guitools/sim_drone.py index 05f3841..711d2b0 100644 --- a/guiTools/spiri_sdk_guitools/sim_drone.py +++ b/guiTools/spiri_sdk_guitools/sim_drone.py @@ -22,10 +22,6 @@ import threading def ros_main() -> None: rclpy.init() - try: - rclpy.spin() - except ExternalShutdownException: - pass app.on_startup(lambda: threading.Thread(target=ros_main).start()) From df5df4b497cd9263563677f117568227577c960b Mon Sep 17 00:00:00 2001 From: Burak Ozter Date: Thu, 7 Nov 2024 13:54:04 -0400 Subject: [PATCH 3/4] FIX: button state when stop/delete and video stream was enabled --- guiTools/spiri_sdk_guitools/sim_drone.py | 9 ++++++++- guiTools/spiri_sdk_guitools/video_button.py | 15 +++++++++++---- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/guiTools/spiri_sdk_guitools/sim_drone.py b/guiTools/spiri_sdk_guitools/sim_drone.py index 711d2b0..3cc29b9 100644 --- a/guiTools/spiri_sdk_guitools/sim_drone.py +++ b/guiTools/spiri_sdk_guitools/sim_drone.py @@ -85,6 +85,7 @@ class Robot: ] self.compose_files = compose_files self.processes = [] + self.video_button = None robots.add(self) async def ui_containers(self, element): @@ -142,7 +143,9 @@ 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") + self.video_button = EnableStreamingButton(sysid=self.sysid).classes( + "m-2" + ) async def delete_robot(): await self.async_stop() @@ -164,6 +167,10 @@ class Robot: return await run.io_bound(self.stop) def stop(self): + # If video was enabled, and we stop. Button state is wrong. + # Stop video if it is enabled. + if isinstance(self.video_button, EnableStreamingButton): + self.video_button.stop_video() # Delete gazebo model self.delete_gz_model(sysid=self.sysid) # Signal all processes to stop diff --git a/guiTools/spiri_sdk_guitools/video_button.py b/guiTools/spiri_sdk_guitools/video_button.py index 07205a0..7c29164 100644 --- a/guiTools/spiri_sdk_guitools/video_button.py +++ b/guiTools/spiri_sdk_guitools/video_button.py @@ -27,18 +27,25 @@ class EnableStreamingButton(ui.element): 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() + self.set_state(state=not self._state) else: - self._state = False - self.update() + self.set_state(state=False) ui.notify("No Video Streaming Available..", type="negative") spinner.delete() + def set_state(self, state: bool): + self._state = state + self.update() + 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"}') + def stop_video(self): + if self._state != False: + self.enable_streaming(sysid=self.sysid, is_streaming=False) + self.set_state(state=False) + @staticmethod def enable_streaming(sysid: int, is_streaming: bool) -> bool: From f37841d7bbf511dab7a9d833f9f3351dd33659ca Mon Sep 17 00:00:00 2001 From: Alex Davies Date: Fri, 8 Nov 2024 09:41:34 -0400 Subject: [PATCH 4/4] Run ui start in thread --- guiTools/spiri_sdk_guitools/sim_drone.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/guiTools/spiri_sdk_guitools/sim_drone.py b/guiTools/spiri_sdk_guitools/sim_drone.py index 3cc29b9..23584dd 100644 --- a/guiTools/spiri_sdk_guitools/sim_drone.py +++ b/guiTools/spiri_sdk_guitools/sim_drone.py @@ -141,7 +141,7 @@ class Robot: with self.robot_ui: ui.label(f"{self.robot_type} {self.sysid}").classes("text-2xl") ui.label(f"""Sysid: {self.sysid}""") - ui.button("Start", on_click=self.start).classes("m-2") + 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( "m-2" @@ -187,6 +187,9 @@ class Robot: all=True, filters={"name": f"robot-sim-{self.robot_type}-{self.sysid}"} ) + async def async_start(self): + return await run.io_bound(self.start) + def start(self): """Starts the simulated drone with a given sysid, each drone must have it's own unique ID.