enable/disable video stream, spawn/delete gazebo entity

.gitignore

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

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
+COPY ./gz_entrypoint.sh /gz_entrypoint.sh
-RUN chmod +x /spawn_drones.sh
+RUN chmod +x /gz_entrypoint.sh
 
 WORKDIR /app
 

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 "$@"

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
 }

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")
+                            ui.label().bind_text(container_status, container).classes(
-                            logelement = ui.expansion("Logs").style('margin: 10px;').classes('w-full outline outline-1')
+                                "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_containers = ui.tab("Containers")
-                    tab_ros = ui.tab('ROS Topics')
+                    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",
+                        "docker-compose",
-                            "--profile",
+                        "--profile",
-                            "uav-sim",
+                        "uav-sim",
-                            "-p",
+                        "-p",
-                            f"robot-sim-{self.robot_type}-{sysid}",
+                        f"robot-sim-{self.robot_type}-{sysid}",
-                            "-f",
+                        "-f",
-                            compose_file.as_posix(),
+                        compose_file.as_posix(),
-                            "up",
+                        "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()

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