fixed local planner and transform bug

Author: alexlin2

dimos/navigation/bt_navigator/navigator.py

@@ -26,8 +26,10 @@
 from dimos.core import Module, In, Out, rpc
 from dimos.msgs.geometry_msgs import PoseStamped
 from dimos.navigation.local_planner.local_planner import BaseLocalPlanner
+from dimos.protocol.tf import TF
 from dimos.utils.logging_config import setup_logger
 from dimos_lcm.std_msgs import Bool
+from dimos.utils.transform_utils import apply_transform
 
 logger = setup_logger("dimos.navigation.bt_navigator")
 
@@ -92,6 +94,9 @@ def __init__(self, publishing_frequency: float = 1.0, **kwargs):
         # Local planner reference (to be connected externally)
         self.local_planner: Optional[BaseLocalPlanner] = None
 
+        # TF listener
+        self.tf = TF()
+
         logger.info("Navigator initialized")
 
     @rpc
@@ -146,14 +151,19 @@ def set_goal(self, goal: PoseStamped) -> bool:
         Returns:
             True if goal was accepted, False otherwise
         """
+        transformed_goal = self._transform_goal_to_odom_frame(goal)
+        if not transformed_goal:
+            logger.error("Failed to transform goal to odometry frame")
+            return False
+
         with self.goal_lock:
-            self.current_goal = goal
+            self.current_goal = transformed_goal
 
         with self.state_lock:
             self.state = NavigatorState.FOLLOWING_PATH
 
         logger.info(
-            f"New goal set: ({goal.position.x:.2f}, {goal.position.y:.2f}, {goal.position.z:.2f})"
+            f"New goal set: ({transformed_goal.position.x:.2f}, {transformed_goal.position.y:.2f}, {transformed_goal.position.z:.2f})"
         )
         return True
 
@@ -163,6 +173,11 @@ def connect_local_planner(self, local_planner):
         self.local_planner = local_planner
         logger.info("Local planner connected")
 
+    @rpc
+    def get_state(self) -> NavigatorState:
+        """Get the current state of the navigator."""
+        return self.state
+
     def _on_odom(self, msg: PoseStamped):
         """Handle incoming odometry messages."""
         self.latest_odom = msg
@@ -171,70 +186,94 @@ def _on_goal_request(self, msg: PoseStamped):
         """Handle incoming goal requests."""
         self.set_goal(msg)
 
+    def _transform_goal_to_odom_frame(self, goal: PoseStamped) -> Optional[PoseStamped]:
+        """Transform goal pose to the odometry frame."""
+        if not self.latest_odom:
+            logger.warning("No odometry available yet, cannot transform goal")
+            return None
+
+        if not goal.frame_id:
+            return goal
+
+        odom_frame = self.latest_odom.frame_id
+        if goal.frame_id == odom_frame:
+            return goal
+
+        try:
+            transform = self.tf.get(
+                parent_frame=odom_frame,
+                child_frame=goal.frame_id,
+                time_point=goal.ts,
+                time_tolerance=1.0,
+            )
+
+            if not transform:
+                logger.error(f"Could not find transform from '{goal.frame_id}' to '{odom_frame}'")
+                return None
+
+            pose = apply_transform(goal, transform)
+            transformed_goal = PoseStamped(
+                position=pose.position,
+                orientation=pose.orientation,
+                frame_id=odom_frame,
+                ts=goal.ts,
+            )
+            return transformed_goal
+
+        except Exception as e:
+            logger.error(f"Failed to transform goal: {e}")
+            return None
+
     def _control_loop(self):
         """Main control loop running in separate thread."""
         while not self.stop_event.is_set():
             with self.state_lock:
                 current_state = self.state
 
             if current_state == NavigatorState.FOLLOWING_PATH:
-                # Publish goal to global planner
                 with self.goal_lock:
                     goal = self.current_goal
 
                 if goal is not None:
                     self.goal.publish(goal)
 
-                    # Check if goal reached
                     if self.local_planner and self.is_goal_reached():
                         logger.info("Goal reached!")
                         reached_msg = Bool()
                         reached_msg.data = True
                         self.goal_reached.publish(reached_msg)
-                        # Reset local planner to clear the path
                         self.local_planner.reset()
                         with self.goal_lock:
                             self.current_goal = None
                         with self.state_lock:
                             self.state = NavigatorState.IDLE
 
             elif current_state == NavigatorState.RECOVERY:
-                # Recovery mode placeholder - transitions back to IDLE
                 with self.state_lock:
                     self.state = NavigatorState.IDLE
 
-            # Sleep for control period
             time.sleep(self.publishing_period)
 
     @rpc
     def is_goal_reached(self) -> bool:
-        """
-        Check if the current goal has been reached.
-
-        Returns:
-            True if goal reached, False otherwise
-        """
+        """Check if the current goal has been reached."""
         if self.local_planner is None:
             return False
 
         try:
-            # Call local planner's is_goal_reached RPC
             return self.local_planner.is_goal_reached()
         except Exception as e:
             logger.error(f"Failed to check goal status: {e}")
             return False
 
     def stop(self):
         """Stop navigation and return to IDLE state."""
-        # Cancel any active goal
         with self.goal_lock:
             self.current_goal = None
 
-        # Set state to IDLE
         with self.state_lock:
             self.state = NavigatorState.IDLE
 
-        # Stop the local planner
         if self.local_planner:
             self.local_planner.reset()
 

dimos/navigation/global_planner/algo.py

@@ -122,7 +122,7 @@ def astar(
     # Convert world coordinates to grid coordinates directly using vector-like inputs
     start_vector = costmap.world_to_grid(start)
     goal_vector = costmap.world_to_grid(goal)
-    logger.info(f"ASTAR {costmap} {start_vector} -> {goal_vector}")
+    logger.debug(f"ASTAR {costmap} {start_vector} -> {goal_vector}")
 
     # Store original positions for reference
     original_start = (int(start_vector.x), int(start_vector.y))
@@ -139,7 +139,7 @@ def astar(
         start_in_obstacle = costmap.grid[int(start_vector.y), int(start_vector.x)] >= cost_threshold
 
     if not start_valid or start_in_obstacle:
-        logger.info("Start position is out of bounds or in an obstacle, finding nearest free cell")
+        logger.debug("Start position is out of bounds or in an obstacle, finding nearest free cell")
         adjusted_start = find_nearest_free_cell(costmap, start, cost_threshold)
         # Update start_vector for later use
         start_vector = Vector(adjusted_start[0], adjusted_start[1])
@@ -152,7 +152,7 @@ def astar(
         goal_in_obstacle = costmap.grid[int(goal_vector.y), int(goal_vector.x)] >= cost_threshold
 
     if not goal_valid or goal_in_obstacle:
-        logger.info("Goal position is out of bounds or in an obstacle, finding nearest free cell")
+        logger.debug("Goal position is out of bounds or in an obstacle, finding nearest free cell")
         adjusted_goal = find_nearest_free_cell(costmap, goal, cost_threshold)
         # Update goal_vector for later use
         goal_vector = Vector(adjusted_goal[0], adjusted_goal[1])

dimos/navigation/local_planner/local_planner.py

@@ -107,7 +107,7 @@ def _start_planning_thread(self):
         self.stop_planning.clear()
         self.planning_thread = threading.Thread(target=self._follow_path_loop, daemon=True)
         self.planning_thread.start()
-        logger.info("Started follow path thread")
+        logger.debug("Started follow path thread")
 
     def _follow_path_loop(self):
         """Main planning loop that runs in a separate thread."""

dimos/protocol/tf/__init__.py

@@ -12,6 +12,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from dimos.protocol.tf.tf import TF, LCMTF, PubSubTF, TFSpec, TFConfig
+from dimos.protocol.tf.tf import TF, LCMTF, PubSubTF, TFSpec, TFConfig, TBuffer, MultiTBuffer
 
-__all__ = ["TF", "LCMTF", "PubSubTF", "TFSpec", "TFConfig"]
+__all__ = ["TF", "LCMTF", "PubSubTF", "TFSpec", "TFConfig", "TBuffer", "MultiTBuffer"]

dimos/protocol/tf/test_tf.py

@@ -18,7 +18,7 @@
 
 from dimos.core import TF
 from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Transform, Vector3
-from dimos.protocol.tf.tf import MultiTBuffer, TBuffer
+from dimos.protocol.tf import MultiTBuffer, TBuffer
 
 
 def test_tf_main():

dimos/robot/unitree_webrtc/type/odometry.py

@@ -11,22 +11,15 @@
 # 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 math
-from datetime import datetime
-from io import BytesIO
-from typing import BinaryIO, Literal, TypeAlias, TypedDict
+import time
+from typing import Literal, TypedDict
 
 from scipy.spatial.transform import Rotation as R
 
 from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Vector3
 from dimos.robot.unitree_webrtc.type.timeseries import (
-    EpochLike,
     Timestamped,
-    to_datetime,
-    to_human_readable,
 )
-from dimos.types.timestamped import to_timestamp
-from dimos.types.vector import Vector, VectorLike
 
 raw_odometry_msg_sample = {
     "type": "msg",
@@ -104,7 +97,7 @@ def from_msg(cls, msg: RawOdometryMessage) -> "Odometry":
             pose["orientation"].get("w"),
         )
 
-        ts = to_timestamp(msg["data"]["header"]["stamp"])
+        ts = time.time()
         return Odometry(position=pos, orientation=rot, ts=ts, frame_id="world")
 
     def __repr__(self) -> str: