Unified policy service node (Variant A: reuse Ros2Input)#128
Closed
kingb wants to merge 2 commits into
Closed
Conversation
…nput - inputs/impl/ros2: map the "kill" string to StateCommand.KILL, giving an in-band emergency exit over ROS2 state input (parity with the joystick L1+R1 kill). Useful for any ros2/injected state-input setup. - policies/dual_mode: guard the SWITCH_MODE _mapping injection with hasattr. Only keyboard/joystick providers expose _mapping; Ros2Input (and injected ROS2 providers) map the "switch_mode" string to SWITCH_MODE natively and are intercepted via the dispatch patch. Without the guard, dual-mode crashed with AttributeError when the command provider was a ROS2 input.
Add a ROS2 service node that runs any holosoma policy (locomotion, WBT, ...) and supports runtime policy swapping -- the run_policy.py experience served over ROS2. Refactors the WBT-only holosoma_node into reusable pieces: - feedback.py: PolicyFeedbackPublisher -- policy-agnostic executed_cmd + heartbeat, driven by the per-tick _on_command_sent hook. Spins each node on its own SingleThreadedExecutor (rclpy.spin shares the global executor and collides with Ros2Input under "Executor is already spinning"). - target_bridge.py: DenseTargetBridge -- WBT-only CmdDense -> TargetSource, attached only to policies that expose _target_source. - service_node.py: builds the policy via _select_policy_class/DualModePolicy (no extension-only policy_type), attaches the dense bridge to WBT-family policies, wires feedback into each policy. Routes the Unitree SDK through its multiprocess interface (sdk_type unitree -> unitree_mp) so the SDK CycloneDDS runs in a child process and never shares this process rclpy (otherwise SDK init heap-corrupts). Strips ROS args before tyro so ros2 launch/run work. Loco input rides the existing Ros2Input (velocity_input/state_input=ros2); no new input code. Adds teleop_with_loco_policy.launch.py (fully parameterized for composition into a parent launch) + the policy_service_node entry point. Validated walking on a real G1 (ROS2 Jazzy) via the launch file.
Contributor
Author
Kedaro
reviewed
Jun 22, 2026
| default_value="policy_service", | ||
| description="ROS2 node name (override when composing multiple instances).", | ||
| ), | ||
| Node( |
Contributor
There was a problem hiding this comment.
My understanding is that service_node removes the need for holsoma_node, is that right? Or at least we can use the same entry point for WBT and loco? Do we still need separate launch files?
tomasz-lewicki
pushed a commit
that referenced
this pull request
Jun 24, 2026
## Summary Makes the Variant B service node (`policy_service_node`) the **single entrypoint** for both locomotion and WBT, and removes the separate `holosoma_node`. This is the unified entrypoint we deferred when picking Variant B in #128/#129. Builds on #124 (`dev/tomasz/tracker_service`) — **base this branch, not main.** ## How it works Most of the WBT path already worked through Variant B: `WholeBodyTrackingPolicy` honors `_target_source` attribute injection (set post-build, read only at runtime in `rl_inference()`), and `ServiceIONode` already subscribes `CmdDense` + serves `get_target()`. The one gap was **policy resolution** — the node resolved via core `_select_policy_class`, which didn't honor `config.task.policy_type` / the `holosoma.policies.by_type` entry-point group. ## Changes - **`dual_mode._select_policy_class`** — resolve `config.task.policy_type` against the `holosoma.policies.by_type` entry-point group *before* the existing robot_type/`motion_command` heuristic. Additive and generic: core reads an optional `policy_type` via `getattr` and resolves classes **by string** through the entry-point group, so it never imports or hardcodes any policy class. - **`teleop_with_holosoma_policy.launch.py`** — drive `policy_service_node` (was `holosoma_node`); keep the `input_type:=smplh|dense` selector + conditional retargeter; expose the `velocity_input`/`state_input`/`interface`/`domain_id`/`node_name` knobs. - **Delete `holosoma_node.py`** + its `console_scripts` entry. Its dense `TargetSource` behavior already lives in `ServiceIONode` (`_attach_target_source`). - **Tests** — resolution-order coverage for `_select_policy_class`; `_attach_target_source`/`_iter_policies` coverage (ROS-guarded so it skips off-container). Docs/diagram updated. ## Testing - `ruff` + `ruff-format`: clean. - `pytest src/holosoma_inference/.../test_select_policy_class.py`: 6/6 pass (real package). Service-node attach test skips off-ROS (runs in the inference CI container).
Contributor
Author
|
We went with implementation B #129 |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Adds a unified ROS2 policy service node that runs any holosoma policy (locomotion, WBT, …) and supports runtime policy swapping — the
run_policy.pyexperience, served over ROS2. Builds on #124, reusing itsholosoma_servicepackage,unitree_mpmultiprocess proxy,TargetSource, and_on_command_sentfeedback hook.This is Variant A (reuse
Ros2Input): the node touches no shared input code — loco velocity/state ride the existingRos2Input; the node only adds a WBT dense-target bridge and a policy-agnostic feedback publisher. A companion PR proposes Variant B (single node owns all I/O via aninjectedinput source) for comparison.What's here
service_node.py— builds the policy via_select_policy_class/DualModePolicy(no extension-onlypolicy_type), routes the Unitree SDK throughunitree_mp(CycloneDDS in a child process, isolated from this process's rclpy), strips ROS args before tyro soros2 launch/runwork.feedback.py—PolicyFeedbackPublisher: policy-agnosticexecuted_cmd+heartbeat, driven by the per-tick_on_command_senthook; each node spun on its ownSingleThreadedExecutor.target_bridge.py—DenseTargetBridge: WBT-onlyCmdDense→TargetSource, attached only to policies exposing_target_source(loco untouched).teleop_with_loco_policy.launch.py— fully parameterized (velocity_input/state_input/interface/domain_id/node_name) for composition into a parent launch.holosoma_inference: ROS2"kill"command (parity with joystick L1+R1), and ahasattrguard indual_modeso the SWITCH_MODE injection doesn't crash on providers without_mapping(ros2/injected).Testing
Validated walking on a real G1 (ROS2 Jazzy) via the
ros2 launchpath: velocity over/cmd_vel, state on the native G1 joystick,executed_cmdat 50 Hz,heartbeatstatus reflecting the active policy.