From dae459e6e005b472400d16486420e1f8f0968330 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Sun, 28 Dec 2025 06:53:51 +0200 Subject: [PATCH 1/5] fix move skill --- dimos/mapping/occupancy/path_resampling.py | 11 +++ dimos/msgs/geometry_msgs/PoseStamped.py | 3 +- .../replanning_a_star/global_planner.py | 23 ++++- .../replanning_a_star/local_planner.py | 18 +++- .../unitree_webrtc/unitree_skill_container.py | 90 +++++++++++++++---- 5 files changed, 122 insertions(+), 23 deletions(-) diff --git a/dimos/mapping/occupancy/path_resampling.py b/dimos/mapping/occupancy/path_resampling.py index f33d6c7cb9..eae386e0f8 100644 --- a/dimos/mapping/occupancy/path_resampling.py +++ b/dimos/mapping/occupancy/path_resampling.py @@ -160,6 +160,17 @@ def smooth_resample_path( Returns: A new Path with smoothly resampled poses """ + + if len(path.poses) == 1: + p = path.poses[0].position + o = goal_pose.orientation + new_pose = PoseStamped( + frame_id=path.frame_id, + position=[p.x, p.y, p.z], + orientation=[o.x, o.y, o.z, o.w], + ) + return Path(frame_id=path.frame_id, poses=[new_pose]) + if len(path) < 2 or spacing <= 0: return path diff --git a/dimos/msgs/geometry_msgs/PoseStamped.py b/dimos/msgs/geometry_msgs/PoseStamped.py index 930bdb213d..25a968e763 100644 --- a/dimos/msgs/geometry_msgs/PoseStamped.py +++ b/dimos/msgs/geometry_msgs/PoseStamped.py @@ -14,6 +14,7 @@ from __future__ import annotations +import math import time from typing import BinaryIO, TypeAlias @@ -83,7 +84,7 @@ def lcm_decode(cls, data: bytes | BinaryIO) -> PoseStamped: def __str__(self) -> str: return ( f"PoseStamped(pos=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], " - f"euler=[{self.roll:.3f}, {self.pitch:.3f}, {self.yaw:.3f}])" + f"euler=[{math.degrees(self.roll):.1f}, {math.degrees(self.pitch):.1f}, {math.degrees(self.yaw):.1f}])" ) def new_transform_to(self, name: str) -> Transform: diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index d134c6998f..04dc22a447 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math from threading import Event, RLock, Thread, current_thread import time @@ -36,6 +37,7 @@ from dimos.navigation.replanning_a_star.position_tracker import PositionTracker from dimos.navigation.replanning_a_star.replan_limiter import ReplanLimiter from dimos.utils.logging_config import setup_logger +from dimos.utils.trigonometry import angle_diff logger = setup_logger() @@ -61,6 +63,8 @@ class GlobalPlanner(Resource): _lock: RLock _safe_goal_tolerance: float = 4.0 + _goal_tolerance: float = 0.2 + _rotation_tolerance: float = math.radians(15) _replan_goal_tolerance: float = 0.5 _max_replan_attempts: int = 10 _stuck_time_window: float = 8.0 @@ -72,7 +76,9 @@ def __init__(self, global_config: GlobalConfig) -> None: self._global_config = global_config self._navigation_map = NavigationMap(self._global_config) - self._local_planner = LocalPlanner(self._global_config, self._navigation_map) + self._local_planner = LocalPlanner( + self._global_config, self._navigation_map, self._goal_tolerance + ) self._position_tracker = PositionTracker(self._stuck_time_window) self._replan_limiter = ReplanLimiter() self._disposables = CompositeDisposable() @@ -114,6 +120,7 @@ def handle_global_costmap(self, msg: OccupancyGrid) -> None: self._navigation_map.update(msg) def handle_goal_request(self, goal: PoseStamped) -> None: + logger.info("Got new goal", goal=str(goal)) with self._lock: self._current_goal = goal self._goal_reached = False @@ -184,7 +191,10 @@ def _thread_entrypoint(self) -> None: if not current_goal or not current_odom: continue - if current_goal.position.distance(current_odom.position) < self._replan_goal_tolerance: + if current_goal.position.distance(current_odom.position) < self._goal_tolerance and abs( + angle_diff(current_goal.orientation.euler[2], current_odom.orientation.euler[2]) + < self._rotation_tolerance + ): logger.info("Close enough to goal. Accepting as arrived.") self.cancel_goal(arrived=True) continue @@ -289,7 +299,16 @@ def _plan_path(self) -> None: ) return + print("-" * 100) + print("current odom", current_odom) + print("current goal", current_goal) + print("safe goal", safe_goal) + print("last pose") + print(path.poses[-1]) + resampled_path = smooth_resample_path(path, current_goal, 0.1) + print(resampled_path.poses[-1]) + print("-" * 100) self.path.on_next(resampled_path) diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index 5a01ef047b..1012703fb7 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -62,16 +62,18 @@ class LocalPlanner(Resource): _state_unique_id: int _global_config: GlobalConfig _navigation_map: NavigationMap + _goal_tolerance: float _controller: Controller _speed: float = 0.55 _control_frequency: float = 10 - _goal_tolerance: float = 0.2 _orientation_tolerance: float = 0.35 _debug_navigation_interval: float = 1.0 _debug_navigation_last: float = 0.0 - def __init__(self, global_config: GlobalConfig, navigation_map: NavigationMap) -> None: + def __init__( + self, global_config: GlobalConfig, navigation_map: NavigationMap, goal_tolerance: float + ) -> None: self.cmd_vel = Subject() self.stopped_navigating = Subject() self.debug_navigation = Subject() @@ -83,6 +85,7 @@ def __init__(self, global_config: GlobalConfig, navigation_map: NavigationMap) - self._state_unique_id = 0 self._global_config = global_config self._navigation_map = navigation_map + self._goal_tolerance = goal_tolerance controller = PController if global_config.simulation else PdController @@ -174,8 +177,15 @@ def _loop(self) -> None: robot_yaw = current_odom.orientation.euler[2] initial_yaw_error = angle_diff(first_yaw, robot_yaw) self._controller.reset_yaw_error(initial_yaw_error) - if abs(initial_yaw_error) < self._orientation_tolerance: - new_state = "path_following" + angle_in_tolerance = abs(initial_yaw_error) < self._orientation_tolerance + if angle_in_tolerance: + position_in_tolerance = ( + path.poses[0].position.distance(current_odom.position) < 0.01 + ) + if position_in_tolerance: + new_state = "final_rotation" + else: + new_state = "path_following" with self._lock: self._change_state(new_state) diff --git a/dimos/robot/unitree_webrtc/unitree_skill_container.py b/dimos/robot/unitree_webrtc/unitree_skill_container.py index b6d22d0458..135b2d3ea1 100644 --- a/dimos/robot/unitree_webrtc/unitree_skill_container.py +++ b/dimos/robot/unitree_webrtc/unitree_skill_container.py @@ -16,6 +16,7 @@ import datetime import difflib +import math import time from typing import TYPE_CHECKING @@ -23,7 +24,8 @@ from dimos.core.core import rpc from dimos.core.skill_module import SkillModule -from dimos.msgs.geometry_msgs import Twist, Vector3 +from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Twist, Vector3 +from dimos.navigation.base import NavigationState from dimos.protocol.skill.skill import skill from dimos.protocol.skill.type import Reducer, Stream from dimos.robot.unitree_webrtc.unitree_skills import UNITREE_WEBRTC_CONTROLS @@ -48,9 +50,18 @@ class UnitreeSkillContainer(SkillModule): _move: RpcCall | None = None _publish_request: RpcCall | None = None + rpc_calls: list[str] = [ + "NavigationInterface.set_goal", + "NavigationInterface.get_state", + "NavigationInterface.is_goal_reached", + "NavigationInterface.cancel_goal", + ] + @rpc def start(self) -> None: super().start() + # Initialize TF early so it can start receiving transforms. + _ = self.tf @rpc def stop(self) -> None: @@ -67,25 +78,72 @@ def set_ConnectionModule_publish_request(self, callable: RpcCall) -> None: self._publish_request.set_rpc(self.rpc) # type: ignore[arg-type] @skill() - def move(self, x: float, y: float = 0.0, yaw: float = 0.0, duration: float = 0.0) -> str: - """Move the robot using direct velocity commands. Determine duration required based on user distance instructions. + def relative_move(self, forward: float = 0.0, left: float = 0.0, degrees: float = 0.0) -> str: + """Move the robot relative to its current position. - Example call: - args = { "x": 0.5, "y": 0.0, "yaw": 0.0, "duration": 2.0 } - move(**args) + The `degrees` arguments refers to the rotation the robot should be at the end, relative to its current rotation. - Args: - x: Forward velocity (m/s) - y: Left/right velocity (m/s) - yaw: Rotational velocity (rad/s) - duration: How long to move (seconds) + Example calls: + + # Move to a point that's 2 meters forward and 1 to the right. + relative_move(forward=2, left=-1, degrees=0) + + # Move back 1 meter, while still facing the same direction. + relative_move(forward=-1, left=0, degrees=0) + + # Rotate 90 degrees to the right (in place) + relative_move(forward=0, left=0, degrees=-90) + + # Move 3 meters left, and face that direction + relative_move(forward=0, left=3, degrees=90) """ - if self._move is None: - return "Error: Robot not connected" - twist = Twist(linear=Vector3(x, y, 0), angular=Vector3(0, 0, yaw)) - self._move(twist, duration=duration) - return f"Started moving with velocity=({x}, {y}, {yaw}) for {duration} seconds" + tf = self.tf.get("world", "base_link") + if tf is None: + return "Failed to get the position of the robot." + + current_pose = tf.to_pose() + + try: + set_goal_rpc, get_state_rpc, is_goal_reached_rpc = self.get_rpc_calls( + "NavigationInterface.set_goal", + "NavigationInterface.get_state", + "NavigationInterface.is_goal_reached", + ) + except Exception: + logger.error("Navigation module not connected properly") + return "Failed to connect to navigation module." + + # Create local offset (forward is x, left is y in robot frame) + local_offset = Vector3(forward, left, 0) + + # Rotate the offset by the robot's current orientation to get it in the map frame + global_offset = current_pose.orientation.rotate_vector(local_offset) + + # Calculate goal position + goal_position = current_pose.position + global_offset + + # Calculate goal orientation by adding the rotation to the current yaw + current_euler = current_pose.orientation.to_euler() + goal_yaw = current_euler.yaw + math.radians(degrees) + goal_euler = Vector3(current_euler.roll, current_euler.pitch, goal_yaw) + goal_orientation = Quaternion.from_euler(goal_euler) + + goal_pose = PoseStamped(position=goal_position, orientation=goal_orientation) + + set_goal_rpc(goal_pose) + + time.sleep(1.0) + + while get_state_rpc() == NavigationState.FOLLOWING_PATH: + time.sleep(0.25) + + time.sleep(1.0) + + if not is_goal_reached_rpc(): + return "Navigation was cancelled or failed" + else: + return "Navigation goal reached" @skill() def wait(self, seconds: float) -> str: From a5c89978ab1495a703242aeb1c84708c4f5a3625 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 31 Dec 2025 08:03:01 +0200 Subject: [PATCH 2/5] fix big --- .../replanning_a_star/global_planner.py | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index 04dc22a447..646deb6f98 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -191,8 +191,11 @@ def _thread_entrypoint(self) -> None: if not current_goal or not current_odom: continue - if current_goal.position.distance(current_odom.position) < self._goal_tolerance and abs( - angle_diff(current_goal.orientation.euler[2], current_odom.orientation.euler[2]) + if ( + current_goal.position.distance(current_odom.position) < self._goal_tolerance + and abs( + angle_diff(current_goal.orientation.euler[2], current_odom.orientation.euler[2]) + ) < self._rotation_tolerance ): logger.info("Close enough to goal. Accepting as arrived.") @@ -299,16 +302,7 @@ def _plan_path(self) -> None: ) return - print("-" * 100) - print("current odom", current_odom) - print("current goal", current_goal) - print("safe goal", safe_goal) - print("last pose") - print(path.poses[-1]) - resampled_path = smooth_resample_path(path, current_goal, 0.1) - print(resampled_path.poses[-1]) - print("-" * 100) self.path.on_next(resampled_path) From d665996d8187209429951f28163180bce270dbfe Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 31 Dec 2025 08:09:29 +0200 Subject: [PATCH 3/5] duplicate test --- dimos/robot/cli/test_dimos_robot_e2e.py | 157 ------------------------ 1 file changed, 157 deletions(-) delete mode 100644 dimos/robot/cli/test_dimos_robot_e2e.py diff --git a/dimos/robot/cli/test_dimos_robot_e2e.py b/dimos/robot/cli/test_dimos_robot_e2e.py deleted file mode 100644 index b44e202aba..0000000000 --- a/dimos/robot/cli/test_dimos_robot_e2e.py +++ /dev/null @@ -1,157 +0,0 @@ -# Copyright 2025 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. - -import os -import signal -import subprocess -import time - -import lcm -import pytest - -from dimos.core.transport import pLCMTransport -from dimos.protocol.service.lcmservice import LCMService - - -class LCMSpy(LCMService): - messages: dict[str, list[bytes]] = {} - - def __init__(self, **kwargs) -> None: - super().__init__(**kwargs) - self.l = lcm.LCM() - - def start(self) -> None: - super().start() - if self.l: - self.l.subscribe(".*", self.msg) - - def wait_for_topic(self, topic: str, timeout: float = 30.0) -> list[bytes]: - start_time = time.time() - while time.time() - start_time < timeout: - if topic in self.messages: - return self.messages[topic] - time.sleep(0.1) - raise TimeoutError(f"Timeout waiting for topic {topic}") - - def wait_for_message_content( - self, topic: str, content_contains: bytes, timeout: float = 30.0 - ) -> None: - start_time = time.time() - while time.time() - start_time < timeout: - if topic in self.messages: - for msg in self.messages[topic]: - if content_contains in msg: - return - time.sleep(0.1) - raise TimeoutError(f"Timeout waiting for message content on topic {topic}") - - def stop(self) -> None: - super().stop() - - def msg(self, topic, data) -> None: - self.messages.setdefault(topic, []).append(data) - - -class DimosRobotCall: - process: subprocess.Popen | None - - def __init__(self) -> None: - self.process = None - - def start(self) -> None: - self.process = subprocess.Popen( - ["dimos", "run", "demo-skill"], - stdout=subprocess.PIPE, - stderr=subprocess.PIPE, - ) - - def stop(self) -> None: - if self.process is None: - return - - try: - # Send the kill signal (SIGTERM for graceful shutdown) - self.process.send_signal(signal.SIGTERM) - - # Record the time when we sent the kill signal - shutdown_start = time.time() - - # Wait for the process to terminate with a 30-second timeout - try: - self.process.wait(timeout=30) - shutdown_duration = time.time() - shutdown_start - - # Verify it shut down in time - assert shutdown_duration <= 30, ( - f"Process took {shutdown_duration:.2f} seconds to shut down, " - f"which exceeds the 30-second limit" - ) - except subprocess.TimeoutExpired: - # If we reach here, the process didn't terminate in 30 seconds - self.process.kill() # Force kill - self.process.wait() # Clean up - raise AssertionError( - "Process did not shut down within 30 seconds after receiving SIGTERM" - ) - - except Exception: - # Clean up if something goes wrong - if self.process.poll() is None: # Process still running - self.process.kill() - self.process.wait() - raise - - -@pytest.fixture -def lcm_spy(): - lcm_spy = LCMSpy() - lcm_spy.start() - yield lcm_spy - lcm_spy.stop() - - -@pytest.fixture -def dimos_robot_call(): - dimos_robot_call = DimosRobotCall() - dimos_robot_call.start() - yield dimos_robot_call - dimos_robot_call.stop() - - -@pytest.fixture -def human_input(): - transport = pLCMTransport("/human_input") - transport.lcm.start() - - def send_human_input(message: str) -> None: - transport.publish(message) - - yield send_human_input - - transport.lcm.stop() - - -@pytest.mark.skipif(bool(os.getenv("CI")), reason="LCM spy doesn't work in CI.") -@pytest.mark.skipif(not os.getenv("OPENAI_API_KEY"), reason="OPENAI_API_KEY not set.") -def test_dimos_robot_demo_e2e(lcm_spy, dimos_robot_call, human_input) -> None: - lcm_spy.wait_for_topic("/rpc/DemoCalculatorSkill/set_LlmAgent_register_skills/res") - lcm_spy.wait_for_topic("/rpc/HumanInput/start/res") - lcm_spy.wait_for_message_content("/agent", b"AIMessage") - - human_input("what is 52983 + 587237") - - lcm_spy.wait_for_message_content("/agent", b"640220") - - assert "/rpc/DemoCalculatorSkill/sum_numbers/req" in lcm_spy.messages - assert "/rpc/DemoCalculatorSkill/sum_numbers/res" in lcm_spy.messages From 1c888a7328ccd4ea284bcd0cafe916b474b5f859 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 31 Dec 2025 08:31:11 +0200 Subject: [PATCH 4/5] fix --- .../unitree_webrtc/unitree_skill_container.py | 40 +++++++++---------- 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/dimos/robot/unitree_webrtc/unitree_skill_container.py b/dimos/robot/unitree_webrtc/unitree_skill_container.py index 135b2d3ea1..b60e21bf43 100644 --- a/dimos/robot/unitree_webrtc/unitree_skill_container.py +++ b/dimos/robot/unitree_webrtc/unitree_skill_container.py @@ -24,7 +24,7 @@ from dimos.core.core import rpc from dimos.core.skill_module import SkillModule -from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Twist, Vector3 +from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Vector3 from dimos.navigation.base import NavigationState from dimos.protocol.skill.skill import skill from dimos.protocol.skill.type import Reducer, Stream @@ -102,8 +102,6 @@ def relative_move(self, forward: float = 0.0, left: float = 0.0, degrees: float if tf is None: return "Failed to get the position of the robot." - current_pose = tf.to_pose() - try: set_goal_rpc, get_state_rpc, is_goal_reached_rpc = self.get_rpc_calls( "NavigationInterface.set_goal", @@ -114,29 +112,15 @@ def relative_move(self, forward: float = 0.0, left: float = 0.0, degrees: float logger.error("Navigation module not connected properly") return "Failed to connect to navigation module." - # Create local offset (forward is x, left is y in robot frame) - local_offset = Vector3(forward, left, 0) - - # Rotate the offset by the robot's current orientation to get it in the map frame - global_offset = current_pose.orientation.rotate_vector(local_offset) - - # Calculate goal position - goal_position = current_pose.position + global_offset - - # Calculate goal orientation by adding the rotation to the current yaw - current_euler = current_pose.orientation.to_euler() - goal_yaw = current_euler.yaw + math.radians(degrees) - goal_euler = Vector3(current_euler.roll, current_euler.pitch, goal_yaw) - goal_orientation = Quaternion.from_euler(goal_euler) - - goal_pose = PoseStamped(position=goal_position, orientation=goal_orientation) + # TODO: Improve this. This is not a nice way to do it. I should + # subscribe to arrival/cancellation events instead. - set_goal_rpc(goal_pose) + set_goal_rpc(self._generate_new_goal(tf.to_pose(), forward, left, degrees)) time.sleep(1.0) while get_state_rpc() == NavigationState.FOLLOWING_PATH: - time.sleep(0.25) + time.sleep(0.1) time.sleep(1.0) @@ -145,6 +129,20 @@ def relative_move(self, forward: float = 0.0, left: float = 0.0, degrees: float else: return "Navigation goal reached" + def _generate_new_goal( + self, current_pose: PoseStamped, forward: float, left: float, degrees: float + ) -> PoseStamped: + local_offset = Vector3(forward, left, 0) + global_offset = current_pose.orientation.rotate_vector(local_offset) + goal_position = current_pose.position + global_offset + + current_euler = current_pose.orientation.to_euler() + goal_yaw = current_euler.yaw + math.radians(degrees) + goal_euler = Vector3(current_euler.roll, current_euler.pitch, goal_yaw) + goal_orientation = Quaternion.from_euler(goal_euler) + + return PoseStamped(position=goal_position, orientation=goal_orientation) + @skill() def wait(self, seconds: float) -> str: """Wait for a specified amount of time. From 683b065e2ff818555e5299f87286941d3126e925 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Wed, 31 Dec 2025 08:40:09 +0200 Subject: [PATCH 5/5] max 100s --- dimos/robot/unitree_webrtc/unitree_skill_container.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/dimos/robot/unitree_webrtc/unitree_skill_container.py b/dimos/robot/unitree_webrtc/unitree_skill_container.py index b60e21bf43..32d005db5b 100644 --- a/dimos/robot/unitree_webrtc/unitree_skill_container.py +++ b/dimos/robot/unitree_webrtc/unitree_skill_container.py @@ -47,7 +47,6 @@ class UnitreeSkillContainer(SkillModule): """Container for Unitree Go2 robot skills using the new framework.""" - _move: RpcCall | None = None _publish_request: RpcCall | None = None rpc_calls: list[str] = [ @@ -119,7 +118,11 @@ def relative_move(self, forward: float = 0.0, left: float = 0.0, degrees: float time.sleep(1.0) + start_time = time.monotonic() + timeout = 100.0 while get_state_rpc() == NavigationState.FOLLOWING_PATH: + if time.monotonic() - start_time > timeout: + return "Navigation timed out" time.sleep(0.1) time.sleep(1.0)