From 54eab3e7d577091df676cc966012c4a585162e84 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Tue, 9 Jun 2026 17:44:52 +0200 Subject: [PATCH 01/24] =?UTF-8?q?I=20=E2=9D=A4=EF=B8=8F=20ROS4HRI?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- _meta/repos/arena.repos | 10 + .../launch/task_generator.launch.py | 12 +- task_generator/task_generator/node.py | 10 +- .../task_generator/simulators/human/JOINTS.md | 76 +++++ .../task_generator/simulators/human/README.md | 27 +- .../task_generator/simulators/human/gait.py | 240 ++++++++++++++ task_generator/tests/test_gait.py | 146 ++++++++ utils/msgs/arena_people_msgs/CMakeLists.txt | 3 +- .../msgs/arena_people_msgs/msg/Pedestrian.msg | 3 + utils/msgs/arena_people_msgs/package.xml | 1 + .../rerun_utils/renderers/pedestrians.py | 14 +- utils/rviz_utils/package.xml | 8 + utils/rviz_utils/rviz_utils/hri/__init__.py | 5 + utils/rviz_utils/rviz_utils/hri/body_pool.py | 208 ++++++++++++ .../rviz_utils/renderers/pedestrians.py | 32 +- .../rviz_utils/scripts/hri_producer.py | 233 +++++++++++++ .../scripts/pedestrian_marker_publisher.py | 313 ------------------ utils/rviz_utils/setup.py | 4 +- 18 files changed, 970 insertions(+), 375 deletions(-) create mode 100644 task_generator/task_generator/simulators/human/JOINTS.md create mode 100644 task_generator/task_generator/simulators/human/gait.py create mode 100644 task_generator/tests/test_gait.py create mode 100644 utils/rviz_utils/rviz_utils/hri/__init__.py create mode 100644 utils/rviz_utils/rviz_utils/hri/body_pool.py create mode 100644 utils/rviz_utils/rviz_utils/scripts/hri_producer.py delete mode 100644 utils/rviz_utils/rviz_utils/scripts/pedestrian_marker_publisher.py diff --git a/_meta/repos/arena.repos b/_meta/repos/arena.repos index f70afe9c..0e34e8df 100644 --- a/_meta/repos/arena.repos +++ b/_meta/repos/arena.repos @@ -99,3 +99,13 @@ repositories: deps/arena_text_crowd: type: git url: https://github.com/ductaingn/arena_text_crowd.git + + # hri_rviz has no jazzy buildfarm binary, vendored + forked + deps/hri/libhri: + type: git + url: https://github.com/voshch/libhri.git + version: jazzy + deps/hri/hri_rviz: + type: git + url: https://github.com/voshch/hri_rviz.git + version: jazzy diff --git a/task_generator/launch/task_generator.launch.py b/task_generator/launch/task_generator.launch.py index 123a7ff1..e6b57bc5 100644 --- a/task_generator/launch/task_generator.launch.py +++ b/task_generator/launch/task_generator.launch.py @@ -240,18 +240,12 @@ def _restore_terminal_titles(): pedestrian_marker_node = launch_ros.actions.Node( package="rviz_utils", - executable="pedestrian_marker_publisher", - name="pedestrian_marker_publisher", + executable="hri_producer", + name="hri_producer", namespace=os.path.dirname(allocated_ns), parameters=[ {"use_sim_time": True}, - {"body_height": 1.6}, - {"body_radius": 0.25}, - {"head_radius": 0.15}, - {"arrow_length": 0.6}, - {"show_labels": True}, - {"show_velocity_arrows": True}, - {"show_orientation_arrows": True}, + {"max_bodies": 32}, ], output="screen", ) diff --git a/task_generator/task_generator/node.py b/task_generator/task_generator/node.py index 220dc272..3eb3cf11 100644 --- a/task_generator/task_generator/node.py +++ b/task_generator/task_generator/node.py @@ -521,21 +521,19 @@ def _publish_viz_manifest(self) -> None: topic="", topic_type="", kind=DisplayKind.TF, - style_json="", + style_json=StyleSpec(enabled=False).to_json(), topic_must_exist=False, ), ] if human_sim != human_sim.__class__.DUMMY: latched = StyleSpec(extra={"rviz": {"Reliability Policy": "Reliable", "Durability Policy": "Transient Local"}}).to_json() - # Canonical human models, shared by every backend and latched to match the - # converter's TRANSIENT_LOCAL publisher (survives paused resets, replays to late rviz). env_displays.append( AdapterDisplay( name="Pedestrians", - topic=f"{env_ns}/pedestrian_markers", - topic_type="visualization_msgs/MarkerArray", + topic=f"{env_ns}/humans", + topic_type="", kind=DisplayKind.PEDESTRIANS, - style_json=latched, + style_json=StyleSpec().to_json(), topic_must_exist=False, group="Pedestrians", ) diff --git a/task_generator/task_generator/simulators/human/JOINTS.md b/task_generator/task_generator/simulators/human/JOINTS.md new file mode 100644 index 00000000..73c08d68 --- /dev/null +++ b/task_generator/task_generator/simulators/human/JOINTS.md @@ -0,0 +1,76 @@ +# Human skeleton joint contract (ROS4HRI `human_description` rig) + +Frozen interface shared by the gait generator (`GaitGenerator`) and the HRI producer node. +Source of truth: `ros4hri/human_description` `urdf/human-tpl.xacro` (Apache-2.0, pinned). + +## URDF generation (per body) + +The producer generates one URDF per body via xacro: + +``` +xacro /human_description/urdf/human-tpl.xacro id:= height:= +``` + +- Only `id` (string) and `height` (float, default `1.65`) are real xacro args; the other + proportion knobs in `create_human_urdf.py` do not map to xacro args and are inert. +- The generated URDF is set as ROS param `human_description_` and consumed by a + `robot_state_publisher` for that body. Root link is `body_` (REP-155 hip-origin frame). + +## Naming convention + +Every joint/link carries a `_` suffix where **` = str(pedestrian.id)`**. +`sensor_msgs/JointState.name[i]` MUST be the full suffixed name (e.g. `l_r_hip_7`) so it +matches the body's generated URDF. The gait generator takes the agent id and suffixes. + +## Articulated joints (20 revolute; all others fixed) + +Publish a position for ALL 20 each tick (unanimated -> 0.0) so robot_state_publisher does +not warn. Fixed joints (`torso`, `head`, `l/r_wrist`, `l/r_ankle`) are NOT in JointState. + +| # | base name | axis | limits [lo, hi] (rad) | role | +|---|---|---|---|---| +| 1 | `waist` | (0,1,0) | [-0.2, 1.0] | torso forward lean | +| 2 | `r_head` | (1,0,0) | [-1.0, 1.0] | head roll | +| 3 | `y_head` | (0,0,1) | [-1.4, 1.4] | head yaw | +| 4 | `p_head` | (0,-1,0) | [-1.5, 1.5] | head pitch | +| 5 | `l_y_shoulder` | (0,0,-1) | [-1.1, 1.9] | L shoulder yaw | +| 6 | `l_p_shoulder` | (1,0,0) | [-0.4, 3.3] | **L arm sagittal swing** | +| 7 | `l_r_shoulder` | (0,0,1) | [-1.7, 1.5] | L shoulder roll | +| 8 | `l_elbow` | (0,-1,0) | [0.0, 2.5] | **L elbow** | +| 9 | `r_y_shoulder` | (0,0,1) | [-1.1, 1.9] | R shoulder yaw | +| 10 | `r_p_shoulder` | (-1,0,0) | [-0.4, 3.3] | **R arm sagittal swing** | +| 11 | `r_r_shoulder` | (0,0,-1) | [-1.7, 1.5] | R shoulder roll | +| 12 | `r_elbow` | (0,-1,0) | [0.0, 2.5] | **R elbow** | +| 13 | `l_y_hip` | (0,0,-1) | [-0.1, 0.6] | L hip yaw | +| 14 | `l_p_hip` | (1,0,0) | [-0.4, 3.3] | L hip abduction | +| 15 | `l_r_hip` | (0,-1,0) | [-0.4, 0.7] | **L leg sagittal swing** | +| 16 | `l_knee` | (0,-1,0) | [-2.5, 0.0] | **L knee** | +| 17 | `r_y_hip` | (0,0,-1) | [-0.1, 0.6] | R hip yaw | +| 18 | `r_p_hip` | (-1,0,0) | [-0.4, 3.3] | R hip abduction | +| 19 | `r_r_hip` | (0,-1,0) | [-0.4, 0.7] | **R leg sagittal swing** | +| 20 | `r_knee` | (0,-1,0) | [-2.5, 0.0] | **R knee** | + +Bold = the gait-driving DOFs. Clamp every emitted value to its `[lo, hi]`. + +## Gait synthesis (recommended starting points; T1 tunes) + +Phase `phi` per agent integrates from speed: `phi += 2*pi*cadence*dt`, where +`cadence ~= clamp(0.4 + 0.55*speed, 0.4, 2.2)` Hz. Legs antiphase; arms contralateral +(arm swings with the opposite leg). Suggested amplitudes: + +- **walk** (`WALKING`, speed-scaled gain `g = clamp(speed/1.2, 0.2, 1.0)`): + - `l_r_hip = 0.45*g*sin(phi)`, `r_r_hip = 0.45*g*sin(phi+pi)` + - knees bend on the back-swing: `l_knee = -0.9*g*max(0, -sin(phi))`, `r_knee` with `phi+pi` + - `l_p_shoulder = 0.35*g*sin(phi+pi)`, `r_p_shoulder = 0.35*g*sin(phi)` (contralateral) + - `l_elbow = 0.3 + 0.2*g`, `r_elbow = 0.3 + 0.2*g` +- **run** (`RUNNING`): same shape, `cadence` higher, amplitudes ~1.6x, elbow bias ~1.2 rad. +- **idle** (`IDLE` and the behavior states `PANIC/SURPRISED/CURIOUS/THREATENING` for now): + near-zero limbs; tiny breathing sway `waist = 0.03*sin(2*pi*0.25*t_phase)`. (Behavior + states get richer posture later; baseline treats them as idle for the rig.) + +State selection comes from `Pedestrian.animation_state`; speed is `hypot(twist.linear.x, y)`. + +## Determinism + +Per-agent phase keyed by id, cleared on despawn. Seed initial `phi` from `id` +(e.g. `(id % 360) * pi/180`) so agents are not in lockstep. Read `dt`, never wall-clock. diff --git a/task_generator/task_generator/simulators/human/README.md b/task_generator/task_generator/simulators/human/README.md index 8d53299f..e82a7bf8 100644 --- a/task_generator/task_generator/simulators/human/README.md +++ b/task_generator/task_generator/simulators/human/README.md @@ -66,28 +66,31 @@ env level (`/`) and shared by every backend: | Topic | Producer | QoS | Role | | --- | --- | --- | --- | -| `arena_peds` | each backend (`publish_arena_peds`) | reliable, volatile | Pedestrian state feed (positions/velocities). Data, not markers. | -| `pedestrian_markers` | `pedestrian_marker_publisher` node | reliable, transient-local, depth 1 | **Canonical** human models (mesh body + orientation/velocity arrows + labels), converted from `arena_peds`. Identical across hunav and arena_humansim. | +| `arena_peds` | each backend (`publish_arena_peds`) | reliable, volatile | Pedestrian state feed (positions/velocities), plus an optional `joint_state` skeleton override (empty by default). Data, not markers. | +| `humans/bodies/tracked`, `humans/persons/*`, `humans/bodies//joint_states` | `hri_producer` node | per REP-155 | **Canonical** ROS4HRI projection of `arena_peds`: id lists, per-person engagement, per-body joint states, per-body URDF on param `human_description_`, TF `body_`. | | `pedestrian_markers/extra` | base class (`publish_markers`) | best-effort, volatile | Backend-internal debug overlay (e.g. arena_humansim forwards its planner viz). Off by default. | | `pedestrian_markers/static` | base class (`publish_static_markers`) | reliable, transient-local, depth 1 | Latched static scene as one combined topic. | | `pedestrian_markers/static_*` | adapter | reliable, transient-local, depth 1 | Latched static scene split per bucket (`/static_walls`, `/static_objects`, ...). | -**Rendering contract.** The canonical human view is `pedestrian_markers`, produced by the standalone -[`pedestrian_marker_publisher`](../../../../utils/rviz_utils/rviz_utils/scripts/pedestrian_marker_publisher.py) -node, which subscribes `arena_peds` and emits mesh humans. It is latched (transient-local, no marker -lifetime) so the models persist across paused resets and replay to a late-joining rviz; the per-frame -`DELETEALL` clears departed pedestrians. Both backends feed the same converter, so the human model is -identical regardless of simulator. `extra` is backend debug, disabled by default; enabling it alongside -the canonical display intentionally double-draws the agents (mesh + debug geometry). +**Rendering contract.** The canonical human view is an animated articulated skeleton, produced by the +[`hri_producer`](../../../../utils/rviz_utils/rviz_utils/scripts/hri_producer.py) node, which subscribes +`arena_peds` and projects it into the ROS4HRI (REP-155) `humans/` namespace: id lists, per-person +engagement, and a per-body `robot_state_publisher` (pooled) driven from `humans/bodies//joint_states` +against the `human_description` URDF rig. The [`hri_rviz/Skeletons3D`](https://github.com/ros4hri/hri_rviz) +display renders one kinematic model per body. Joint states come from `arena_peds.joint_state` when an +upstream backend supplies one (override, e.g. Isaac real bones), otherwise the producer synthesizes a +fallback gait per the rig contract in [`JOINTS.md`](JOINTS.md). `extra` is backend debug, disabled by +default. **Display kinds** (`arena_viz.DisplayKind`): -- `PEDESTRIANS` — the canonical `pedestrian_markers` display only; its renderer assumes the - `pedestrian_*` marker namespaces. +- `PEDESTRIANS` — the canonical `hri_rviz/Skeletons3D` skeleton display, keyed on the env `humans/` + namespace. Note: the upstream display uses absolute `/humans` paths via libhri, so per-env namespacing + is a known limitation. - `MARKER_ARRAY` — generic MarkerArray passthrough, no namespace assumptions; used for `extra` and all `static*` layers. The auto-rviz manifest ([`node.py` `_publish_viz_manifest`](../../node.py)) groups these into a -**Pedestrians** folder (canonical display + `extra`, off) and a separate **Static** folder (`static`, +**Pedestrians** folder (skeleton display + `extra`, off) and a separate **Static** folder (`static`, `static_walls`, `static_objects`). The `pedestrian_markers/` prefix on the debug/static layers is historical, they are overlays, not pedestrian data. diff --git a/task_generator/task_generator/simulators/human/gait.py b/task_generator/task_generator/simulators/human/gait.py new file mode 100644 index 00000000..25858dbc --- /dev/null +++ b/task_generator/task_generator/simulators/human/gait.py @@ -0,0 +1,240 @@ +from __future__ import annotations + +import math +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from builtin_interfaces.msg import Time + from sensor_msgs.msg import JointState +else: + try: + from sensor_msgs.msg import JointState + except ImportError: + JointState = None # type: ignore[assignment,misc] + +# Animation state constants matching Pedestrian.msg +_IDLE = 0 +_WALKING = 1 +_RUNNING = 2 +# PANIC=3, SURPRISED=4, CURIOUS=5, THREATENING=6 -> treated as idle + +# Joint limits: (lo, hi) in radians, ordered to match JOINT_NAMES. +_LIMITS: tuple[tuple[float, float], ...] = ( + (-0.2, 1.0), # waist + (-1.0, 1.0), # r_head + (-1.4, 1.4), # y_head + (-1.5, 1.5), # p_head + (-1.1, 1.9), # l_y_shoulder + (-0.4, 3.3), # l_p_shoulder + (-1.7, 1.5), # l_r_shoulder + (0.0, 2.5), # l_elbow + (-1.1, 1.9), # r_y_shoulder + (-0.4, 3.3), # r_p_shoulder + (-1.7, 1.5), # r_r_shoulder + (0.0, 2.5), # r_elbow + (-0.1, 0.6), # l_y_hip + (-0.4, 3.3), # l_p_hip + (-0.4, 0.7), # l_r_hip + (-2.5, 0.0), # l_knee + (-0.1, 0.6), # r_y_hip + (-0.4, 3.3), # r_p_hip + (-0.4, 0.7), # r_r_hip + (-2.5, 0.0), # r_knee +) + +_IDX: dict[str, int] = {} + + +class GaitGenerator: + """Deterministic per-agent gait synthesis for the ROS4HRI human_description rig.""" + + JOINT_NAMES: tuple[str, ...] = ( + "waist", + "r_head", + "y_head", + "p_head", + "l_y_shoulder", + "l_p_shoulder", + "l_r_shoulder", + "l_elbow", + "r_y_shoulder", + "r_p_shoulder", + "r_r_shoulder", + "r_elbow", + "l_y_hip", + "l_p_hip", + "l_r_hip", + "l_knee", + "r_y_hip", + "r_p_hip", + "r_r_hip", + "r_knee", + ) + + def __init__(self) -> None: + self._phase: dict[int, float] = {} + + def _get_phase(self, agent_id: int) -> float: + if agent_id not in self._phase: + self._phase[agent_id] = (agent_id % 360) * math.pi / 180.0 + return self._phase[agent_id] + + def _set_phase(self, agent_id: int, phi: float) -> None: + self._phase[agent_id] = phi + + def forget(self, agent_id: int) -> None: + """Drop accumulated phase state for a despawned agent.""" + self._phase.pop(agent_id, None) + + def compute( + self, + agent_id: int, + animation_state: int, + speed: float, + dt: float, + ) -> dict[str, float]: + """Return base-joint-name -> angle for all 20 joints, clamped to limits. + + Phase advances by dt each call and is keyed per agent_id. + animation_state: int matching Pedestrian.msg constants (IDLE=0, WALKING=1, RUNNING=2). + """ + angles: dict[str, float] = {name: 0.0 for name in self.JOINT_NAMES} + + if animation_state == _WALKING: + angles = self._gait_walk(agent_id, speed, dt) + elif animation_state == _RUNNING: + angles = self._gait_run(agent_id, speed, dt) + else: + angles = self._gait_idle(agent_id, dt) + + return {name: _clamp(angles.get(name, 0.0), _LIMITS[i][0], _LIMITS[i][1]) for i, name in enumerate(self.JOINT_NAMES)} + + def _gait_walk(self, agent_id: int, speed: float, dt: float) -> dict[str, float]: + cadence = _clamp(0.4 + 0.55 * speed, 0.4, 2.2) + phi = self._get_phase(agent_id) + phi += 2.0 * math.pi * cadence * dt + self._set_phase(agent_id, phi) + + g = _clamp(speed / 1.2, 0.2, 1.0) + + l_r_hip = 0.45 * g * math.sin(phi) + r_r_hip = 0.45 * g * math.sin(phi + math.pi) + l_knee = -0.9 * g * max(0.0, -math.sin(phi)) + r_knee = -0.9 * g * max(0.0, -math.sin(phi + math.pi)) + l_p_shoulder = 0.35 * g * math.sin(phi + math.pi) + r_p_shoulder = 0.35 * g * math.sin(phi) + elbow_bias = 0.3 + 0.2 * g + + return { + "waist": 0.0, + "r_head": 0.0, + "y_head": 0.0, + "p_head": 0.0, + "l_y_shoulder": 0.0, + "l_p_shoulder": l_p_shoulder, + "l_r_shoulder": 0.0, + "l_elbow": elbow_bias, + "r_y_shoulder": 0.0, + "r_p_shoulder": r_p_shoulder, + "r_r_shoulder": 0.0, + "r_elbow": elbow_bias, + "l_y_hip": 0.0, + "l_p_hip": 0.0, + "l_r_hip": l_r_hip, + "l_knee": l_knee, + "r_y_hip": 0.0, + "r_p_hip": 0.0, + "r_r_hip": r_r_hip, + "r_knee": r_knee, + } + + def _gait_run(self, agent_id: int, speed: float, dt: float) -> dict[str, float]: + cadence = _clamp(0.4 + 0.55 * speed, 0.4, 2.2) + phi = self._get_phase(agent_id) + phi += 2.0 * math.pi * cadence * dt + self._set_phase(agent_id, phi) + + g = _clamp(speed / 1.2, 0.2, 1.0) + amp = 1.6 * g + + l_r_hip = 0.45 * amp * math.sin(phi) + r_r_hip = 0.45 * amp * math.sin(phi + math.pi) + l_knee = -0.9 * amp * max(0.0, -math.sin(phi)) + r_knee = -0.9 * amp * max(0.0, -math.sin(phi + math.pi)) + l_p_shoulder = 0.35 * amp * math.sin(phi + math.pi) + r_p_shoulder = 0.35 * amp * math.sin(phi) + elbow_bias = 1.2 + + return { + "waist": 0.0, + "r_head": 0.0, + "y_head": 0.0, + "p_head": 0.0, + "l_y_shoulder": 0.0, + "l_p_shoulder": l_p_shoulder, + "l_r_shoulder": 0.0, + "l_elbow": elbow_bias, + "r_y_shoulder": 0.0, + "r_p_shoulder": r_p_shoulder, + "r_r_shoulder": 0.0, + "r_elbow": elbow_bias, + "l_y_hip": 0.0, + "l_p_hip": 0.0, + "l_r_hip": l_r_hip, + "l_knee": l_knee, + "r_y_hip": 0.0, + "r_p_hip": 0.0, + "r_r_hip": r_r_hip, + "r_knee": r_knee, + } + + def _gait_idle(self, agent_id: int, dt: float) -> dict[str, float]: + phi = self._get_phase(agent_id) + phi += 2.0 * math.pi * 0.25 * dt + self._set_phase(agent_id, phi) + + waist = 0.03 * math.sin(phi) + + return { + "waist": waist, + "r_head": 0.0, + "y_head": 0.0, + "p_head": 0.0, + "l_y_shoulder": 0.0, + "l_p_shoulder": 0.0, + "l_r_shoulder": 0.0, + "l_elbow": 0.0, + "r_y_shoulder": 0.0, + "r_p_shoulder": 0.0, + "r_r_shoulder": 0.0, + "r_elbow": 0.0, + "l_y_hip": 0.0, + "l_p_hip": 0.0, + "l_r_hip": 0.0, + "l_knee": 0.0, + "r_y_hip": 0.0, + "r_p_hip": 0.0, + "r_r_hip": 0.0, + "r_knee": 0.0, + } + + def joint_state( + self, + body_id: str, + angles: dict[str, float], + stamp: Time | None = None, + ) -> JointState: + """Build a sensor_msgs/JointState from a compute() result. + + Joint names are suffixed with _ to match the generated URDF. + """ + msg = JointState() + if stamp is not None: + msg.header.stamp = stamp + msg.name = [f"{name}_{body_id}" for name in self.JOINT_NAMES] + msg.position = [angles[name] for name in self.JOINT_NAMES] + return msg + + +def _clamp(value: float, lo: float, hi: float) -> float: + return max(lo, min(hi, value)) diff --git a/task_generator/tests/test_gait.py b/task_generator/tests/test_gait.py new file mode 100644 index 00000000..c872ce22 --- /dev/null +++ b/task_generator/tests/test_gait.py @@ -0,0 +1,146 @@ +from __future__ import annotations + +import math + +import pytest + +from task_generator.simulators.human.gait import GaitGenerator + +# Animation state constants matching Pedestrian.msg +_IDLE = 0 +_WALKING = 1 +_RUNNING = 2 +_PANIC = 3 +_SURPRISED = 4 +_CURIOUS = 5 +_THREATENING = 6 + +_JOINT_COUNT = 20 + + +@pytest.fixture() +def gen() -> GaitGenerator: + return GaitGenerator() + + +def test_joint_names_count() -> None: + assert len(GaitGenerator.JOINT_NAMES) == _JOINT_COUNT + + +def test_joint_names_unique() -> None: + assert len(set(GaitGenerator.JOINT_NAMES)) == _JOINT_COUNT + + +def test_all_joints_present_walking(gen: GaitGenerator) -> None: + result = gen.compute(agent_id=1, animation_state=_WALKING, speed=1.0, dt=0.05) + assert set(result.keys()) == set(GaitGenerator.JOINT_NAMES) + + +def test_all_joints_present_idle(gen: GaitGenerator) -> None: + result = gen.compute(agent_id=1, animation_state=_IDLE, speed=0.0, dt=0.05) + assert set(result.keys()) == set(GaitGenerator.JOINT_NAMES) + + +def test_all_joints_present_running(gen: GaitGenerator) -> None: + result = gen.compute(agent_id=1, animation_state=_RUNNING, speed=3.0, dt=0.05) + assert set(result.keys()) == set(GaitGenerator.JOINT_NAMES) + + +@pytest.mark.parametrize("state", [_IDLE, _WALKING, _RUNNING, _PANIC, _SURPRISED, _CURIOUS, _THREATENING]) +def test_all_values_finite(gen: GaitGenerator, state: int) -> None: + result = gen.compute(agent_id=2, animation_state=state, speed=1.0, dt=0.05) + for name, val in result.items(): + assert math.isfinite(val), f"{name} is not finite: {val}" + + +_LIMITS: tuple[tuple[float, float], ...] = ( + (-0.2, 1.0), + (-1.0, 1.0), + (-1.4, 1.4), + (-1.5, 1.5), + (-1.1, 1.9), + (-0.4, 3.3), + (-1.7, 1.5), + (0.0, 2.5), + (-1.1, 1.9), + (-0.4, 3.3), + (-1.7, 1.5), + (0.0, 2.5), + (-0.1, 0.6), + (-0.4, 3.3), + (-0.4, 0.7), + (-2.5, 0.0), + (-0.1, 0.6), + (-0.4, 3.3), + (-0.4, 0.7), + (-2.5, 0.0), +) + + +@pytest.mark.parametrize("state", [_IDLE, _WALKING, _RUNNING]) +def test_values_within_limits(gen: GaitGenerator, state: int) -> None: + result = gen.compute(agent_id=3, animation_state=state, speed=1.5, dt=0.05) + for i, name in enumerate(GaitGenerator.JOINT_NAMES): + lo, hi = _LIMITS[i] + val = result[name] + assert lo <= val <= hi, f"{name}={val} outside [{lo}, {hi}]" + + +def test_phase_monotonic_across_calls(gen: GaitGenerator) -> None: + """A gait-driving joint should vary across successive WALKING calls.""" + values: list[float] = [] + for _ in range(30): + result = gen.compute(agent_id=10, animation_state=_WALKING, speed=1.2, dt=0.05) + values.append(result["l_r_hip"]) + # With 30 steps at cadence ~1.06 Hz and dt=0.05, phase advances ~1.6 rad total. + # The driven joint cannot be constant for that arc. + assert max(values) - min(values) > 0.01, "l_r_hip did not change over 30 steps" + + +def test_phase_completes_cycle(gen: GaitGenerator) -> None: + """After a full gait cycle the hip joint should return near its start value.""" + g = GaitGenerator() + cadence = 0.4 + 0.55 * 1.2 # ~1.06 Hz + period = 1.0 / cadence + steps = 200 + dt = period / steps + first = g.compute(agent_id=5, animation_state=_WALKING, speed=1.2, dt=0.0)["l_r_hip"] + for _ in range(steps): + last = g.compute(agent_id=5, animation_state=_WALKING, speed=1.2, dt=dt)["l_r_hip"] + assert math.isclose(first, last, abs_tol=0.05), f"cycle not closed: {first} vs {last}" + + +def test_different_agents_not_in_lockstep() -> None: + """Two agents with different ids must start at different phases.""" + g = GaitGenerator() + r1 = g.compute(agent_id=1, animation_state=_WALKING, speed=1.2, dt=0.0) + r2 = g.compute(agent_id=37, animation_state=_WALKING, speed=1.2, dt=0.0) + assert r1["l_r_hip"] != r2["l_r_hip"], "agents 1 and 37 are in lockstep at t=0" + + +def test_idle_limb_joints_near_zero(gen: GaitGenerator) -> None: + result = gen.compute(agent_id=1, animation_state=_IDLE, speed=0.0, dt=0.0) + limb_joints = [ + "l_r_hip", "r_r_hip", "l_knee", "r_knee", + "l_p_shoulder", "r_p_shoulder", "l_elbow", "r_elbow", + ] + for name in limb_joints: + assert abs(result[name]) < 1e-9, f"idle {name}={result[name]} not near zero" + + +def test_forget_resets_phase(gen: GaitGenerator) -> None: + gen.compute(agent_id=99, animation_state=_WALKING, speed=1.0, dt=0.1) + gen.forget(99) + # After forget, next call re-seeds from id, giving the same value as a fresh generator. + fresh = GaitGenerator() + r_fresh = fresh.compute(agent_id=99, animation_state=_WALKING, speed=1.0, dt=0.0) + r_reset = gen.compute(agent_id=99, animation_state=_WALKING, speed=1.0, dt=0.0) + assert math.isclose(r_fresh["l_r_hip"], r_reset["l_r_hip"], abs_tol=1e-12) + + +def test_behavior_states_treated_as_idle(gen: GaitGenerator) -> None: + """PANIC/SURPRISED/CURIOUS/THREATENING should produce idle-like limb angles.""" + for state in (_PANIC, _SURPRISED, _CURIOUS, _THREATENING): + result = gen.compute(agent_id=1, animation_state=state, speed=0.0, dt=0.0) + assert abs(result["l_r_hip"]) < 1e-9, f"state {state}: l_r_hip not idle" + assert abs(result["r_r_hip"]) < 1e-9, f"state {state}: r_r_hip not idle" diff --git a/utils/msgs/arena_people_msgs/CMakeLists.txt b/utils/msgs/arena_people_msgs/CMakeLists.txt index 28ebf380..9c6c6743 100644 --- a/utils/msgs/arena_people_msgs/CMakeLists.txt +++ b/utils/msgs/arena_people_msgs/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) set(srv_files @@ -30,7 +31,7 @@ set(msg_files rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs + DEPENDENCIES builtin_interfaces geometry_msgs sensor_msgs std_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/utils/msgs/arena_people_msgs/msg/Pedestrian.msg b/utils/msgs/arena_people_msgs/msg/Pedestrian.msg index 0435ce00..d145d91b 100644 --- a/utils/msgs/arena_people_msgs/msg/Pedestrian.msg +++ b/utils/msgs/arena_people_msgs/msg/Pedestrian.msg @@ -18,3 +18,6 @@ uint8 CURIOUS = 5 uint8 THREATENING = 6 uint8 animation_state +# Empty = synthesize fallback gait, non-empty = upstream override. +sensor_msgs/JointState joint_state + diff --git a/utils/msgs/arena_people_msgs/package.xml b/utils/msgs/arena_people_msgs/package.xml index 401d568a..652cdbe1 100644 --- a/utils/msgs/arena_people_msgs/package.xml +++ b/utils/msgs/arena_people_msgs/package.xml @@ -11,6 +11,7 @@ builtin_interfaces geometry_msgs + sensor_msgs std_msgs rosidl_default_generators diff --git a/utils/rerun_utils/rerun_utils/renderers/pedestrians.py b/utils/rerun_utils/rerun_utils/renderers/pedestrians.py index 61b13746..a13ee7c5 100644 --- a/utils/rerun_utils/rerun_utils/renderers/pedestrians.py +++ b/utils/rerun_utils/rerun_utils/renderers/pedestrians.py @@ -1,21 +1,17 @@ -"""DisplayKind.PEDESTRIANS renderer: visualization_msgs/MarkerArray → rerun. +"""DisplayKind.PEDESTRIANS renderer: stub pending rerun HRI integration. -Consumes the converted-marker stream produced by pedestrian_marker_publisher. +TODO: subscribe to {env}/humans/bodies/tracked (hri_msgs/IdsList) and log +per-body poses from TF; rerun has no hri plugin equivalent of hri_rviz/Skeletons3D. """ from __future__ import annotations -from arena_viz import DisplayKind, StyleSpec +from arena_viz import DisplayKind from task_generator_msgs.msg import AdapterDisplay, RobotDescriptor -from rerun_utils.entity_paths import display_path from rerun_utils.renderers._registry import RendererCtx, register -from rerun_utils.renderers.marker_array import subscribe_marker_array @register(DisplayKind.PEDESTRIANS) def render_pedestrians(d: AdapterDisplay, robot: RobotDescriptor | None, ctx: RendererCtx) -> None: - style = StyleSpec.from_json(d.style_json) - if not style.enabled: - return - subscribe_marker_array(ctx, d, display_path(ctx.env_id, robot, d.name)) + pass diff --git a/utils/rviz_utils/package.xml b/utils/rviz_utils/package.xml index 90f43b33..f43a0724 100644 --- a/utils/rviz_utils/package.xml +++ b/utils/rviz_utils/package.xml @@ -65,6 +65,14 @@ std_msgs task_generator_msgs arena_viz + arena_people_msgs + hri_msgs + human_description + robot_state_publisher + tf2_ros + xacro + hri_rviz + task_generator diff --git a/utils/rviz_utils/rviz_utils/hri/__init__.py b/utils/rviz_utils/rviz_utils/hri/__init__.py new file mode 100644 index 00000000..13735d60 --- /dev/null +++ b/utils/rviz_utils/rviz_utils/hri/__init__.py @@ -0,0 +1,5 @@ +"""ROS4HRI helper sub-package for the hri_producer node.""" + +from .body_pool import BodyPool + +__all__ = ["BodyPool"] diff --git a/utils/rviz_utils/rviz_utils/hri/body_pool.py b/utils/rviz_utils/rviz_utils/hri/body_pool.py new file mode 100644 index 00000000..85341cbf --- /dev/null +++ b/utils/rviz_utils/rviz_utils/hri/body_pool.py @@ -0,0 +1,208 @@ +"""Pooled robot_state_publisher lifecycle manager for ROS4HRI body frames. + +Design: POOLED RSP SUBPROCESSES. + +Each slot in the pool holds a pre-spawned robot_state_publisher process. +On body assignment the slot receives a new robot_description (via `--ros-args +-p robot_description:=...`) and a remapped joint_states topic. Recycling +reuses the process slot rather than spawning and tearing down an OS process +each episode, which matters when pedestrian counts oscillate across resets. + +Why not FK-in-producer: robot_state_publisher already performs correct URDF +parsing, joint-limit-free FK, and TF broadcasting. Re-implementing that +inside the producer would duplicate well-tested C++ logic and couple us to +urdf_parser_py version skew. +""" + +from __future__ import annotations + +import dataclasses +import logging +import os +import subprocess +import tempfile +from typing import TYPE_CHECKING + +import xacro +import yaml +from ament_index_python.packages import get_package_share_directory + +if TYPE_CHECKING: + import rclpy.node + +_LOG = logging.getLogger(__name__) + +_DEFAULT_HEIGHT = 1.65 + + +@dataclasses.dataclass +class _Slot: + proc: subprocess.Popen[bytes] + body_id: str | None = None + params_file: str | None = None + + +class BodyPool: + """Fixed-size pool of robot_state_publisher child processes. + + Each slot is assigned to one body_id at a time. Assignment restarts the + rsp with a freshly generated URDF. Clearance sends SIGTERM. Callers must + call `teardown()` on shutdown. + """ + + def __init__( + self, + node: rclpy.node.Node, + humans_ns: str, + max_bodies: int = 32, + ) -> None: + self._node = node + self._humans_ns = humans_ns.rstrip("/") + self._max_bodies = max_bodies + self._slots: list[_Slot] = [] + self._id_to_slot: dict[str, _Slot] = {} + self._urdf: dict[str, str] = {} + self._hd_share: str = get_package_share_directory("human_description") + + def _generate_urdf(self, body_id: str, height: float) -> str: + xacro_path = f"{self._hd_share}/urdf/human-tpl.xacro" + doc = xacro.process_file(xacro_path, mappings={"id": body_id, "height": str(height)}) + return doc.toprettyxml(indent=" ") + + @staticmethod + def _write_params(body_id: str, urdf: str) -> str: + params = {"/**": {"ros__parameters": {"robot_description": urdf, "use_sim_time": True}}} + fd, path = tempfile.mkstemp(prefix=f"rsp_{body_id}_", suffix=".yaml") + with os.fdopen(fd, "w") as handle: + # width disables line-wrapping: rcl's YAML param parser cannot handle the + # backslash continuations PyYAML emits for a long robot_description scalar. + yaml.safe_dump(params, handle, width=10**9) + return path + + @staticmethod + def _cleanup_params(path: str | None) -> None: + if not path: + return + try: + os.unlink(path) + except OSError: + pass + + def _spawn_rsp(self, body_id: str, urdf: str) -> tuple[subprocess.Popen[bytes], str]: + joint_states_topic = f"{self._humans_ns}/bodies/{body_id}/joint_states" + params_file = self._write_params(body_id, urdf) + cmd = [ + "ros2", + "run", + "robot_state_publisher", + "robot_state_publisher", + "--ros-args", + "--log-level", + "warn", + "--params-file", + params_file, + "--remap", + f"__node:=rsp_{body_id}", + "--remap", + f"joint_states:={joint_states_topic}", + ] + proc = subprocess.Popen(cmd, stdout=subprocess.DEVNULL) + _LOG.debug("spawned rsp pid=%d for body %s", proc.pid, body_id) + return proc, params_file + + def _wait_terminate(self, proc: subprocess.Popen[bytes]) -> None: + if proc.returncode is None: + proc.terminate() + try: + proc.wait(timeout=3.0) + except subprocess.TimeoutExpired: + proc.kill() + proc.wait() + + def _terminate_slot(self, slot: _Slot) -> None: + self._wait_terminate(slot.proc) + self._cleanup_params(slot.params_file) + _LOG.debug("terminated rsp pid=%d (was body %s)", slot.proc.pid, slot.body_id) + slot.body_id = None + slot.params_file = None + + def assign(self, body_id: str, height: float = _DEFAULT_HEIGHT) -> bool: + """Assign body_id to a pool slot, starting its rsp. + + Returns True if assigned, False if the pool is exhausted. + """ + if body_id in self._id_to_slot: + return True + + urdf = self._generate_urdf(body_id, height) + self._urdf[body_id] = urdf + + free_slot: _Slot | None = None + for slot in self._slots: + if slot.body_id is None: + free_slot = slot + break + + if free_slot is not None: + self._wait_terminate(free_slot.proc) + self._cleanup_params(free_slot.params_file) + free_slot.proc, free_slot.params_file = self._spawn_rsp(body_id, urdf) + free_slot.body_id = body_id + self._id_to_slot[body_id] = free_slot + return True + + if len(self._slots) >= self._max_bodies: + _LOG.warning( + "body pool exhausted (max_bodies=%d), dropping body %s", + self._max_bodies, + body_id, + ) + return False + + proc, params_file = self._spawn_rsp(body_id, urdf) + slot = _Slot(proc=proc, body_id=body_id, params_file=params_file) + self._slots.append(slot) + self._id_to_slot[body_id] = slot + return True + + def release(self, body_id: str) -> None: + """Release a body_id, terminating its rsp slot.""" + slot = self._id_to_slot.pop(body_id, None) + self._urdf.pop(body_id, None) + if slot is None: + return + self._terminate_slot(slot) + + def urdf_for(self, body_id: str) -> str | None: + return self._urdf.get(body_id) + + def active_ids(self) -> frozenset[str]: + return frozenset(self._id_to_slot.keys()) + + def sync(self, current_ids: set[str], heights: dict[str, float]) -> None: + """Reconcile pool state against the current set of body ids. + + Assigns new ids and releases ids no longer present. + """ + existing = self.active_ids() + for bid in current_ids - existing: + self.assign(bid, heights.get(bid, _DEFAULT_HEIGHT)) + for bid in existing - current_ids: + self.release(bid) + + def teardown(self) -> None: + """Terminate all active rsp processes. Call on node shutdown.""" + for slot in self._slots: + if slot.proc.returncode is None: + slot.proc.terminate() + for slot in self._slots: + try: + slot.proc.wait(timeout=3.0) + except subprocess.TimeoutExpired: + slot.proc.kill() + slot.proc.wait() + for slot in self._slots: + self._cleanup_params(slot.params_file) + self._slots.clear() + self._id_to_slot.clear() + self._urdf.clear() diff --git a/utils/rviz_utils/rviz_utils/renderers/pedestrians.py b/utils/rviz_utils/rviz_utils/renderers/pedestrians.py index 9485e887..853e1c34 100644 --- a/utils/rviz_utils/rviz_utils/renderers/pedestrians.py +++ b/utils/rviz_utils/rviz_utils/renderers/pedestrians.py @@ -1,4 +1,4 @@ -"""Renderer for DisplayKind.PEDESTRIANS (visualization_msgs/MarkerArray).""" +"""Renderer for DisplayKind.PEDESTRIANS (hri_rviz/Skeletons3D).""" from __future__ import annotations @@ -12,32 +12,18 @@ def render_pedestrians(d: AdapterDisplay, robot: RobotDescriptor | None) -> dict[str, object] | None: style = StyleSpec.from_json(d.style_json) rviz_extra: dict[str, object] = style.extra.get("rviz", {}) - reliability = rviz_extra.get("Reliability Policy", "Best Effort") - durability = rviz_extra.get("Durability Policy", "Volatile") - namespaces = rviz_extra.get( - "Namespaces", - { - "pedestrian_meshes": True, - "pedestrian_orientation": True, - "pedestrian_velocity": True, - "pedestrian_labels": True, - }, - ) result: dict[str, object] = { - "Class": "rviz_default_plugins/MarkerArray", + "Class": "hri_rviz/Skeletons3D", "Name": d.name, "Enabled": style.enabled, - "Topic": { - "Value": d.topic, - "Depth": 20, - "History Policy": "Keep Last", - "Reliability Policy": reliability, - "Durability Policy": durability, - }, - "Namespaces": namespaces, - "Value": True, + "Alpha": rviz_extra.get("Alpha", 1.0), + "Update Interval": rviz_extra.get("Update Interval", 0), + "Visual Enabled": rviz_extra.get("Visual Enabled", True), + "Collision Enabled": rviz_extra.get("Collision Enabled", False), + "Tf Prefix": rviz_extra.get("Tf Prefix", ""), + "Namespace": d.topic.removesuffix("/humans"), } for k, v in rviz_extra.items(): - if k not in ("Reliability Policy", "Durability Policy", "Namespaces"): + if k not in ("Alpha", "Update Interval", "Visual Enabled", "Collision Enabled", "Tf Prefix", "Namespace"): result[k] = v return result diff --git a/utils/rviz_utils/rviz_utils/scripts/hri_producer.py b/utils/rviz_utils/rviz_utils/scripts/hri_producer.py new file mode 100644 index 00000000..efdfe37e --- /dev/null +++ b/utils/rviz_utils/rviz_utils/scripts/hri_producer.py @@ -0,0 +1,233 @@ +#!/usr/bin/env python3 +"""ROS4HRI producer node: projects Arena pedestrians into the REP-155 /humans/... representation. + +Replaces pedestrian_marker_publisher. Publishes hri_msgs tracks, per-person +metadata, per-body joint_states, and TF for each body frame. Drives a pool of +robot_state_publisher subprocesses (rviz_utils.hri.body_pool) so hri_rviz can +render animated skeletons. +""" + +from __future__ import annotations + +import math + +import rclpy +from arena_people_msgs.msg import Pedestrian, Pedestrians +from arena_rclpy_mixins.spin import spin_node +from geometry_msgs.msg import TransformStamped +from hri_msgs.msg import EngagementLevel, IdsList +from rclpy.node import Node +from rclpy.qos import ( + QoSDurabilityPolicy, + QoSHistoryPolicy, + QoSProfile, + QoSReliabilityPolicy, +) +from sensor_msgs.msg import JointState +from std_msgs.msg import Bool, Float32, String +from task_generator.simulators.human.gait import GaitGenerator +from tf2_ros import TransformBroadcaster + +from rviz_utils.hri import BodyPool + +_DEFAULT_HEIGHT = 1.65 + +_PEDS_QOS = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.VOLATILE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, +) + +_LATCHED_QOS = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, +) + +_LIVE_QOS = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.VOLATILE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, +) + + +def _engagement_level(animation_state: int) -> EngagementLevel: + """Map Pedestrian.animation_state to hri_msgs/EngagementLevel.""" + msg = EngagementLevel() + if animation_state in (Pedestrian.WALKING, Pedestrian.RUNNING): + msg.level = EngagementLevel.ENGAGED + elif animation_state == Pedestrian.IDLE: + msg.level = EngagementLevel.DISENGAGED + else: + msg.level = EngagementLevel.ENGAGING + return msg + + +class HriProducer(Node): + """Projects arena_people_msgs/Pedestrians into the REP-155 /humans/... API.""" + + def __init__(self) -> None: + super().__init__("hri_producer") + + self._ns = self.get_namespace().rstrip("/") + + env_tag = self._ns.rsplit("/", 1)[-1] or "env" + self._body_prefix = f"{env_tag}_agent_" + + self.declare_parameter("max_bodies", 32) + max_bodies: int = self.get_parameter("max_bodies").value + self._humans_ns = f"{self._ns}/humans" + + self._tf_broadcaster = TransformBroadcaster(self) + self._pool = BodyPool(self, self._humans_ns, max_bodies=max_bodies) + self._gait = GaitGenerator() + self._prev_stamp_sec: dict[str, float] = {} + + self._bodies_tracked_pub = self.create_publisher( + IdsList, f"{self._humans_ns}/bodies/tracked", _LATCHED_QOS + ) + self._persons_tracked_pub = self.create_publisher( + IdsList, f"{self._humans_ns}/persons/tracked", _LATCHED_QOS + ) + + self._body_js_pub: dict[str, rclpy.publisher.Publisher] = {} + self._body_urdf_pub: dict[str, rclpy.publisher.Publisher] = {} + self._person_body_id_pub: dict[str, rclpy.publisher.Publisher] = {} + self._person_anon_pub: dict[str, rclpy.publisher.Publisher] = {} + self._person_conf_pub: dict[str, rclpy.publisher.Publisher] = {} + self._person_eng_pub: dict[str, rclpy.publisher.Publisher] = {} + + self.create_subscription( + Pedestrians, + f"{self._ns}/arena_peds", + self._on_peds, + _PEDS_QOS, + ) + + self.get_logger().info(f"hri_producer ready, env ns={self._ns!r}") + + def _body_id(self, ped_id: int) -> str: + return f"{self._body_prefix}{ped_id}" + + def _ped_id(self, body_id: str) -> int: + return int(body_id.removeprefix(self._body_prefix)) + + def _ensure_body_publishers(self, body_id: str) -> None: + if body_id in self._body_js_pub: + return + self._body_js_pub[body_id] = self.create_publisher( + JointState, + f"{self._humans_ns}/bodies/{body_id}/joint_states", + _LIVE_QOS, + ) + # libhri reads the body URDF from this latched topic, not the param. + urdf_pub = self.create_publisher( + String, f"{self._humans_ns}/bodies/{body_id}/urdf", _LATCHED_QOS + ) + self._body_urdf_pub[body_id] = urdf_pub + urdf = self._pool.urdf_for(body_id) + if urdf is not None: + urdf_pub.publish(String(data=urdf)) + + def _destroy_body_publishers(self, body_id: str) -> None: + for registry in ( + self._body_js_pub, + self._body_urdf_pub, + self._person_body_id_pub, + self._person_anon_pub, + self._person_conf_pub, + self._person_eng_pub, + ): + pub = registry.pop(body_id, None) + if pub is not None: + self.destroy_publisher(pub) + + def _ensure_person_publishers(self, person_id: str) -> None: + if person_id in self._person_body_id_pub: + return + base = f"{self._humans_ns}/persons/{person_id}" + self._person_body_id_pub[person_id] = self.create_publisher( + String, f"{base}/body_id", _LATCHED_QOS + ) + self._person_anon_pub[person_id] = self.create_publisher( + Bool, f"{base}/anonymous", _LATCHED_QOS + ) + self._person_conf_pub[person_id] = self.create_publisher( + Float32, f"{base}/location_confidence", _LIVE_QOS + ) + self._person_eng_pub[person_id] = self.create_publisher( + EngagementLevel, f"{base}/engagement_status", _LIVE_QOS + ) + self._person_body_id_pub[person_id].publish(String(data=person_id)) + self._person_anon_pub[person_id].publish(Bool(data=False)) + + def _on_peds(self, msg: Pedestrians) -> None: + stamp = self.get_clock().now().to_msg() + parent_frame = msg.header.frame_id + + current_ids: set[str] = {self._body_id(ped.id) for ped in msg.pedestrians} + heights: dict[str, float] = {self._body_id(ped.id): _DEFAULT_HEIGHT for ped in msg.pedestrians} + + for bid in self._pool.active_ids() - current_ids: + self._gait.forget(self._ped_id(bid)) + self._prev_stamp_sec.pop(bid, None) + self._destroy_body_publishers(bid) + + self._pool.sync(current_ids, heights) + + ids_list = IdsList(ids=sorted(current_ids)) + self._bodies_tracked_pub.publish(ids_list) + self._persons_tracked_pub.publish(ids_list) + + for ped in msg.pedestrians: + bid = self._body_id(ped.id) + + self._ensure_body_publishers(bid) + self._ensure_person_publishers(bid) + + tf = TransformStamped() + tf.header.stamp = stamp + tf.header.frame_id = parent_frame + tf.child_frame_id = f"body_{bid}" + tf.transform.translation.x = ped.pose.position.x + tf.transform.translation.y = ped.pose.position.y + tf.transform.translation.z = ped.pose.position.z + tf.transform.rotation = ped.pose.orientation + self._tf_broadcaster.sendTransform(tf) + + if ped.joint_state.name: + js = JointState() + js.header.stamp = stamp + js.header.frame_id = "" + js.name = list(ped.joint_state.name) + js.position = list(ped.joint_state.position) + js.velocity = list(ped.joint_state.velocity) + js.effort = list(ped.joint_state.effort) + self._body_js_pub[bid].publish(js) + else: + now_sec = stamp.sec + stamp.nanosec * 1e-9 + prev_sec = self._prev_stamp_sec.get(bid) + dt = (now_sec - prev_sec) if prev_sec is not None and now_sec > prev_sec else 0.1 + self._prev_stamp_sec[bid] = now_sec + speed = math.hypot(ped.twist.linear.x, ped.twist.linear.y) + angles = self._gait.compute(ped.id, ped.animation_state, speed, dt) + self._body_js_pub[bid].publish(self._gait.joint_state(bid, angles, stamp=stamp)) + + self._person_conf_pub[bid].publish(Float32(data=1.0)) + self._person_eng_pub[bid].publish(_engagement_level(ped.animation_state)) + + def destroy_node(self) -> None: + self._pool.teardown() + super().destroy_node() + + +def main(args: list[str] | None = None) -> None: + rclpy.init(args=args) + spin_node(HriProducer()) + + +if __name__ == "__main__": + main() diff --git a/utils/rviz_utils/rviz_utils/scripts/pedestrian_marker_publisher.py b/utils/rviz_utils/rviz_utils/scripts/pedestrian_marker_publisher.py deleted file mode 100644 index d287c934..00000000 --- a/utils/rviz_utils/rviz_utils/scripts/pedestrian_marker_publisher.py +++ /dev/null @@ -1,313 +0,0 @@ -#!/usr/bin/env python3 - -import math - -import rclpy -from arena_people_msgs.msg import Pedestrian, Pedestrians -from arena_rclpy_mixins.spin import spin_node -from geometry_msgs.msg import Vector3 -from rclpy.node import Node -from rclpy.qos import ( - QoSDurabilityPolicy, - QoSHistoryPolicy, - QoSProfile, - QoSReliabilityPolicy, -) -from std_msgs.msg import ColorRGBA, Header -from tf_transformations import quaternion_from_euler, quaternion_multiply -from visualization_msgs.msg import Marker, MarkerArray - - -class PedestrianMarkerPublisher(Node): - """ - Node that converts arena_people_msgs/Pedestrians to visualization_msgs/MarkerArray - for better pedestrian visualization in RViz2 - """ - - def __init__(self): - super().__init__("pedestrian_marker_publisher") - - # Get namespace parameter for multi-environment support - namespace = self.get_namespace() - - # QoS Settings for arena_peds topic - pedestrians_qos = QoSProfile( - reliability=QoSReliabilityPolicy.BEST_EFFORT, - durability=QoSDurabilityPolicy.VOLATILE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=10, - ) - - # QoS Settings for marker output: latched so the human models persist across - # paused resets and replay to a late-joining rviz. Per-frame DELETEALL handles - # removal of departed pedestrians, so the markers carry no self-expiry. - marker_qos = QoSProfile( - reliability=QoSReliabilityPolicy.RELIABLE, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - ) - - # Build namespaced topic names - pedestrians_topic = f"{namespace}/arena_peds" - marker_topic = f"{namespace}/pedestrian_markers" - - # Subscriber to arena_peds topic - self.pedestrians_subscriber = self.create_subscription(Pedestrians, pedestrians_topic, self.pedestrians_callback, pedestrians_qos) - - # Publisher for pedestrian markers - self.marker_publisher = self.create_publisher(MarkerArray, marker_topic, marker_qos) - - # Parameters for visualization - self.declare_parameter("body_height", 1.6) # Height of pedestrian body - self.declare_parameter("body_radius", 0.25) # Radius of pedestrian body - self.declare_parameter("head_radius", 0.15) # Radius of pedestrian head - self.declare_parameter("arrow_length", 0.6) # Length of direction arrow - self.declare_parameter("show_labels", True) # Show name labels - self.declare_parameter("show_velocity_arrows", True) # Show velocity arrows - self.declare_parameter("show_orientation_arrows", True) # Show orientation arrows - self.declare_parameter( - "mesh_resource", - "package://pal_gazebo_worlds/models/citizen_extras_male_03/meshes/mesh.dae", - ) - - self.get_logger().info("Pedestrian Marker Publisher initialized") - self.get_logger().info(f"Subscribing to {pedestrians_topic}, publishing to {marker_topic}") - - def pedestrians_callback(self, msg: Pedestrians): - """Convert Pedestrians message to MarkerArray with stylized human figures""" - - marker_array = MarkerArray() - markers = [] - - # Get parameters - body_height = self.get_parameter("body_height").value - body_radius = self.get_parameter("body_radius").value - arrow_length = self.get_parameter("arrow_length").value - show_labels = self.get_parameter("show_labels").value - show_velocity_arrows = self.get_parameter("show_velocity_arrows").value - show_orientation_arrows = self.get_parameter("show_orientation_arrows").value - - # Clear existing markers first (important for dynamic number of people) - delete_marker = Marker() - delete_marker.header = msg.header - delete_marker.action = Marker.DELETEALL - markers.append(delete_marker) - - for pedestrian in msg.pedestrians: - pedestrian_id = pedestrian.id - - # Determine colors based on animation state - body_color, head_color = self._get_pedestrian_colors(pedestrian) - - # 1. Body (mesh) - body_marker = self._create_body_marker( - pedestrian, - pedestrian_id, - body_color, - body_height, - body_radius, - msg.header, - ) - markers.append(body_marker) - - # 3. Velocity Arrow (if pedestrian is moving) - if show_velocity_arrows: - velocity_magnitude = math.sqrt(pedestrian.twist.linear.x**2 + pedestrian.twist.linear.y**2) - if velocity_magnitude > 0.1: # Only show if moving fast enough - arrow_marker = self._create_velocity_arrow(pedestrian, pedestrian_id, arrow_length, body_height, msg.header) - markers.append(arrow_marker) - - # 4. Orientation Arrow (shows where pedestrian is facing) - if show_orientation_arrows: - orientation_marker = self._create_orientation_arrow(pedestrian, pedestrian_id, arrow_length, body_height, msg.header) - markers.append(orientation_marker) - - # 5. Name Label - if show_labels: - label_marker = self._create_name_label(pedestrian, pedestrian_id, body_height, msg.header) - markers.append(label_marker) - - marker_array.markers = markers - self.marker_publisher.publish(marker_array) - - def _get_pedestrian_colors(self, pedestrian: Pedestrian) -> tuple[ColorRGBA, ColorRGBA]: - """Get colors based on pedestrian animation state""" - - # Map animation states to colors - if pedestrian.animation_state == Pedestrian.WALKING: - body_color = ColorRGBA(r=0.2, g=0.7, b=0.2, a=1.0) # Green - head_color = ColorRGBA(r=0.9, g=0.7, b=0.5, a=1.0) # Skin tone - elif pedestrian.animation_state == Pedestrian.IDLE: - body_color = ColorRGBA(r=0.5, g=0.5, b=0.5, a=1.0) # Gray - head_color = ColorRGBA(r=0.9, g=0.7, b=0.5, a=1.0) # Skin tone - elif pedestrian.animation_state == Pedestrian.PANIC: - body_color = ColorRGBA(r=1.0, g=0.2, b=0.2, a=1.0) # Red - head_color = ColorRGBA(r=1.0, g=0.6, b=0.6, a=1.0) # Light red - elif pedestrian.animation_state == Pedestrian.SURPRISED: - body_color = ColorRGBA(r=1.0, g=1.0, b=0.2, a=1.0) # Yellow - head_color = ColorRGBA(r=0.9, g=0.7, b=0.5, a=1.0) # Skin tone - elif pedestrian.animation_state == Pedestrian.CURIOUS: - body_color = ColorRGBA(r=0.2, g=0.2, b=1.0, a=1.0) # Blue - head_color = ColorRGBA(r=0.9, g=0.7, b=0.5, a=1.0) # Skin tone - elif pedestrian.animation_state == Pedestrian.THREATENING: - body_color = ColorRGBA(r=0.8, g=0.0, b=0.8, a=1.0) # Purple - head_color = ColorRGBA(r=0.9, g=0.7, b=0.5, a=1.0) # Skin tone - else: - # Default colors - body_color = ColorRGBA(r=0.3, g=0.3, b=0.8, a=1.0) # Blue - head_color = ColorRGBA(r=0.9, g=0.7, b=0.5, a=1.0) # Skin tone - - return body_color, head_color - - def _create_body_marker( - self, - pedestrian: Pedestrian, - pedestrian_id: int, - color: ColorRGBA, - body_height: float, - body_radius: float, - header: Header, - ) -> Marker: - """Create mesh marker for pedestrian body""" - marker = Marker() - marker.header = header - marker.ns = "pedestrian_meshes" - marker.id = pedestrian_id - marker.type = Marker.MESH_RESOURCE - marker.mesh_resource = self.get_parameter("mesh_resource").value - marker.mesh_use_embedded_materials = True - marker.action = Marker.ADD - - # Position - use pedestrian position - marker.pose.position.x = pedestrian.pose.position.x - marker.pose.position.y = pedestrian.pose.position.y - marker.pose.position.z = 0.01 # Center - - # Align the mesh model's yaw with the true ped's pose (pi/2 offset) - q_org = [ - pedestrian.pose.orientation.x, - pedestrian.pose.orientation.y, - pedestrian.pose.orientation.z, - pedestrian.pose.orientation.w, - ] - rotation = quaternion_from_euler(0, 0, math.pi / 2) - q = quaternion_multiply(q_org, rotation) - - marker.pose.orientation.x = q[0] - marker.pose.orientation.y = q[1] - marker.pose.orientation.z = q[2] - marker.pose.orientation.w = q[3] - - marker.scale = Vector3(x=body_height, y=body_height, z=body_height) - - # Color - # marker.color = color - - return marker - - def _create_orientation_arrow( - self, - pedestrian: Pedestrian, - pedestrian_id: int, - arrow_length: float, - body_height: float, - header: Header, - ) -> Marker: - """Create arrow marker showing pedestrian orientation""" - marker = Marker() - marker.header = header - marker.ns = "pedestrian_orientation" - marker.id = pedestrian_id - marker.type = Marker.ARROW - marker.action = Marker.ADD - - # Position at pedestrian location - marker.pose.position.x = pedestrian.pose.position.x - marker.pose.position.y = pedestrian.pose.position.y - marker.pose.position.z = body_height / 2 - - # Orientation from pedestrian orientation - marker.pose.orientation = pedestrian.pose.orientation - - # Size - marker.scale = Vector3(x=arrow_length, y=0.15, z=0.15) - - # Color (blue for orientation) - marker.color = ColorRGBA(r=0.0, g=0.0, b=1.0, a=0.8) # Semi-transparent blue - - return marker - - def _create_velocity_arrow( - self, - pedestrian: Pedestrian, - pedestrian_id: int, - arrow_length: float, - body_height: float, - header: Header, - ) -> Marker: - """Create arrow marker showing pedestrian velocity direction""" - marker = Marker() - marker.header = header - marker.ns = "pedestrian_velocity" - marker.id = pedestrian_id - marker.type = Marker.ARROW - marker.action = Marker.ADD - - # Position at pedestrian location - marker.pose.position.x = pedestrian.pose.position.x - marker.pose.position.y = pedestrian.pose.position.y - marker.pose.position.z = body_height / 2 + 0.2 - - # Orientation from velocity direction - vel_x = pedestrian.twist.linear.x - vel_y = pedestrian.twist.linear.y - yaw = math.atan2(vel_y, vel_x) - - marker.pose.orientation.z = math.sin(yaw / 2) - marker.pose.orientation.w = math.cos(yaw / 2) - - # Size (arrow length proportional to speed) - velocity_magnitude = math.sqrt(vel_x**2 + vel_y**2) - actual_length = min(arrow_length * velocity_magnitude, arrow_length) - marker.scale = Vector3(x=actual_length, y=0.1, z=0.1) - - # Color (bright for visibility) - marker.color = ColorRGBA(r=1.0, g=0.5, b=0.0, a=1.0) # Orange - - return marker - - def _create_name_label(self, pedestrian: Pedestrian, pedestrian_id: int, body_height: float, header: Header) -> Marker: - """Create text marker with pedestrian name""" - marker = Marker() - marker.header = header - marker.ns = "pedestrian_labels" - marker.id = pedestrian_id - marker.type = Marker.TEXT_VIEW_FACING - marker.action = Marker.ADD - - # Position above head - marker.pose.position.x = pedestrian.pose.position.x - marker.pose.position.y = pedestrian.pose.position.y - marker.pose.position.z = 0.0 + body_height + 0.5 # Ground + body + label offset - marker.pose.orientation.w = 1.0 - - # Text content - use pedestrian name - marker.text = f"Agent {pedestrian.name}" - - # Size - marker.scale.z = 0.3 # Text height - - # Color (white for visibility) - marker.color = ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0) - - return marker - - -def main(args: list[str] | None = None) -> None: - rclpy.init(args=args) - spin_node(PedestrianMarkerPublisher()) - - -if __name__ == "__main__": - main() diff --git a/utils/rviz_utils/setup.py b/utils/rviz_utils/setup.py index 8721024e..b793710d 100644 --- a/utils/rviz_utils/setup.py +++ b/utils/rviz_utils/setup.py @@ -8,7 +8,7 @@ name=package_name, version='0.0.0', # Packages to export - packages=[package_name, f'{package_name}.renderers'], + packages=[package_name, f'{package_name}.renderers', f'{package_name}.hri'], # Files we want to install, specifically launch files data_files=[ # Install marker file in the package index @@ -49,7 +49,7 @@ 'console_scripts': [ 'rviz_config = rviz_utils.scripts.rviz_config:main', 'visualize_robot_model = rviz_utils.scripts.visualize_robot_model:main', - 'pedestrian_marker_publisher = rviz_utils.scripts.pedestrian_marker_publisher:main', + 'hri_producer = rviz_utils.scripts.hri_producer:main', ], }, ) From c2fcc693ee3c439ba7bc3acebaa83f9ae4f4aff0 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Tue, 9 Jun 2026 22:38:46 +0200 Subject: [PATCH 02/24] old python compat for isaac --- pyproject.toml | 1 + utils/arena_rclpy_mixins/arena_rclpy_mixins/Async.py | 4 ++-- .../arena_rclpy_mixins/ROSParamServer.py | 6 +++--- .../arena_rclpy_mixins/arena_rclpy_mixins/registry.py | 10 +++++----- 4 files changed, 11 insertions(+), 10 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index aa2ba453..25bdb88f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -101,6 +101,7 @@ ignore = ["E501", "E402", "F841", "E731", "PYI051", "E741", "UP035"] [tool.ruff.lint.per-file-ignores] "arena_simulation_setup/**/*.py" = ["UP"] +"utils/arena_rclpy_mixins/**/*.py" = ["UP046", "UP047"] [tool.ruff.lint.flake8-annotations] suppress-none-returning = true diff --git a/utils/arena_rclpy_mixins/arena_rclpy_mixins/Async.py b/utils/arena_rclpy_mixins/arena_rclpy_mixins/Async.py index d83ba7dd..ef28942c 100644 --- a/utils/arena_rclpy_mixins/arena_rclpy_mixins/Async.py +++ b/utils/arena_rclpy_mixins/arena_rclpy_mixins/Async.py @@ -190,7 +190,7 @@ async def timeout(cls, coro: typing.Awaitable[T], timeout_sec: float) -> T | Non ServiceT = typing.TypeVar('ServiceT') -class ClientWrapper[ServiceT]: +class ClientWrapper(typing.Generic[ServiceT]): """Async wrapper around rclpy.client.Client.""" def __init__(self, node: AsyncNode, client: rclpy.client.Client, timeout: float = 60.0): @@ -236,7 +236,7 @@ async def ensure(self, timeout_sec: float | None = None) -> bool: ActionT = typing.TypeVar('ActionT') -class ActionClientWrapper[ActionT]: +class ActionClientWrapper(typing.Generic[ActionT]): """Async wrapper around rclpy.action.ActionClient.""" def __init__(self, node: AsyncNode, action_client: rclpy.action.ActionClient, timeout: float = 60.0): diff --git a/utils/arena_rclpy_mixins/arena_rclpy_mixins/ROSParamServer.py b/utils/arena_rclpy_mixins/arena_rclpy_mixins/ROSParamServer.py index 35a3b781..9e4a2dee 100644 --- a/utils/arena_rclpy_mixins/arena_rclpy_mixins/ROSParamServer.py +++ b/utils/arena_rclpy_mixins/arena_rclpy_mixins/ROSParamServer.py @@ -12,7 +12,7 @@ U = typing.TypeVar('U') -class ROSParamT[T](abc.ABC): +class ROSParamT(abc.ABC, typing.Generic[T]): @abc.abstractmethod def __init__( self, @@ -59,7 +59,7 @@ def destroy(self) -> None: """ -class _ROSParam[T](ROSParamT[T]): +class _ROSParam(ROSParamT[T]): """ Wrapper that handles callbacks. """ @@ -140,7 +140,7 @@ def __init__( counter = 0 -class _rosparam[T]: +class _rosparam(typing.Generic[T]): """ Light-weight stateless interface for singular typed rosparam actions (short-lived). Runtime checks are not performed. diff --git a/utils/arena_rclpy_mixins/arena_rclpy_mixins/registry.py b/utils/arena_rclpy_mixins/arena_rclpy_mixins/registry.py index a3778eb6..457a5c63 100644 --- a/utils/arena_rclpy_mixins/arena_rclpy_mixins/registry.py +++ b/utils/arena_rclpy_mixins/arena_rclpy_mixins/registry.py @@ -10,11 +10,11 @@ import typing -Key = typing.TypeVar('Key') -Value = typing.TypeVar('Value') +K = typing.TypeVar('K') +V = typing.TypeVar('V') -class ClassRegistry[K, V]: +class ClassRegistry(typing.Generic[K, V]): """Lazy sync registry: loader returns a class, result cached on first get. Typical use: @@ -57,7 +57,7 @@ def __contains__(self, key: object) -> bool: return key in self._loaders -class FactoryRegistry[K, V]: +class FactoryRegistry(typing.Generic[K, V]): """Sync factory registry: loader constructs a fresh instance per get. Not cached. Typical use: @@ -97,7 +97,7 @@ def __contains__(self, key: object) -> bool: return key in self._registry -class AsyncFactoryRegistry[K, V]: +class AsyncFactoryRegistry(typing.Generic[K, V]): """Async factory registry: loader constructs a fresh instance per get. Not cached. Typical use: From fd4a04d2c10334071410e0b23f365ade80dcabbf Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Tue, 9 Jun 2026 23:21:18 +0200 Subject: [PATCH 03/24] add gazebo skeletones --- _meta/repos/gazebo.repos | 6 +- .../simulator/sim/gazebo/gazebo.launch.py | 70 +++++++++++++++++++ arena_simulation_setup/README.md | 26 +++++++ .../Common/Pedestrian/arenian/arenian.sdf | 30 ++++---- 4 files changed, 118 insertions(+), 14 deletions(-) diff --git a/_meta/repos/gazebo.repos b/_meta/repos/gazebo.repos index 30dda3e1..276ad2ff 100644 --- a/_meta/repos/gazebo.repos +++ b/_meta/repos/gazebo.repos @@ -1,4 +1,8 @@ -repositories: {} +repositories: + gazebo/arena_gz_plugins: + type: git + url: https://github.com/voshch/arena_gz_plugins.git + version: jazzy # gazebo/ros_gz: # type: git # url: https://github.com/voshch/ros_gz.git diff --git a/arena_bringup/launch/simulator/sim/gazebo/gazebo.launch.py b/arena_bringup/launch/simulator/sim/gazebo/gazebo.launch.py index 3eef1cd9..9d9aeaa8 100644 --- a/arena_bringup/launch/simulator/sim/gazebo/gazebo.launch.py +++ b/arena_bringup/launch/simulator/sim/gazebo/gazebo.launch.py @@ -1,6 +1,9 @@ import itertools import os import subprocess +import sys +import tempfile +import xml.etree.ElementTree as ET from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -43,6 +46,14 @@ def generate_launch_description(): 'headless', ) + # Injects PedSkeletonPlugin into the world SDF so gpu_lidar perceives + # articulated pedestrians. ped_skeleton_enabled:=false to disable. + ped_skeleton_enabled = LaunchArgument( + 'ped_skeleton_enabled', + default_value='true', + description='Enable pedestrian skeletal articulation plugin (PedSkeletonPlugin)', + ) + # Set environment variables package_root = get_package_share_directory('arena_bringup') ss_root = get_package_share_directory('arena_simulation_setup') @@ -120,9 +131,48 @@ def generate_launch_description(): else_value=desired_world, ) + def _inject_ped_skeleton_plugin(world_sdf_path: str, enabled: bool) -> str: + """Return a patched world SDF path with PedSkeletonPlugin injected. + + Parses the world SDF, appends the plugin element to the element, + writes a temp file, and returns its path. When disabled, returns the + original path unchanged. + """ + if not enabled: + return world_sdf_path + + try: + tree = ET.parse(world_sdf_path) + root = tree.getroot() + world_el = root.find('world') + if world_el is None: + return world_sdf_path + + plugin_el = ET.SubElement(world_el, 'plugin') + plugin_el.set('filename', 'PedSkeletonPlugin') + plugin_el.set('name', 'arena_gz_plugins::PedSkeletonPlugin') + enabled_el = ET.SubElement(plugin_el, 'enabled') + enabled_el.text = 'true' + + tmp = tempfile.NamedTemporaryFile( + mode='wb', suffix='.sdf', delete=False, + prefix='arena_world_', + ) + tree.write(tmp, xml_declaration=True, encoding='utf-8') + tmp.close() + return tmp.name + + except Exception as exc: + # Non-fatal: fall back to original world without the plugin. + print(f'[gazebo.launch] PedSkeletonPlugin injection failed: {exc}', file=sys.stderr) + return world_sdf_path + def _launch_gazebo(context, *args, **kwargs): resolved_world = context.perform_substitution(world_path) headless_val = context.perform_substitution(headless.substitution) + skeleton_val = context.perform_substitution(ped_skeleton_enabled.substitution) + skeleton_on = skeleton_val.lower() not in ('false', '0') + resolved_world = _inject_ped_skeleton_plugin(resolved_world, skeleton_on) engine = _select_render_engine() gz_args = resolved_world + f" -r --render-engine {engine}" if headless_val.lower() in ("true", "1"): @@ -158,6 +208,22 @@ def _launch_gazebo(context, *args, **kwargs): }], ) + # Point gz-sim at /lib so it can load PedSkeletonPlugin. + try: + arena_gz_plugins_lib = os.path.join( + get_package_share_directory('arena_gz_plugins'), '..', '..', 'lib' + ) + arena_gz_plugins_lib = os.path.normpath(arena_gz_plugins_lib) + except Exception: + arena_gz_plugins_lib = '' + + existing_plugin_path = os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', '') + gz_plugin_path = ( + f"{arena_gz_plugins_lib}:{existing_plugin_path}" + if arena_gz_plugins_lib and existing_plugin_path + else (arena_gz_plugins_lib or existing_plugin_path) + ) + # Return the LaunchDescription with all the nodes/actions return LaunchDescription( @@ -165,12 +231,16 @@ def _launch_gazebo(context, *args, **kwargs): use_sim_time, world, headless, + ped_skeleton_enabled, # SetEnvironmentVariable( # "GZ_SIM_PHYSICS_ENGINE_PATH", GZ_SIM_PHYSICS_ENGINE_PATH # ), SetEnvironmentVariable( "GZ_SIM_RESOURCE_PATH", GZ_SIM_RESOURCE_PATHS_COMBINED ), + SetEnvironmentVariable( + "GZ_SIM_SYSTEM_PLUGIN_PATH", gz_plugin_path + ), gazebo, # robot_state_publisher, # joint_state_publisher, diff --git a/arena_simulation_setup/README.md b/arena_simulation_setup/README.md index 113af64a..90e44350 100644 --- a/arena_simulation_setup/README.md +++ b/arena_simulation_setup/README.md @@ -30,6 +30,32 @@ pedestrian agents, and environment templates through the types defined here. Scripts live under [scripts/](scripts). +## Pedestrian assets and skeletal articulation + +### arenian actor + +`assets/Common/Pedestrian/arenian/arenian.sdf` defines the default Gazebo +pedestrian as an SDF `` with a `walk.dae` skin (Mingfei/Fuel) and a named +`walk` animation clip. Earlier it was a static ``; the actor form is +required so Gazebo registers the skinned mesh and the clip that `PedSkeletonPlugin` +scrubs. + +**gpu_lidar implication.** Arena's lidar sensor is `gpu_lidar`, a rendering +sensor that forces a server-side render scene even in headless mode. Because +`PedSkeletonPlugin` positions and animates the actor in that scene, the robot's +lidar perceives a moving, articulated body, not a static box or capsule, making +the in-sim animation perceptually load-bearing in headless RL training, not only +cosmetic. + +`PedSkeletonPlugin` (package `arena_gz_plugins`) is the gz-sim 8 C++ plugin that +subscribes `arena_peds`, positions each actor via `TrajectoryPose`, and scrubs the +`walk` clip via `AnimationTime` phased to the ped's speed. gz-sim 8 has no +supported per-bone path for actors, so the true articulated gait stays the +ROS4HRI/rviz and Isaac view. Enabled by default via the `ped_skeleton_enabled` +launch arg in `arena_bringup`. The plugin lives in its own repo, +[arena_gz_plugins](https://github.com/voshch/arena_gz_plugins), pulled into the +workspace via `_meta/repos/gazebo.repos`. + ## Internals Key `Identifier` types from `arena_simulation_setup.tree`: diff --git a/arena_simulation_setup/assets/Common/Pedestrian/arenian/arenian.sdf b/arena_simulation_setup/assets/Common/Pedestrian/arenian/arenian.sdf index f8ac8320..c34b8e0b 100644 --- a/arena_simulation_setup/assets/Common/Pedestrian/arenian/arenian.sdf +++ b/arena_simulation_setup/assets/Common/Pedestrian/arenian/arenian.sdf @@ -1,16 +1,20 @@ - - true - - - 0 0 1.0 0 0 0 - - - https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae - - - - - + + + 0 0 0 0 0 0 + + + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae + 1.0 + + + + + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae + 1.0 + false + + From 512ec6b9824affd2db1e4866924f2101267f71eb Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Wed, 10 Jun 2026 00:47:51 +0200 Subject: [PATCH 04/24] isaac peds update --- arena_isaac | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/arena_isaac b/arena_isaac index abb123a6..619ee5dd 160000 --- a/arena_isaac +++ b/arena_isaac @@ -1 +1 @@ -Subproject commit abb123a6bdeba49e4814b76793abccef35ac436e +Subproject commit 619ee5dd88d0fba654f0416f9097ad4088004b35 From 3bea90cf2743f495b2ba26f85a1f8faa961e1832 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Wed, 10 Jun 2026 00:50:34 +0200 Subject: [PATCH 05/24] full viz --- .../task_generator/simulators/human/README.md | 23 +++++++++++--- .../simulators/human/__init__.py | 27 ++++++++++++++-- .../task_generator/simulators/human/gait.py | 8 ++--- utils/rviz_utils/README.md | 31 +++++++++++++++++++ .../rviz_utils/scripts/hri_producer.py | 9 ++++-- 5 files changed, 83 insertions(+), 15 deletions(-) create mode 100644 utils/rviz_utils/README.md diff --git a/task_generator/task_generator/simulators/human/README.md b/task_generator/task_generator/simulators/human/README.md index e82a7bf8..83b68f07 100644 --- a/task_generator/task_generator/simulators/human/README.md +++ b/task_generator/task_generator/simulators/human/README.md @@ -59,6 +59,17 @@ Every subclass must implement: | `_remove_robot_impl(robots)` | human-sim side: deregister robots | | `_move_robot_impl(robots)` | human-sim side: update robot positions | +## GaitGenerator as articulation ground truth + +`BaseHumanSimulator.publish_arena_peds` is the single point where skeletal +joint angles are committed to the `arena_peds` bus. For each `Pedestrian` in +the outgoing message it checks `ped.joint_state.name`: + +- **non-empty**: upstream backend supplied its own joint state, published unchanged (override path: an upstream producer that already computes joint angles). +- **empty**: `publish_arena_peds` calls `GaitGenerator.compute` + `GaitGenerator.joint_state` and fills the field with bare semantic joint names (20 DOF, ~9 active per gait mode, no body suffix). + +The filled field feeds the ROS4HRI skeleton in rviz through `hri_producer`. The 3D engines do not read it: Isaac animates pedestrians with its native omni.anim.people AnimGraph and Gazebo clip-scrubs `walk.dae`, both driven by pedestrian pose and twist. So `GaitGenerator` is the ROS-side articulation ground truth, while the in-engine meshes play plausible locomotion that is not bone-for-bone identical to it. + ## Visualization topics Pedestrian visualization flows through one data feed plus a few marker layers, all at @@ -66,7 +77,7 @@ env level (`/`) and shared by every backend: | Topic | Producer | QoS | Role | | --- | --- | --- | --- | -| `arena_peds` | each backend (`publish_arena_peds`) | reliable, volatile | Pedestrian state feed (positions/velocities), plus an optional `joint_state` skeleton override (empty by default). Data, not markers. | +| `arena_peds` | each backend (`publish_arena_peds`) | reliable, volatile | Pedestrian state feed (positions/velocities/joint_state). `joint_state` carries bare semantic joint names filled by `GaitGenerator` unless the backend overrides it (non-empty = upstream wins). | | `humans/bodies/tracked`, `humans/persons/*`, `humans/bodies//joint_states` | `hri_producer` node | per REP-155 | **Canonical** ROS4HRI projection of `arena_peds`: id lists, per-person engagement, per-body joint states, per-body URDF on param `human_description_`, TF `body_`. | | `pedestrian_markers/extra` | base class (`publish_markers`) | best-effort, volatile | Backend-internal debug overlay (e.g. arena_humansim forwards its planner viz). Off by default. | | `pedestrian_markers/static` | base class (`publish_static_markers`) | reliable, transient-local, depth 1 | Latched static scene as one combined topic. | @@ -77,10 +88,12 @@ env level (`/`) and shared by every backend: `arena_peds` and projects it into the ROS4HRI (REP-155) `humans/` namespace: id lists, per-person engagement, and a per-body `robot_state_publisher` (pooled) driven from `humans/bodies//joint_states` against the `human_description` URDF rig. The [`hri_rviz/Skeletons3D`](https://github.com/ros4hri/hri_rviz) -display renders one kinematic model per body. Joint states come from `arena_peds.joint_state` when an -upstream backend supplies one (override, e.g. Isaac real bones), otherwise the producer synthesizes a -fallback gait per the rig contract in [`JOINTS.md`](JOINTS.md). `extra` is backend debug, disabled by -default. +display renders one kinematic model per body. + +`hri_producer` is now a relay: it re-suffixes joint names from `arena_peds.joint_state` per body ID and +publishes them directly. The producer's own `GaitGenerator` instance is a **fallback only** for peds whose +`joint_state` arrives empty (e.g. the Isaac adapter, which does not fill joint_state on the bus). +`extra` is backend debug, disabled by default. **Display kinds** (`arena_viz.DisplayKind`): - `PEDESTRIANS` — the canonical `hri_rviz/Skeletons3D` skeleton display, keyed on the env `humans/` diff --git a/task_generator/task_generator/simulators/human/__init__.py b/task_generator/task_generator/simulators/human/__init__.py index cfa1c166..0907f7f2 100644 --- a/task_generator/task_generator/simulators/human/__init__.py +++ b/task_generator/task_generator/simulators/human/__init__.py @@ -3,6 +3,7 @@ import abc import asyncio import itertools +import math import typing from collections.abc import Iterable, Mapping, Sequence @@ -19,6 +20,7 @@ from task_generator.constants import Constants from task_generator.manager.realizer import Realizer from task_generator.shared import Door, DynamicObstacle, Obstacle, Region, Robot, Wall +from task_generator.simulators.human.gait import GaitGenerator from task_generator.simulators.human.utils import ( KnownObstacle, KnownObstacles, @@ -87,6 +89,8 @@ def __init__(self, *args: object, namespace: Namespace, simulator: BaseSim, real ) self._ped_positions_xy: dict[str, tuple[float, float]] = {} + self._gait = GaitGenerator() + self._gait_prev_stamp: dict[int, float] = {} self.node.create_subscription( Pedestrians, self._namespace("arena_peds"), @@ -95,8 +99,27 @@ def __init__(self, *args: object, namespace: Namespace, simulator: BaseSim, real ) self._simulator.attach_human_simulator(self) - def publish_arena_peds(self, msg: Pedestrians): - """Publish pedestrian states.""" + def publish_arena_peds(self, msg: Pedestrians) -> None: + """Fill bare-name joint_state for each ped missing one, then publish.""" + now = self.node.get_clock().now() + now_sec = now.nanoseconds * 1e-9 + stamp = now.to_msg() + + current_ids: set[int] = {ped.id for ped in msg.pedestrians} + for stale in set(self._gait_prev_stamp) - current_ids: + self._gait.forget(stale) + del self._gait_prev_stamp[stale] + + for ped in msg.pedestrians: + if ped.joint_state.name: + continue + prev = self._gait_prev_stamp.get(ped.id) + dt = (now_sec - prev) if prev is not None and now_sec > prev else 0.1 + self._gait_prev_stamp[ped.id] = now_sec + speed = math.hypot(ped.twist.linear.x, ped.twist.linear.y) + angles = self._gait.compute(ped.id, ped.animation_state, speed, dt) + ped.joint_state = self._gait.joint_state(angles, stamp=stamp) + self._arena_peds_publisher.publish(msg) def publish_markers(self, markers: MarkerArray) -> None: diff --git a/task_generator/task_generator/simulators/human/gait.py b/task_generator/task_generator/simulators/human/gait.py index 25858dbc..ee02e959 100644 --- a/task_generator/task_generator/simulators/human/gait.py +++ b/task_generator/task_generator/simulators/human/gait.py @@ -220,18 +220,14 @@ def _gait_idle(self, agent_id: int, dt: float) -> dict[str, float]: def joint_state( self, - body_id: str, angles: dict[str, float], stamp: Time | None = None, ) -> JointState: - """Build a sensor_msgs/JointState from a compute() result. - - Joint names are suffixed with _ to match the generated URDF. - """ + """Build a sensor_msgs/JointState from a compute() result with bare semantic names.""" msg = JointState() if stamp is not None: msg.header.stamp = stamp - msg.name = [f"{name}_{body_id}" for name in self.JOINT_NAMES] + msg.name = list(self.JOINT_NAMES) msg.position = [angles[name] for name in self.JOINT_NAMES] return msg diff --git a/utils/rviz_utils/README.md b/utils/rviz_utils/README.md new file mode 100644 index 00000000..dbd1c388 --- /dev/null +++ b/utils/rviz_utils/README.md @@ -0,0 +1,31 @@ +# rviz_utils + +ROS4HRI visualization bridge and rviz config utilities for Arena. + +## hri_producer + +[`rviz_utils/scripts/hri_producer.py`](rviz_utils/scripts/hri_producer.py) + +Subscribes `/arena_peds` and projects each pedestrian into the +REP-155 `/humans/` namespace: tracked-id lists, per-person engagement, +per-body `joint_states`, URDF latched on `bodies//urdf`, and TF +`body_`. Drives a pool of `robot_state_publisher` subprocesses +([`hri/body_pool.py`](rviz_utils/hri/body_pool.py)) so `hri_rviz/Skeletons3D` +can render animated skeletons. + +**Relay mode (primary path).** When `arena_peds.joint_state.name` is non-empty, +`hri_producer` re-suffixes each bare semantic joint name with the body ID +(`_`) and publishes directly. The joint data originates from +`GaitGenerator` inside `BaseHumanSimulator.publish_arena_peds`. + +**Fallback gait.** When `joint_state` arrives empty (e.g. from the Isaac +adapter, which does not fill joint_state on the bus), `hri_producer` runs its +own `GaitGenerator` instance to synthesize a gait from the pedestrian's speed +and `animation_state`. The fallback applies per-body in the same message, so +mixed messages (some peds with joint_state, some without) are handled correctly. + +Joint name convention: bare names on the bus (`l_r_hip`, `l_knee`, ...) are +suffixed per body before publishing to `joint_states` so they match the +`human_description` URDF rig. See +[`task_generator/simulators/human/gait.py`](../../task_generator/task_generator/simulators/human/gait.py) +for the full 20-joint semantic name set. diff --git a/utils/rviz_utils/rviz_utils/scripts/hri_producer.py b/utils/rviz_utils/rviz_utils/scripts/hri_producer.py index efdfe37e..56b98ea6 100644 --- a/utils/rviz_utils/rviz_utils/scripts/hri_producer.py +++ b/utils/rviz_utils/rviz_utils/scripts/hri_producer.py @@ -202,7 +202,7 @@ def _on_peds(self, msg: Pedestrians) -> None: js = JointState() js.header.stamp = stamp js.header.frame_id = "" - js.name = list(ped.joint_state.name) + js.name = [f"{n}_{bid}" for n in ped.joint_state.name] js.position = list(ped.joint_state.position) js.velocity = list(ped.joint_state.velocity) js.effort = list(ped.joint_state.effort) @@ -214,7 +214,12 @@ def _on_peds(self, msg: Pedestrians) -> None: self._prev_stamp_sec[bid] = now_sec speed = math.hypot(ped.twist.linear.x, ped.twist.linear.y) angles = self._gait.compute(ped.id, ped.animation_state, speed, dt) - self._body_js_pub[bid].publish(self._gait.joint_state(bid, angles, stamp=stamp)) + bare_js = self._gait.joint_state(angles, stamp=stamp) + js = JointState() + js.header = bare_js.header + js.name = [f"{n}_{bid}" for n in bare_js.name] + js.position = list(bare_js.position) + self._body_js_pub[bid].publish(js) self._person_conf_pub[bid].publish(Float32(data=1.0)) self._person_eng_pub[bid].publish(_engagement_level(ped.animation_state)) From 62ea894acb348632aa457c1d8e1695445dfa9b87 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 14:00:34 +0200 Subject: [PATCH 06/24] overlapping waypoints allowed --- .../task_generator/tasks/obstacles/__init__.py | 2 +- .../task_generator/tasks/obstacles/random/impl.py | 13 +++++++++---- .../task_generator/tasks/robots/random/impl.py | 12 +++++++----- 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/task_generator/task_generator/tasks/obstacles/__init__.py b/task_generator/task_generator/tasks/obstacles/__init__.py index 4646bbf1..e39bb820 100644 --- a/task_generator/task_generator/tasks/obstacles/__init__.py +++ b/task_generator/task_generator/tasks/obstacles/__init__.py @@ -29,7 +29,7 @@ async def extend(self, kind: ObstacleKind, model: str, pose: Pose | None = None, obstacle: Obstacle | DynamicObstacle = Obstacle(name=name, model=model, pose=resolved_pose, level_id=level_id) await self._ctx.environment_manager.spawn_obstacles([obstacle]) else: - waypoints = self._ctx.world_manager.get_positions_on_map(n=2, safe_dist=1, level_id=level_id) + waypoints = self._ctx.world_manager.get_positions_on_map(n=2, safe_dist=0, forbid=False, level_id=level_id) obstacle = DynamicObstacle(name=name, model=model, waypoints=waypoints, pose=resolved_pose, level_id=level_id) await self._ctx.environment_manager.spawn_dynamic_obstacles([obstacle]) return obstacle.sim_path diff --git a/task_generator/task_generator/tasks/obstacles/random/impl.py b/task_generator/task_generator/tasks/obstacles/random/impl.py index a611a4f9..320fa251 100644 --- a/task_generator/task_generator/tasks/obstacles/random/impl.py +++ b/task_generator/task_generator/tasks/obstacles/random/impl.py @@ -129,13 +129,18 @@ def index(model: str) -> int: return index waypoints_per_ped = 2 - points = self._ctx.world_manager.get_positions_on_map( - n=N_STATIC_OBSTACLES + N_INTERACTIVE_OBSTACLES + N_DYNAMIC_OBSTACLES * (1 + waypoints_per_ped), + waypoint_points = self._ctx.world_manager.get_positions_on_map( + n=N_DYNAMIC_OBSTACLES * waypoints_per_ped, + safe_dist=0, + forbid=False, + ) + spawn_points = self._ctx.world_manager.get_positions_on_map( + n=N_STATIC_OBSTACLES + N_INTERACTIVE_OBSTACLES + N_DYNAMIC_OBSTACLES, safe_dist=1, ) - positions = map(lambda pos: Pose(pos, orientation=Orientation.from_yaw(2 * np.pi * self.node.conf.General.RNG.value.random())), points[: (N_STATIC_OBSTACLES + N_INTERACTIVE_OBSTACLES + N_DYNAMIC_OBSTACLES)]) - waypoints = iter(points[(N_STATIC_OBSTACLES + N_INTERACTIVE_OBSTACLES + N_DYNAMIC_OBSTACLES) :]) + positions = map(lambda pos: Pose(pos, orientation=Orientation.from_yaw(2 * np.pi * self.node.conf.General.RNG.value.random())), spawn_points) + waypoints = iter(waypoint_points) obstacles: list[Obstacle] = [] diff --git a/task_generator/task_generator/tasks/robots/random/impl.py b/task_generator/task_generator/tasks/robots/random/impl.py index 9137b7d1..995ed517 100644 --- a/task_generator/task_generator/tasks/robots/random/impl.py +++ b/task_generator/task_generator/tasks/robots/random/impl.py @@ -23,14 +23,16 @@ async def reset(self, **kwargs: object) -> None: ) if len(ROBOT_POSITIONS) < len(self._ctx.robots): - to_generate = 2 * (len(self._ctx.robots) - len(ROBOT_POSITIONS)) + n_missing = len(self._ctx.robots) - len(ROBOT_POSITIONS) + rng = self.node.conf.General.RNG.value - orientations = 2 * math.pi * self.node.conf.General.RNG.value.random(to_generate) - positions = self._ctx.world_manager.get_positions_on_map(n=to_generate, safe_dist=biggest_robot) + goal_positions = self._ctx.world_manager.get_positions_on_map(n=n_missing, safe_dist=0) + start_positions = self._ctx.world_manager.get_positions_on_map(n=n_missing, safe_dist=biggest_robot) - generated_positions = [Pose(position, Orientation.from_yaw(orientation)) for (orientation, position) in zip(orientations, positions, strict=False)] + starts = [Pose(p, Orientation.from_yaw(2 * math.pi * rng.random())) for p in start_positions] + goals = [Pose(p, Orientation.from_yaw(2 * math.pi * rng.random())) for p in goal_positions] - ROBOT_POSITIONS += list(zip(generated_positions[::2], generated_positions[1::2], strict=False)) + ROBOT_POSITIONS += list(zip(starts, goals, strict=False)) for robot, pos in zip(self._ctx.robots.values(), ROBOT_POSITIONS, strict=False): self._start_poses[robot.name] = pos[0] From b8d443c5284520f09b1779062feb3113fd4f321c Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 14:03:42 +0200 Subject: [PATCH 07/24] default arena humansim --- task_generator/launch/task_generator.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/task_generator/launch/task_generator.launch.py b/task_generator/launch/task_generator.launch.py index ced06a91..3db33642 100644 --- a/task_generator/launch/task_generator.launch.py +++ b/task_generator/launch/task_generator.launch.py @@ -118,7 +118,7 @@ def generate_launch_description(): human = LaunchArgument( name="human", default_value="", - description="empty = derive from arena_sim ({dummy: dummy, gazebo|isaac: hunav})", + description="empty = derive from arena_sim ({dummy: dummy, gazebo|isaac: arena})", ) robot = LaunchArgument(name="robot", default_value="auto") tm_robots = LaunchArgument(name="tm_robots", default_value="explore") @@ -194,7 +194,7 @@ def _restore_terminal_titles(): human_val = launch.utilities.perform_substitutions( context, launch.utilities.normalize_to_list_of_substitutions(human.substitution) - ) or {"dummy": "dummy", "gazebo": "hunav", "isaac": "hunav"}.get(arena_sim, "dummy") + ) or {"dummy": "dummy", "gazebo": "arena", "isaac": "arena"}.get(arena_sim, "dummy") mobile_val = launch.utilities.perform_substitutions( context, launch.utilities.normalize_to_list_of_substitutions(mobile.substitution) ) or {"dummy": "none"}.get(arena_sim, "nav2") From 2cc9c47b57b6823ad086238b8a57e84aa3e24cdd Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 14:05:51 +0200 Subject: [PATCH 08/24] warn on legacy scenario load --- .../src/arena_simulation_setup/tree/World/Scenario.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/arena_simulation_setup/src/arena_simulation_setup/tree/World/Scenario.py b/arena_simulation_setup/src/arena_simulation_setup/tree/World/Scenario.py index 7566d5e0..d3c93d4c 100644 --- a/arena_simulation_setup/src/arena_simulation_setup/tree/World/Scenario.py +++ b/arena_simulation_setup/src/arena_simulation_setup/tree/World/Scenario.py @@ -141,6 +141,12 @@ def load(self, converter: ArenaConverter = converter) -> Scenario: legacy_exc: Exception try: scenario = self.load_legacy() + warnings.warn( + f"Scenario {self.scenario_path}: new-format load failed, falling back to legacy. Underlying error:\n" + f"{''.join(traceback.format_exception(type(load_exc), load_exc, load_exc.__traceback__))}", + UserWarning, + stacklevel=2, + ) warnings.warn("Loading Scenario in legacy format.", DeprecationWarning, stacklevel=2) return scenario except Exception as e: From cb5fae37c81708ab402f57619ff5033ee127c7ea Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 14:42:58 +0200 Subject: [PATCH 09/24] universal string-based waypoint sampling --- .../arena_simulation_setup/shared/entities.py | 5 +- .../tree/World/World.py | 54 ++++----- .../arena_simulation_setup/utils/cattrs.py | 16 ++- .../arena_simulation_setup/utils/geometry.py | 30 ++++- .../utils/resolution.py | 39 +++++++ .../tests/unit/test_resolution.py | 105 ++++++++++++++++++ .../tasks/obstacles/environment/impl.py | 15 +-- 7 files changed, 221 insertions(+), 43 deletions(-) create mode 100644 arena_simulation_setup/src/arena_simulation_setup/utils/resolution.py create mode 100644 arena_simulation_setup/tests/unit/test_resolution.py diff --git a/arena_simulation_setup/src/arena_simulation_setup/shared/entities.py b/arena_simulation_setup/src/arena_simulation_setup/shared/entities.py index a7c0fb23..0c5b5d0c 100644 --- a/arena_simulation_setup/src/arena_simulation_setup/shared/entities.py +++ b/arena_simulation_setup/src/arena_simulation_setup/shared/entities.py @@ -18,6 +18,7 @@ converter, ) from arena_simulation_setup.utils.geometry import Pose, Position, Scale +from arena_simulation_setup.utils.resolution import resolve_zone_point @attrs.define(auto_attribs=True, kw_only=True) @@ -35,7 +36,9 @@ def position(self, value: Position) -> None: self.z = value.z @classmethod - def from_any(cls, obj: Union[Position, dict, 'Waypoint']) -> 'Waypoint': + def from_any(cls, obj: Union[Position, dict, str, 'Waypoint']) -> 'Waypoint': + if isinstance(obj, str): + return cls.from_position(pos=resolve_zone_point(obj)) if isinstance(obj, Waypoint): return obj if isinstance(obj, Position): diff --git a/arena_simulation_setup/src/arena_simulation_setup/tree/World/World.py b/arena_simulation_setup/src/arena_simulation_setup/tree/World/World.py index 751ee238..1ce471e6 100644 --- a/arena_simulation_setup/src/arena_simulation_setup/tree/World/World.py +++ b/arena_simulation_setup/src/arena_simulation_setup/tree/World/World.py @@ -30,7 +30,7 @@ MaterialIdentifier, ) from arena_simulation_setup.utils.cattrs import ArenaConverter, converter -from arena_simulation_setup.utils.geometry import Orientation, Pose, Position, sample_point_in_polygon +from arena_simulation_setup.utils.geometry import PointResolver, Position from .Map import Map from .Scenario import RegionAssignment, ScenarioView @@ -181,6 +181,29 @@ def lookup_zone_polygon(self, name: str) -> list[Position] | None: return elevator.cabin_corners() return None + def zone_ref_names(self) -> list[str]: + """Every name resolvable by lookup_zone_polygon (zones, doors, elevators).""" + names: list[str] = [] + for zone in self.zones: + names.append(zone.name) + names.extend(door.name for door in zone.doors) + names.extend(elevator.name for elevator in zone.elevators) + return names + + def point_resolver( + self, + rng: np.random.Generator, + *, + is_valid: typing.Callable[[Position], bool] | None = None, + ) -> PointResolver: + """Resolver that samples a point inside a named zone/door/elevator polygon.""" + return PointResolver( + lookup=self.lookup_zone_polygon, + rng=rng, + is_valid=is_valid, + candidates=self.zone_ref_names, + ) + def zone_converter( self, rng: np.random.Generator, @@ -189,33 +212,13 @@ def zone_converter( ) -> ArenaConverter: """Return a converter that resolves zone/door/elevator names to geometry. - String values for Pose/Position fields are resolved by sampling a - random point within the named zone polygon. RegionAssignment dicts - with a ``ref`` key get their polygon resolved from the floor. + String values for Pose/Position/Waypoint fields are resolved by the active + PointResolver (a random point sampled within the named zone polygon). + RegionAssignment dicts with a ``ref`` key get their polygon resolved here. """ lookup = self.lookup_zone_polygon - - base_pose_hook = converter.get_structure_hook(Pose) - base_position_hook = converter.get_structure_hook(Position) base_region_hook = converter.get_structure_hook(RegionAssignment) - def pose_hook(v: object, t: type) -> Pose: - if isinstance(v, str): - polygon = lookup(v) - if polygon is None: - raise ValueError(f"zone ref '{v}' not found in world") - pt = sample_point_in_polygon(polygon, rng, is_valid=is_valid) - return Pose(position=pt, orientation=Orientation.identity()) - return base_pose_hook(v, t) - - def position_hook(v: object, t: type) -> Position: - if isinstance(v, str): - polygon = lookup(v) - if polygon is None: - raise ValueError(f"zone ref '{v}' not found in world") - return sample_point_in_polygon(polygon, rng, is_valid=is_valid) - return base_position_hook(v, t) - def region_hook(v: object, t: type) -> RegionAssignment: if isinstance(v, dict) and 'ref' in v: ref = v.pop('ref') @@ -226,8 +229,7 @@ def region_hook(v: object, t: type) -> RegionAssignment: return base_region_hook(v, t) c = converter.copy() - c.register_structure_hook(Pose, pose_hook) - c.register_structure_hook(Position, position_hook) + c.set_resolver(self.point_resolver(rng, is_valid=is_valid)) c.register_structure_hook(RegionAssignment, region_hook) return c diff --git a/arena_simulation_setup/src/arena_simulation_setup/utils/cattrs.py b/arena_simulation_setup/src/arena_simulation_setup/utils/cattrs.py index f97b8387..6d8c12e3 100644 --- a/arena_simulation_setup/src/arena_simulation_setup/utils/cattrs.py +++ b/arena_simulation_setup/src/arena_simulation_setup/utils/cattrs.py @@ -9,6 +9,11 @@ import cattrs +from arena_simulation_setup.utils.resolution import activate_resolver + +if typing.TYPE_CHECKING: + from arena_simulation_setup.utils.geometry import PointResolver + _active_converter: contextvars.ContextVar[ArenaConverter] = contextvars.ContextVar('_active_converter') @@ -19,6 +24,11 @@ def __init__(self, *args: object, **kwargs: object) -> None: super().__init__(*args, **kwargs) self._encoders: dict[type, typing.Callable] = {} self._decoders: list[tuple[typing.Callable[[type], bool], typing.Callable]] = [] + self._resolver: PointResolver | None = None + + def set_resolver(self, resolver: PointResolver | None) -> None: + """Bind a zone resolver, activated while this converter structures values.""" + self._resolver = resolver @property def type_encoders(self) -> dict[type, typing.Callable]: @@ -69,14 +79,16 @@ def predicate(t: type, c: type = cl) -> bool: def structure(self, obj: object, cl: type) -> object: token = _active_converter.set(self) try: - return super().structure(obj, cl) + with activate_resolver(self._resolver): + return super().structure(obj, cl) finally: _active_converter.reset(token) def structure_attrs_fromdict(self, obj: Mapping[str, object], cl: type) -> object: token = _active_converter.set(self) try: - return super().structure_attrs_fromdict(obj, cl) + with activate_resolver(self._resolver): + return super().structure_attrs_fromdict(obj, cl) finally: _active_converter.reset(token) diff --git a/arena_simulation_setup/src/arena_simulation_setup/utils/geometry.py b/arena_simulation_setup/src/arena_simulation_setup/utils/geometry.py index ab0ab3a0..0ea3d401 100644 --- a/arena_simulation_setup/src/arena_simulation_setup/utils/geometry.py +++ b/arena_simulation_setup/src/arena_simulation_setup/utils/geometry.py @@ -9,6 +9,7 @@ from typing_extensions import Self from arena_simulation_setup.utils.cattrs import Idempotent, Parseable +from arena_simulation_setup.utils.resolution import resolve_zone_point try: import geometry_msgs @@ -110,8 +111,12 @@ class Position(Parseable, Idempotent, Vector3): def parse(cls, value: geometry_msgs.msg.Point | Sequence[float]) -> Self: """ parse value into Position - formats: [x,y], [x,y,z] + formats: [x,y], [x,y,z], zone ref name """ + if isinstance(value, str): + point = resolve_zone_point(value) + return cls(x=point.x, y=point.y, z=point.z) + if isinstance(value, geometry_msgs.msg.Point): return cls.from_msg(value) @@ -284,8 +289,11 @@ class Pose(Parseable, Idempotent): def parse(cls, value: geometry_msgs.msg.Pose | Sequence[float] | Sequence[Sequence[float]]) -> Self: """ parse value into Pose - formats: [x,y], [x,y,yaw], [x,y,z,roll,pitch,yaw], [x,y,z,w,x,y,z], [[*position], [*orientation]] + formats: [x,y], [x,y,yaw], [x,y,z,roll,pitch,yaw], [x,y,z,w,x,y,z], [[*position], [*orientation]], zone ref name """ + if isinstance(value, str): + return cls(position=resolve_zone_point(value), orientation=Orientation.identity()) + if isinstance(value, geometry_msgs.msg.Pose): return cls.from_msg(value) @@ -456,6 +464,24 @@ def _sample_once() -> Position: return last +@attrs.define +class PointResolver: + """Resolves zone/door/elevator names to a sampled point within their polygon.""" + + lookup: typing.Callable[[str], list[Position] | None] + rng: np.random.Generator + is_valid: typing.Callable[[Position], bool] | None = None + candidates: typing.Callable[[], typing.Iterable[str]] | None = None + + def resolve(self, name: str) -> Position: + polygon = self.lookup(name) + if polygon is None: + known = sorted(self.candidates()) if self.candidates is not None else [] + hint = f"; known refs: {', '.join(known)}" if known else "" + raise ValueError(f"zone ref {name!r} not found in world{hint}") + return sample_point_in_polygon(polygon, self.rng, is_valid=self.is_valid) + + def angle_diff(a: float, b: float) -> float: """ returns difference of angles diff --git a/arena_simulation_setup/src/arena_simulation_setup/utils/resolution.py b/arena_simulation_setup/src/arena_simulation_setup/utils/resolution.py new file mode 100644 index 00000000..35ef22e9 --- /dev/null +++ b/arena_simulation_setup/src/arena_simulation_setup/utils/resolution.py @@ -0,0 +1,39 @@ +from __future__ import annotations + +import contextlib +import contextvars +import typing + +if typing.TYPE_CHECKING: + from arena_simulation_setup.utils.geometry import PointResolver, Position + +_active_resolver: contextvars.ContextVar[PointResolver | None] = contextvars.ContextVar( + "_active_resolver", + default=None, +) + + +def current_resolver() -> PointResolver | None: + """Return the resolver for the world currently being parsed, or None.""" + return _active_resolver.get() + + +@contextlib.contextmanager +def activate_resolver(resolver: PointResolver | None) -> typing.Iterator[None]: + """Bind *resolver* as the active world resolution context for the duration of the block.""" + token = _active_resolver.set(resolver) + try: + yield + finally: + _active_resolver.reset(token) + + +def resolve_zone_point(name: str) -> Position: + """Resolve a zone/door/elevator name to a point using the active world context. + + Raises ValueError when no context is active. + """ + resolver = _active_resolver.get() + if resolver is None: + raise ValueError(f"zone ref {name!r} cannot be resolved: no world resolution context active") + return resolver.resolve(name) diff --git a/arena_simulation_setup/tests/unit/test_resolution.py b/arena_simulation_setup/tests/unit/test_resolution.py new file mode 100644 index 00000000..55067ebc --- /dev/null +++ b/arena_simulation_setup/tests/unit/test_resolution.py @@ -0,0 +1,105 @@ +from __future__ import annotations + +import numpy as np +import pytest + +from arena_simulation_setup.shared.entities import Waypoint +from arena_simulation_setup.utils.cattrs import converter +from arena_simulation_setup.utils.geometry import PointResolver, Pose, Position +from arena_simulation_setup.utils.resolution import activate_resolver, current_resolver + + +def _square() -> list[Position]: + return [Position(0.0, 0.0), Position(2.0, 0.0), Position(2.0, 2.0), Position(0.0, 2.0)] + + +def _resolver(is_valid=None) -> PointResolver: + polygons = {"zone_a": _square()} + return PointResolver( + lookup=polygons.get, + rng=np.random.default_rng(0), + is_valid=is_valid, + candidates=lambda: ["zone_a", "door_1"], + ) + + +def _inside(p: Position) -> bool: + return 0.0 <= p.x <= 2.0 and 0.0 <= p.y <= 2.0 + + +# --------------------------------------------------------------------------- +# PointResolver +# --------------------------------------------------------------------------- + + +def test_point_resolver_samples_inside_polygon(): + pt = _resolver().resolve("zone_a") + assert _inside(pt) + + +def test_point_resolver_unknown_ref_lists_candidates(): + with pytest.raises(ValueError, match="zone_a, door_1|door_1, zone_a"): + _resolver().resolve("nope") + + +def test_point_resolver_respects_is_valid(): + pt = _resolver(is_valid=lambda p: p.x >= 1.0).resolve("zone_a") + assert pt.x >= 1.0 + + +# --------------------------------------------------------------------------- +# Context-scoped resolution in parse / from_any +# --------------------------------------------------------------------------- + + +def test_no_active_context_by_default(): + assert current_resolver() is None + + +def test_position_parse_string_requires_context(): + with pytest.raises(ValueError, match="no world resolution context active"): + Position.parse("zone_a") + + +def test_position_parse_string_with_context(): + with activate_resolver(_resolver()): + pt = Position.parse("zone_a") + assert isinstance(pt, Position) + assert _inside(pt) + + +def test_pose_parse_string_with_context(): + with activate_resolver(_resolver()): + pose = Pose.parse("zone_a") + assert _inside(pose.position) + assert pose.orientation.w == pytest.approx(1.0) + + +def test_waypoint_from_any_string_with_context(): + with activate_resolver(_resolver()): + wp = Waypoint.from_any("zone_a") + assert isinstance(wp, Waypoint) + assert _inside(wp) + + +def test_waypoint_from_any_string_requires_context(): + with pytest.raises(ValueError, match="no world resolution context active"): + Waypoint.from_any("zone_a") + + +def test_non_string_inputs_unaffected_without_context(): + assert Position.parse([1.0, 2.0, 3.0]) == Position(1.0, 2.0, 3.0) + assert Waypoint.from_any(Position(1.0, 2.0)) == Waypoint(x=1.0, y=2.0, z=0.0) + + +# --------------------------------------------------------------------------- +# cattrs converter integration +# --------------------------------------------------------------------------- + + +def test_converter_resolves_string_waypoints(): + c = converter.copy() + c.set_resolver(_resolver()) + wps = c.structure(["zone_a", "zone_a"], list[Waypoint]) + assert len(wps) == 2 + assert all(isinstance(w, Waypoint) and _inside(w) for w in wps) diff --git a/task_generator/task_generator/tasks/obstacles/environment/impl.py b/task_generator/task_generator/tasks/obstacles/environment/impl.py index fe90b561..e81877f0 100644 --- a/task_generator/task_generator/tasks/obstacles/environment/impl.py +++ b/task_generator/task_generator/tasks/obstacles/environment/impl.py @@ -7,7 +7,6 @@ import numpy as np import shapely from arena_rclpy_mixins.ROSParamServer import ROSParamT -from arena_simulation_setup.utils.geometry import sample_point_in_polygon from shapely.geometry import Point from task_generator.shared import DynamicObstacle, Obstacle, Orientation, Pose, Position @@ -429,17 +428,9 @@ def _parse_environment(self, environment_file: str) -> _ParsedConfig: obstacle_x = x + rot_x obstacle_y = y + rot_y - # Resolve waypoints: strings are zone labels, lists are coordinates - rng = self.node.conf.General.RNG.value - resolved_waypoints = [] - for wp in entity.get("waypoints", []): - if isinstance(wp, str): - vertices = world.lookup_zone_polygon(wp) - if vertices is None: - raise ValueError(f"zone ref '{wp}' not found in world") - resolved_waypoints.append(sample_point_in_polygon(vertices, rng)) - else: - resolved_waypoints.append(Position(x=wp[0], y=wp[1])) + # strings are zone refs, lists are coordinates + resolver = world.point_resolver(self.node.conf.General.RNG.value) + resolved_waypoints = [resolver.resolve(wp) if isinstance(wp, str) else Position(x=wp[0], y=wp[1]) for wp in entity.get("waypoints", [])] obs_name = f"G_{group_name}_{idx}_{n_groups}_{entity['model']}_{g}" new_obstacle = DynamicObstacle( From c70b2270bff0e6a5ecceccffb75de14ececec411 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 14:55:48 +0200 Subject: [PATCH 10/24] only show first resolution failure per pedestrian model --- .../task_generator/simulators/human/__init__.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/task_generator/task_generator/simulators/human/__init__.py b/task_generator/task_generator/simulators/human/__init__.py index 0907f7f2..1bb80179 100644 --- a/task_generator/task_generator/simulators/human/__init__.py +++ b/task_generator/task_generator/simulators/human/__init__.py @@ -65,6 +65,7 @@ def __init__(self, *args: object, namespace: Namespace, simulator: BaseSim, real self._known_doors = KnownObstacles[Door]() self._wall_counter = itertools.count() self._known_regions: dict[str, Region] = {} + self._warned_unresolved_models: set[str] = set() self._arena_peds_publisher = self.node.create_publisher(Pedestrians, self._namespace("arena_peds"), 10) self._marker_publisher = self.node.create_publisher( @@ -258,13 +259,17 @@ async def _resolve(obs: DynamicObstacle) -> DynamicObstacle: if model.type is not ModelType.UNKNOWN: return obs except Exception as e: - self._logger.warning( - f"pedestrian {obs.name!r}: model {obs.model.name!r} unresolved ({e}); using fallback", - ) + if obs.model.name not in self._warned_unresolved_models: + self._warned_unresolved_models.add(obs.model.name) + self._logger.warning( + f"pedestrian model {obs.model.name!r} unresolved ({e}); using fallback", + ) else: - self._logger.warning( - f"pedestrian {obs.name!r}: model {obs.model.name!r} has no SDF; using fallback", - ) + if obs.model.name not in self._warned_unresolved_models: + self._warned_unresolved_models.add(obs.model.name) + self._logger.warning( + f"pedestrian model {obs.model.name!r} has no SDF; using fallback", + ) return attrs.evolve(obs, model=PedestrianIdentifier.parse(self._PEDESTRIAN_FALLBACK)) return await asyncio.gather(*(_resolve(o) for o in obstacles)) From 652c3fb1724292db0a87c0ec59286fc8a2a4529a Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 14:56:00 +0200 Subject: [PATCH 11/24] fix scenario region coords --- .../scenarios/default/scenario.yaml | 24 +++++++++++++++---- .../hospital_1/scenarios/flow/scenario.yaml | 20 +++++++++++----- .../scenarios/humansim_default/scenario.yaml | 24 +++++++++++++++---- .../scenarios/humansim_flow/scenario.yaml | 20 +++++++++++----- .../scenarios/humansim_showcase/scenario.yaml | 8 +++---- .../scenarios/showcase/scenario.yaml | 8 +++---- 6 files changed, 76 insertions(+), 28 deletions(-) diff --git a/arena_simulation_setup/worlds/hospital_1/scenarios/default/scenario.yaml b/arena_simulation_setup/worlds/hospital_1/scenarios/default/scenario.yaml index 9ca2db4c..39d6f53d 100644 --- a/arena_simulation_setup/worlds/hospital_1/scenarios/default/scenario.yaml +++ b/arena_simulation_setup/worlds/hospital_1/scenarios/default/scenario.yaml @@ -96,24 +96,40 @@ regions: reception_sink: type: sink - ref: reception + polygon: + - {x: 5.6, y: 1.0} + - {x: 7.4, y: 1.0} + - {x: 7.4, y: 4.0} + - {x: 5.6, y: 4.0} config: capacity: -1 waiting_sink: type: sink - ref: waiting_area + polygon: + - {x: 21.2, y: 4.6} + - {x: 22.8, y: 4.6} + - {x: 22.8, y: 7.8} + - {x: 21.2, y: 7.8} config: capacity: 8 pharmacy_sink: type: sink - ref: pharmacy + polygon: + - {x: 5.8, y: 27.0} + - {x: 7.5, y: 27.0} + - {x: 7.5, y: 28.3} + - {x: 5.8, y: 28.3} config: capacity: 5 nurse_rest_sink: type: sink - ref: nurse_resting_room + polygon: + - {x: 14.3, y: 6.0} + - {x: 16.3, y: 6.0} + - {x: 16.3, y: 9.0} + - {x: 14.3, y: 9.0} config: capacity: 3 diff --git a/arena_simulation_setup/worlds/hospital_1/scenarios/flow/scenario.yaml b/arena_simulation_setup/worlds/hospital_1/scenarios/flow/scenario.yaml index 056eddf5..df8dafae 100644 --- a/arena_simulation_setup/worlds/hospital_1/scenarios/flow/scenario.yaml +++ b/arena_simulation_setup/worlds/hospital_1/scenarios/flow/scenario.yaml @@ -24,7 +24,11 @@ dynamic: regions: entrance_source: type: source - ref: main_hallway_entrance + polygon: + - {x: 9.25, y: 0.2} + - {x: 10.75, y: 0.2} + - {x: 10.75, y: 1.2} + - {x: 9.25, y: 1.2} config: rate_profile: - t: 0.0 @@ -39,17 +43,21 @@ regions: reception_sink: type: sink - ref: reception + polygon: + - {x: 5.6, y: 1.0} + - {x: 7.4, y: 1.0} + - {x: 7.4, y: 4.0} + - {x: 5.6, y: 4.0} config: capacity: -1 hallway_spawn: type: source polygon: - - {x: 18.0, y: 2.0} - - {x: 22.0, y: 2.0} - - {x: 22.0, y: 6.0} - - {x: 18.0, y: 6.0} + - {x: 21.0, y: 1.0} + - {x: 23.0, y: 1.0} + - {x: 23.0, y: 2.8} + - {x: 21.0, y: 2.8} config: rate_profile: - t: 0.0 diff --git a/arena_simulation_setup/worlds/hospital_1/scenarios/humansim_default/scenario.yaml b/arena_simulation_setup/worlds/hospital_1/scenarios/humansim_default/scenario.yaml index 9ca2db4c..39d6f53d 100644 --- a/arena_simulation_setup/worlds/hospital_1/scenarios/humansim_default/scenario.yaml +++ b/arena_simulation_setup/worlds/hospital_1/scenarios/humansim_default/scenario.yaml @@ -96,24 +96,40 @@ regions: reception_sink: type: sink - ref: reception + polygon: + - {x: 5.6, y: 1.0} + - {x: 7.4, y: 1.0} + - {x: 7.4, y: 4.0} + - {x: 5.6, y: 4.0} config: capacity: -1 waiting_sink: type: sink - ref: waiting_area + polygon: + - {x: 21.2, y: 4.6} + - {x: 22.8, y: 4.6} + - {x: 22.8, y: 7.8} + - {x: 21.2, y: 7.8} config: capacity: 8 pharmacy_sink: type: sink - ref: pharmacy + polygon: + - {x: 5.8, y: 27.0} + - {x: 7.5, y: 27.0} + - {x: 7.5, y: 28.3} + - {x: 5.8, y: 28.3} config: capacity: 5 nurse_rest_sink: type: sink - ref: nurse_resting_room + polygon: + - {x: 14.3, y: 6.0} + - {x: 16.3, y: 6.0} + - {x: 16.3, y: 9.0} + - {x: 14.3, y: 9.0} config: capacity: 3 diff --git a/arena_simulation_setup/worlds/hospital_1/scenarios/humansim_flow/scenario.yaml b/arena_simulation_setup/worlds/hospital_1/scenarios/humansim_flow/scenario.yaml index 4e0de241..a30ce1c5 100644 --- a/arena_simulation_setup/worlds/hospital_1/scenarios/humansim_flow/scenario.yaml +++ b/arena_simulation_setup/worlds/hospital_1/scenarios/humansim_flow/scenario.yaml @@ -27,7 +27,11 @@ dynamic: regions: entrance_source: type: source - ref: main_hallway_entrance + polygon: + - {x: 9.25, y: 0.2} + - {x: 10.75, y: 0.2} + - {x: 10.75, y: 1.2} + - {x: 9.25, y: 1.2} config: rate_profile: - t: 0.0 @@ -42,17 +46,21 @@ regions: reception_sink: type: sink - ref: reception + polygon: + - {x: 5.6, y: 1.0} + - {x: 7.4, y: 1.0} + - {x: 7.4, y: 4.0} + - {x: 5.6, y: 4.0} config: capacity: -1 hallway_spawn: type: source polygon: - - {x: 18.0, y: 2.0} - - {x: 22.0, y: 2.0} - - {x: 22.0, y: 6.0} - - {x: 18.0, y: 6.0} + - {x: 21.0, y: 1.0} + - {x: 23.0, y: 1.0} + - {x: 23.0, y: 2.8} + - {x: 21.0, y: 2.8} config: rate_profile: - t: 0.0 diff --git a/arena_simulation_setup/worlds/hospital_1/scenarios/humansim_showcase/scenario.yaml b/arena_simulation_setup/worlds/hospital_1/scenarios/humansim_showcase/scenario.yaml index 2bcc3d33..204f8b84 100644 --- a/arena_simulation_setup/worlds/hospital_1/scenarios/humansim_showcase/scenario.yaml +++ b/arena_simulation_setup/worlds/hospital_1/scenarios/humansim_showcase/scenario.yaml @@ -127,9 +127,9 @@ regions: pharmacy_sink: type: sink polygon: - - {x: 6.0, y: 26.8} - - {x: 6.8, y: 25.8} - - {x: 6.8, y: 26.7} - - {x: 6.0, y: 26.7} + - {x: 5.8, y: 27.0} + - {x: 7.5, y: 27.0} + - {x: 7.5, y: 28.3} + - {x: 5.8, y: 28.3} config: capacity: 4 diff --git a/arena_simulation_setup/worlds/hospital_1/scenarios/showcase/scenario.yaml b/arena_simulation_setup/worlds/hospital_1/scenarios/showcase/scenario.yaml index 2f82ceea..4ef8f97f 100644 --- a/arena_simulation_setup/worlds/hospital_1/scenarios/showcase/scenario.yaml +++ b/arena_simulation_setup/worlds/hospital_1/scenarios/showcase/scenario.yaml @@ -127,9 +127,9 @@ regions: pharmacy_sink: type: sink polygon: - - {x: 6.0, y: 26.8} - - {x: 6.8, y: 25.8} - - {x: 6.8, y: 26.7} - - {x: 6.0, y: 26.7} + - {x: 5.8, y: 27.0} + - {x: 7.5, y: 27.0} + - {x: 7.5, y: 28.3} + - {x: 5.8, y: 28.3} config: capacity: 4 From 0501f57f094b8d8d423698e273042b10c3970804 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 15:05:44 +0200 Subject: [PATCH 12/24] open doors for humans --- .../simulators/human/arena_humansim/arena_humansim.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py b/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py index 3b119216..dd5ac2a3 100644 --- a/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py +++ b/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py @@ -955,13 +955,14 @@ async def _spawn_walls_impl(self, walls: Mapping[str, Wall]) -> bool: return await self._add_walls(walls) async def _spawn_doors_impl(self, doors: Mapping[str, Door]) -> bool: - return await self._add_walls(doors) + # doors stay passable, update_doors closes them with __door_ walls + return True async def _remove_walls_impl(self, names: Sequence[str]) -> bool: return await self._remove_walls(names) async def _remove_doors_impl(self, names: Sequence[str]) -> bool: - return await self._remove_walls(names) + return True async def _add_walls(self, walls: Mapping[str, Wall]) -> bool: """Send wall/door segments to arena_humansim via AddWalls.""" From 3944ff8841a65944b8c40e5daf3fbc86dbecf31f Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 16:26:09 +0200 Subject: [PATCH 13/24] better humansim/gazebo integration --- .../sim/gazebo_simulator/gazebo_simulator.py | 18 ++-- .../Common/Pedestrian/arenian/arenian.sdf | 8 ++ .../human/arena_humansim/arena_humansim.py | 100 +++--------------- 3 files changed, 31 insertions(+), 95 deletions(-) diff --git a/arena_runtime/arena_runtime/arena_runtime/sim/gazebo_simulator/gazebo_simulator.py b/arena_runtime/arena_runtime/arena_runtime/sim/gazebo_simulator/gazebo_simulator.py index 231fba9a..24284edf 100644 --- a/arena_runtime/arena_runtime/arena_runtime/sim/gazebo_simulator/gazebo_simulator.py +++ b/arena_runtime/arena_runtime/arena_runtime/sim/gazebo_simulator/gazebo_simulator.py @@ -18,7 +18,7 @@ import rclpy.impl.rcutils_logger import rclpy.time import tf2_ros -from arena_people_msgs.msg import Pedestrian, Pedestrians +from arena_people_msgs.msg import Pedestrians from arena_rclpy_mixins import ArenaMixinNode from arena_rclpy_mixins.Async import ClientWrapper from arena_rclpy_mixins.shared import Namespace @@ -247,7 +247,8 @@ async def obstacle_move(self, obstacles: Sequence[Obstacle]) -> Sequence[bool]: return await asyncio.gather(*map(self._move_entity, obstacles)) async def pedestrian_move(self, pedestrians: Sequence[DynamicObstacle]) -> Sequence[bool]: - return await asyncio.gather(*map(self._move_entity, pedestrians)) + # Ped pose is owned by PedSkeletonPlugin (driven from the arena_peds topic). + return tuple(True for _ in pedestrians) async def robot_move(self, robots: Sequence[Robot]) -> Sequence[bool]: async def impl(robot: Robot) -> bool: @@ -261,7 +262,8 @@ async def obstacle_delete(self, obstacles: Sequence[Obstacle]) -> Sequence[bool] return await asyncio.gather(*(self._delete_entity(o.sim_path) for o in obstacles)) async def pedestrian_delete(self, pedestrians: Sequence[DynamicObstacle]) -> Sequence[bool]: - return await asyncio.gather(*(self._delete_entity(p.sim_path) for p in pedestrians)) + # PedSkeletonPlugin removes a ped's actor when its name leaves arena_peds. + return tuple(True for _ in pedestrians) async def robot_delete(self, robots: Sequence[Robot]) -> Sequence[bool]: for robot in robots: @@ -269,14 +271,8 @@ async def robot_delete(self, robots: Sequence[Robot]) -> Sequence[bool]: return await asyncio.gather(*(self._delete_entity(robot.sim_path) for robot in robots)) async def pedestrian_update(self, pedestrians: Pedestrians) -> Sequence[bool]: - async def impl(ped: Pedestrian) -> bool: - req = SetEntityPose.Request() - req.entity = EntityMsg(name=ped.name, type=EntityMsg.MODEL) - req.pose = ped.pose - res = await self._service_set_entity_pose.call_timeout(req) - return bool(res and res.success) - - return await asyncio.gather(*(impl(p) for p in pedestrians.pedestrians)) + # Ped pose is owned by PedSkeletonPlugin (driven from the arena_peds topic). + return tuple(True for _ in pedestrians.pedestrians) async def spawn_floors(self, floors: Sequence[Floor]) -> bool: for floor in floors: diff --git a/arena_simulation_setup/assets/Common/Pedestrian/arenian/arenian.sdf b/arena_simulation_setup/assets/Common/Pedestrian/arenian/arenian.sdf index c34b8e0b..a4c0ae38 100644 --- a/arena_simulation_setup/assets/Common/Pedestrian/arenian/arenian.sdf +++ b/arena_simulation_setup/assets/Common/Pedestrian/arenian/arenian.sdf @@ -16,5 +16,13 @@ 1.0 false + + + + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/stand.dae + 1.0 + false + diff --git a/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py b/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py index dd5ac2a3..0f25646f 100644 --- a/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py +++ b/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py @@ -2,7 +2,6 @@ import asyncio import math -import os import traceback from collections.abc import Mapping, Sequence @@ -211,16 +210,11 @@ def __init__(self, *args: object, **kwargs: object) -> None: # IDs managed by the bridge (scenario-defined agents) self._bridge_agent_ids: set[int] = set() - # IDs of flow agents currently live in the physics sim (no pool slot) - self._physim_agent_ids: set[int] = set() + # IDs of flow agents (source/sink) with a live actor in the simulator + self._flow_agent_ids: set[int] = set() # agent_id → human-readable name (scenario YAML name or flow source label) self._agent_names: dict[int, str] = {} - # Pre-spawned pedestrian pool for flow agents (source/sink) - self._pool_free: list[str] = [] # pool pedestrian names, available - self._pool_active: dict[int, str] = {} # flow_agent_id → pool ped name - self._pool_spawned: bool = False - self._tick_loop_task: asyncio.Task | None = None self._update_loop_task: asyncio.Task | None = None self._feedback_loop_task: asyncio.Task | None = None @@ -444,11 +438,8 @@ def _make_flow_dynamic_obstacle(self, agent: AgentStateMsg) -> DynamicObstacle: ) async def _pedestrian_update_loop(self): - """Push latest pedestrian positions to the underlying simulator (e.g. flatland). - - Also manages physics-sim bodies for agents created/removed by - arena_humansim sources and sinks at runtime, using a pre-spawned pool - when available and falling back to runtime spawn/delete on exhaustion. + """Spawn an actor for each flow agent a source creates, delete it when a + sink removes the agent, and push the latest pedestrian set to the simulator. """ try: with self.node.sim_time_rate(self.TICK_RATE) as (done, rate): @@ -463,52 +454,26 @@ async def _pedestrian_update_loop(self): if states is not None: current_ids = {a.agent_id for a in states.agents} flow_ids = current_ids - self._bridge_agent_ids - known_flow = set(self._pool_active.keys()) | self._physim_agent_ids - new_ids = flow_ids - known_flow - gone_ids = known_flow - current_ids + new_ids = flow_ids - self._flow_agent_ids + gone_ids = self._flow_agent_ids - current_ids if new_ids: agents_by_id = {a.agent_id: a for a in states.agents} - to_move: list[DynamicObstacle] = [] to_spawn: list[DynamicObstacle] = [] for aid in new_ids: - agent = agents_by_id[aid] - if self._pool_free: - pool_name = self._pool_free.pop() - self._pool_active[aid] = pool_name - obs = self._runtime_obstacle( - name=pool_name, - pose=Pose(Position(agent.pose.x, agent.pose.y)), - ) - self._agent_names[aid] = obs.sim_path - to_move.append(obs) - else: - self._physim_agent_ids.add(aid) - obs = self._make_flow_dynamic_obstacle(agent) - self._agent_names[aid] = obs.sim_path - to_spawn.append(obs) - if to_move: - await self._simulator.pedestrian_move(to_move) - if to_spawn: - await self._simulator.pedestrian_spawn(await self._ensure_spawnable(to_spawn)) + self._flow_agent_ids.add(aid) + obs = self._make_flow_dynamic_obstacle(agents_by_id[aid]) + self._agent_names[aid] = obs.sim_path + to_spawn.append(obs) + await self._simulator.pedestrian_spawn(await self._ensure_spawnable(to_spawn)) if gone_ids: - to_park: list[DynamicObstacle] = [] to_delete: list[DynamicObstacle] = [] - parking = Pose(Position(self.POOL_PARKING_X, self.POOL_PARKING_Y)) for aid in gone_ids: + self._flow_agent_ids.discard(aid) self._agent_names.pop(aid, None) - if aid in self._pool_active: - pool_name = self._pool_active.pop(aid) - self._pool_free.append(pool_name) - to_park.append(self._runtime_obstacle(name=pool_name, pose=parking)) - else: - self._physim_agent_ids.discard(aid) - to_delete.append(self._runtime_obstacle(name=f"flow_{aid}", pose=parking)) - if to_park: - await self._simulator.pedestrian_move(to_park) - if to_delete: - await self._simulator.pedestrian_delete(to_delete) + to_delete.append(self._runtime_obstacle(name=f"flow_{aid}", pose=Pose(Position(0.0, 0.0)))) + await self._simulator.pedestrian_delete(to_delete) await self._simulator.pedestrian_update(peds) except asyncio.CancelledError: @@ -599,41 +564,11 @@ def _polygon_centroid(self, polygon: Sequence[Position]) -> tuple[float, float]: cy = sum(p.y for p in polygon) / len(polygon) return cx, cy - POOL_PARKING_X = 9999.0 - POOL_PARKING_Y = 9999.0 - async def _add_regions_impl(self, regions: Sequence[Region]) -> bool: results = await asyncio.gather( *(self._add_region_single(r) for r in regions), ) - if results and not all(results): - return False - - # Compute pool size from finite source capacities - pool_size = 0 - for region in regions: - if region.type != "source": - continue - mc = region.config.get("max_concurrent", -1) - if mc < 0: - # Unlimited source — can't pre-allocate, rely on runtime spawn - continue - pool_size += mc - - optimize = os.environ.get("OPTIMIZE_SPEED", "false").lower() in ("1", "true", "yes") - if optimize and pool_size > 0 and not self._pool_spawned: - await self._spawn_pool(pool_size) - - return True - - async def _spawn_pool(self, size: int): - """Pre-spawn pool pedestrians at the parking position.""" - parking = Pose(Position(self.POOL_PARKING_X, self.POOL_PARKING_Y)) - pool_peds = [self._runtime_obstacle(name=f"_flow_pool_{i}", pose=parking) for i in range(size)] - await self._simulator.pedestrian_spawn(await self._ensure_spawnable(pool_peds)) - self._pool_free = [p.name for p in pool_peds] - self._pool_spawned = True - self._logger.info(f"Pre-spawned {size} pool pedestrians") + return all(results) async def _add_region_single(self, region: Region) -> bool: if region.type == "source": @@ -932,11 +867,8 @@ async def _remove_pedestrians_impl(self) -> bool: response = await self._remove_client.call_timeout(request) self._next_id = 1 self._bridge_agent_ids.clear() - self._physim_agent_ids.clear() - self._pool_free.clear() - self._pool_active.clear() + self._flow_agent_ids.clear() self._agent_names.clear() - self._pool_spawned = False self._prev_agent_states = None self._curr_agent_states = None self._arena_pedestrians = Pedestrians() From 95bfd1f67cc6d8ca568d03506ee714e4ab9d3300 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 17:17:23 +0200 Subject: [PATCH 14/24] fix preload_world --- arena_simulation_setup/scripts/preload_world | 47 +++++++++++--------- 1 file changed, 26 insertions(+), 21 deletions(-) diff --git a/arena_simulation_setup/scripts/preload_world b/arena_simulation_setup/scripts/preload_world index 2001def5..32172e11 100755 --- a/arena_simulation_setup/scripts/preload_world +++ b/arena_simulation_setup/scripts/preload_world @@ -16,31 +16,36 @@ import traceback from pathlib import Path import yaml - from arena_simulation_setup.tree import DynamicPaths, Identifier from arena_simulation_setup.tree.assets.Object import ObjectIdentifier from arena_simulation_setup.tree.assets.Pedestrian import PedestrianIdentifier from arena_simulation_setup.tree.Wall import WallIdentifier -from arena_simulation_setup.tree.World import World, WorldIdentifier +from arena_simulation_setup.tree.World import ( + MultiLevelWorldView, + WorldDescription, + WorldIdentifier, +) -def _world_identifiers(world_desc: object) -> list[Identifier]: +def _world_identifiers(world_desc: WorldDescription) -> list[Identifier]: out: list[Identifier] = [] - for zone in world_desc.zones: - out.append(zone.material) - for wall in zone.walls: - if wall.material is not None: - out.append(wall.material) - if wall.kind: - out.append(WallIdentifier(wall.kind)) - for door in zone.doors: - out.append(door.material) - for elev in zone.elevators: - out.append(elev.material) - for entity in zone.entities.static: - out.append(entity.model) - for entity in zone.entities.dynamic: - out.append(entity.model) + for level in world_desc.all_levels: + for zone in level.zones: + out.append(zone.material) + out.append(zone.ceiling_material) + for wall in zone.walls: + if wall.material is not None: + out.append(wall.material) + if wall.kind: + out.append(WallIdentifier(wall.kind)) + for door in zone.doors: + out.append(door.material) + for elev in zone.elevators: + out.append(elev.material) + for entity in zone.entities.static: + out.append(entity.model) + for entity in zone.entities.dynamic: + out.append(entity.model) return out @@ -136,7 +141,7 @@ async def _resolve_all(identifiers: list[Identifier]) -> tuple[int, int]: return n_ok, total -def _resolve_world(world_name: str) -> World: +def _resolve_world(world_name: str) -> MultiLevelWorldView: """Locate a world directory, preferring the resolver chain and falling back to ARENA_DIR.""" try: return WorldIdentifier(world_name).resolve_sync() @@ -149,7 +154,7 @@ def _resolve_world(world_name: str) -> World: candidate = Path(arena_dir) / 'arena_simulation_setup' / 'worlds' / world_name if not (candidate / 'world.yaml').is_file(): raise FileNotFoundError(f'world {world_name!r} not found at {candidate}') - return World(candidate) + return MultiLevelWorldView(candidate) def main() -> int: @@ -167,7 +172,7 @@ def main() -> int: DynamicPaths.WORLD.path = world.path try: - world_desc = world.load() + world_desc = world.load(validate=False) except Exception: print(f'failed to load {world.world_path}:', file=sys.stderr) traceback.print_exc() From e09cda6550fcb84e1f47e042396bb27d0338bfa4 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 18:14:11 +0200 Subject: [PATCH 15/24] gate simulators on feature being installed --- _meta/docker/features/gazebo | 6 ++- _meta/features/gazebo | 6 ++- _meta/tools/source | 9 ++++ arena_bringup/launch/simulator/sim/README.md | 45 ++++++++++--------- .../simulator/sim/isaac/isaac.launch.py | 34 -------------- .../launch/simulator/sim/sim.launch.py | 45 ++++++++++--------- 6 files changed, 68 insertions(+), 77 deletions(-) delete mode 100644 arena_bringup/launch/simulator/sim/isaac/isaac.launch.py diff --git a/_meta/docker/features/gazebo b/_meta/docker/features/gazebo index f2a579cd..34d3800f 100644 --- a/_meta/docker/features/gazebo +++ b/_meta/docker/features/gazebo @@ -20,7 +20,7 @@ source_fn(){ help(){ - echo "Usage: $name " + echo "Usage: $name " } case "$1" in @@ -40,6 +40,10 @@ case "$1" in source_fn return $? ;; + launch) + shift + exec ros2 launch arena_bringup gazebo.launch.py "$@" + ;; *) help exit 1 diff --git a/_meta/features/gazebo b/_meta/features/gazebo index cb1863cb..880709bd 100644 --- a/_meta/features/gazebo +++ b/_meta/features/gazebo @@ -55,7 +55,7 @@ uninstall(){ # === MAIN SCRIPT === help(){ - echo "Usage: $name " + echo "Usage: $name " } if [ $# -lt 1 ]; then help @@ -79,6 +79,10 @@ case "$1" in source_fn return $? ;; + launch) + shift + exec ros2 launch arena_bringup gazebo.launch.py "$@" + ;; *) help exit 1 diff --git a/_meta/tools/source b/_meta/tools/source index 8fee762a..9c1e5020 100644 --- a/_meta/tools/source +++ b/_meta/tools/source @@ -352,6 +352,15 @@ if [ -z ${ARENA_SOURCED+x} ] ; then source "$feature_path" uninstall "$@" result=$? ;; + launch) + if ! _feature_registry has "$fname"; then + echo "$fname is not installed; run 'arena feature $fname install' first." >&2 + result=1 + else + ($SHELL -c "source '$SOURCE_FILE' > /dev/null 2>&1 && $shell '$feature_path' $*") + result=$? + fi + ;; *) ($SHELL -c "source '$SOURCE_FILE' > /dev/null 2>&1 && $shell '$feature_path' $*") result=$? diff --git a/arena_bringup/launch/simulator/sim/README.md b/arena_bringup/launch/simulator/sim/README.md index e2176690..419a37d8 100644 --- a/arena_bringup/launch/simulator/sim/README.md +++ b/arena_bringup/launch/simulator/sim/README.md @@ -9,14 +9,14 @@ Called from `arena_runtime.launch.py` with `simulator`, `use_sim_time`, `world`, `SelectAction` is a key->action registry resolved at launch time. The selector expression is `LaunchConfiguration('simulator')`. Each registered key maps to -a `GroupAction` or `IncludeLaunchDescription`. +a `GroupAction` or `ExecuteProcess`. ``` simulator key -> action ────────────────────────────────────────────────────────────────── dummy -> static_transform_publisher (map -> dummy TF only) -gazebo -> gazebo/gazebo.launch.py -isaac -> isaac/isaac.launch.py +gazebo -> ExecuteProcess: `arena feature gazebo launch` +isaac -> ExecuteProcess: `arena feature isaac launch` ``` The `simulator` `LaunchArgument` is declared *after* the `SelectAction` is @@ -24,19 +24,26 @@ built so that `choices` can be derived from `launch_simulator.keys` - the keys registered above. Passing an unregistered value causes a launch-time validation error. -## Per-sim subdir layout +## Feature-gated delegation + +Both real simulators run via their feature `launch` verb: `sim.launch.py` +delegates with an `ExecuteProcess` (`bash -c 'arena feature launch ...'`, +`on_exit=Shutdown`). The verb refuses unless the feature is installed +(`arena feature install`), so the dispatcher is the single live entry and +the verb is never a dead path. Forwarded args: gazebo gets +`use_sim_time`/`headless`/`world`, isaac gets `headless`/`log_level`. + +The real bringup files live wherever the feature owns them: ``` -launch/simulator/sim/ -├── sim.launch.py - dispatcher (this file's parent) -├── gazebo/ -│ └── gazebo.launch.py - gz-sim 8 bringup + clock bridge -└── isaac/ - └── isaac.launch.py - delegates to `arena feature isaac launch` +arena_bringup: launch/simulator/sim/gazebo/gazebo.launch.py (gz-sim 8 + clock bridge) +arena_isaac: run_isaacsim.launch.py (Isaac Sim app) ``` -### gazebo/gazebo.launch.py +### gazebo.launch.py (arena_bringup) +- Reached via `arena feature gazebo launch`, so it only runs when the gazebo + feature is installed. - Stages models via `arena_simulation_setup model_staging` at Python-load time. - Sets `GZ_SIM_RESOURCE_PATH` and `GAZEBO_MODEL_PATH` from the staging directory plus `arena_robots` and any declared deps. @@ -47,19 +54,17 @@ launch/simulator/sim/ renderer). When `headless=True`, passes `-s` (server-only). - Starts a `ros_gz_bridge parameter_bridge` for `/clock`. -Forwarded args: `use_sim_time`, `world`, `headless`. - -### isaac/isaac.launch.py +### run_isaacsim.launch.py (arena_isaac) -- Runs `arena feature isaac launch` via `ExecuteProcess` (bash). -- Triggers `Shutdown` on process exit. -- Forwarded arg: `use_sim_time` (passed via launch arguments; `headless` is - currently commented out). +- Reached via `arena feature isaac launch` (which, in docker, brings up the + separate `isaac` compose service first). ## Adding a new simulator -1. Create `launch/simulator/sim//.launch.py`. -2. In `sim.launch.py`, call `launch_simulator.add("", IncludeLaunchDescription(...))`. +1. Add a feature with a `launch` verb that exec's the real bringup + (in `arena_bringup` or the feature's own package). +2. In `sim.launch.py`, call `launch_simulator.add("", ExecuteProcess(...))` + delegating to `arena feature launch`. 3. The new key appears in `launch_simulator.keys` automatically, so the `simulator` argument's `choices` list updates without extra changes. 4. Add the corresponding `Constants.SimSimulator.` entry in diff --git a/arena_bringup/launch/simulator/sim/isaac/isaac.launch.py b/arena_bringup/launch/simulator/sim/isaac/isaac.launch.py deleted file mode 100644 index ae8d15f1..00000000 --- a/arena_bringup/launch/simulator/sim/isaac/isaac.launch.py +++ /dev/null @@ -1,34 +0,0 @@ -import launch.actions -import launch.substitutions -from arena_bringup.substitutions import LaunchArgument -from launch import LaunchDescription -from launch.substitutions import TextSubstitution - - -def generate_launch_description(): - ld = [] - LaunchArgument.auto_append(ld) - log_level = LaunchArgument( - name='log_level', - default_value='debug', - description='Logging level', - ) - headless = LaunchArgument( - name='headless', - default_value='False', - ) - return LaunchDescription([ - *ld, - launch.actions.ExecuteProcess( - cmd=['bash', '-c', [ - TextSubstitution(text='arena feature isaac launch headless:='), - headless.substitution, - TextSubstitution(text=' log_level:='), - log_level.substitution, - ]], - sigterm_timeout=launch.substitutions.LaunchConfiguration('sigterm_timeout', default='5'), - sigkill_timeout=launch.substitutions.LaunchConfiguration('sigkill_timeout', default='5'), - on_exit=[launch.actions.Shutdown()], - output='log', - ) - ]) diff --git a/arena_bringup/launch/simulator/sim/sim.launch.py b/arena_bringup/launch/simulator/sim/sim.launch.py index a247eb57..8a741d9d 100644 --- a/arena_bringup/launch/simulator/sim/sim.launch.py +++ b/arena_bringup/launch/simulator/sim/sim.launch.py @@ -1,7 +1,5 @@ import launch.actions import launch.substitutions -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare import launch import launch_ros.actions @@ -43,30 +41,35 @@ def generate_launch_description(): launch_simulator.add( SimSimulator.GAZEBO.value, - launch.actions.IncludeLaunchDescription( - PathJoinSubstitution([ - FindPackageShare('arena_bringup'), - 'launch', 'simulator', 'sim', 'gazebo', 'gazebo.launch.py', - ]), - launch_arguments={ - **use_sim_time.dict, - **headless.dict, - **world.dict, - }.items(), + launch.actions.ExecuteProcess( + cmd=['bash', '-c', [ + launch.substitutions.TextSubstitution(text='arena feature gazebo launch use_sim_time:='), + use_sim_time.substitution, + launch.substitutions.TextSubstitution(text=' headless:='), + headless.substitution, + launch.substitutions.TextSubstitution(text=' world:='), + world.substitution, + ]], + sigterm_timeout=launch.substitutions.LaunchConfiguration('sigterm_timeout', default='5'), + sigkill_timeout=launch.substitutions.LaunchConfiguration('sigkill_timeout', default='5'), + on_exit=[launch.actions.Shutdown()], + output='log', ) ) launch_simulator.add( SimSimulator.ISAAC.value, - launch.actions.IncludeLaunchDescription( - PathJoinSubstitution([ - FindPackageShare('arena_bringup'), - 'launch', 'simulator', 'sim', 'isaac', 'isaac.launch.py', - ]), - launch_arguments={ - 'use_sim_time': use_sim_time.substitution, - 'headless': headless.substitution, - }.items(), + launch.actions.ExecuteProcess( + cmd=['bash', '-c', [ + launch.substitutions.TextSubstitution(text='arena feature isaac launch headless:='), + headless.substitution, + launch.substitutions.TextSubstitution(text=' log_level:='), + launch.substitutions.LaunchConfiguration('log_level', default='debug'), + ]], + sigterm_timeout=launch.substitutions.LaunchConfiguration('sigterm_timeout', default='5'), + sigkill_timeout=launch.substitutions.LaunchConfiguration('sigkill_timeout', default='5'), + on_exit=[launch.actions.Shutdown()], + output='log', ) ) From 15ff15585a31fa1b7f7e3341aff4ebd2da165263 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 18:29:05 +0200 Subject: [PATCH 16/24] fix multi-ped spawn during load --- .../sim/gazebo_simulator/gazebo_simulator.py | 20 ++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/arena_runtime/arena_runtime/arena_runtime/sim/gazebo_simulator/gazebo_simulator.py b/arena_runtime/arena_runtime/arena_runtime/sim/gazebo_simulator/gazebo_simulator.py index 24284edf..cf66d571 100644 --- a/arena_runtime/arena_runtime/arena_runtime/sim/gazebo_simulator/gazebo_simulator.py +++ b/arena_runtime/arena_runtime/arena_runtime/sim/gazebo_simulator/gazebo_simulator.py @@ -173,6 +173,11 @@ async def _list_models(self) -> list[str]: class GazeboSimulator(BaseSim): + # gz create CLI ack timeout (ms), kept generous so a slow server-side ack + # under heavy load is not mistaken for a spawn failure, which would leak an + # untracked orphan model. + _SPAWN_TIMEOUT_MS = 60000 + def __init__(self, *args: object, namespace: Namespace, **kwargs: object) -> None: super().__init__(*args, namespace=namespace, **kwargs) @@ -430,14 +435,27 @@ async def _spawn_model(self, name: str, model: Model, pose: Pose) -> bool: req_payload = f'sdf_filename: "{sdf_path}", name: "{name}", pose: {{ position: {{ x: {pose.position.x}, y: {pose.position.y}, z: {pose.position.z} }} orientation: {{ x: {pose.orientation.x}, y: {pose.orientation.y}, z: {pose.orientation.z}, w: {pose.orientation.w} }} }}' - process = await asyncio.create_subprocess_exec('gz', 'service', '-s', '/world/default/create', '--reqtype', 'gz.msgs.EntityFactory', '--reptype', 'gz.msgs.Boolean', '--timeout', '2000', '--req', req_payload, stdout=asyncio.subprocess.PIPE, stderr=asyncio.subprocess.PIPE) + process = await asyncio.create_subprocess_exec('gz', 'service', '-s', '/world/default/create', '--reqtype', 'gz.msgs.EntityFactory', '--reptype', 'gz.msgs.Boolean', '--timeout', str(self._SPAWN_TIMEOUT_MS), '--req', req_payload, stdout=asyncio.subprocess.PIPE, stderr=asyncio.subprocess.PIPE) _, stderr = await process.communicate() if process.returncode == 0: self._spawned_names.add(name) return True + + # A non-zero return covers both a timed-out ack and an outright + # rejection, but the create may have landed server-side anyway. + # Reconcile against the live model list: track it if it exists, else + # best-effort delete in case the create lands just after this check, + # so a slow ack never leaves an orphan that survives every reset. + if name in await self._list_models(): + self._spawned_names.add(name) + return True + self._logger.error(f"Failed to spawn {name}. Error: {stderr.decode().strip()}") + req = DeleteEntity.Request() + req.entity = EntityMsg(name=name, type=EntityMsg.MODEL) + await self._service_delete_entity.call_timeout(req) return False return await self._spawn_sdf(name, model.description, pose) From 862579faae9f43f5e555ece6b49ca9e7bf9ea49e Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 18:32:24 +0200 Subject: [PATCH 17/24] adjust ped height based on model --- utils/rviz_utils/package.xml | 2 + utils/rviz_utils/rviz_utils/hri/body_pool.py | 50 +++++++++++++++++++ .../rviz_utils/scripts/hri_producer.py | 2 +- 3 files changed, 53 insertions(+), 1 deletion(-) diff --git a/utils/rviz_utils/package.xml b/utils/rviz_utils/package.xml index f43a0724..43a18760 100644 --- a/utils/rviz_utils/package.xml +++ b/utils/rviz_utils/package.xml @@ -71,6 +71,8 @@ robot_state_publisher tf2_ros xacro + urdfdom_py + python3-numpy hri_rviz task_generator diff --git a/utils/rviz_utils/rviz_utils/hri/body_pool.py b/utils/rviz_utils/rviz_utils/hri/body_pool.py index 85341cbf..225ece6b 100644 --- a/utils/rviz_utils/rviz_utils/hri/body_pool.py +++ b/utils/rviz_utils/rviz_utils/hri/body_pool.py @@ -18,14 +18,17 @@ import dataclasses import logging +import math import os import subprocess import tempfile from typing import TYPE_CHECKING +import numpy as np import xacro import yaml from ament_index_python.packages import get_package_share_directory +from urdf_parser_py import urdf as urdf_parser if TYPE_CHECKING: import rclpy.node @@ -35,6 +38,45 @@ _DEFAULT_HEIGHT = 1.65 +def _origin_matrix(origin: urdf_parser.Pose | None) -> np.ndarray: + """4x4 homogeneous transform for a URDF joint origin (xyz + rpy).""" + m = np.eye(4) + if origin is None: + return m + r, p, y = origin.rpy or (0.0, 0.0, 0.0) + cr, sr = math.cos(r), math.sin(r) + cp, sp = math.cos(p), math.sin(p) + cy, sy = math.cos(y), math.sin(y) + m[:3, :3] = ( + (cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr), + (sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr), + (-sp, cp * sr, cp * cr), + ) + m[:3, 3] = origin.xyz or (0.0, 0.0, 0.0) + return m + + +def _hip_to_foot_offset(urdf_xml: str) -> float: + """Distance to lift the hip-origin root so the lowest link frame sits at z=0. + + Forward kinematics with every joint at zero (the standing pose), composing + only joint origin transforms. The rig root `body` is the REP-155 hip frame, + so the legs reach below it, this is how far down the lowest link extends. + """ + robot = urdf_parser.URDF.from_xml_string(urdf_xml) + root = robot.get_root() + world: dict[str, np.ndarray] = {root: np.eye(4)} + min_z = 0.0 + stack = [root] + while stack: + parent = stack.pop() + for joint_name, child in robot.child_map.get(parent, ()): + world[child] = world[parent] @ _origin_matrix(robot.joint_map[joint_name].origin) + min_z = min(min_z, world[child][2, 3]) + stack.append(child) + return -min_z + + @dataclasses.dataclass class _Slot: proc: subprocess.Popen[bytes] @@ -62,6 +104,7 @@ def __init__( self._slots: list[_Slot] = [] self._id_to_slot: dict[str, _Slot] = {} self._urdf: dict[str, str] = {} + self._foot_offset: dict[str, float] = {} self._hd_share: str = get_package_share_directory("human_description") def _generate_urdf(self, body_id: str, height: float) -> str: @@ -136,6 +179,7 @@ def assign(self, body_id: str, height: float = _DEFAULT_HEIGHT) -> bool: urdf = self._generate_urdf(body_id, height) self._urdf[body_id] = urdf + self._foot_offset[body_id] = _hip_to_foot_offset(urdf) free_slot: _Slot | None = None for slot in self._slots: @@ -169,6 +213,7 @@ def release(self, body_id: str) -> None: """Release a body_id, terminating its rsp slot.""" slot = self._id_to_slot.pop(body_id, None) self._urdf.pop(body_id, None) + self._foot_offset.pop(body_id, None) if slot is None: return self._terminate_slot(slot) @@ -176,6 +221,10 @@ def release(self, body_id: str) -> None: def urdf_for(self, body_id: str) -> str | None: return self._urdf.get(body_id) + def foot_offset(self, body_id: str) -> float: + """Hip-to-floor lift for body_id, 0.0 if not assigned.""" + return self._foot_offset.get(body_id, 0.0) + def active_ids(self) -> frozenset[str]: return frozenset(self._id_to_slot.keys()) @@ -206,3 +255,4 @@ def teardown(self) -> None: self._slots.clear() self._id_to_slot.clear() self._urdf.clear() + self._foot_offset.clear() diff --git a/utils/rviz_utils/rviz_utils/scripts/hri_producer.py b/utils/rviz_utils/rviz_utils/scripts/hri_producer.py index 56b98ea6..9e65894e 100644 --- a/utils/rviz_utils/rviz_utils/scripts/hri_producer.py +++ b/utils/rviz_utils/rviz_utils/scripts/hri_producer.py @@ -194,7 +194,7 @@ def _on_peds(self, msg: Pedestrians) -> None: tf.child_frame_id = f"body_{bid}" tf.transform.translation.x = ped.pose.position.x tf.transform.translation.y = ped.pose.position.y - tf.transform.translation.z = ped.pose.position.z + tf.transform.translation.z = ped.pose.position.z + self._pool.foot_offset(bid) tf.transform.rotation = ped.pose.orientation self._tf_broadcaster.sendTransform(tf) From d9774926bed155037e12d83f6e79b8a788217898 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 19:54:14 +0200 Subject: [PATCH 18/24] fix gazebo peds robustness (vol. 2) --- .../sim/gazebo_simulator/gazebo_simulator.py | 3 +- .../simulators/human/__init__.py | 33 ++++++++++++++++--- .../human/arena_humansim/arena_humansim.py | 1 + .../simulators/human/hunav/hunav.py | 1 + .../msgs/arena_people_msgs/msg/Pedestrian.msg | 3 ++ 5 files changed, 35 insertions(+), 6 deletions(-) diff --git a/arena_runtime/arena_runtime/arena_runtime/sim/gazebo_simulator/gazebo_simulator.py b/arena_runtime/arena_runtime/arena_runtime/sim/gazebo_simulator/gazebo_simulator.py index cf66d571..2a9a6929 100644 --- a/arena_runtime/arena_runtime/arena_runtime/sim/gazebo_simulator/gazebo_simulator.py +++ b/arena_runtime/arena_runtime/arena_runtime/sim/gazebo_simulator/gazebo_simulator.py @@ -227,7 +227,8 @@ async def obstacle_spawn(self, obstacles: Sequence[Obstacle]) -> Sequence[bool]: return await asyncio.gather(*map(self._spawn_entity, obstacles)) async def pedestrian_spawn(self, pedestrians: Sequence[DynamicObstacle]) -> Sequence[bool]: - return await asyncio.gather(*map(self._spawn_entity, pedestrians)) + # Ped actors are created and removed by PedSkeletonPlugin from arena_peds. + return tuple(True for _ in pedestrians) async def robot_spawn(self, robots: Sequence[Robot]) -> Sequence[bool]: diff --git a/task_generator/task_generator/simulators/human/__init__.py b/task_generator/task_generator/simulators/human/__init__.py index 1bb80179..270b3fb3 100644 --- a/task_generator/task_generator/simulators/human/__init__.py +++ b/task_generator/task_generator/simulators/human/__init__.py @@ -66,6 +66,7 @@ def __init__(self, *args: object, namespace: Namespace, simulator: BaseSim, real self._wall_counter = itertools.count() self._known_regions: dict[str, Region] = {} self._warned_unresolved_models: set[str] = set() + self._ped_model_uris: dict[str, str] = {} self._arena_peds_publisher = self.node.create_publisher(Pedestrians, self._namespace("arena_peds"), 10) self._marker_publisher = self.node.create_publisher( @@ -112,6 +113,7 @@ def publish_arena_peds(self, msg: Pedestrians) -> None: del self._gait_prev_stamp[stale] for ped in msg.pedestrians: + ped.model_uri = self._ped_model_uris.get(ped.name, "") if ped.joint_state.name: continue prev = self._gait_prev_stamp.get(ped.id) @@ -250,13 +252,22 @@ async def spawn_dynamic_obstacles(self, obstacles: typing.Sequence[DynamicObstac _PEDESTRIAN_FALLBACK: typing.ClassVar[str] = "arenian" async def _ensure_spawnable(self, obstacles: Sequence[DynamicObstacle]) -> Sequence[DynamicObstacle]: - """Swap unresolvable ped models for _PEDESTRIAN_FALLBACK.""" + """Swap unresolvable ped models for _PEDESTRIAN_FALLBACK and record each + ped's resolved SDF path, which publish_arena_peds stamps as model_uri so + PedSkeletonPlugin can create the actor.""" + + async def _sdf_path(obs: DynamicObstacle) -> str | None: + view = await obs.model.resolve() + model = await view.model.get(ModelType.SDF) + if model.type is ModelType.UNKNOWN or model.path is None: + return None + return str(model.path) async def _resolve(obs: DynamicObstacle) -> DynamicObstacle: try: - view = await obs.model.resolve() - model = await view.model.get(ModelType.SDF) - if model.type is not ModelType.UNKNOWN: + path = await _sdf_path(obs) + if path is not None: + self._record_ped_model(obs, path) return obs except Exception as e: if obs.model.name not in self._warned_unresolved_models: @@ -270,10 +281,22 @@ async def _resolve(obs: DynamicObstacle) -> DynamicObstacle: self._logger.warning( f"pedestrian model {obs.model.name!r} has no SDF; using fallback", ) - return attrs.evolve(obs, model=PedestrianIdentifier.parse(self._PEDESTRIAN_FALLBACK)) + fallback = attrs.evolve(obs, model=PedestrianIdentifier.parse(self._PEDESTRIAN_FALLBACK)) + try: + fallback_path = await _sdf_path(fallback) + except Exception: + fallback_path = None + self._record_ped_model(fallback, fallback_path or "") + return fallback return await asyncio.gather(*(_resolve(o) for o in obstacles)) + def _record_ped_model(self, obs: DynamicObstacle, model_uri: str) -> None: + """Map both a ped's name and sim_path to its SDF path, since the published + ped name is one or the other depending on the simulator.""" + self._ped_model_uris[obs.name] = model_uri + self._ped_model_uris[obs.sim_path] = model_uri + async def spawn_world( self, walls: Sequence[Wall], diff --git a/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py b/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py index 0f25646f..0e8e71a8 100644 --- a/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py +++ b/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py @@ -869,6 +869,7 @@ async def _remove_pedestrians_impl(self) -> bool: self._bridge_agent_ids.clear() self._flow_agent_ids.clear() self._agent_names.clear() + self._ped_model_uris.clear() self._prev_agent_states = None self._curr_agent_states = None self._arena_pedestrians = Pedestrians() diff --git a/task_generator/task_generator/simulators/human/hunav/hunav.py b/task_generator/task_generator/simulators/human/hunav/hunav.py index 4a0df110..bfaf79a5 100644 --- a/task_generator/task_generator/simulators/human/hunav/hunav.py +++ b/task_generator/task_generator/simulators/human/hunav/hunav.py @@ -502,6 +502,7 @@ def _clear_local_data(self): # Clear agents container (if not already done) self._agents_container.agents.clear() # type: ignore self._arena_pedestrians_container.pedestrians.clear() # type: ignore + self._ped_model_uris.clear() self._last_updated_agents = None self._last_smooth_yaws = {} diff --git a/utils/msgs/arena_people_msgs/msg/Pedestrian.msg b/utils/msgs/arena_people_msgs/msg/Pedestrian.msg index d145d91b..128d9967 100644 --- a/utils/msgs/arena_people_msgs/msg/Pedestrian.msg +++ b/utils/msgs/arena_people_msgs/msg/Pedestrian.msg @@ -21,3 +21,6 @@ uint8 animation_state # Empty = synthesize fallback gait, non-empty = upstream override. sensor_msgs/JointState joint_state +# Resolved local SDF path for this ped's actor, read once per name by the spawner. +string model_uri + From 3fd32aaa052d9e1e77124616004dfaef0099ade3 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 19:56:57 +0200 Subject: [PATCH 19/24] startup robustness --- .../arena_runtime/arena_runtime/README.md | 23 +++-- .../arena_runtime/arena_runtime/arena_node.py | 40 ++++++--- .../arena_runtime/arena_runtime/registry.py | 26 +++++- .../tests/unit/test_sweep_policy.py | 87 +++++++++++++++++++ .../tree/World/Scenario.py | 3 +- 5 files changed, 154 insertions(+), 25 deletions(-) create mode 100644 arena_runtime/arena_runtime/tests/unit/test_sweep_policy.py diff --git a/arena_runtime/arena_runtime/arena_runtime/README.md b/arena_runtime/arena_runtime/arena_runtime/README.md index 65a021ac..58814fd1 100644 --- a/arena_runtime/arena_runtime/arena_runtime/README.md +++ b/arena_runtime/arena_runtime/arena_runtime/README.md @@ -16,7 +16,7 @@ Launched by `arena_runtime.launch.py` (namespace `/arena`, node name `arena`). Allocates integer env IDs and translates world extents into non-overlapping simulator slots. State per record: `env_id`, `fqn`, `placed`, `reference`, -`slot_extent`, `prespawn`, `ready`, `draining`, `bootstrapped`, `last_heartbeat`. +`slot_extent`, `prespawn`, `ready`, `draining`, `last_heartbeat`. Two-phase allocation: @@ -87,8 +87,12 @@ Action: `purge_env` ([`PurgeEnv.action`](../../arena_runtime_msgs/action/PurgeEn `ArenaNode` also subscribes per env to `//transition_event` (to track ACTIVE/INACTIVE/FINALIZED state) and `//state/heartbeat` (to detect -stale envs). A periodic timer evicts any bootstrapped env whose heartbeat exceeds -`heartbeat_timeout_sec` (default 5 s). +stale envs). A periodic timer applies `sweep_verdict` per record: a managed env +whose wrapper process exited is evicted immediately, a pre-ready env (still +bootstrapping, e.g. loading its world) is only evicted after +`bootstrap_timeout_sec` (default 600 s) of heartbeat silence, and a ready env +is evicted after `heartbeat_timeout_sec` (default 5 s), stretched to +`reset_hold_timeout_sec` (default 30 s) while it holds a "reset" hold. When `env_n > 0` at startup the node self-orchestrates an initial fleet via `_spawn_initial_envs`; each spawned env has `headless=false` (stdout visible). @@ -104,14 +108,15 @@ When `env_n > 0` at startup the node self-orchestrates an initial fleet via `reallocated=true` in the response means the env must translate all entity coordinates by the new `reference` offset. 4. **Heartbeat** — env publishes [`Heartbeat.msg`](../../arena_runtime_msgs/msg/Heartbeat.msg) - on `/state/heartbeat`; `arena_node` marks the record `bootstrapped` on - first receipt and resets the timeout clock on each tick. + on `/state/heartbeat`; `arena_node` resets the timeout clock on each tick. + The clock starts at `reserve()` time, so an env is covered from registration. 5. **Despawn** — caller sends `despawn_env`; `arena_node` publishes a `ShutdownRequest` the env observes and acts on via its own lifecycle. -6. **Eviction** — on heartbeat timeout or lifecycle FINALIZED, `arena_node` - calls `start_eviction` (marks draining, releases holds, cleans up namespace - via `SimLifecycle`), then `complete_eviction` (frees the ID for reuse) and - publishes a final `ShutdownRequest`. +6. **Eviction** — on a `sweep_verdict` reason or lifecycle FINALIZED, + `arena_node` calls `start_eviction` (marks draining, releases holds), + publishes a `ShutdownRequest`, awaits env disposal (reaping the wrapper + process for managed envs), and only then cleans up the namespace via + `SimLifecycle` and calls `complete_eviction` (frees the ID for reuse). ## See also diff --git a/arena_runtime/arena_runtime/arena_runtime/arena_node.py b/arena_runtime/arena_runtime/arena_runtime/arena_node.py index c68b97a9..1bacb249 100644 --- a/arena_runtime/arena_runtime/arena_runtime/arena_node.py +++ b/arena_runtime/arena_runtime/arena_runtime/arena_node.py @@ -33,7 +33,7 @@ from arena_runtime.constants import SimSimulator from arena_runtime.holds import HoldRegistry -from arena_runtime.registry import EnvRegistry, _extent_eq +from arena_runtime.registry import EnvRegistry, _extent_eq, sweep_verdict from arena_runtime.sim import LifecycleRegistry, SimLifecycle from arena_runtime.sim.dummy_simulator import DummyHost @@ -125,6 +125,7 @@ async def setup(self) -> None: period = self.rosparam[float].get("heartbeat_period_sec", 1.0) self.rosparam[float].get("heartbeat_timeout_sec", 5.0) self.rosparam[float].get("reset_hold_timeout_sec", 30.0) + self.rosparam[float].get("bootstrap_timeout_sec", 600.0) slot_buffer = self.rosparam[float].get("slot_buffer", 5.0) self._env_registry = EnvRegistry(slot_buffer=slot_buffer) @@ -345,15 +346,24 @@ def _cb_heartbeat_check(self) -> None: async def _check_heartbeats(self) -> None: timeout = self.rosparam[float].get("heartbeat_timeout_sec", 5.0) reset_timeout = self.rosparam[float].get("reset_hold_timeout_sec", 30.0) + bootstrap_timeout = self.rosparam[float].get("bootstrap_timeout_sec", 600.0) now = self.wall_time for env_id, record in self._env_registry.items(): - if record.draining or not record.bootstrapped: - continue elapsed = (now - Time.from_msg(record.last_heartbeat)).to_seconds() + env = self._envs.get(env_id) + process_alive = env.popen.poll() is None if env is not None and env.popen is not None else None # an env mid-reset blocks its loop for seconds; tolerate longer while it holds "reset", but still cap it - effective = reset_timeout if self._holds.has(record.fqn, "reset") else timeout - if elapsed > effective: - await self._evict(env_id, "heartbeat_timeout") + reason = sweep_verdict( + record, + elapsed=elapsed, + has_reset_hold=self._holds.has(record.fqn, "reset"), + process_alive=process_alive, + heartbeat_timeout=timeout, + reset_timeout=reset_timeout, + bootstrap_timeout=bootstrap_timeout, + ) + if reason is not None: + await self._evict(env_id, reason) async def _evict(self, env_id: int, reason: str) -> None: if not self._env_registry.start_eviction(env_id): @@ -373,16 +383,20 @@ async def _evict(self, env_id: int, reason: str) -> None: await self._lifecycle.unpause() self._publish_state() - await self._lifecycle.cleanup_namespace(self._lifecycle.env_prefix(env_id)) + self._publish_shutdown_request(env_id, reason) env = self._envs.pop(env_id, None) - self._env_registry.complete_eviction(env_id) - self._publish_envs() + if env is not None: + if env.popen is not None: + await env.dispose(self, grace_seconds=15.0) + else: + await env.dispose(self, grace_seconds=0.0) + await asyncio.sleep(2.0) # give external env time to observe shutdown_request - self._publish_shutdown_request(env_id, reason) + await self._lifecycle.cleanup_namespace(self._lifecycle.env_prefix(env_id)) - if env is not None: - asyncio.create_task(env.dispose(self, grace_seconds=15.0)) + self._env_registry.complete_eviction(env_id) + self._publish_envs() self.get_logger().info(f"evicted env_{env_id} ({reason})") @@ -489,6 +503,7 @@ async def _cb_register_env( env_id, namespace = self._env_registry.reserve( requested_env_id=requested, requested_ns=requested_ns, + now=self.wall_time.to_msg(), ) except ValueError as e: response.success = False @@ -597,6 +612,7 @@ async def _cb_spawn_env( env_id, namespace = self._env_registry.reserve( requested_env_id=None, requested_ns=requested_ns, + now=self.wall_time.to_msg(), ) except ValueError as e: response.success = False diff --git a/arena_runtime/arena_runtime/arena_runtime/registry.py b/arena_runtime/arena_runtime/arena_runtime/registry.py index d2222c44..bdcbea4b 100644 --- a/arena_runtime/arena_runtime/arena_runtime/registry.py +++ b/arena_runtime/arena_runtime/arena_runtime/registry.py @@ -27,7 +27,6 @@ class EnvRecord: prespawn: tuple[float, float] = (0.0, 0.0) ready: bool = False draining: bool = False - bootstrapped: bool = False last_heartbeat: builtin_interfaces.msg.Time = attrs.Factory(builtin_interfaces.msg.Time) @@ -59,6 +58,8 @@ def reserve( self, requested_env_id: int | None = None, requested_ns: str | None = None, + *, + now: builtin_interfaces.msg.Time, ) -> tuple[int, str]: if requested_env_id is not None: if requested_env_id in self._records: @@ -82,6 +83,7 @@ def reserve( self._records[env_id] = EnvRecord( env_id=env_id, fqn=f"/{namespace}", + last_heartbeat=now, ) return env_id, namespace @@ -229,7 +231,6 @@ def update_heartbeat(self, env_id: int, stamp: builtin_interfaces.msg.Time) -> N record = self._records.get(env_id) if record is not None: record.last_heartbeat = stamp - record.bootstrapped = True def update_ready(self, env_id: int, ready: bool) -> None: record = self._records.get(env_id) @@ -253,3 +254,24 @@ def snapshot(self) -> list[arena_runtime_msgs.msg.EnvRecord]: msg.last_heartbeat = record.last_heartbeat result.append(msg) return result + + +def sweep_verdict( + record: EnvRecord, + *, + elapsed: float, + has_reset_hold: bool, + process_alive: bool | None, + heartbeat_timeout: float, + reset_timeout: float, + bootstrap_timeout: float, +) -> str | None: + """Return the eviction reason, or None to keep the env alive.""" + if record.draining: + return None + if process_alive is False: + return "process_exited" + if not record.ready: + return "bootstrap_timeout" if elapsed > bootstrap_timeout else None + budget = reset_timeout if has_reset_hold else heartbeat_timeout + return "heartbeat_timeout" if elapsed > budget else None diff --git a/arena_runtime/arena_runtime/tests/unit/test_sweep_policy.py b/arena_runtime/arena_runtime/tests/unit/test_sweep_policy.py new file mode 100644 index 00000000..e3a96486 --- /dev/null +++ b/arena_runtime/arena_runtime/tests/unit/test_sweep_policy.py @@ -0,0 +1,87 @@ +"""Pure-logic tests for sweep_verdict and reserve() heartbeat stamping. + +Skipped if arena_runtime / builtin_interfaces aren't importable (no sourced +overlay). +""" +from __future__ import annotations + +import pytest + +pytest.importorskip("builtin_interfaces.msg") +pytest.importorskip("arena_runtime.registry") + +import builtin_interfaces.msg # noqa: E402 + +from arena_runtime.registry import EnvRecord, EnvRegistry, sweep_verdict # noqa: E402 + + +def _record(*, ready: bool = True, draining: bool = False) -> EnvRecord: + return EnvRecord(env_id=0, fqn="/test", ready=ready, draining=draining) + + +def _time(sec: int = 0) -> builtin_interfaces.msg.Time: + t = builtin_interfaces.msg.Time() + t.sec = sec + return t + + +def _verdict(record: EnvRecord, elapsed: float, *, has_reset_hold: bool = False, process_alive: bool | None = None) -> str | None: + return sweep_verdict( + record, + elapsed=elapsed, + has_reset_hold=has_reset_hold, + process_alive=process_alive, + heartbeat_timeout=5.0, + reset_timeout=30.0, + bootstrap_timeout=600.0, + ) + + +def test_draining_returns_none(): + assert _verdict(_record(draining=True), 9999.0) is None + + +def test_process_exited_trumps_ready(): + assert _verdict(_record(ready=True), 0.1, process_alive=False) == "process_exited" + + +def test_pre_ready_under_bootstrap_timeout_returns_none(): + assert _verdict(_record(ready=False), 599.9) is None + + +def test_pre_ready_over_bootstrap_timeout_evicts(): + assert _verdict(_record(ready=False), 600.1) == "bootstrap_timeout" + + +def test_ready_no_hold_under_timeout_returns_none(): + assert _verdict(_record(ready=True), 4.9) is None + + +def test_ready_no_hold_over_timeout_evicts(): + assert _verdict(_record(ready=True), 5.1) == "heartbeat_timeout" + + +def test_ready_reset_hold_mid_window_returns_none(): + assert _verdict(_record(ready=True), 20.0, has_reset_hold=True) is None + + +def test_ready_reset_hold_over_reset_timeout_evicts(): + assert _verdict(_record(ready=True), 30.1, has_reset_hold=True) == "heartbeat_timeout" + + +def test_process_alive_none_falls_through_to_phase(): + assert _verdict(_record(ready=True), 5.1, process_alive=None) == "heartbeat_timeout" + assert _verdict(_record(ready=True), 4.9, process_alive=None) is None + + +def test_process_alive_true_pre_ready_still_uses_bootstrap_budget(): + assert _verdict(_record(ready=False), 599.9, process_alive=True) is None + assert _verdict(_record(ready=False), 600.1, process_alive=True) == "bootstrap_timeout" + + +def test_reserve_stamps_last_heartbeat(): + reg = EnvRegistry() + env_id, _ns = reg.reserve(now=_time(sec=42)) + record = reg.get(env_id) + assert record is not None + assert record.last_heartbeat.sec == 42 diff --git a/arena_simulation_setup/src/arena_simulation_setup/tree/World/Scenario.py b/arena_simulation_setup/src/arena_simulation_setup/tree/World/Scenario.py index d3c93d4c..57a3af69 100644 --- a/arena_simulation_setup/src/arena_simulation_setup/tree/World/Scenario.py +++ b/arena_simulation_setup/src/arena_simulation_setup/tree/World/Scenario.py @@ -142,8 +142,7 @@ def load(self, converter: ArenaConverter = converter) -> Scenario: try: scenario = self.load_legacy() warnings.warn( - f"Scenario {self.scenario_path}: new-format load failed, falling back to legacy. Underlying error:\n" - f"{''.join(traceback.format_exception(type(load_exc), load_exc, load_exc.__traceback__))}", + f"Scenario {self.scenario_path}: new-format load failed, falling back to legacy. Underlying error:\n{''.join(traceback.format_exception(type(load_exc), load_exc, load_exc.__traceback__))}", UserWarning, stacklevel=2, ) From 6f52a009e578af6b65d5864efac19f27d2aa75c2 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Thu, 11 Jun 2026 22:51:41 +0200 Subject: [PATCH 20/24] FIX VIIIIIZ --- _meta/tools/viz | 9 +++++++-- arena_bringup/arena_bringup/viz_backends.py | 10 +++++----- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/_meta/tools/viz b/_meta/tools/viz index 127c6fb4..9b3f543b 100755 --- a/_meta/tools/viz +++ b/_meta/tools/viz @@ -57,7 +57,8 @@ def discover_envs() -> list[str]: def matches(ns: str, target: str) -> bool: - return ns == target or ns == f"/{target}" or os.path.basename(ns) == target + 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]: @@ -156,7 +157,7 @@ def main() -> int: if len(nodes) > 1: print("arena viz: multiple envs running, pass or --all:", file=sys.stderr) for n in nodes: - print(f" {n}", file=sys.stderr) + print(f" {os.path.dirname(n)}", file=sys.stderr) return 1 chosen = nodes[0] else: @@ -167,3 +168,7 @@ def main() -> int: print(f"arena viz: attaching to {chosen}", file=sys.stderr) os.execvp(cmds[0][0], cmds[0]) return attach_all([chosen], viz_args) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/arena_bringup/arena_bringup/viz_backends.py b/arena_bringup/arena_bringup/viz_backends.py index 1f3410a7..389da096 100644 --- a/arena_bringup/arena_bringup/viz_backends.py +++ b/arena_bringup/arena_bringup/viz_backends.py @@ -47,11 +47,11 @@ def fleet_size(ns: str) -> int: def env_idx_from_ns(ns: str) -> int: - """Parse the trailing integer from an env namespace (`/env_3` → 3). 0 on failure.""" - base = os.path.basename(ns) - head, _, tail = base.rpartition('_') - if head and tail.isdigit(): - return int(tail) + """Parse the env index from a namespace (`/arena/env_3/task_generator_node` → 3). 0 on failure.""" + for part in reversed(ns.strip('/').split('/')): + head, _, tail = part.rpartition('_') + if head and tail.isdigit(): + return int(tail) return 0 From 714372ebb184d6c8fd6cb77a7e5e8e1390932e4c Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Fri, 12 Jun 2026 01:18:16 +0200 Subject: [PATCH 21/24] FIX PEEEEEDS --- arena_isaac | 2 +- .../arena_runtime/sim/isaac_simulator.py | 27 ++++++++----------- 2 files changed, 12 insertions(+), 17 deletions(-) diff --git a/arena_isaac b/arena_isaac index 619ee5dd..78e7e440 160000 --- a/arena_isaac +++ b/arena_isaac @@ -1 +1 @@ -Subproject commit 619ee5dd88d0fba654f0416f9097ad4088004b35 +Subproject commit 78e7e4402ebcd85358699c0bac57d6df11eb68c5 diff --git a/arena_runtime/arena_runtime/arena_runtime/sim/isaac_simulator.py b/arena_runtime/arena_runtime/arena_runtime/sim/isaac_simulator.py index 67bc669a..453131e8 100644 --- a/arena_runtime/arena_runtime/arena_runtime/sim/isaac_simulator.py +++ b/arena_runtime/arena_runtime/arena_runtime/sim/isaac_simulator.py @@ -193,7 +193,6 @@ def __init__(self, *args: object, **kwargs: object) -> None: env_prefix = f"env_{self._env_id}" self._NS_PRIM = Namespace(env_prefix)('Obstacles') - self._NS_PEDESTRIAN = Namespace(env_prefix)('Pedestrians') self._NS_ROBOT = Namespace(env_prefix)('Robots') self._NS_WALL = Namespace(env_prefix)('Walls') self._NS_FLOOR = Namespace(env_prefix)('Floors') @@ -420,7 +419,7 @@ async def pedestrian_move(self, pedestrians: Sequence[DynamicObstacle]) -> Seque req = MovePedestrians.Request( pedestrians=[ Pedestrian( - name=self._NS_PEDESTRIAN(p.name), + name=p.sim_path, pose=p.pose.to_msg(), ) for p in pedestrians @@ -445,7 +444,7 @@ async def obstacle_delete(self, obstacles: Sequence[Obstacle]) -> Sequence[bool] return await asyncio.gather(*(self._delete_entity(self._NS_PRIM(o.name)) for o in obstacles)) async def pedestrian_delete(self, pedestrians: Sequence[DynamicObstacle]) -> Sequence[bool]: - res = await self._clients.DeletePedestrians.call_timeout(DeletePedestrians.Request(names=[self._NS_PEDESTRIAN(p.name) for p in pedestrians])) + res = await self._clients.DeletePedestrians.call_timeout(DeletePedestrians.Request(names=[p.sim_path for p in pedestrians])) if res is None: return tuple(False for _ in pedestrians) return tuple(r == DeletePedestrians.Response.SUCCESS for r in res.results) @@ -686,7 +685,7 @@ async def pedestrian_spawn(self, pedestrians: Sequence[DynamicObstacle]) -> Sequ items.append( SpawnPedestrian( pedestrian=Pedestrian( - name=self._NS_PEDESTRIAN(pedestrian.name), + name=pedestrian.sim_path, pose=pedestrian.pose.to_msg(), ), model_ref=available_models[model_name], @@ -704,7 +703,7 @@ async def pedestrian_spawn(self, pedestrians: Sequence[DynamicObstacle]) -> Sequ arena_people_msgs.msg.Pedestrians( pedestrians=[ arena_people_msgs.msg.Pedestrian( - name=ped.name, + name=ped.sim_path, pose=ped.pose.to_msg(), ) for status, ped in zip(success, pedestrians, strict=False) @@ -716,20 +715,16 @@ async def pedestrian_spawn(self, pedestrians: Sequence[DynamicObstacle]) -> Sequ return success async def pedestrian_update(self, pedestrians: arena_people_msgs.msg.Pedestrians) -> Sequence[bool]: - req = UpdatePedestrians.Request( - pedestrians=[ - Pedestrian( - name=self._NS_PEDESTRIAN(ped.name), - pose=ped.pose, - twist=ped.twist, - ) - for ped in pedestrians.pedestrians - ] - ) + # Published ped names are already sim_paths, matching the spawn key. + req = UpdatePedestrians.Request(pedestrians=list(pedestrians.pedestrians)) res = await self._clients.UpdatePedestrians.call_timeout(req) if res is None: return tuple(False for _ in pedestrians.pedestrians) - return tuple(r == UpdatePedestrians.Response.SUCCESS for r in res.results) + results = tuple(r == UpdatePedestrians.Response.SUCCESS for r in res.results) + if not all(results): + missed = [p.name for p, ok in zip(pedestrians.pedestrians, results, strict=False) if not ok] + self._logger.warning(f"UpdatePedestrians missed {missed}", throttle_duration_sec=10.0) + return results async def _delete_entity(self, name: str) -> bool: self._logger.debug(f"Attempting to delete prim {name}") From efdc462aa28241b69d74faa2cc3f536b6fa4d7d3 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Fri, 12 Jun 2026 19:20:02 +0200 Subject: [PATCH 22/24] latency-compensate ped commands --- arena_isaac | 2 +- .../arena_runtime/arena_runtime/sim/isaac_simulator.py | 2 +- utils/msgs/arena_people_msgs/srv/UpdatePedestrians.srv | 2 ++ 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/arena_isaac b/arena_isaac index 78e7e440..15a6b0b9 160000 --- a/arena_isaac +++ b/arena_isaac @@ -1 +1 @@ -Subproject commit 78e7e4402ebcd85358699c0bac57d6df11eb68c5 +Subproject commit 15a6b0b9661185d75b34f78acf965c212904ffa6 diff --git a/arena_runtime/arena_runtime/arena_runtime/sim/isaac_simulator.py b/arena_runtime/arena_runtime/arena_runtime/sim/isaac_simulator.py index 453131e8..ab3465a6 100644 --- a/arena_runtime/arena_runtime/arena_runtime/sim/isaac_simulator.py +++ b/arena_runtime/arena_runtime/arena_runtime/sim/isaac_simulator.py @@ -716,7 +716,7 @@ async def pedestrian_spawn(self, pedestrians: Sequence[DynamicObstacle]) -> Sequ async def pedestrian_update(self, pedestrians: arena_people_msgs.msg.Pedestrians) -> Sequence[bool]: # Published ped names are already sim_paths, matching the spawn key. - req = UpdatePedestrians.Request(pedestrians=list(pedestrians.pedestrians)) + req = UpdatePedestrians.Request(pedestrians=list(pedestrians.pedestrians), stamp=pedestrians.header.stamp) res = await self._clients.UpdatePedestrians.call_timeout(req) if res is None: return tuple(False for _ in pedestrians.pedestrians) diff --git a/utils/msgs/arena_people_msgs/srv/UpdatePedestrians.srv b/utils/msgs/arena_people_msgs/srv/UpdatePedestrians.srv index 825cd0df..3fa149d2 100644 --- a/utils/msgs/arena_people_msgs/srv/UpdatePedestrians.srv +++ b/utils/msgs/arena_people_msgs/srv/UpdatePedestrians.srv @@ -1,6 +1,8 @@ # Continuous per-tick state. Receivers treat as smooth motion — # no animation-phase reset, no trajectory re-seed. arena_people_msgs/Pedestrian[] pedestrians +# Sim-time stamp the poses were computed at, zero = unstamped. +builtin_interfaces/Time stamp --- uint8 SUCCESS=0 uint8 NOT_FOUND=1 From ee7ac05a252b4fd4f9107cb281866e857970a8ef Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Fri, 12 Jun 2026 19:20:22 +0200 Subject: [PATCH 23/24] re-seed pathpoints only on turns --- arena_isaac | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/arena_isaac b/arena_isaac index 15a6b0b9..969c44bd 160000 --- a/arena_isaac +++ b/arena_isaac @@ -1 +1 @@ -Subproject commit 15a6b0b9661185d75b34f78acf965c212904ffa6 +Subproject commit 969c44bd9f90ff1eb0b703d1188f854ba4983b78 From 1e13058b6ddc4b36997d2222b6d114bb30205b31 Mon Sep 17 00:00:00 2001 From: Vova Shcherbyna Date: Fri, 12 Jun 2026 20:05:53 +0200 Subject: [PATCH 24/24] stamp interpolated peds with pose time --- .../simulators/human/arena_humansim/arena_humansim.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py b/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py index 0e8e71a8..e4038faf 100644 --- a/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py +++ b/task_generator/task_generator/simulators/human/arena_humansim/arena_humansim.py @@ -302,9 +302,11 @@ def _interpolate_agent_states(self, now_ns: int) -> AgentStatesMsg | None: inv = 1.0 - alpha prev_by_id = {a.agent_id: a for a in prev.agents} + # stamp with the time the lerped pose corresponds to, receivers dead-reckon from it + pose_ns = prev_ns + int(alpha * dt_ns) msg = AgentStatesMsg() - msg.header.stamp.sec = int(now_ns // int(1e9)) - msg.header.stamp.nanosec = int(now_ns % int(1e9)) + msg.header.stamp.sec = int(pose_ns // int(1e9)) + msg.header.stamp.nanosec = int(pose_ns % int(1e9)) msg.header.frame_id = "map" for curr_a in curr.agents: