Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 10 additions & 7 deletions dimos/navigation/movement_manager/movement_manager.py
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
from typing import Any

from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped]
from pydantic import Field
from reactivex.disposable import Disposable

from dimos.core.core import rpc
Expand All @@ -38,15 +39,17 @@

logger = setup_logger()

# without this you can (basically) click into infinity in rerun (not good for the planner)
MAX_CLICK_HORIZONTAL_M = 500.0
MAX_CLICK_VERTICAL_M = 50.0


class MovementManagerConfig(ModuleConfig):
tele_cooldown_sec: float = 1.0
tele_cmd_vel_scaling: Twist = Twist(Vector3(1, 1, 1), Vector3(1, 1, 1))

# Clamp clicked goals so a stray click can't send the planner toward infinity in
# rerun. Configurable for people rendering very large maps; must stay positive so a
# zero/negative override can't silently reject every click.
max_click_horizontal_m: float = Field(500.0, gt=0)
max_click_vertical_m: float = Field(50.0, gt=0)


class MovementManager(Module):
"""Combine tele_cmd_vel (keyboard controls) and nav_cmd_vel in a sane way, output cmd_vel"""
Expand Down Expand Up @@ -86,9 +89,9 @@ def _on_click(self, msg: PointStamped) -> None:
logger.warning("Ignored invalid click", x=msg.x, y=msg.y, z=msg.z)
return
if (
abs(msg.x) > MAX_CLICK_HORIZONTAL_M
or abs(msg.y) > MAX_CLICK_HORIZONTAL_M
or abs(msg.z) > MAX_CLICK_VERTICAL_M
abs(msg.x) > self.config.max_click_horizontal_m
or abs(msg.y) > self.config.max_click_horizontal_m
or abs(msg.z) > self.config.max_click_vertical_m
):
logger.warning("Ignored out-of-range click", x=msg.x, y=msg.y, z=msg.z)
return
Expand Down
28 changes: 28 additions & 0 deletions dimos/navigation/movement_manager/test_movement_manager.py
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,34 @@ def test_invalid_clicks_rejected(manager_and_captured):
assert captured.goal == []


def test_click_limit_rejects_when_lowered(manager_and_captured):
"""A click within the default range is rejected once the limit is lowered below it."""
manager, captured = manager_and_captured
manager.config.max_click_horizontal_m = 10.0
manager._on_click(_click(x=50.0, y=0.0, z=0.0))
assert captured.goal == []
assert captured.way_point == []


def test_click_limit_accepts_when_raised(manager_and_captured):
"""A click beyond the default range is accepted once the limit is raised above it."""
manager, captured = manager_and_captured
manager.config.max_click_horizontal_m = 1000.0
far_click = _click(x=600.0, y=0.0, z=0.0)
manager._on_click(far_click)
assert captured.goal == [far_click]
assert captured.way_point == [far_click]
Comment on lines +127 to +143
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P2 The two new tests only exercise max_click_horizontal_m; max_click_vertical_m has no equivalent coverage for the dynamic-config path. A click like _click(x=0.0, y=0.0, z=30.0) with max_click_vertical_m lowered to 10.0 would be a symmetric counterpart and would guard against a future regression where only one field gets wired to self.config.



def test_vertical_click_limit_rejects_when_lowered(manager_and_captured):
"""A click within the default vertical range is rejected once the limit is lowered below it."""
manager, captured = manager_and_captured
manager.config.max_click_vertical_m = 5.0
manager._on_click(_click(x=0.0, y=0.0, z=20.0))
assert captured.goal == []
assert captured.way_point == []


def test_tele_cmd_vel_scaling(manager_and_captured):
"""tele_cmd_vel_scaling multiplies each teleop twist component independently."""
manager, captured = manager_and_captured
Expand Down