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..c7f3d2b 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,9 @@ 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("-","_")
+ self.world_name = "citadel_hill"
async def ui_containers(self, element):
docker_elements = {}
@@ -109,6 +112,10 @@ class Robot:
ui.label().bind_text(container_status, container).classes(
"text-lg"
)
+ #Show the command the container is running
+ # ui.label(container.attrs["Config"]["Cmd"])
+ cmd_widget = ui.codemirror(" ".join(container.attrs["Config"]["Cmd"]), language="bash",theme="basicDark").classes('h-auto max-h-32')
+ cmd_widget.enabled = False
logelement = (
ui.expansion("Logs")
.style("margin: 10px;")
@@ -129,8 +136,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}/") or topic[0].startswith(f"/world/{self.world_name}/model/{self.robot_name}/"):
+ ui.label(topic[0])
await asyncio.sleep(10)
async def ui(self, element):
@@ -143,7 +152,7 @@ class Robot:
ui.label(f"""Sysid: {self.sysid}""")
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(
+ self.video_button = EnableStreamingButton(robot_name=self.robot_name).classes(
"m-2"
)
@@ -159,7 +168,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 +180,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 +192,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 +215,24 @@ 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,
+ WORLD_NAME="citadel_hill",
):
- 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 +246,9 @@ class Robot:
stderr=subprocess.PIPE,
)
- @staticmethod
- def spawn_gz_model(sysid):
+ @logger.catch
+ def spawn_gz_model(self):
+ sysid = self.sysid
logger.info("")
env = os.environ
GSTREAMER_UDP_PORT = env["GSTREAMER_UDP_PORT"]
@@ -252,7 +265,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 +282,14 @@ class Robot:
ros2_gz_create_proc.kill()
return
- @staticmethod
- def delete_gz_model(sysid):
+ @logger.catch
+ 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/guiTools/spiri_sdk_guitools/video_button.py b/guiTools/spiri_sdk_guitools/video_button.py
index 7c29164..6146163 100644
--- a/guiTools/spiri_sdk_guitools/video_button.py
+++ b/guiTools/spiri_sdk_guitools/video_button.py
@@ -4,13 +4,13 @@ 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"
+ENABLE_STREAMING_TOPIC = "/world/{world_name}/model/{robot_name}/link/pitch_link/sensor/camera/image/enable_streaming"
class EnableStreamingButton(ui.element):
- def __init__(self, sysid, state: bool = False) -> None:
+ def __init__(self, robot_name, state: bool = False) -> None:
super().__init__()
- self.sysid = sysid
+ self.robot_name = robot_name
self._state = state
self.button = None
with self.classes():
@@ -24,7 +24,7 @@ class EnableStreamingButton(ui.element):
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)
+ result = await run.cpu_bound(self.enable_streaming, self.robot_name, not self._state)
if result:
ui.notify("Success", type="positive]")
self.set_state(state=not self._state)
@@ -43,17 +43,17 @@ class EnableStreamingButton(ui.element):
def stop_video(self):
if self._state != False:
- self.enable_streaming(sysid=self.sysid, is_streaming=False)
+ self.enable_streaming(robot_name=self.robot_name, is_streaming=False)
self.set_state(state=False)
@staticmethod
- def enable_streaming(sysid: int, is_streaming: bool) -> bool:
+ def enable_streaming(robot_name: str, 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)],
+ + [ENABLE_STREAMING_TOPIC.format(world_name=world_name, robot_name=robot_name)],
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
text=True,
@@ -72,7 +72,7 @@ class EnableStreamingButton(ui.element):
"gz",
"topic",
"-t",
- ENABLE_STREAMING_TOPIC.format(world_name=world_name, sysid=sysid),
+ ENABLE_STREAMING_TOPIC.format(world_name=world_name, robot_name=robot_name),
"-m",
"gz.msgs.Boolean",
"-p",
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/robots/spiri-mu/virtual_camera/Dockerfile b/robots/spiri-mu/virtual_camera/Dockerfile
new file mode 100644
index 0000000..ffbfbdc
--- /dev/null
+++ b/robots/spiri-mu/virtual_camera/Dockerfile
@@ -0,0 +1,5 @@
+FROM git.spirirobotics.com/spiri/services-ros2-mavros:main
+
+RUN apt-get update
+RUN apt-get --yes install ros-${ROS_DISTRO}-ros-gz-bridge ros-${ROS_DISTRO}-compressed-image-transport ros-${ROS_DISTRO}-rmw-cyclonedds-cpp
+
diff --git a/robots/spiri-mu/virtual_camera/docker-compose.yaml b/robots/spiri-mu/virtual_camera/docker-compose.yaml
new file mode 100644
index 0000000..1969fd3
--- /dev/null
+++ b/robots/spiri-mu/virtual_camera/docker-compose.yaml
@@ -0,0 +1,10 @@
+
+services:
+ front-gimbal:
+ ipc: host
+ network_mode: host
+ # image: git.spirirobotics.com/spiri/services-ros2-mavros:main
+ build: ./
+ environment:
+ - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+ command: ros2 run ros_gz_image image_bridge /world/${WORLD_NAME}/model/${ROBOT_NAME}/link/pitch_link/sensor/camera/image
\ No newline at end of file
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)