Unified policy service node (Variant B: single node owns all I/O)#129
Merged
tomasz-lewicki merged 2 commits intoJun 22, 2026
Conversation
Core holosoma_inference changes enabling an external owner to drive a policy over ROS2: - config/task: add "injected" to InputSource. - inputs/create_input: "injected" returns a provider pre-attached by an external owner (policy._injected_velocity_input / _injected_command_provider) instead of constructing one -- so a ROS2 node that owns all I/O can supply the vel/state providers. - inputs/impl/ros2: map the "kill" string to StateCommand.KILL (in-band emergency exit over ROS2 state input, parity with joystick L1+R1). - policies/dual_mode: guard the SWITCH_MODE _mapping injection with hasattr; only keyboard/joystick providers expose _mapping, while ros2/injected providers map "switch_mode" natively and are intercepted via the dispatch patch. Without the guard, dual-mode crashed with AttributeError on 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. Unlike the reuse-Ros2Input design, a single ServiceIONode owns every subscription/publisher and injects the velocity+state providers into the policy (input source "injected"). - injected_inputs.py: InjectedRos2Input -- one object implementing BOTH VelCmdProvider and StateCommandProvider (like Ros2Input), subscribing on the node the service owns. A single object is required because the base policy shares one provider for both roles when velocity_input == state_input, then calls poll_commands on it. - service_node.py: ServiceIONode carries vel/state subscriptions, the WBT dense target, and executed_cmd/heartbeat feedback on one node, spun once. Builds the policy via _select_policy_class/DualModePolicy (no extension-only policy_type), injecting providers before __init__ (dual-mode injects the primary; the secondary shares via _shared_hardware_source). Routes the Unitree SDK through its multiprocess interface (unitree_mp) so the SDK CycloneDDS runs in a child process and never shares this process rclpy. Strips ROS args before tyro so ros2 launch/run work. Adds teleop_with_loco_policy.launch.py (fully parameterized) + the policy_service_node entry point. Validated walking on a real G1 (ROS2 Jazzy) via the launch file.
Contributor
Author
tomasz-lewicki
approved these changes
Jun 22, 2026
Comment on lines
+162
to
+177
| if config.secondary is not None: | ||
| # Replicate DualModePolicy's construction with injection into primary. | ||
| dm = object.__new__(DualModePolicy) | ||
| primary_cls = _select_policy_class(config) | ||
| secondary_cls = _select_policy_class(config.secondary) | ||
| logger.info(f"Dual-mode: primary={primary_cls.__name__}, secondary={secondary_cls.__name__}") | ||
| dm.primary = _new_with_injected(primary_cls, config, io) | ||
| secondary = object.__new__(secondary_cls) | ||
| secondary._shared_hardware_source = dm.primary | ||
| secondary.__init__(config=config.secondary) | ||
| dm.secondary = secondary | ||
| dm.active = dm.primary | ||
| dm.active_label = "primary" | ||
| dm._setup_command_intercept() | ||
| logger.info("Dual-mode ready. Publish 'switch_mode' to swap policies.") | ||
| return dm |
Contributor
There was a problem hiding this comment.
In general, I'd love it if we can get rid of this leaky state here with an explicit FSM, so we don't have to deal with this _shared_hardware_source shim.
I have a prototype here: #117
tomasz-lewicki
approved these changes
Jun 22, 2026
tomasz-lewicki
merged commit c61cc9d
into
amazon-far:dev/tomasz/tracker_service
8 of 9 checks passed
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).
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 B (single node owns all I/O): a single
ServiceIONodeowns every subscription/publisher — velocity, state, dense target, and feedback — and injects the input providers into the policy via a newinjectedinput source. A companion PR proposes Variant A (reuseRos2Input, no shared-input changes); pick one.What's here
injectedinput source (sharedholosoma_inference):create_inputreturns a provider pre-attached by an external owner (policy._injected_*) instead of constructing one;InputSourceenum gains"injected".injected_inputs.py—InjectedRos2Input: one object implementing bothVelCmdProviderandStateCommandProvider(likeRos2Input), subscribing on the node the service owns. Single object is required because the base policy shares one provider for both roles whenvelocity_input == state_input.service_node.py—ServiceIONodecarries all I/O on one node spun once; builds the policy via_select_policy_class/DualModePolicyinjecting providers before__init__(dual-mode injects primary; secondary shares via_shared_hardware_source). Routes the Unitree SDK throughunitree_mp; strips ROS args before tyro.teleop_with_loco_policy.launch.py— fully parameterized for composition into a parent launch."kill"command +dual_modehasattrguard.Variant A vs B
Ros2Input. Two rclpy nodes coexist.injectedsource in sharedinputs/.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.