Spiri
/
spiri-sdk
12
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases 1 Wiki Activity

enable/disable video stream, spawn/delete gazebo entity

This commit is contained in:
Burak Ozter 2024-11-07 13:09:37 -04:00
parent f4f5b0890e
commit a752ed603e
6 changed files with 201 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

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

4
guiTools/Dockerfile
View File

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

6
guiTools/gz_entrypoint.sh Normal file
View File

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

2
guiTools/spiri_sdk_guitools/launcher.py
View File

@ -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
}

139
guiTools/spiri_sdk_guitools/sim_drone.py
View File

@ -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()

75
guiTools/spiri_sdk_guitools/video_button.py Normal file
View File

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