-
Notifications
You must be signed in to change notification settings - Fork 547
Jeff/fix/rconnect2 #1784
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Open
jeff-hykin
wants to merge
50
commits into
dev
Choose a base branch
from
jeff/fix/rconnect2
base: dev
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Open
Jeff/fix/rconnect2 #1784
Changes from 18 commits
Commits
Show all changes
50 commits
Select commit
Hold shift + click to select a range
9423ae4
all vis changes
jeff-hykin 6d6151c
fixup
jeff-hykin ac62445
add a conventions.md to dev docs
jeff-hykin 36492b1
fixup cmd_vel timer edgecase leaks
jeff-hykin 306bd2b
switch cmd_vel_scaling to be on rerun
jeff-hykin f7277d9
add convention
jeff-hykin 75bf186
fixup
jeff-hykin 29875f8
missed edgecase
jeff-hykin dd10ae4
fix webvis logic
jeff-hykin a3c5666
make pydantic happy
jeff-hykin 9683ea6
global config patch
jeff-hykin 247571f
cleanup
jeff-hykin bffc95d
better helper message
jeff-hykin 43d6f78
Merge branch 'dev' into jeff/fix/rconnect2
jeff-hykin a009186
-
jeff-hykin 20d455a
Merge branch 'jeff/fix/rconnect2' of github.com:dimensionalOS/dimos i…
jeff-hykin 0b962a3
merge dev
jeff-hykin a22150d
uv lock was incorrect somehow
jeff-hykin a8f9653
add conventions
jeff-hykin 769c8b8
switch to rosnav's MovementManager
jeff-hykin a3a2ab5
refactor: move rerun constants into dimos/visualization/rerun/config.py
jeff-hykin e7106aa
simplify tests and make them good
jeff-hykin f6de436
import improve
jeff-hykin 1dce3fe
restore constants.py
jeff-hykin f41ffc8
refactor(cli): move cast, get_args, RerunOpenOption to top-level imports
jeff-hykin 881fc04
import psutil
jeff-hykin b35242c
-
jeff-hykin 3b745dc
-
jeff-hykin 009aff9
cleanup test
jeff-hykin 474f10e
clean
jeff-hykin 72500ff
add scaling
jeff-hykin 4e38462
-
jeff-hykin 4591285
add scaling check
jeff-hykin 76d50e4
address main comments
jeff-hykin 073d0a7
refactor(ws-server): add ViewerMsg TypedDict union for typed message …
jeff-hykin cdd07b5
remove junk comments
jeff-hykin 4d2072b
-
jeff-hykin 39c2769
re-apply the SIGINT fix
jeff-hykin 3d0aa56
Merge remote-tracking branch 'origin/dev' into jeff/fix/rconnect2
jeff-hykin ef579e8
-
jeff-hykin 83ad060
update docs
jeff-hykin 059564b
fixes
jeff-hykin 293642e
Merge remote-tracking branch 'origin/dev' into jeff/fix/rconnect2
jeff-hykin 38b4e29
test: ignore .ignore.enhance overlay in section-marker scan
jeff-hykin 6165d65
fix: cast pipe() return to satisfy mypy no-any-return check
jeff-hykin 92c02d9
refactor: move websockets imports to top of test files
jeff-hykin 473dc5b
refactor: address remaining review nits
jeff-hykin c1b845a
fix: correct import sort order for ruff
jeff-hykin aac49ba
revert .ignore.enhance exclusion, use .hidden instead
jeff-hykin aab885e
Merge branch 'dev' into jeff/fix/rconnect2
jeff-hykin File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
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
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
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
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
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
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
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
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,166 @@ | ||
| # Copyright 2026 Dimensional Inc. | ||
| # | ||
| # Licensed under the Apache License, Version 2.0 (the "License"); | ||
| # you may not use this file except in compliance with the License. | ||
| # You may obtain a copy of the License at | ||
| # | ||
| # http://www.apache.org/licenses/LICENSE-2.0 | ||
| # | ||
| # Unless required by applicable law or agreed to in writing, software | ||
| # distributed under the License is distributed on an "AS IS" BASIS, | ||
| # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| # See the License for the specific language governing permissions and | ||
| # limitations under the License. | ||
|
|
||
| """CmdVelMux: merges nav and teleop velocity commands. | ||
|
|
||
| Teleop (tele_cmd_vel) takes priority over autonomous navigation | ||
| (nav_cmd_vel). When teleop is active, nav commands are suppressed | ||
| and a stop_movement signal is published. After a cooldown period | ||
| with no teleop input, nav commands resume. | ||
| """ | ||
|
|
||
| from __future__ import annotations | ||
|
|
||
| import threading | ||
| from typing import Any | ||
| import weakref | ||
|
|
||
| from dimos_lcm.std_msgs import Bool | ||
| from reactivex.disposable import Disposable | ||
|
|
||
| from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT | ||
| from dimos.core.core import rpc | ||
| from dimos.core.module import Module, ModuleConfig | ||
| from dimos.core.stream import In, Out | ||
| from dimos.msgs.geometry_msgs.Twist import Twist | ||
| from dimos.utils.logging_config import setup_logger | ||
|
|
||
| logger = setup_logger() | ||
|
|
||
|
|
||
| class CmdVelMuxConfig(ModuleConfig): | ||
| tele_cooldown_sec: float = 1.0 | ||
|
|
||
|
|
||
| class CmdVelMux(Module): | ||
| """Multiplexes nav_cmd_vel and tele_cmd_vel into a single cmd_vel output. | ||
|
|
||
| When teleop input arrives, stop_movement is published so downstream | ||
| modules (planner, explorer) can cancel their active goals. | ||
|
|
||
| config.tele_cooldown_sec | ||
| nav_cmd_vel will be ignored for tele_cooldown_sec seconds after | ||
| the last teleop command | ||
|
|
||
| dev notes: each new tele_cmd_vel message restarts the cooldown | ||
| so under continuous teleop (e.g. 50 Hz joystick) the cooldown | ||
| is never actually reached; it only fires once the operator stops. | ||
|
|
||
| Ports: | ||
| nav_cmd_vel (In[Twist]): Velocity from the autonomous planner. | ||
| tele_cmd_vel (In[Twist]): Velocity from keyboard/joystick teleop. | ||
| cmd_vel (Out[Twist]): Merged output — teleop wins when active. | ||
| stop_movement (Out[Bool]): Published once per cooldown window, on | ||
| the first teleop message; downstream nav modules should cancel | ||
| their active goal when they see it. | ||
| """ | ||
|
|
||
| config: CmdVelMuxConfig | ||
|
|
||
| nav_cmd_vel: In[Twist] | ||
| tele_cmd_vel: In[Twist] | ||
| cmd_vel: Out[Twist] | ||
| stop_movement: Out[Bool] | ||
|
|
||
| def __init__(self, **kwargs: Any) -> None: | ||
| super().__init__(**kwargs) | ||
| self._teleop_active = False | ||
| self._lock = threading.Lock() | ||
| self._timer: threading.Timer | None = None | ||
| # Monotonic token identifying the current cooldown timer. Each new | ||
| # _on_teleop bumps this; _end_teleop short-circuits if its captured | ||
| # generation doesn't match — a cheap fix for stale Timer callbacks. | ||
| self._timer_gen = 0 | ||
|
|
||
| def __getstate__(self) -> dict[str, Any]: | ||
| state: dict[str, Any] = super().__getstate__() # type: ignore[no-untyped-call] | ||
| state.pop("_lock", None) | ||
| state.pop("_timer", None) | ||
| return state | ||
|
|
||
| def __setstate__(self, state: dict[str, Any]) -> None: | ||
| super().__setstate__(state) | ||
| self._lock = threading.Lock() | ||
| self._timer = None | ||
| self._timer_gen = 0 | ||
|
|
||
| def __del__(self) -> None: | ||
| # Cancel any pending cooldown timer so the daemon thread doesn't | ||
| # outlive the mux and trip pytest's thread-leak detector. | ||
| timer = getattr(self, "_timer", None) | ||
| if timer is not None: | ||
| timer.cancel() | ||
| timer.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) | ||
|
jeff-hykin marked this conversation as resolved.
Outdated
|
||
|
|
||
| @rpc | ||
| def start(self) -> None: | ||
| super().start() | ||
| self.register_disposable(Disposable(self.nav_cmd_vel.subscribe(self._on_nav))) | ||
| self.register_disposable(Disposable(self.tele_cmd_vel.subscribe(self._on_teleop))) | ||
|
|
||
| @rpc | ||
| def stop(self) -> None: | ||
| with self._lock: | ||
| self._timer_gen += 1 # invalidate any pending _end_teleop | ||
| if self._timer is not None: | ||
| self._timer.cancel() | ||
| self._timer = None | ||
| super().stop() | ||
|
|
||
| def _on_nav(self, msg: Twist) -> None: | ||
| with self._lock: | ||
| if self._teleop_active: | ||
| return | ||
| self.cmd_vel.publish(msg) | ||
|
|
||
| def _on_teleop(self, msg: Twist) -> None: | ||
| was_active: bool | ||
| with self._lock: | ||
| was_active = self._teleop_active | ||
| self._teleop_active = True | ||
| if self._timer is not None: | ||
| # Cancel + join so the superseded Timer thread exits promptly | ||
| # rather than accumulating under rapid teleop (50 Hz) and | ||
| # tripping pytest's thread-leak detector. | ||
| self._timer.cancel() | ||
| self._timer.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) | ||
| self._timer_gen += 1 | ||
| my_gen = self._timer_gen | ||
| # weakref prevents the Timer thread from keeping the mux alive | ||
| # via a bound-method reference — otherwise mux.__del__ can't | ||
| # run at test scope exit. | ||
| self_ref = weakref.ref(self) | ||
|
jeff-hykin marked this conversation as resolved.
Outdated
|
||
|
|
||
| def _end() -> None: | ||
| obj = self_ref() | ||
| if obj is not None: | ||
| obj._end_teleop(my_gen) | ||
|
|
||
| self._timer = threading.Timer(self.config.tele_cooldown_sec, _end) | ||
| self._timer.daemon = True | ||
| self._timer.start() | ||
|
jeff-hykin marked this conversation as resolved.
Outdated
|
||
|
|
||
| if not was_active: | ||
| self.stop_movement.publish(Bool(data=True)) | ||
| logger.info("Teleop active — published stop_movement") | ||
|
|
||
| self.cmd_vel.publish(msg) | ||
|
|
||
| def _end_teleop(self, expected_gen: int) -> None: | ||
| with self._lock: | ||
| if expected_gen != self._timer_gen: | ||
| # Superseded by a newer timer (or cleared by stop()). | ||
| return | ||
| self._teleop_active = False | ||
| self._timer = None | ||
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
Oops, something went wrong.
Oops, something went wrong.
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.
Uh oh!
There was an error while loading. Please reload this page.