diff --git a/dimos/navigation/movement_manager/movement_manager.py b/dimos/navigation/movement_manager/movement_manager.py index ed12dc93ac..9e069cd69e 100644 --- a/dimos/navigation/movement_manager/movement_manager.py +++ b/dimos/navigation/movement_manager/movement_manager.py @@ -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 @@ -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""" @@ -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 diff --git a/dimos/navigation/movement_manager/test_movement_manager.py b/dimos/navigation/movement_manager/test_movement_manager.py index e266ad3e98..1d734c215f 100644 --- a/dimos/navigation/movement_manager/test_movement_manager.py +++ b/dimos/navigation/movement_manager/test_movement_manager.py @@ -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] + + +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