arena robot and fleet panel

_meta/tools/robot

@@ -0,0 +1,266 @@
+#!/usr/bin/env python3
+"""Spawn, list, and despawn robots in a running arena fleet at runtime.
+
+Usage:
+  arena robot <model> [name:=N] [mobile:=M] [arm:=A] [count:=K] [key:=value ...]
+                      [--env <id>|--ns <ns>] [--nowait]
+  arena robot rm <name> [--env <id>|--ns <ns>] [--nowait]
+  arena robot ls [--env <id>|--ns <ns>|--all]
+
+<key>:=<value> tokens describe the robot and are forwarded to the spawn service:
+`name` selects the instance name, `count` spawns that many, and every other key
+is passed through to Robot.parse (e.g. mobile:=manual, arm:=none). --flags
+control the command; the model is the sole positional for spawn.
+
+Spawns go live immediately (idle, in the staging area) and join the episode at the
+next reset. Pass immediate:=false to defer the whole spawn to the next reset instead.
+Despawns take effect on the next reset. By default the command waits until the robot
+appears in or disappears from the fleet (10s heartbeat), and --nowait returns as soon
+as the request is accepted.
+
+Discovers envs by polling `ros2 topic list` for `<ns>/state/robots`, waiting
+forever (10s heartbeat) until a match appears.
+"""
+
+import argparse
+import contextlib
+import os
+import subprocess
+import sys
+import time
+
+ROBOTS_SUFFIX = "/state/robots"
+
+
+def discover_envs() -> list[str]:
+    try:
+        out = subprocess.check_output(["ros2", "topic", "list"], stderr=subprocess.DEVNULL, text=True)
+    except (subprocess.CalledProcessError, FileNotFoundError):
+        return []
+    return [
+        line[: -len(ROBOTS_SUFFIX)]
+        for line in (s.strip() for s in out.splitlines())
+        if line.endswith(ROBOTS_SUFFIX)
+    ]
+
+
+def matches(ns: str, target: str) -> bool:
+    env_ns = os.path.dirname(ns)
+    return target in (ns, env_ns, env_ns.lstrip("/")) or os.path.basename(env_ns) == target
+
+
+def wait_for_env(target: str | None) -> list[str]:
+    waited = 0
+    while True:
+        nodes = discover_envs()
+        if nodes and (target is None or any(matches(n, target) for n in nodes)):
+            return nodes
+        time.sleep(1)
+        waited += 1
+        if waited % 10 == 0:
+            who = f"env '{target}'" if target is not None else "an env"
+            print(f"arena robot: waiting for {who} ({waited}s elapsed)", file=sys.stderr)
+
+
+def resolve_single(target: str | None) -> str:
+    nodes = wait_for_env(target)
+    if target is None:
+        if len(nodes) > 1:
+            print("arena robot: multiple envs running, pass --env <id> or --ns <ns>:", file=sys.stderr)
+            for n in nodes:
+                print(f"  {os.path.dirname(n)}", file=sys.stderr)
+            raise SystemExit(1)
+        return nodes[0]
+    return next(n for n in nodes if matches(n, target))
+
+
+@contextlib.contextmanager
+def ros_node():
+    import rclpy
+
+    own = not rclpy.ok()
+    if own:
+        rclpy.init(args=[])
+    node = rclpy.create_node("arena_robot_cli")
+    try:
+        yield node
+    finally:
+        node.destroy_node()
+        if own:
+            rclpy.shutdown()
+
+
+def call_service(node, srv_type, srv_name: str, request, timeout: float = 10.0):
+    import rclpy
+
+    client = node.create_client(srv_type, srv_name)
+    if not client.wait_for_service(timeout_sec=timeout):
+        raise SystemExit(f"arena robot: service {srv_name} unavailable")
+    future = client.call_async(request)
+    while rclpy.ok() and not future.done():
+        rclpy.spin_once(node, timeout_sec=0.1)
+    return future.result()
+
+
+def read_fleet(node, node_ns: str, timeout: float = 5.0):
+    import rclpy
+    import rclpy.qos
+    from task_generator_msgs.msg import RobotFleet
+
+    latest: list = []
+    qos = rclpy.qos.QoSProfile(depth=1, durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL)
+    sub = node.create_subscription(RobotFleet, node_ns + ROBOTS_SUFFIX, lambda m: latest.append(m), qos)
+    deadline = time.monotonic() + timeout
+    while rclpy.ok() and not latest and time.monotonic() < deadline:
+        rclpy.spin_once(node, timeout_sec=0.1)
+    node.destroy_subscription(sub)
+    return list(latest[-1].robots) if latest else []
+
+
+def wait_for_presence(node, node_ns: str, names: set[str], present: bool, label: str) -> None:
+    import rclpy
+    import rclpy.qos
+    from task_generator_msgs.msg import RobotFleet
+
+    state: dict[str, set[str] | None] = {"names": None}
+    qos = rclpy.qos.QoSProfile(depth=1, durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL)
+    sub = node.create_subscription(
+        RobotFleet,
+        node_ns + ROBOTS_SUFFIX,
+        lambda m: state.update(names={r.name for r in m.robots}),
+        qos,
+    )
+    waited = 0.0
+    next_beat = 10.0
+    try:
+        while rclpy.ok():
+            rclpy.spin_once(node, timeout_sec=0.1)
+            live = state["names"]
+            if live is not None and all((n in live) == present for n in names):
+                return
+            waited += 0.1
+            if waited >= next_beat:
+                print(f"arena robot: waiting for {label} ({int(waited)}s elapsed)", file=sys.stderr)
+                next_beat += 10.0
+    finally:
+        node.destroy_subscription(sub)
+
+
+def cmd_spawn(model: str, kv: dict[str, str], opts: argparse.Namespace) -> int:
+    from diagnostic_msgs.msg import KeyValue
+    from task_generator_msgs.srv import SpawnRobot
+
+    count = int(kv.pop("count", "1"))
+    name = kv.pop("name", "")
+    immediate = kv.pop("immediate", "true").lower() not in ("false", "0", "no")
+    if count > 1 and name:
+        print("arena robot: name:= cannot be combined with count:= > 1", file=sys.stderr)
+        return 1
+
+    node_ns = resolve_single(opts.target)
+    spawned: set[str] = set()
+    with ros_node() as node:
+        for _ in range(count):
+            req = SpawnRobot.Request()
+            req.model = model
+            req.name = name
+            req.immediate = immediate
+            req.args = [KeyValue(key=k, value=v) for k, v in kv.items()]
+            resp = call_service(node, SpawnRobot, node_ns + "/runtime/spawn_robot", req)
+            if resp is None or not resp.success:
+                msg = resp.error_msg if resp else "no response"
+                print(f"arena robot: spawn failed: {msg}", file=sys.stderr)
+                return 1
+            print(resp.name)
+            spawned.add(resp.name)
+        if opts.nowait:
+            return 0
+        wait_for_presence(node, node_ns, spawned, present=True, label=f"{', '.join(sorted(spawned))} to spawn")
+    return 0
+
+
+def cmd_rm(name: str, opts: argparse.Namespace) -> int:
+    from task_generator_msgs.srv import DespawnRobot
+
+    node_ns = resolve_single(opts.target)
+    with ros_node() as node:
+        req = DespawnRobot.Request()
+        req.name = name
+        resp = call_service(node, DespawnRobot, node_ns + "/runtime/despawn_robot", req)
+        if resp is None or not resp.success:
+            msg = resp.error_msg if resp else "no response"
+            print(f"arena robot: despawn failed: {msg}", file=sys.stderr)
+            return 1
+        if opts.nowait:
+            return 0
+        wait_for_presence(node, node_ns, {name}, present=False, label=f"'{name}' to despawn")
+    return 0
+
+
+def cmd_ls(opts: argparse.Namespace) -> int:
+    nodes = wait_for_env(None) if opts.all_envs else [resolve_single(opts.target)]
+    with ros_node() as node:
+        for ns in nodes:
+            print(os.path.dirname(ns))
+            robots = read_fleet(node, ns)
+            if not robots:
+                print("  (empty)")
+            for r in robots:
+                print(f"  {r.name}\t{r.model}\t{r.ns}")
+    return 0
+
+
+def _add_target(parser: argparse.ArgumentParser) -> None:
+    parser.add_argument("--env", dest="env", type=int)
+    parser.add_argument("--ns", dest="ns")
+
+
+def _resolve_target(args: argparse.Namespace) -> str | None:
+    if args.env is not None and args.ns:
+        raise SystemExit("arena robot: pass either --env or --ns, not both")
+    if args.env is not None:
+        return f"env_{args.env}"
+    return args.ns
+
+
+def main() -> int:
+    raw = sys.argv[1:]
+    kv: dict[str, str] = {}
+    passthrough: list[str] = []
+    for tok in raw:
+        if ":=" in tok:
+            key, _, value = tok.partition(":=")
+            kv[key] = value
+        else:
+            passthrough.append(tok)
+
+    sub = passthrough[0] if passthrough else None
+
+    if sub == "ls":
+        parser = argparse.ArgumentParser(prog="arena robot ls")
+        _add_target(parser)
+        parser.add_argument("--all", dest="all_envs", action="store_true")
+        args = parser.parse_args(passthrough[1:])
+        args.target = None if args.all_envs else _resolve_target(args)
+        return cmd_ls(args)
+
+    if sub == "rm":
+        parser = argparse.ArgumentParser(prog="arena robot rm")
+        parser.add_argument("name")
+        _add_target(parser)
+        parser.add_argument("--nowait", action="store_true")
+        args = parser.parse_args(passthrough[1:])
+        args.target = _resolve_target(args)
+        return cmd_rm(args.name, args)
+
+    parser = argparse.ArgumentParser(prog="arena robot", description=__doc__)
+    parser.add_argument("model")
+    _add_target(parser)
+    parser.add_argument("--nowait", action="store_true")
+    args = parser.parse_args(passthrough)
+    args.target = _resolve_target(args)
+    return cmd_spawn(args.model, kv, args)
+
+
+if __name__ == "__main__":
+    sys.exit(main())

_meta/tools/source

@@ -264,6 +264,9 @@ if [ -z ${ARENA_SOURCED+x} ] ; then
             ;;
             cam)
                 ros2 run arena_runtime cam "$@"
+            ;;
+            robot)
+                "${TOOLS_DIR}/robot" "$@"
                 result=$?
             ;;
             cleanup)

arena_bringup/BRINGUP.md

@@ -297,6 +297,32 @@ and exit non-zero with a hint to use `--all` or `<env_id>`.
 
 ---
 
+## arena robot
+
+Add, remove, and list robots in a running fleet at runtime, on top of the
+launch-time `robot:=` seed.
+
+```bash
+arena robot jackal                       # spawn one jackal (unique auto name)
+arena robot jackal mobile:=manual        # per-instance adapter selection
+arena robot jackal count:=2              # two jackals
+arena robot rm jackal_0                  # despawn by name
+arena robot ls                           # list the current fleet
+```
+
+`<key>:=<value>` tokens describe the robot and are forwarded to the spawn
+service: `name` sets the instance name, `count` spawns that many, and every
+other key reaches `Robot.parse` (e.g. `mobile:=`, `arm:=`). `--flags` control
+the command. Target an env with `--env <id>` or `--ns <ns>` (required when more
+than one env is running); `arena robot ls --all` lists every env.
+
+Spawns and despawns apply on the next episode reset. By default the command
+waits until the robot appears in / disappears from `state/robots` (10s warning
+cadence); `--nowait` returns as soon as the request is accepted (printing the
+assigned name).
+
+---
+
 ## Common options
 
 ```bash
@@ -404,6 +430,7 @@ common entry points. Verbs relevant to bringup:
 | `arena runtime [args]` | `arena_runtime.launch.py` | Runtime-only launch (sim + `arena_node`, no envs). |
 | `arena env [args]` | `task_generator.launch.py` | Attach one task-generator env to a running runtime. |
 | `arena viz [target]` | `ros2 run rviz_utils rviz_config` | Attach rviz to a running env; see [arena viz](#arena-viz). |
+| `arena robot <model>\|rm\|ls` | `runtime/spawn_robot`, `runtime/despawn_robot`, `state/robots` | Spawn, despawn, or list robots in a running fleet; see [arena robot](#arena-robot). |
 | `arena cleanup <env_id>` | `/arena/cleanup_namespace` service | Force-clean an env's namespace by id (calls the service for both the `env_<id>_` and `env_<id>/` prefixes, covering gazebo and isaac layouts). |
 | `arena train [args]` | `arena_training` feature launcher | RL training entry, see section 7 above. |
 

task_generator/package.xml

@@ -16,6 +16,7 @@
 
   <exec_depend>arena_rclpy_mixins</exec_depend>
   <exec_depend>arena_bringup</exec_depend>
+  <exec_depend>controller_manager_msgs</exec_depend>
   <exec_depend>arena_planners</exec_depend>
   <exec_depend>arena_runtime</exec_depend>
   <exec_depend>arena_runtime_msgs</exec_depend>