diff --git a/_meta/tools/robot b/_meta/tools/robot new file mode 100755 index 00000000..07178e06 --- /dev/null +++ b/_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 [name:=N] [mobile:=M] [arm:=A] [count:=K] [key:=value ...] + [--env |--ns ] [--nowait] + arena robot rm [--env |--ns ] [--nowait] + arena robot ls [--env |--ns |--all] + +:= 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 `/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 or --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()) diff --git a/_meta/tools/source b/_meta/tools/source index a2d2d669..297ee3ff 100644 --- a/_meta/tools/source +++ b/_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) diff --git a/arena_bringup/BRINGUP.md b/arena_bringup/BRINGUP.md index 71ae0f65..7b749c8b 100644 --- a/arena_bringup/BRINGUP.md +++ b/arena_bringup/BRINGUP.md @@ -297,6 +297,32 @@ and exit non-zero with a hint to use `--all` or ``. --- +## 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 +``` + +`:=` 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 ` or `--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 \|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 ` | `/arena/cleanup_namespace` service | Force-clean an env's namespace by id (calls the service for both the `env__` and `env_/` prefixes, covering gazebo and isaac layouts). | | `arena train [args]` | `arena_training` feature launcher | RL training entry, see section 7 above. | diff --git a/task_generator/package.xml b/task_generator/package.xml index 65fefa47..64afb410 100644 --- a/task_generator/package.xml +++ b/task_generator/package.xml @@ -16,6 +16,7 @@ arena_rclpy_mixins arena_bringup + controller_manager_msgs arena_planners arena_runtime arena_runtime_msgs diff --git a/task_generator/task_generator/manager/robot_manager/robot_manager.py b/task_generator/task_generator/manager/robot_manager/robot_manager.py index 9d75a664..1193e6e7 100644 --- a/task_generator/task_generator/manager/robot_manager/robot_manager.py +++ b/task_generator/task_generator/manager/robot_manager/robot_manager.py @@ -8,6 +8,7 @@ import ament_index_python import arena_bringup.extensions.NodeLogLevelExtension as NodeLogLevelExtension import attrs +import controller_manager_msgs.srv import geometry_msgs.msg import launch import launch.launch_description_sources @@ -18,6 +19,7 @@ import rclpy.publisher import rclpy.timer import tf2_ros +from arena_rclpy_mixins.Async import LaunchHandle from arena_rclpy_mixins.shared import Namespace from arena_robots.Robot import RobotView from arena_runtime._node import NodeInterface @@ -50,6 +52,9 @@ _MOVEIT_QUIET_RULES = '+[**/moveit/**:error]' +_CONTROLLER_MANAGER_GRACE = 30.0 +_CONTROLLER_POLL = 0.2 + class RobotManager(NodeInterface): """Manages the goal and start position of a robot for all task modes.""" @@ -70,6 +75,7 @@ class RobotManager(NodeInterface): _phase_index: int _unsupported_kinds_logged: set[TaskKind] _abort_episode: Callable[[str], None] | None + _launch_handle: LaunchHandle | None @property def robot(self) -> Robot: @@ -134,6 +140,7 @@ def __init__( self._robot.extra.setdefault('namespace', self.namespace) self._publish_goal_task: asyncio.Task | None = None + self._launch_handle: LaunchHandle | None = None # Deferred to break the import cycle between this module and # task_generator.tasks (which eagerly loads context.py → RobotManager). @@ -504,8 +511,25 @@ async def _launch_robot(self, node_paths: set[str]): ) launch_description.add_action(launch.actions.GroupAction(adapter_actions)) - await self.node.do_launch(launch_description) + self._launch_handle = await self.node.do_launch_tracked(launch_description) await asyncio.gather(*(a.wait_until_ready(self, node_paths) for a in self._adapter_instances)) + await self.wait_controllers_active() + + async def wait_controllers_active(self) -> None: + """Block until every spawned controller reports active. A controller_manager that + never appears within the grace window (dummy sim) means there is nothing to wait for.""" + client = self.node.create_client_wrapper( + controller_manager_msgs.srv.ListControllers, + str(self.namespace("controller_manager", "list_controllers")), + timeout=_CONTROLLER_MANAGER_GRACE, + ) + if not await client.ensure(timeout_sec=_CONTROLLER_MANAGER_GRACE): + return + while True: + resp = await client.call_timeout(controller_manager_msgs.srv.ListControllers.Request()) + if resp is not None and resp.controller and all(c.state == "active" for c in resp.controller): + return + await asyncio.sleep(_CONTROLLER_POLL) async def update(self): # TODO implement record data dir @@ -519,4 +543,7 @@ async def destroy(self): for adapter, result in zip(self._adapter_instances, results, strict=True): if isinstance(result, Exception): self._logger.warning(f"adapter {adapter.kind!r} teardown failed: {result!r}") + if self._launch_handle is not None: + await self._launch_handle.shutdown() + self._launch_handle = None await self._environment_manager.remove_robot((self.robot,)) diff --git a/task_generator/task_generator/manager/robot_manager/robots_manager.py b/task_generator/task_generator/manager/robot_manager/robots_manager.py index 5de0f793..a9594fd5 100644 --- a/task_generator/task_generator/manager/robot_manager/robots_manager.py +++ b/task_generator/task_generator/manager/robot_manager/robots_manager.py @@ -10,7 +10,6 @@ import geometry_msgs.msg import rclpy import task_generator_msgs.msg -from arena_rclpy_mixins.ROSParamServer import ROSParamT from arena_rclpy_mixins.shared import Namespace from arena_runtime._node import NodeInterface @@ -132,7 +131,6 @@ class RobotsManager(NodeInterface): """ _initialpose: typing.Generator - _robot_configurations: ROSParamT[_RobotDiff] _diff: _RobotDiff @property @@ -375,6 +373,11 @@ async def set_up(self): await asyncio.gather(*futures) self._diff.to_add.clear() + self._publish_fleet() + self.publish_queue() + + def _publish_fleet(self) -> None: + """Publish the live fleet on state/robots, refresh robot_names, viz, and frame aliases.""" self.node.rosparam[list[str]].set('robot_names', [robot.name for robot in self.managers.values()]) fleet = task_generator_msgs.msg.RobotFleet( @@ -418,32 +421,91 @@ def __init__(self, *args: object, environment_manager: EnvironmentManager, **kwa self._pending_launch: list[RobotManager] = [] self._abort_episode: Callable[[str], None] | None = None - self._robot_configurations = self.node.ROSParam[_RobotDiff]( - 'robot', - type_=rclpy.Parameter.Type.STRING, - parse=self._parse_robot_configurations, - ) + # Launch-time seed: read the `robot` param once to populate the initial + # fleet, then undeclare it so only spawn/despawn deltas mutate the fleet. + self.node.rosparam[str].declare_safe('robot', rclpy.Parameter.Type.STRING) + self._parse_robot_configurations(self.node.rosparam[str].get('robot', '')) + self.node.undeclare_parameter('robot') + + @property + def has_pending_additions(self) -> bool: + """Whether the next set_up will add robots, whose controllers must provision while the sim steps.""" + return bool(self._diff.to_add) async def launch_pending(self) -> None: """Bring up navstacks for managers queued by set_up. Caller controls when this fires - so LaunchService.run_async()'s main-loop block doesn't starve concurrent work - (e.g. spawn_world_obstacles).""" + so LaunchService.run_async()'s main-loop block doesn't starve concurrent work.""" if not self._pending_launch: return pending = self._pending_launch self._pending_launch = [] node_paths: set[str] = set() - with self.provide_node_paths(node_paths) as fetch_task: - await asyncio.wait( - ( - fetch_task, - asyncio.gather(*(m.launch(node_paths) for m in pending)), - ), - return_when=asyncio.FIRST_COMPLETED, - ) + with self.provide_node_paths(node_paths): + await asyncio.gather(*(m.launch(node_paths) for m in pending)) + + async def spawn_now(self, name: str, pose: Pose | None) -> None: + """Provision one queued robot into the live world now, idle, at its resolved on-map pose + (no staging detour, which would teleport it off-map and disturb nav2). It joins the + episode at the next reset like any other robot.""" + config = attrs.evolve(self._diff.to_add.pop(name)) + config.name = name + if pose is not None: + config.pose = pose + manager = RobotManager( + node=self.node, + namespace=Namespace(self.node.get_namespace())(self.node.get_name()), + environment_manager=self._environment_manager, + robot=config, + ) + if self._abort_episode is not None: + manager.bind_abort(self._abort_episode) + await manager.set_up_robot() + self.managers[name] = manager + node_paths: set[str] = set() + with self.provide_node_paths(node_paths): + await manager.launch(node_paths) + self._publish_fleet() def add_pending(self, name: str, robot: Robot) -> None: """Queue a robot for full set_up_robot on the next reset_cycle.""" if name in self._managers or name in self._diff.to_add: raise ValueError(f"robot {name!r} already exists") self._diff.to_add[name] = robot + + def remove_pending(self, name: str) -> None: + """Toggle a robot's despawn-pending state, the single fleet-removal surface. + + Cancels a queued spawn, un-stages a queued despawn, or stages a live + robot for teardown on the next reset_cycle. + """ + if name in self._diff.to_add: + del self._diff.to_add[name] + return + if name in self._diff.to_remove: + self._diff.to_remove.remove(name) + return + if name not in self._managers: + raise ValueError(f"robot {name!r} does not exist") + self._diff.to_remove.append(name) + + def publish_queue(self) -> None: + """Publish the pending spawn/despawn diff (the queue) on state/robots/pending.""" + msg = task_generator_msgs.msg.RobotQueue( + spawn=[ + task_generator_msgs.msg.RobotDescriptor(name=name, model=robot.model.name) + for name, robot in self._diff.to_add.items() + ], + despawn=[ + task_generator_msgs.msg.RobotDescriptor(name=name, model=self._managers[name].model_name) + for name in self._diff.to_remove + ], + ) + self.node._pub_state_robots_pending.publish(msg) + + def next_name(self, model: str) -> str: + """Lowest-free `_` not live, queued to add, or queued to remove.""" + taken = set(self._managers) | set(self._diff.to_add) | set(self._diff.to_remove) + n = 0 + while f"{model}_{n}" in taken: + n += 1 + return f"{model}_{n}" diff --git a/task_generator/task_generator/node.py b/task_generator/task_generator/node.py index 7ef1c34a..b26a0356 100644 --- a/task_generator/task_generator/node.py +++ b/task_generator/task_generator/node.py @@ -89,7 +89,6 @@ class TaskModeOverrides: tm_modules: list[str] = attrs.Factory(list) keep_modules: bool = False world: str = "" - robots: list[str] = attrs.Factory(list) @attrs.define @@ -228,6 +227,12 @@ def __init__(self): _LATCHED, ) + self._pub_state_robots_pending = self.create_publisher( + task_generator_msgs.msg.RobotQueue, + self.service_namespace("state", "robots", "pending"), + _LATCHED, + ) + self._pub_state_queue = self.create_publisher( task_generator_msgs.msg.EpisodeRecord, self.service_namespace("state", "queue"), @@ -259,6 +264,7 @@ def __init__(self): self._episode_task: asyncio.Task | None = None self._publish_bootstrap_queue_state() + self._pub_state_robots_pending.publish(task_generator_msgs.msg.RobotQueue()) def _publish_bootstrap_queue_state(self) -> None: # Latch a minimal state/queue at construction so the rviz panel can @@ -652,20 +658,18 @@ def _publish_queue_state(self) -> None: current_modules = [m.value for m in self.conf.TaskMode.TM_MODULES.value] record_world = self._episodes.current.world loaded_world = self._world_manager.loaded_world - live_robots = [m.model_name for m in self._robots_manager.managers.values()] + queued_robots = [m.model_name for m in self._robots_manager.managers.values()] if overrides is None: queued_tm_robots = current_robots queued_tm_obstacles = current_obstacles queued_tm_modules = current_modules queued_world = record_world or loaded_world - queued_robots = live_robots else: queued_tm_robots = overrides.tm_robots or current_robots queued_tm_obstacles = overrides.tm_obstacles or current_obstacles queued_tm_modules = current_modules if overrides.keep_modules else overrides.tm_modules queued_world = overrides.world or record_world or loaded_world - queued_robots = overrides.robots or live_robots obstacles_live = self._params_for_mode(queued_tm_obstacles) robots_live = self._params_for_mode(queued_tm_robots) @@ -715,9 +719,6 @@ async def _build_next_record(self, world: str, seed: int) -> None: world = world or overrides.world # World swap itself happens inside `_run_reset_cycle`'s hold window. - if overrides is not None and overrides.robots: - self.rosparam[str].set("robot", ",".join(overrides.robots)) - resolved_world = world or self._world_manager.loaded_world or self._episodes.current.world run_seed = self.rosparam[str].get("run_seed", "") or self._episodes.run_seed resolved_seed = seed if seed >= 0 else _derive_seed(run_seed, resolved_world, new_id) @@ -977,11 +978,6 @@ def reject(field: str, value: str, enum_cls: type) -> None: existing.keep_modules = False if request.world: existing.world = request.world - if request.robots: - seen = dict.fromkeys(existing.robots) - for r in request.robots: - seen.setdefault(r, None) - existing.robots = list(seen) self._episodes.pending_overrides = existing for p in request.obstacles_params: @@ -1105,6 +1101,10 @@ async def _cb_spawn_robot( pose = self._pose_from_request(request.pose) if request.use_pose else None args = {kv.key: kv.value for kv in request.args} name_out = await self._task.tm_robots.extend(request.model, request.name or None, pose, args=args) + if request.immediate: + async with self._reset_lock: + await self._robots_manager.spawn_now(name_out, pose) + self._robots_manager.publish_queue() self._flip_integrity() response.name = name_out response.success = True @@ -1113,6 +1113,21 @@ async def _cb_spawn_robot( response.error_msg = str(e) return response + async def _cb_despawn_robot( + self, + request: task_generator_msgs.srv.DespawnRobot.Request, + response: task_generator_msgs.srv.DespawnRobot.Response, + ) -> task_generator_msgs.srv.DespawnRobot.Response: + try: + self._robots_manager.remove_pending(request.name) + self._robots_manager.publish_queue() + self._flip_integrity() + response.success = True + except Exception as e: + response.success = False + response.error_msg = str(e) + return response + # ACTION SERVER def _goal_callback(self, goal_request: object) -> GoalResponse: @@ -1378,4 +1393,10 @@ async def _set_up_services(self): self._cb_spawn_robot, ) + self.create_service( + task_generator_msgs.srv.DespawnRobot, + self.service_namespace("runtime", "despawn_robot"), + self._cb_despawn_robot, + ) + self._logger.info("Services set up") diff --git a/task_generator/task_generator/tasks/robots/__init__.py b/task_generator/task_generator/tasks/robots/__init__.py index afbe3763..2222905f 100644 --- a/task_generator/task_generator/tasks/robots/__init__.py +++ b/task_generator/task_generator/tasks/robots/__init__.py @@ -1,5 +1,4 @@ import asyncio -import uuid from task_generator.shared import Pose from task_generator.shared import Robot as RobotEntity @@ -34,9 +33,13 @@ async def reset(self, **kwargs: object) -> None: self._start_poses = {} async def set_position(self, pose: Pose): - """Teleport every robot to ``pose``.""" - for robot_manager in self._ctx.robots.values(): - await robot_manager.move(pose) + """Handle an external pose-estimate override for the robots in this mode. + + Default: teleport only the most recently added robot in scope to ``pose``. + """ + robots = self._ctx.robots + if robots: + await next(reversed(robots.values())).move(pose) async def set_goal(self, pose: Pose): """Dispatch a single-phase GOTO request targeting ``pose`` on every robot.""" @@ -51,7 +54,7 @@ async def extend( args: dict[str, str] | None = None, ) -> str: resolved_pose = pose if pose is not None else await random_placement(self._ctx) - assigned_name = name or f"{model}_{uuid.uuid4().hex[:6]}" + assigned_name = name or self.node._robots_manager.next_name(model) value: dict[str, object] = dict(args or {}) value['model'] = model value['name'] = assigned_name diff --git a/task_generator/task_generator/tasks/task.py b/task_generator/task_generator/tasks/task.py index a3f2373d..b7f955fa 100644 --- a/task_generator/task_generator/tasks/task.py +++ b/task_generator/task_generator/tasks/task.py @@ -1,4 +1,5 @@ import asyncio +import contextlib import typing from collections.abc import Sequence @@ -201,8 +202,10 @@ async def _reset_episode(self, **kwargs: object) -> None: self.node.conf.General.RNG.reseed(int(kwargs["seed"])) - await self.robots_manager.set_up() - await self.robots_manager.launch_pending() + provision = self.node.unpause_window() if self.robots_manager.has_pending_additions else contextlib.nullcontext() + async with provision: + await self.robots_manager.set_up() + await self.robots_manager.launch_pending() await self.environment_manager.before_reset_episode() @@ -220,6 +223,16 @@ async def _reset_episode(self, **kwargs: object) -> None: await self.__tm_robots.reset(**kwargs) + obstacles, dynamic_obstacles = await self.__tm_obstacles.reset(**kwargs) + + async def respawn(): + await asyncio.gather( + self.environment_manager.spawn_dynamic_obstacles(dynamic_obstacles), + self.environment_manager.spawn_obstacles(obstacles), + ) + + await self.environment_manager.respawn(respawn) + robot_outcomes = await asyncio.gather( *( mgr.reset( @@ -237,16 +250,6 @@ async def _reset_episode(self, **kwargs: object) -> None: if isinstance(outcome, BaseException): self._logger.warning(f"robot {name!r} adapter reset failed: {outcome!r}") - obstacles, dynamic_obstacles = await self.__tm_obstacles.reset(**kwargs) - - async def respawn(): - await asyncio.gather( - self.environment_manager.spawn_dynamic_obstacles(dynamic_obstacles), - self.environment_manager.spawn_obstacles(obstacles), - ) - - await self.environment_manager.respawn(respawn) - for module in self.__modules: module.after_reset() finally: diff --git a/task_generator/tests/ros/test_node_services.py b/task_generator/tests/ros/test_node_services.py index 3002af1c..99cb87ec 100644 --- a/task_generator/tests/ros/test_node_services.py +++ b/task_generator/tests/ros/test_node_services.py @@ -297,75 +297,6 @@ def _noop() -> None: assert stub._episodes.pending_overrides.world == "map_b" -# --------------------------------------------------------------------------- -# _cb_queue_episode: robots field union semantics -# --------------------------------------------------------------------------- - - -def test_queue_episode_robots_empty_array_no_op() -> None: - import asyncio - - import task_generator_msgs.srv - from task_generator.node import EpisodeRuntime, TaskGenerator - - stub = type("Stub", (), {})() - stub._episodes = EpisodeRuntime() - stub._staged_obstacles_params = {} - stub._staged_robots_params = {} - - def _noop() -> None: - pass - - stub._publish_queue_state = _noop - - first = task_generator_msgs.srv.QueueEpisode.Request() - first.action = task_generator_msgs.srv.QueueEpisode.Request.MERGE - first.robots = ["robot_a"] - first.keep_modules = True - asyncio.run(TaskGenerator._cb_queue_episode(stub, first, task_generator_msgs.srv.QueueEpisode.Response())) - - second = task_generator_msgs.srv.QueueEpisode.Request() - second.action = task_generator_msgs.srv.QueueEpisode.Request.MERGE - second.robots = [] - second.keep_modules = True - asyncio.run(TaskGenerator._cb_queue_episode(stub, second, task_generator_msgs.srv.QueueEpisode.Response())) - - assert stub._episodes.pending_overrides is not None - assert stub._episodes.pending_overrides.robots == ["robot_a"] - - -def test_queue_episode_robots_union_dedup_order_preserved() -> None: - import asyncio - - import task_generator_msgs.srv - from task_generator.node import EpisodeRuntime, TaskGenerator - - stub = type("Stub", (), {})() - stub._episodes = EpisodeRuntime() - stub._staged_obstacles_params = {} - stub._staged_robots_params = {} - - def _noop() -> None: - pass - - stub._publish_queue_state = _noop - - first = task_generator_msgs.srv.QueueEpisode.Request() - first.action = task_generator_msgs.srv.QueueEpisode.Request.MERGE - first.robots = ["robot_a", "robot_b"] - first.keep_modules = True - asyncio.run(TaskGenerator._cb_queue_episode(stub, first, task_generator_msgs.srv.QueueEpisode.Response())) - - second = task_generator_msgs.srv.QueueEpisode.Request() - second.action = task_generator_msgs.srv.QueueEpisode.Request.MERGE - second.robots = ["robot_b", "robot_c"] - second.keep_modules = True - asyncio.run(TaskGenerator._cb_queue_episode(stub, second, task_generator_msgs.srv.QueueEpisode.Response())) - - assert stub._episodes.pending_overrides is not None - assert stub._episodes.pending_overrides.robots == ["robot_a", "robot_b", "robot_c"] - - # --------------------------------------------------------------------------- # _cb_queue_episode: action != MERGE rejected # --------------------------------------------------------------------------- diff --git a/utils/arena_rclpy_mixins/README.md b/utils/arena_rclpy_mixins/README.md index a7b1a445..dc218b31 100644 --- a/utils/arena_rclpy_mixins/README.md +++ b/utils/arena_rclpy_mixins/README.md @@ -24,7 +24,7 @@ Sourced from `__init__.py` plus the submodules it re-exports. | `ClientWrapper` | class | Async `call_timeout()` / sync `call_timeout_sync()` around a service client | Service calls with timeout logging | | `ActionClientWrapper` | class | `send_goal()`, `send_goal_timeout()`, `await_result()`, `send_and_await()`, `cancel()`, `ensure()` | Action client with timeout wrappers | | `AsyncUtil` | class | `AsyncUtil.timeout(coro, sec)` — `asyncio.wait_for` that returns `None` on timeout | One-off timeout wrapper | -| `AsyncLaunchManager` | class | Async `launch_description()`, `kill_all()` — manages active `LaunchService` tasks | Nodes that programmatically launch sub-processes | +| `AsyncLaunchManager` / `LaunchHandle` | class | `launch_description()` returns a `LaunchHandle` (awaitable to completion; `shutdown()` gracefully terminates child processes); `kill_all()` shuts down all | Nodes that programmatically launch sub-processes | | `Namespace` | class | `str` subclass with `/`-join `__call__`, `.simulation_ns`, `.robot_ns`, `.remove_double_slash()` | Building topic / service paths | | `FrameNamespace` | class | `Namespace` subclass with `.sanitize()` (replaces non-alphanumeric with `_`) | TF frame name construction | | `ParamNamespace` | class | `Namespace` with `.`-join `__call__`, converts to/from slash namespaces | ROS param key construction | diff --git a/utils/arena_rclpy_mixins/arena_rclpy_mixins/Async.py b/utils/arena_rclpy_mixins/arena_rclpy_mixins/Async.py index ef28942c..f8a05e99 100644 --- a/utils/arena_rclpy_mixins/arena_rclpy_mixins/Async.py +++ b/utils/arena_rclpy_mixins/arena_rclpy_mixins/Async.py @@ -19,25 +19,46 @@ T = typing.TypeVar('T') +class LaunchHandle: + """Handle to a running LaunchService, with graceful teardown. + + Cancelling the run_async task only abandons the run loop (LaunchService + breaks out without calling _shutdown), leaving child processes orphaned. + shutdown() emits the Shutdown event so they are terminated properly. + """ + + def __init__(self, service: launch.LaunchService, task: asyncio.Task) -> None: + self.task = task + self._service = service + + def __await__(self) -> typing.Generator[typing.Any, None, int]: + """Await the launch to completion (e.g. until its process exits).""" + return self.task.__await__() + + async def shutdown(self) -> None: + coro = self._service.shutdown() + if coro is not None: + await coro + await asyncio.gather(self.task, return_exceptions=True) + + class AsyncLaunchManager: def __init__(self): - self.active_tasks: set[asyncio.Task] = set() + self.active_launches: set[LaunchHandle] = set() - async def launch_description(self, description: launch.LaunchDescription) -> asyncio.Task: + async def launch_description(self, description: launch.LaunchDescription) -> LaunchHandle: ls = launch.LaunchService(noninteractive=True) ls.include_launch_description(description) task = asyncio.create_task(ls.run_async()) - self.active_tasks.add(task) - task.add_done_callback(self.active_tasks.discard) - return task + handle = LaunchHandle(ls, task) + self.active_launches.add(handle) + task.add_done_callback(lambda _: self.active_launches.discard(handle)) + return handle async def kill_all(self): - if not self.active_tasks: + if not self.active_launches: return - for task in self.active_tasks: - if not task.done(): - task.cancel() - await asyncio.gather(*self.active_tasks, return_exceptions=True) + await asyncio.gather(*(h.shutdown() for h in list(self.active_launches)), return_exceptions=True) class AsyncNode(TimeNode, rclpy.node.Node): @@ -77,6 +98,11 @@ async def _launcher(): asyncio.run_coroutine_threadsafe(_launcher(), self.__loop) + async def do_launch_tracked(self, launch_description: launch.LaunchDescription) -> LaunchHandle: + """Like do_launch, but returns a handle so the caller can gracefully shut down just + this launch group (terminating its child processes) on demand.""" + return await self._launch_manager.launch_description(launch_description) + async def kill_launches(self) -> None: await self._launch_manager.kill_all() diff --git a/utils/msgs/task_generator_msgs/CMakeLists.txt b/utils/msgs/task_generator_msgs/CMakeLists.txt index 1fbd0861..eb516608 100644 --- a/utils/msgs/task_generator_msgs/CMakeLists.txt +++ b/utils/msgs/task_generator_msgs/CMakeLists.txt @@ -18,6 +18,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} msg/EpisodeRecord.msg msg/RobotDescriptor.msg msg/RobotFleet.msg + msg/RobotQueue.msg + srv/DespawnRobot.srv srv/GetTaskModes.srv srv/Pause.srv srv/QueryDynamicObstacles.srv diff --git a/utils/msgs/task_generator_msgs/README.md b/utils/msgs/task_generator_msgs/README.md index 112b0598..73cb88cd 100644 --- a/utils/msgs/task_generator_msgs/README.md +++ b/utils/msgs/task_generator_msgs/README.md @@ -13,7 +13,8 @@ Runtime types (env registry, holds, world confirm, cleanup, purge) live in [`are | `Pause.srv` | Toggle pause from external callers. | | `GetTaskModes.srv` | Return currently active task-mode strings. | | `QueryWorlds.srv` / `QueryScenarios.srv` / `QueryEnvironments.srv` / `QueryParametrizeds.srv` / `QueryRobots.srv` / `QueryStaticObstacles.srv` / `QueryDynamicObstacles.srv` / `QueryTaskModes.srv` | Listing of available shortnames for the corresponding asset class. | -| `SpawnStatic.srv` / `SpawnDynamic.srv` / `SpawnRobot.srv` | Inject a static obstacle / dynamic pedestrian / additional robot into the running episode via `TM_Obstacles.extend` / `TM_Robots.extend`. `SpawnRobot` accepts an optional `args` (`diagnostic_msgs/KeyValue[]`) forwarded to `Robot.parse` (e.g. `mobile`, `mobile.local_planner`, `mobile.agent`). | +| `SpawnStatic.srv` / `SpawnDynamic.srv` / `SpawnRobot.srv` | Inject a static obstacle / dynamic pedestrian / additional robot into the running episode via `TM_Obstacles.extend` / `TM_Robots.extend`. `SpawnRobot` accepts an optional `args` (`diagnostic_msgs/KeyValue[]`) forwarded to `Robot.parse` (e.g. `mobile`, `mobile.local_planner`, `mobile.agent`), and an `immediate` flag that provisions the robot into the live world now (idle) instead of committing on the next reset. | +| `DespawnRobot.srv` | Single fleet-removal surface: stages a live robot for teardown on the next reset, un-stages a queued despawn, or cancels a queued spawn (toggles `state/robots/pending`). | ## Messages (`msg/`) @@ -22,6 +23,7 @@ Runtime types (env registry, holds, world confirm, cleanup, purge) live in [`are | `EpisodeRecord.msg` | One episode: id, world, seed, task modes, `robots[]`, `outcome_state` (`QUEUED` / `RUNNING` / `SUCCESS` / `FAILED` / `SKIPPED` / `FATAL`), `outcome_info` (live status string, may be republished mid-episode via `Task.set_info`), integrity flag, plus `obstacles_params` / `robots_params` (effective per-mode params, with staged dict overlay for queued records). Published latched on `state/episode` and `state/queue`. | | `RobotDescriptor.msg` | Per-robot description (model, ns, frame, capabilities). | | `RobotFleet.msg` | All currently-active `RobotDescriptor`s in the env. Published latched on `state/robots`. | +| `RobotQueue.msg` | Robots staged for spawn/despawn (the pending fleet delta), applied on the next reset. Published latched on `state/robots/pending`. | ## Actions (`action/`) diff --git a/utils/msgs/task_generator_msgs/msg/RobotQueue.msg b/utils/msgs/task_generator_msgs/msg/RobotQueue.msg new file mode 100644 index 00000000..83fa55d6 --- /dev/null +++ b/utils/msgs/task_generator_msgs/msg/RobotQueue.msg @@ -0,0 +1,3 @@ +# Robots staged for spawn/despawn, applied on the next reset. Latched on state/robots/pending. +RobotDescriptor[] spawn # queued additions, name + model, ns/frame assigned at reset +RobotDescriptor[] despawn # queued removals of live robots (full descriptor) diff --git a/utils/msgs/task_generator_msgs/srv/DespawnRobot.srv b/utils/msgs/task_generator_msgs/srv/DespawnRobot.srv new file mode 100644 index 00000000..59830668 --- /dev/null +++ b/utils/msgs/task_generator_msgs/srv/DespawnRobot.srv @@ -0,0 +1,6 @@ +# Remove a robot from the fleet at runtime via TM_Robots removal. +# Instance name as published in RobotFleet. +string name +--- +bool success +string error_msg diff --git a/utils/msgs/task_generator_msgs/srv/QueueEpisode.srv b/utils/msgs/task_generator_msgs/srv/QueueEpisode.srv index 6274dcf5..f1e78bdd 100644 --- a/utils/msgs/task_generator_msgs/srv/QueueEpisode.srv +++ b/utils/msgs/task_generator_msgs/srv/QueueEpisode.srv @@ -13,8 +13,6 @@ bool keep_modules # Empty keeps prior queued value. string world -# Incremental: union with prior queued robots set; dedup; empty array no-op. -string[] robots # Leaf-keyed per-mode params; per-key upsert. rcl_interfaces/Parameter[] obstacles_params diff --git a/utils/msgs/task_generator_msgs/srv/SpawnRobot.srv b/utils/msgs/task_generator_msgs/srv/SpawnRobot.srv index 466d204a..29f1bcd3 100644 --- a/utils/msgs/task_generator_msgs/srv/SpawnRobot.srv +++ b/utils/msgs/task_generator_msgs/srv/SpawnRobot.srv @@ -5,6 +5,8 @@ string name geometry_msgs/PoseStamped pose # False = task mode picks placement. bool use_pose +# True = provision into the live world now (idle), else commit on the next reset. +bool immediate # Forwarded to Robot.parse; unrecognized keys silently ignored. diagnostic_msgs/KeyValue[] args --- diff --git a/utils/msgs/task_generator_msgs/test/test_schemas.py b/utils/msgs/task_generator_msgs/test/test_schemas.py index 889dbf88..93d359c9 100644 --- a/utils/msgs/task_generator_msgs/test/test_schemas.py +++ b/utils/msgs/task_generator_msgs/test/test_schemas.py @@ -147,7 +147,6 @@ def test_queue_episode_srv(): req.tm_modules = ["benchmark", "staged"] req.keep_modules = False req.world = "maze" - req.robots = ["husky"] p = Parameter() p.name = "static.n" @@ -164,7 +163,6 @@ def test_queue_episode_srv(): assert req.tm_modules == ["benchmark", "staged"] assert req.keep_modules is False assert req.world == "maze" - assert list(req.robots) == ["husky"] assert len(req.obstacles_params) == 1 assert req.obstacles_params[0].name == "static.n" @@ -238,6 +236,7 @@ def test_spawn_robot_srv(): req.name = "" req.pose = PoseStamped() req.use_pose = False + req.immediate = True req.args = [ KeyValue(key="mobile.local_planner", value="teb"), KeyValue(key="mobile.agent", value=""), @@ -245,6 +244,7 @@ def test_spawn_robot_srv(): assert req.model == "turtlebot3" assert req.name == "" + assert req.immediate is True assert len(req.args) == 2 assert req.args[0].key == "mobile.local_planner" assert req.args[0].value == "teb" @@ -258,6 +258,35 @@ def test_spawn_robot_srv(): assert res.success is True +def test_despawn_robot_srv(): + from task_generator_msgs.srv import DespawnRobot + + req = DespawnRobot.Request() + req.name = "turtlebot3_0" + + assert req.name == "turtlebot3_0" + + res = DespawnRobot.Response() + res.success = True + res.error_msg = "" + + assert res.success is True + + +def test_robot_queue_msg(): + from task_generator_msgs.msg import RobotDescriptor, RobotQueue + + msg = RobotQueue() + msg.spawn = [RobotDescriptor(name="jackal_1", model="jackal")] + msg.despawn = [RobotDescriptor(name="jackal_0", model="jackal")] + + assert len(msg.spawn) == 1 + assert msg.spawn[0].name == "jackal_1" + assert msg.spawn[0].model == "jackal" + assert len(msg.despawn) == 1 + assert msg.despawn[0].name == "jackal_0" + + def test_run_episode_action_goal(): from task_generator_msgs.action import RunEpisode diff --git a/utils/rviz_utils/config/rviz_default.rviz b/utils/rviz_utils/config/rviz_default.rviz index adb7798c..2cefbdc6 100644 --- a/utils/rviz_utils/config/rviz_default.rviz +++ b/utils/rviz_utils/config/rviz_default.rviz @@ -36,6 +36,9 @@ Panels: - Class: rviz_display_control::Reconciler Name: DisplayControl Topic: {task_generator_node}/state/display_set + - Class: task_generator_gui::FleetPanel + Name: FleetPanel + Target: {task_generator_node} Visualization Manager: Class: "" Displays: [] @@ -124,18 +127,20 @@ Visualization Manager: X: 0 Y: 0 Window Geometry: + DisplayControl: + collapsed: false Displays: collapsed: false + FleetPanel: + collapsed: false Height: 1008 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001c3000005b9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008800fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000055000005b90000012a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002c8000005b9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc00000055000005b9000002e001000029fa000000000100000003fb00000024005400610073006b00470065006e0065007200610074006f007200500061006e0065006c0100000000ffffffff000001a900fffffffb000000260057006f0072006c006400470065006e0065007200610074006f007200500061006e0065006c0100000000ffffffff000001a100fffffffb0000000a005600690065007700730100000669000001170000011800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000b400000004cfc0100000002fb0000000800540069006d0065010000000000000b400000038300fffffffb0000000800540069006d00650100000000000004500000000000000000000006a3000005b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001c300000348fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000328000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c0044006900730070006c006100790043006f006e00740072006f006c01000003690000001a0000001600ffffff00000001000001c400000348fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003b00000348000001bd0100001afa000000000100000004fb00000024005400610073006b00470065006e0065007200610074006f007200500061006e0065006c0100000000ffffffff0000017600fffffffb000000140046006c00650065007400500061006e0065006c0100000000ffffffff000000e100fffffffb000000260057006f0072006c006400470065006e0065007200610074006f007200500061006e0065006c0100000000ffffffff0000010a00fffffffb0000000a005600690065007700730100000669000001170000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004cfc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000003ed0000034800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false TaskGeneratorPanel: collapsed: false - WorldGeneratorPanel: - collapsed: false Time: collapsed: false Tool Properties: @@ -143,5 +148,7 @@ Window Geometry: Views: collapsed: false Width: 1920 + WorldGeneratorPanel: + collapsed: false X: 0 - Y: 0 \ No newline at end of file + Y: 0 diff --git a/utils/task_generator_gui/CMakeLists.txt b/utils/task_generator_gui/CMakeLists.txt index f68546b8..cd5c465a 100644 --- a/utils/task_generator_gui/CMakeLists.txt +++ b/utils/task_generator_gui/CMakeLists.txt @@ -24,12 +24,14 @@ qt5_wrap_cpp(MOC_FILES include/task_generator_gui/task_generator_panel.hpp include/task_generator_gui/spawn_pedestrian_tool.hpp include/task_generator_gui/world_generator_panel.hpp + include/task_generator_gui/fleet_panel.hpp include/Qt-MultiSelectComboBox/MultiSelectComboBox.h ) add_library(task_generator_panel src/task_generator_panel.cpp src/task_generator_panel_getset_params.cpp + src/fleet_panel.cpp src/task_generator_panel_dynamic_params.cpp src/utils/dynamic_param_tree.cpp src/spawn_pedestrian_tool.cpp diff --git a/utils/task_generator_gui/include/task_generator_gui/fleet_panel.hpp b/utils/task_generator_gui/include/task_generator_gui/fleet_panel.hpp new file mode 100644 index 00000000..ffe2249a --- /dev/null +++ b/utils/task_generator_gui/include/task_generator_gui/fleet_panel.hpp @@ -0,0 +1,85 @@ +#ifndef TASK_GENERATOR_GUI_FLEET_PANEL_HPP +#define TASK_GENERATOR_GUI_FLEET_PANEL_HPP + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +#include "task_generator_msgs/srv/query_robots.hpp" +#include "task_generator_msgs/srv/spawn_robot.hpp" +#include "task_generator_msgs/srv/despawn_robot.hpp" +#include "task_generator_msgs/msg/robot_fleet.hpp" +#include "task_generator_msgs/msg/robot_queue.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace task_generator_gui +{ + +class FleetPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit FleetPanel(QWidget* parent = nullptr); + ~FleetPanel() override; + + void onInitialize() override; + void load(const rviz_common::Config& config) override; + + void whenReady(std::function ready_check, + std::function action, + std::chrono::milliseconds period = std::chrono::milliseconds(200)); + +protected: + std::shared_ptr node_ptr; + rclcpp::Node::SharedPtr node; + + std::string task_generator_node; + + rclcpp::Client::SharedPtr query_robots_client; + rclcpp::Client::SharedPtr spawn_robot_client; + rclcpp::Client::SharedPtr despawn_robot_client; + + rclcpp::Subscription::SharedPtr robot_fleet_sub; + rclcpp::Subscription::SharedPtr robot_queue_sub; + + task_generator_msgs::msg::RobotFleet::SharedPtr last_fleet; + task_generator_msgs::msg::RobotQueue::SharedPtr last_pending; + + std::string selected_robot_model; + std::vector robot_models; + + QComboBox* robot_combobox{nullptr}; + QPushButton* spawn_robot_button{nullptr}; + QGroupBox* fleet_group{nullptr}; + QVBoxLayout* fleet_layout{nullptr}; + QGroupBox* queue_group{nullptr}; + QVBoxLayout* queue_layout{nullptr}; + + void setupUi(); + void rebuildFleet(); + void rebuildQueue(); + void sendDespawn(const std::string& name); + +private Q_SLOTS: + void onRobotChanged(const QString& text); + void spawnRobotButtonActivated(); +}; + +} // namespace task_generator_gui + +#endif // TASK_GENERATOR_GUI_FLEET_PANEL_HPP diff --git a/utils/task_generator_gui/include/task_generator_gui/task_generator_panel.hpp b/utils/task_generator_gui/include/task_generator_gui/task_generator_panel.hpp index bd3ea8dd..b4664cf0 100644 --- a/utils/task_generator_gui/include/task_generator_gui/task_generator_panel.hpp +++ b/utils/task_generator_gui/include/task_generator_gui/task_generator_panel.hpp @@ -16,7 +16,6 @@ #include "task_generator_msgs/srv/query_scenarios.hpp" #include "task_generator_msgs/srv/query_task_modes.hpp" #include "task_generator_msgs/srv/query_worlds.hpp" -#include "task_generator_msgs/srv/query_robots.hpp" #include "task_generator_msgs/srv/pause.hpp" #include "task_generator_msgs/srv/reset_episode.hpp" #include "task_generator_msgs/srv/queue_episode.hpp" @@ -87,7 +86,6 @@ namespace task_generator_gui void onInitialize() override; void load(const rviz_common::Config &config) override; - void getRobots(); void getWorlds(); void getTMObstaclesParams(); @@ -96,7 +94,6 @@ namespace task_generator_gui void setTMObstaclesParamsRequest(task_generator_msgs::srv::QueueEpisode::Request &req); void setTMRobotsParamsRequest(task_generator_msgs::srv::QueueEpisode::Request &req); void getParams(); - void setRobot(); // Build a QueueEpisode request from current widget state. task_generator_msgs::srv::QueueEpisode::Request::SharedPtr buildQueueEpisodeRequest(); @@ -156,7 +153,6 @@ namespace task_generator_gui rclcpp::Client::SharedPtr query_dynamic_obstacles_client; rclcpp::Client::SharedPtr query_scenarios_client; rclcpp::Client::SharedPtr query_worlds_client; - rclcpp::Client::SharedPtr query_robots_client; rclcpp::Client::SharedPtr query_task_modes_client; // --- Lifecycle service clients --- @@ -186,10 +182,8 @@ namespace task_generator_gui // --- state/paused subscription (latched) --- rclcpp::Subscription::SharedPtr paused_state_sub; - std::string selected_robot_model; std::string staged_world; - std::vector robot_models; std::vector worlds; std::vector obstacles_modes_; std::vector robots_modes_; @@ -203,8 +197,6 @@ namespace task_generator_gui std::unique_ptr dynamic_param_tree_robots_; // Dirty-tracking flags: set when the user edits a widget. - // The robot combobox is a picker for the Spawn Robot button, not a queued-state - // mirror, so changing it does not mark the panel dirty. bool obstacles_params_dirty_{false}; bool robots_params_dirty_{false}; bool world_dirty_{false}; @@ -229,9 +221,7 @@ namespace task_generator_gui QTreeWidget *robots_tree; QComboBox *obstacles_task_mode_combobox; QComboBox *robot_task_mode_combobox; - QComboBox *robot_combobox; QComboBox *world_combobox; - QPushButton *spawn_robot_button; QPushButton *discard_button; QPushButton *queue_button; @@ -254,9 +244,7 @@ namespace task_generator_gui private Q_SLOTS: void onQueueClicked(); - void spawnRobotButtonActivated(); - void onRobotChanged(const QString &text); void onWorldChanged(const QString &text); void onObstaclesTaskModeChanged(const QString &text); diff --git a/utils/task_generator_gui/rviz_common_plugins.xml b/utils/task_generator_gui/rviz_common_plugins.xml index c8a09c37..257dc1e7 100644 --- a/utils/task_generator_gui/rviz_common_plugins.xml +++ b/utils/task_generator_gui/rviz_common_plugins.xml @@ -8,4 +8,7 @@ Panel for generating worlds: pick algorithm, set seed and world name, edit per-algorithm parameters, then generate and stage the result. + + Spawn, list, and despawn robots in the running fleet at runtime. + diff --git a/utils/task_generator_gui/src/fleet_panel.cpp b/utils/task_generator_gui/src/fleet_panel.cpp new file mode 100644 index 00000000..b64bae7d --- /dev/null +++ b/utils/task_generator_gui/src/fleet_panel.cpp @@ -0,0 +1,289 @@ +#include "task_generator_gui/fleet_panel.hpp" +#include "rviz_common/display_context.hpp" + +#include +#include +#include + +namespace task_generator_gui +{ + +static void clearLayout(QLayout* layout) +{ + QLayoutItem* item; + while ((item = layout->takeAt(0)) != nullptr) + { + if (item->widget()) + item->widget()->deleteLater(); + delete item; + } +} + +FleetPanel::FleetPanel(QWidget* parent) +: Panel(parent) +{ +} + +FleetPanel::~FleetPanel() = default; + +void FleetPanel::onInitialize() +{ + node_ptr = getDisplayContext()->getRosNodeAbstraction().lock(); + node = node_ptr->get_raw_node(); + node->get_logger().set_level(rclcpp::Logger::Level::Warn); +} + +void FleetPanel::load(const rviz_common::Config& config) +{ + rviz_common::Panel::load(config); + + QString result; + if (config.mapGetString("Target", &result)) + task_generator_node = result.toStdString(); + else + task_generator_node = "/task_generator_node"; + + query_robots_client = node->create_client( + task_generator_node + "/query/robots"); + spawn_robot_client = node->create_client( + task_generator_node + "/runtime/spawn_robot"); + despawn_robot_client = node->create_client( + task_generator_node + "/runtime/despawn_robot"); + + { + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local(); + robot_fleet_sub = node->create_subscription( + task_generator_node + "/state/robots", + qos, + [this](const task_generator_msgs::msg::RobotFleet::SharedPtr msg) + { + QMetaObject::invokeMethod(this, [this, msg]() + { + last_fleet = msg; + rebuildFleet(); + }, Qt::QueuedConnection); + }); + } + + { + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local(); + robot_queue_sub = node->create_subscription( + task_generator_node + "/state/robots/pending", + qos, + [this](const task_generator_msgs::msg::RobotQueue::SharedPtr msg) + { + QMetaObject::invokeMethod(this, [this, msg]() + { + last_pending = msg; + rebuildFleet(); + rebuildQueue(); + }, Qt::QueuedConnection); + }); + } + + setupUi(); + + whenReady( + [c = query_robots_client]() { return c->service_is_ready(); }, + [this]() + { + query_robots_client->async_send_request( + std::make_shared(), + [this](rclcpp::Client::SharedFuture f) + { + auto resp = f.get(); + if (!resp) return; + QMetaObject::invokeMethod(this, [this, ids = resp->ids]() + { + robot_models = ids; + if (selected_robot_model.empty() && !robot_models.empty()) + selected_robot_model = robot_models[0]; + QSignalBlocker blocker(robot_combobox); + robot_combobox->clear(); + for (const auto& r : robot_models) + robot_combobox->addItem(QString::fromStdString(r)); + robot_combobox->setCurrentText(QString::fromStdString(selected_robot_model)); + robot_combobox->setEnabled(true); + }, Qt::QueuedConnection); + }); + }); +} + +void FleetPanel::whenReady(std::function ready_check, + std::function action, + std::chrono::milliseconds period) +{ + if (ready_check()) { action(); return; } + auto holder = std::make_shared(); + std::function tick = + [holder, check = std::move(ready_check), act = std::move(action)]() mutable + { + if (!check()) return; + if (*holder) (*holder)->cancel(); + holder->reset(); + act(); + }; + *holder = node->create_wall_timer(period, std::move(tick)); +} + +void FleetPanel::setupUi() +{ + auto* root = new QVBoxLayout(this); + + auto* row = new QHBoxLayout(); + robot_combobox = new QComboBox(); + robot_combobox->addItem("Loading..."); + robot_combobox->setEnabled(false); + connect(robot_combobox, &QComboBox::currentTextChanged, this, &FleetPanel::onRobotChanged); + row->addWidget(robot_combobox, 1); + spawn_robot_button = new QPushButton("Spawn"); + connect(spawn_robot_button, &QPushButton::clicked, this, &FleetPanel::spawnRobotButtonActivated); + row->addWidget(spawn_robot_button); + root->addLayout(row); + + fleet_group = new QGroupBox("Fleet"); + fleet_layout = new QVBoxLayout(); + fleet_group->setLayout(fleet_layout); + root->addWidget(fleet_group, 1); + + queue_group = new QGroupBox("Queued"); + queue_layout = new QVBoxLayout(); + queue_group->setLayout(queue_layout); + queue_group->setVisible(false); + root->addWidget(queue_group); +} + +void FleetPanel::sendDespawn(const std::string& name) +{ + auto req = std::make_shared(); + req->name = name; + despawn_robot_client->async_send_request( + req, + [this, name](rclcpp::Client::SharedFuture f) + { + auto resp = f.get(); + if (resp && !resp->success) + RCLCPP_WARN(node->get_logger(), + "despawn_robot failed (%s): %s", + name.c_str(), resp->error_msg.c_str()); + }); +} + +void FleetPanel::rebuildFleet() +{ + if (!fleet_layout || !last_fleet) + return; + + std::set pending_despawn; + if (last_pending) + for (const auto& robot : last_pending->despawn) + pending_despawn.insert(robot.name); + + clearLayout(fleet_layout); + + for (const auto& robot : last_fleet->robots) + { + auto row_widget = new QWidget(); + auto row_layout = new QHBoxLayout(); + row_layout->setContentsMargins(0, 0, 0, 0); + auto label = new QLabel( + QString::fromStdString(robot.name) + + " (" + QString::fromStdString(robot.model) + ")"); + auto btn = new QPushButton(); + std::string name = robot.name; + if (pending_despawn.count(name)) + { + btn->setText("pending..."); + btn->setEnabled(false); + } + else + { + btn->setText("Despawn"); + connect(btn, &QPushButton::clicked, this, [this, name, btn]() + { + btn->setEnabled(false); + sendDespawn(name); + }); + } + row_layout->addWidget(label); + row_layout->addStretch(); + row_layout->addWidget(btn); + row_widget->setLayout(row_layout); + fleet_layout->addWidget(row_widget); + } + fleet_layout->addStretch(); +} + +void FleetPanel::rebuildQueue() +{ + if (!queue_layout) + return; + + clearLayout(queue_layout); + + auto add_row = [this](const QString& prefix, const std::string& name, const std::string& model) + { + auto row_widget = new QWidget(); + auto row_layout = new QHBoxLayout(); + row_layout->setContentsMargins(0, 0, 0, 0); + auto label = new QLabel( + prefix + " " + QString::fromStdString(name) + + " (" + QString::fromStdString(model) + ")"); + auto btn = new QPushButton("Cancel"); + std::string n = name; + connect(btn, &QPushButton::clicked, this, [this, n, btn]() + { + btn->setEnabled(false); + sendDespawn(n); + }); + row_layout->addWidget(label); + row_layout->addStretch(); + row_layout->addWidget(btn); + row_widget->setLayout(row_layout); + queue_layout->addWidget(row_widget); + }; + + bool empty = !last_pending || (last_pending->spawn.empty() && last_pending->despawn.empty()); + if (last_pending) + { + for (const auto& robot : last_pending->spawn) + add_row("+", robot.name, robot.model); + for (const auto& robot : last_pending->despawn) + add_row("-", robot.name, robot.model); + } + queue_group->setVisible(!empty); + queue_layout->addStretch(); +} + +void FleetPanel::onRobotChanged(const QString& text) +{ + selected_robot_model = text.toStdString(); +} + +void FleetPanel::spawnRobotButtonActivated() +{ + if (selected_robot_model.empty()) + return; + + auto req = std::make_shared(); + req->model = selected_robot_model; + req->name = ""; + req->use_pose = false; + + spawn_robot_client->async_send_request( + req, + [this](rclcpp::Client::SharedFuture f) + { + auto resp = f.get(); + if (resp && !resp->success) + RCLCPP_WARN(node->get_logger(), + "spawn_robot failed: %s", resp->error_msg.c_str()); + }); +} + +} // namespace task_generator_gui + +#include +PLUGINLIB_EXPORT_CLASS(task_generator_gui::FleetPanel, rviz_common::Panel) diff --git a/utils/task_generator_gui/src/task_generator_panel.cpp b/utils/task_generator_gui/src/task_generator_panel.cpp index 204c2e86..1bd6ed3c 100644 --- a/utils/task_generator_gui/src/task_generator_panel.cpp +++ b/utils/task_generator_gui/src/task_generator_panel.cpp @@ -46,8 +46,6 @@ namespace task_generator_gui task_generator_node + "/query/scenarios"); query_worlds_client = node->create_client( task_generator_node + "/query/worlds"); - query_robots_client = node->create_client( - task_generator_node + "/query/robots"); query_task_modes_client = node->create_client( task_generator_node + "/query/task_modes"); @@ -145,31 +143,6 @@ namespace task_generator_gui // Bootstrap queries are gated on service readiness so they survive the // race where rviz loads before task_generator_node has advertised. - whenReady( - [c = query_robots_client]() { return c->service_is_ready(); }, - [this]() - { - query_robots_client->async_send_request( - std::make_shared(), - [this](rclcpp::Client::SharedFuture f) - { - auto resp = f.get(); - if (!resp) return; - QMetaObject::invokeMethod(this, [this, ids = resp->ids]() - { - robot_models = ids; - if (selected_robot_model.empty() && !robot_models.empty()) - selected_robot_model = robot_models[0]; - QSignalBlocker blocker(robot_combobox); - robot_combobox->clear(); - for (const auto &r : robot_models) - robot_combobox->addItem(QString::fromStdString(r)); - robot_combobox->setCurrentText(QString::fromStdString(selected_robot_model)); - robot_combobox->setEnabled(true); - }, Qt::QueuedConnection); - }); - }); - whenReady( [c = query_worlds_client]() { return c->service_is_ready(); }, [this]() @@ -301,14 +274,6 @@ namespace task_generator_gui void TaskGeneratorPanel::setupUi() { - // Robot combobox — disabled until robots arrive. - robot_combobox = setupComboBoxWithLabel(this->root_layout, QStringList{"Loading..."}, QString("Robot")); - robot_combobox->setEnabled(false); - connect(robot_combobox, &QComboBox::currentTextChanged, this, &TaskGeneratorPanel::onRobotChanged); - spawn_robot_button = new QPushButton("Spawn Robot"); - connect(spawn_robot_button, &QPushButton::clicked, this, &TaskGeneratorPanel::spawnRobotButtonActivated); - robot_combobox->parentWidget()->layout()->addWidget(spawn_robot_button); - // World combobox — disabled until worlds arrive. world_combobox = setupComboBoxWithLabel(this->root_layout, QStringList{"Loading..."}, QString("World")); world_combobox->setEnabled(false); @@ -479,11 +444,6 @@ namespace task_generator_gui return tree; } - void TaskGeneratorPanel::onRobotChanged(const QString &text) - { - selected_robot_model = text.toStdString(); - } - void TaskGeneratorPanel::onWorldChanged(const QString &text) { staged_world = text.toStdString(); @@ -692,16 +652,6 @@ namespace task_generator_gui dynamic_param_tree_robots_->rebuild("task." + new_mode); } - if (!rec.robots.empty()) - { - selected_robot_model = rec.robots.back(); - if (robot_combobox) - { - QSignalBlocker b(robot_combobox); - robot_combobox->setCurrentText(QString::fromStdString(selected_robot_model)); - } - } - for (const auto &p : rec.obstacles_params) { auto it = param_widgets_obstacles_.find(p.name); @@ -820,11 +770,6 @@ namespace task_generator_gui } } - void TaskGeneratorPanel::spawnRobotButtonActivated() - { - setRobot(); - } - } // namespace task_generator_gui #include diff --git a/utils/task_generator_gui/src/task_generator_panel_getset_params.cpp b/utils/task_generator_gui/src/task_generator_panel_getset_params.cpp index b703f3a6..02b98d52 100644 --- a/utils/task_generator_gui/src/task_generator_panel_getset_params.cpp +++ b/utils/task_generator_gui/src/task_generator_panel_getset_params.cpp @@ -10,31 +10,6 @@ namespace task_generator_gui { - void TaskGeneratorPanel::getRobots() - { - query_robots_client->async_send_request( - std::make_shared(), - [this](rclcpp::Client::SharedFuture f) - { - auto resp = f.get(); - if (!resp) return; - QMetaObject::invokeMethod(this, [this, ids = resp->ids]() - { - robot_models = ids; - if (selected_robot_model.empty() && !robot_models.empty()) - selected_robot_model = robot_models[0]; - if (robot_combobox) - { - QSignalBlocker blocker(robot_combobox); - robot_combobox->clear(); - for (const auto &r : robot_models) - robot_combobox->addItem(QString::fromStdString(r)); - robot_combobox->setCurrentText(QString::fromStdString(selected_robot_model)); - } - }, Qt::QueuedConnection); - }); - } - void TaskGeneratorPanel::getWorlds() { query_worlds_client->async_send_request( @@ -93,7 +68,6 @@ namespace task_generator_gui void TaskGeneratorPanel::getParams() { - getRobots(); getWorlds(); updateTabs(); getTMObstaclesParams(); @@ -130,28 +104,6 @@ namespace task_generator_gui }); } - void TaskGeneratorPanel::setRobot() - { - if (selected_robot_model.empty()) - return; - - auto req = std::make_shared(); - req->action = task_generator_msgs::srv::QueueEpisode::Request::MERGE; - req->keep_modules = true; - req->robots = {selected_robot_model}; - - queue_episode_client->async_send_request( - req, - [this](rclcpp::Client::SharedFuture f) - { - auto resp = f.get(); - if (resp && !resp->success) - RCLCPP_WARN(node->get_logger(), - "queue_episode rejected (spawn robot): %s", - resp->error_msg.c_str()); - }); - } - std::vector TaskGeneratorPanel::convert(const QStringList &qList) { std::vector result; diff --git a/utils/task_generator_mcp/README.md b/utils/task_generator_mcp/README.md index 1089b46c..51089ea0 100644 --- a/utils/task_generator_mcp/README.md +++ b/utils/task_generator_mcp/README.md @@ -55,7 +55,7 @@ Transport: stdio (v0). Works with Claude Desktop, `mcp-cli`, and any stdio-MCP c |---|---|---|---| | `runtime_spawn_static` | `model: str`, `pose?: {position, orientation, frame_id}` | `{id, success, error_msg}` | Spawn a static obstacle. Omit pose for random placement. | | `runtime_spawn_dynamic` | `model: str`, `pose?` | `{id, success, error_msg}` | Spawn a dynamic obstacle / pedestrian. | -| `runtime_spawn_robot` | `model: str`, `name?: str`, `pose?` | `{name, success, error_msg}` | Spawn a robot (experimental). | +| `runtime_spawn_robot` | `model: str`, `name?: str`, `pose?`, `immediate?: bool` | `{name, success, error_msg}` | Spawn a robot (experimental). `immediate` provisions it live and idle now, else it commits on the next reset. | Task-mode enum values for `tm_robots` / `tm_obstacles` / `tm_modules` are compiled from `task_generator.constants.Constants.TaskMode.*` at server startup — the tool schema `enum` field always reflects the live codebase. diff --git a/utils/task_generator_mcp/task_generator_mcp/ros_bridge.py b/utils/task_generator_mcp/task_generator_mcp/ros_bridge.py index addcf4b5..f29a1245 100644 --- a/utils/task_generator_mcp/task_generator_mcp/ros_bridge.py +++ b/utils/task_generator_mcp/task_generator_mcp/ros_bridge.py @@ -16,6 +16,7 @@ from task_generator_msgs.action import RunEpisode from task_generator_msgs.msg import EpisodeRecord from task_generator_msgs.srv import ( + DespawnRobot, GetTaskModes, QueryDynamicObstacles, QueryEnvironments, @@ -78,6 +79,7 @@ def _path(relative: str) -> str: self.client_spawn_static: ClientWrapper[SpawnStatic] = self.create_client_wrapper(SpawnStatic, _path("runtime/spawn_static")) self.client_spawn_dynamic: ClientWrapper[SpawnDynamic] = self.create_client_wrapper(SpawnDynamic, _path("runtime/spawn_dynamic")) self.client_spawn_robot: ClientWrapper[SpawnRobot] = self.create_client_wrapper(SpawnRobot, _path("runtime/spawn_robot")) + self.client_despawn_robot: ClientWrapper[DespawnRobot] = self.create_client_wrapper(DespawnRobot, _path("runtime/despawn_robot")) # action client for lifecycle/run_episode self.action_run_episode: ActionClientWrapper[RunEpisode] = self.create_action_client_wrapper(RunEpisode, _path("lifecycle/run_episode")) diff --git a/utils/task_generator_mcp/task_generator_mcp/tools.py b/utils/task_generator_mcp/task_generator_mcp/tools.py index 6ac977c6..f64ac2bb 100644 --- a/utils/task_generator_mcp/task_generator_mcp/tools.py +++ b/utils/task_generator_mcp/task_generator_mcp/tools.py @@ -13,6 +13,7 @@ from task_generator.constants import Constants from task_generator_msgs.action import RunEpisode from task_generator_msgs.srv import ( + DespawnRobot, GetTaskModes, QueryDynamicObstacles, QueryEnvironments, @@ -217,7 +218,7 @@ async def list_tools() -> list[Tool]: ), Tool( name="config_queue_episode", - description="Queue episode configuration. Empty string for any field preserves the prior queued value. tm_modules replaces the current set unless keep_modules=true. robots is unioned with the prior queued robots set.", + description="Queue episode configuration. Empty string for any field preserves the prior queued value. tm_modules replaces the current set unless keep_modules=true.", inputSchema={ "type": "object", "properties": { @@ -230,11 +231,6 @@ async def list_tools() -> list[Tool]: }, "keep_modules": {"type": "boolean", "default": False, "description": "When true, tm_modules is ignored and the prior queued module set is preserved."}, "world": {"type": "string", "default": "", "description": "World id for the queued episode. Empty keeps prior queued value."}, - "robots": { - "type": "array", - "items": {"type": "string"}, - "description": "Robot model names to union into the queued robots set. Empty array is a no-op.", - }, }, }, ), @@ -301,10 +297,22 @@ async def list_tools() -> list[Tool]: "model": {"type": "string", "description": "Robot model shortname (see query_robots)."}, "name": {"type": "string", "default": "", "description": "Robot name; empty auto-generates."}, "pose": {**_POSE_SCHEMA, "description": "Optional spawn pose. Omit for placement by active task mode."}, + "immediate": {"type": "boolean", "default": False, "description": "Provision into the live world now (idle), else commit on the next reset."}, }, "required": ["model"], }, ), + Tool( + name="runtime_despawn_robot", + description="Despawn a robot by name.", + inputSchema={ + "type": "object", + "properties": { + "name": {"type": "string", "description": "Robot name to despawn."}, + }, + "required": ["name"], + }, + ), ] @server.call_tool() @@ -416,8 +424,6 @@ async def _dispatch(name: str, args: dict[str, object], bridge: RosBridge) -> ob req.tm_modules = list(modules) if modules is not None else [] req.keep_modules = bool(args.get("keep_modules", False)) req.world = args.get("world", "") - robots = args.get("robots") - req.robots = list(robots) if robots is not None else [] resp = await bridge.client_queue_episode.call_timeout(req) if resp is None: return {"error": "timeout"} @@ -487,10 +493,19 @@ async def _dispatch(name: str, args: dict[str, object], bridge: RosBridge) -> ob req = SpawnRobot.Request() req.model = args["model"] req.name = args.get("name", "") + req.immediate = bool(args.get("immediate", False)) req.pose, req.use_pose = bridge.pose_dict_to_msg(args.get("pose")) resp = await bridge.client_spawn_robot.call_timeout(req) if resp is None: return {"error": "timeout"} return {"name": resp.name, "success": resp.success, "error_msg": resp.error_msg} + if name == "runtime_despawn_robot": + req = DespawnRobot.Request() + req.name = args["name"] + resp = await bridge.client_despawn_robot.call_timeout(req) + if resp is None: + return {"error": "timeout"} + return {"success": resp.success, "error_msg": resp.error_msg} + return {"error": f"unknown tool: {name}"}