much better peds

_meta/docker/features/gazebo

@@ -20,7 +20,7 @@ source_fn(){
 
 
 help(){
-    echo "Usage: $name <install|update|uninstall|source>"
+    echo "Usage: $name <install|update|uninstall|source|launch>"
 }
 
 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

_meta/features/gazebo

@@ -55,7 +55,7 @@ uninstall(){
 
 # === MAIN SCRIPT ===
 help(){
-  echo "Usage: $name <install|update|uninstall|source>"
+  echo "Usage: $name <install|update|uninstall|source|launch>"
 }
 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

_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

_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

_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=$?

_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 <env_id> 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())

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
 
 

arena_bringup/launch/simulator/sim/README.md

@@ -9,34 +9,41 @@ 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
 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 <sim> launch ...'`,
+`on_exit=Shutdown`). The verb refuses unless the feature is installed
+(`arena feature <sim> 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/<name>/<name>.launch.py`.
-2. In `sim.launch.py`, call `launch_simulator.add("<name>", 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("<name>", ExecuteProcess(...))`
+   delegating to `arena feature <name> 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.<NAME>` entry in

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 <world> 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,19 +208,39 @@ def _launch_gazebo(context, *args, **kwargs):
         }],
     )
 
+    # Point gz-sim at <install>/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(
         [
             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,