control: per-episode hardware lifecycle driven by SessionManager by shantanuparab-tr · Pull Request #248 · TrossenRobotics/trossen_sdk

shantanuparab-tr · 2026-06-09T22:36:02Z

Summary

Moves the per-episode teleop + staging choreography out of the example apps and into the SDK: the SessionManager now owns a per-episode hardware lifecycle, components opt in and implement their own per-episode work, and the three AI examples collapse to a single add_teleop() registration. Supersedes #246.

Changes

In include/trossen_sdk/hw/hardware_component.hpp: add opt-in, no-op per-episode lifecycle hooks on_pre_episode() / on_episode_started() / on_episode_ended() plus a virtual is_episode_lifecycle_enabled() (default false). The base parses no config — each component answers the getter from its own config. The per-episode lifecycle runs on both a clean stop and a discard/re-record (user episode-ended callbacks stay clean-stop only); a component only participates if it backs a registered producer.
In include/trossen_sdk/hw/producer_base.hpp + src/runtime/producer_registry.cpp + src/runtime/push_producer_registry.cpp: producers retain their backing HardwareComponent and expose it via hardware(); the registries set it once after construction, so the SessionManager can reach every component's hooks with no per-producer code.
In include/trossen_sdk/runtime/session_manager.hpp + src/runtime/session_manager.cpp: add add_teleop() + a teleop_controllers_ list and collect_lifecycle_components_() (the unique, opted-in, producer-backed components). Drive the per-episode bracket — pause mirrors → on_pre_episode() (arms re-home) → re-arm in the pre-episode phase; teleop() + on_episode_started() once recording is live; reset_teleop() + on_episode_ended() on episode end; stop_teleop() at shutdown. Opted-in components stage in parallel (independent hardware via std::async), so the episode-boundary wait is bounded by the slowest arm rather than the sum. A staging hook that throws is caught and routed through the same cleanup_and_abort() path as every other start_episode() failure.
In include/trossen_sdk/hw/arm/trossen_arm_component.hpp + src/hw/arm/trossen_arm_component.cpp: override on_pre_episode() → stage(), parse episode_lifecycle_enabled, and override is_episode_lifecycle_enabled(). Rename teleop_moving_time_s → staging_time_s (it governs any SDK-commanded point-to-point move — staging to home and the return-to-rest move — not just teleop). On teleop stop, hold the measured pose before returning to rest so the arm doesn't drop under gravity.
In include/trossen_sdk/hw/teleop/teleop_controller.hpp + src/hw/teleop/teleop_controller.cpp: rename pause_mirror() → pause_teleop() for naming consistency, and join a stale control thread on restart so repeated teleop() across episodes cannot std::terminate.
In include/trossen_sdk/configuration/types/hardware/arm_config.hpp: carry staged_position, staging_time_s, and episode_lifecycle_enabled through from_json/to_json.
In examples/trossen_{solo,stationary,mobile}_ai/{trossen_*_ai.cpp,config.json,README.md}: hand controllers to the manager via add_teleop() and remove the per-episode pause/stage/re-arm, mirror-start, reset, and stop choreography the manager now owns. Example configs stay opt-out (no episode_lifecycle_enabled); each README's "Optional: home staging" section documents how to enable staging per arm.

Test Plan

Builds cleanly (make build)
Tests pass — test_config, test_teleop_controller, and the session-manager suite
Lint passes (pre-commit on changed files — cpplint, codespell, json, whitespace)
Tested on hardware — with staging enabled, opted-in arms re-home at the start of every episode; on Ctrl+C the arm holds its pose (no gravity drop) before returning to rest

Breaking Changes

The new lifecycle fields — episode_lifecycle_enabled, staged_position, and staging_time_s — are optional and default off/empty, so configs that don't set them load unchanged. One existing key is renamed, not aliased: teleop_moving_time_s → staging_time_s. A config still using the old teleop_moving_time_s key will have it silently ignored and fall back to the 2.0s default; update such configs to staging_time_s. This is a deliberate clean break (pre-1.0, no config-compat promise), not a dual-key alias.

ABI is not preserved — HardwareComponent gains virtuals and the producer bases gain a member; downstream consumers rebuild on upgrade (pre-1.0, no ABI promise). The teleop-stop sequence is a deliberate runtime behavior change — the arm holds position on stop instead of going briefly limp.

Related Issues

Relates to TDS-188

Add opt-in per-episode lifecycle hooks (on_pre_episode / on_episode_started / on_episode_ended) to HardwareComponent, gated by a virtual is_episode_lifecycle_enabled() that components answer from their own config. Producers expose their backing component (PolledProducer/PushProducer::hardware()), set once by the producer registries, so the SessionManager can reach it. The SessionManager owns the whole choreography: register controllers via add_teleop(), then per episode it pauses teleop mirrors, runs each opted-in component's on_pre_episode() (arms re-home to staged_position), re-arms teleop, starts mirroring when recording goes live, resets between episodes, and stops teleop at shutdown. TrossenArmComponent implements on_pre_episode()->stage() and parses episode_lifecycle_enabled; rename teleop_moving_time_s to slew_time_s; arm holds pose on teleop stop and the control thread is joined on restart.

Hand teleop controllers to the SessionManager via add_teleop() and drop the per-episode pause/stage/re-arm, mirror start, reset, and stop_teleop choreography the manager now owns. Staging is opt-in per arm via episode_lifecycle_enabled (solo enables it alongside its staged_position); docs updated accordingly.

Update ArmConfig staging-field tests for the slew_time_s rename and the TeleopController tests for pause_teleop; drop the obsolete SdkConfig stage_each_episode cases.

reset_teleop() and components' on_episode_ended() run on the discard/re-record path as well as on clean stop, so teleop and components land in a consistent between-episode state either way; user episode-ended callbacks remain clean-stop only. The lifecycle participants are snapshotted under episode_mutex_ and the hooks run after unlocking (matching the observers/callbacks discipline; no hardware calls under the lock). Document the producer-backing precondition on is_episode_lifecycle_enabled() (a component participates only if it backs a registered producer).

Each opted-in component's on_pre_episode() drives independent hardware and stage() is a blocking point-to-point move, so run them concurrently via std::async. Episode-boundary latency is now bounded by the slowest component rather than the sum. Futures rethrow on get() and join on destruction, so a hook exception still propagates and no staging thread is orphaned.

Copilot

Pull request overview

This PR moves per-episode teleop + hardware “staging” orchestration out of the example applications and into the SDK by having SessionManager drive an opt-in per-episode lifecycle on producer-backed HardwareComponents, while also tightening the teleop controller’s thread lifecycle to support repeated start/stop across episodes.

Changes:

Introduces an opt-in per-episode lifecycle API on HardwareComponent and has SessionManager orchestrate pause/stage/re-arm + started/ended hooks (with parallel pre-episode staging via std::async).
Extends producers/registries to retain and expose their backing HardwareComponent so the manager can find lifecycle participants without per-producer glue.
Updates teleop + arm behavior and configuration: pause_teleop(), stale-thread reaping on restart, arm staging via on_pre_episode(), hold-pose on teleop stop, and config/tests/examples updated accordingly.

Reviewed changes

Copilot reviewed 22 out of 22 changed files in this pull request and generated 5 comments.

Show a summary per file

File	Description
`include/trossen_sdk/hw/hardware_component.hpp`	Adds per-episode lifecycle hooks + opt-in gate.
`include/trossen_sdk/hw/producer_base.hpp`	Producers retain/expose backing `HardwareComponent` via `hardware()`.
`src/runtime/producer_registry.cpp`	Sets the producer’s backing hardware component at creation time.
`src/runtime/push_producer_registry.cpp`	Sets the push producer’s backing hardware component at creation time.
`include/trossen_sdk/runtime/session_manager.hpp`	Adds `add_teleop()` and declares lifecycle-component collection helper.
`src/runtime/session_manager.cpp`	Orchestrates per-episode lifecycle (pause teleop, parallel pre-episode hooks, teleop start, episode end/reset, shutdown stop).
`include/trossen_sdk/hw/teleop/teleop_controller.hpp`	Adds `pause_teleop()` API and documents behavior.
`src/hw/teleop/teleop_controller.cpp`	Implements `pause_teleop()` and joins stale threads on restart.
`include/trossen_sdk/hw/arm/trossen_arm_component.hpp`	Adds episode lifecycle opt-in + renames timing parameter to `slew_time_s`.
`src/hw/arm/trossen_arm_component.cpp`	Implements `on_pre_episode()` staging, blocking stage move, and hold-pose on teleop stop.
`include/trossen_sdk/configuration/types/hardware/arm_config.hpp`	Adds staging/lifecycle/timing fields to arm config JSON round-trip.
`tests/test_config.cpp`	Adds ArmConfig JSON parsing/round-trip/omission tests.
`tests/test_teleop_controller.cpp`	Adds regression tests for teleop restart after exception and pause/resume behavior.
`examples/trossen_stationary_ai/trossen_stationary_ai.cpp`	Switches to `SessionManager::add_teleop()` and removes manual episode choreography.
`examples/trossen_solo_ai/trossen_solo_ai.cpp`	Switches to `SessionManager::add_teleop()` and removes manual episode choreography.
`examples/trossen_mobile_ai/trossen_mobile_ai.cpp`	Switches to `SessionManager::add_teleop()` and removes manual episode choreography.
`examples/trossen_stationary_ai/README.md`	Documents opt-in per-episode home staging behavior.
`examples/trossen_solo_ai/README.md`	Documents opt-in per-episode home staging behavior.
`examples/trossen_mobile_ai/README.md`	Documents opt-in per-episode home staging behavior.
`examples/trossen_stationary_ai/config.json`	Formatting-only change (blank line).
`examples/trossen_mobile_ai/config.json`	Formatting-only change (blank line).
`examples/trossen_solo_ai/config.json`	Updates arm config keys to `slew_time_s` + enables episode lifecycle in the example.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

+    if (j.contains("slew_time_s")) {
+      j.at("slew_time_s").get_to(c.slew_time_s);
+    }


+  if (config.contains("slew_time_s")) {
+    slew_time_s_ = config.at("slew_time_s").get<float>();
+    if (slew_time_s_ < 0.0f || !std::isfinite(slew_time_s_)) {
      throw std::runtime_error(
-        "TrossenArmComponent: 'teleop_moving_time_s' must be non-negative and finite");
+        "TrossenArmComponent: 'slew_time_s' must be non-negative and finite");
    }
  }


+  // SDK-driven per-episode preparation (runs after user pre-episode callbacks, so it
+  // is skipped if the episode was aborted above). Pause teleop mirrors so staging
+  // cannot fight the control loop, run each opted-in component's pre-episode hook
+  // (e.g. arms re-home to their staged pose), then re-arm teleop modes before the
+  // mirror restarts at episode start.
+  for (auto& ctrl : teleop_controllers_) ctrl->pause_teleop();
+  // Run the components' pre-episode hooks in parallel. Each drives independent
+  // hardware (its own arm/driver), and staging is a blocking point-to-point move, so
+  // running them concurrently bounds the episode-boundary wait by the slowest component
+  // instead of the sum. Futures rethrow on get() and join on destruction, so a hook
+  // exception still propagates and no staging thread is left orphaned.
+  {
+    std::vector<std::future<void>> staging;
+    for (auto& comp : collect_lifecycle_components_()) {
+      staging.push_back(std::async(std::launch::async, [comp]() { comp->on_pre_episode(); }));
+    }
+    for (auto& f : staging) f.get();
+  }
+  for (auto& ctrl : teleop_controllers_) ctrl->prepare_teleop();


+  ctrl.teleop();
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(ctrl.is_running());
+
+  // Restart with no intervening pause_teleop()/stop_teleop(): must reap the
+  // stale joinable thread instead of terminating.
+  ctrl.teleop();
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(ctrl.is_running());


+  // Staging to a home pose is driven by the application (see the examples'
+  // on_pre_episode hook), not the controller, so that it happens before the
+  // mirror loop becomes active and can be re-run per episode when teleop is
+  // disabled.


shantanuparab-tr · 2026-06-15T16:46:14Z

This stack of pull requests is managed by Graphite. Learn more about stacking.

lukeschmitt-tr · 2026-06-16T14:09:47Z

 {
  "robot_name": "trossen_mobile_ai",

+


Suggested change

lukeschmitt-tr · 2026-06-16T16:38:33Z

+  /// Longer = slower, gentler motion. Sized so that a large start-to-goal
+  /// difference does not exceed joint velocity limits or produce violent
+  /// motion. Optional; 2.0s is a safe default for the supported arms.
+  float slew_time_s{2.0f};


I don't think slew time makes much sense as the config name here. Don't we already have something like this? teleop time or similar? staging_time_s might also work

We had teleop time but it wasn't fitting well as it wasn't actually used for teleoperation. I'll change it to staging

lukeschmitt-tr · 2026-06-16T16:54:48Z

+   * Used to re-stage robots to their home pose between episodes without tearing
+   * down teleop.
+   */
+  void pause_teleop();


How is this different from reset_teleop?

reset_teleop() calls post_episode() and keeps the mirror loop running (reposition the leader while recording is stopped); pause_teleop() joins the control thread (stops the mirror) but does not call end_teleop(), so drivers stay armed for re-staging between episodes.

- session_manager: catch exceptions from parallel staging hooks and route through cleanup_and_abort(), matching the rest of start_episode()'s error handling instead of letting a hook throw escape the function - arm/config: rename slew_time_s -> staging_time_s (clearer; Luke's review). teleop_moving_time_s -> staging_time_s is a clean break, not aliased - teleop_controller: fix stale comment that pointed at the examples' hook; staging is now SDK-driven via HardwareComponent::on_pre_episode() - test_teleop_controller: replace fixed sleep_for waits with a bounded poll so the exception-restart tests are deterministic on slow CI - examples: keep configs at main's baseline (no pre-enabled episode_lifecycle_enabled); READMEs document the opt-in

- session_manager: catch exceptions from parallel staging hooks and route through cleanup_and_abort(), matching the rest of start_episode()'s error handling instead of letting a hook throw escape the function - arm/config: rename slew_time_s -> staging_time_s (clearer; Luke's review). teleop_moving_time_s -> staging_time_s is a clean break, not aliased - teleop_controller: fix stale comment that pointed at the examples' hook; staging is now SDK-driven via HardwareComponent::on_pre_episode() - test_teleop_controller: replace fixed sleep_for waits with a bounded poll so the exception-restart tests are deterministic on slow CI - examples: ship configs with no staging setup (no episode_lifecycle_enabled, staged_position, or staging_time_s); READMEs document the opt-in

Expand the 'Optional: home staging' section in the solo, stationary, and mobile example READMEs with a ready-to-paste snippet (the demo staged_position we used), explicit placement (hardware -> arms -> <arm_name> in config.json), an in-context arm block, and a field reference for staged_position / staging_time_s / episode_lifecycle_enabled.

shantanuparab-tr changed the title ~~Per-episode hardware lifecycle driven by SessionManager~~ [TDS-188] control: per-episode hardware lifecycle driven by SessionManager Jun 9, 2026

shantanuparab-tr self-assigned this Jun 9, 2026

shantanuparab-tr added the enhancement New feature or request label Jun 9, 2026

shantanuparab-tr changed the title ~~[TDS-188] control: per-episode hardware lifecycle driven by SessionManager~~ control: per-episode hardware lifecycle driven by SessionManager Jun 9, 2026

shantanuparab-tr force-pushed the 06-09-hardware-episode-lifecycle branch 2 times, most recently from 2db562f to 1c6401a Compare June 9, 2026 23:06

shantanuparab-tr requested a review from lukeschmitt-tr June 9, 2026 23:06

shantanuparab-tr added 5 commits June 9, 2026 18:28

tests: align config and teleop tests with slew_time_s and pause_teleop

ea302b2

Update ArmConfig staging-field tests for the slew_time_s rename and the TeleopController tests for pause_teleop; drop the obsolete SdkConfig stage_each_episode cases.

shantanuparab-tr force-pushed the 06-09-hardware-episode-lifecycle branch from 1c6401a to af66c59 Compare June 9, 2026 23:29

shantanuparab-tr marked this pull request as ready for review June 9, 2026 23:33

Copilot AI review requested due to automatic review settings June 9, 2026 23:33

Copilot started reviewing on behalf of shantanuparab-tr June 9, 2026 23:33 View session

Copilot AI reviewed Jun 9, 2026

View reviewed changes

shantanuparab-tr mentioned this pull request Jun 10, 2026

control: per-episode arm staging to a home pose and hold-pose on teleop stop #246

Closed

4 tasks

lukeschmitt-tr requested changes Jun 16, 2026

View reviewed changes

shantanuparab-tr force-pushed the 06-09-hardware-episode-lifecycle branch from d3af5ba to 1d1b28a Compare June 16, 2026 21:42

shantanuparab-tr requested a review from lukeschmitt-tr June 16, 2026 21:52

lukeschmitt-tr approved these changes Jun 16, 2026

View reviewed changes

shantanuparab-tr merged commit 6e2eadd into main Jun 16, 2026
4 checks passed

shantanuparab-tr deleted the 06-09-hardware-episode-lifecycle branch June 16, 2026 21:56

shantanuparab-tr requested a review from abhichothani42 July 17, 2026 00:21

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

control: per-episode hardware lifecycle driven by SessionManager#248

control: per-episode hardware lifecycle driven by SessionManager#248
shantanuparab-tr merged 7 commits into
mainfrom
06-09-hardware-episode-lifecycle

shantanuparab-tr commented Jun 9, 2026 •

edited

Loading

Uh oh!

Copilot AI left a comment

Uh oh!

shantanuparab-tr commented Jun 15, 2026 •

edited

Loading

Uh oh!

lukeschmitt-tr Jun 16, 2026

Uh oh!

lukeschmitt-tr Jun 16, 2026

Uh oh!

shantanuparab-tr Jun 16, 2026

Uh oh!

lukeschmitt-tr Jun 16, 2026

Uh oh!

shantanuparab-tr Jun 16, 2026

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

Uh oh!

Conversation

shantanuparab-tr commented Jun 9, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Summary

Changes

Test Plan

Breaking Changes

Related Issues

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

shantanuparab-tr commented Jun 15, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

lukeschmitt-tr Jun 16, 2026

Choose a reason for hiding this comment

Uh oh!

lukeschmitt-tr Jun 16, 2026

Choose a reason for hiding this comment

Uh oh!

shantanuparab-tr Jun 16, 2026

Choose a reason for hiding this comment

Uh oh!

lukeschmitt-tr Jun 16, 2026

Choose a reason for hiding this comment

Uh oh!

shantanuparab-tr Jun 16, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

shantanuparab-tr commented Jun 9, 2026 •

edited

Loading

shantanuparab-tr commented Jun 15, 2026 •

edited

Loading