From 389b199b14e13f5e2ec7fc996dad15652d1aeaba Mon Sep 17 00:00:00 2001 From: Yash Sinha Date: Mon, 12 Jan 2026 13:20:51 -0800 Subject: [PATCH 1/5] Semantic navigation fix spatial perception and spatial_vector_db --- .../memory/spatial_vector_db.py | 4 +-- dimos/perception/spatial_perception.py | 27 ++++++++++++------- 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/dimos/agents_deprecated/memory/spatial_vector_db.py b/dimos/agents_deprecated/memory/spatial_vector_db.py index 0c8774cd95..c482076325 100644 --- a/dimos/agents_deprecated/memory/spatial_vector_db.py +++ b/dimos/agents_deprecated/memory/spatial_vector_db.py @@ -225,8 +225,8 @@ def _process_query_results(self, results) -> list[dict]: # type: ignore[no-unty ) # Get the image from visual memory - image = self.visual_memory.get(lookup_id) - result["image"] = image + #image = self.visual_memory.get(lookup_id) + #result["image"] = image processed_results.append(result) diff --git a/dimos/perception/spatial_perception.py b/dimos/perception/spatial_perception.py index 013d242ba8..e229a903ea 100644 --- a/dimos/perception/spatial_perception.py +++ b/dimos/perception/spatial_perception.py @@ -33,6 +33,7 @@ from dimos.agents_deprecated.memory.visual_memory import VisualMemory from dimos.constants import DIMOS_PROJECT_ROOT from dimos.core import DimosCluster, In, Module, rpc +from dimos.core.skill_module import SkillModule from dimos.msgs.sensor_msgs import Image from dimos.types.robot_location import RobotLocation from dimos.utils.logging_config import setup_logger @@ -50,7 +51,7 @@ logger = setup_logger() -class SpatialMemory(Module): +class SpatialMemory(SkillModule): """ A Dask module for building and querying Robot spatial memory. @@ -186,6 +187,10 @@ def __init__( @rpc def start(self) -> None: super().start() + _ = self.tf + import time + + time.sleep(1.0) # Subscribe to LCM streams def set_video(image_msg: Image) -> None: @@ -216,10 +221,14 @@ def stop(self) -> None: def _process_frame(self) -> None: """Process the latest frame with pose data if available.""" - tf = self.tf.get("map", "base_link") - if self._latest_video_frame is None or tf is None: + tf = self.tf.get("world", "base_link") + if self._latest_video_frame is None: + logger.warning(" No video frame available") return + if tf is None: + logger.warning("No TF transform (map -> base_link) available") + # Create Pose object with position and orientation current_pose = tf.to_pose() @@ -285,11 +294,11 @@ def _process_frame(self) -> None: self.last_record_time = current_time self.stored_frame_count += 1 - logger.info( - f"Stored frame at position ({current_pose.position.x:.2f}, {current_pose.position.y:.2f}, {current_pose.position.z:.2f}), " - f"rotation ({euler.x:.2f}, {euler.y:.2f}, {euler.z:.2f}) " - f"stored {self.stored_frame_count}/{self.frame_count} frames" - ) + #logger.info( + # f"Stored frame at position ({current_pose.position.x:.2f}, {current_pose.position.y:.2f}, {current_pose.position.z:.2f}), " + # f"rotation ({euler.x:.2f}, {euler.y:.2f}, {euler.z:.2f}) " + # f"stored {self.stored_frame_count}/{self.frame_count} frames" + #) # Periodically save visual memory to disk if self._visual_memory is not None and self.visual_memory_path is not None: @@ -501,7 +510,7 @@ def add_named_location( Returns: True if successfully added, False otherwise """ - tf = self.tf.get("map", "base_link") + tf = self.tf.get("world", "base_link") if not tf: logger.error("No position available for robot location") return False From 8d2b11ab9f828c26215adbc4a1edf833b0b0dc29 Mon Sep 17 00:00:00 2001 From: Yash Sinha Date: Mon, 12 Jan 2026 13:59:45 -0800 Subject: [PATCH 2/5] commented in logs that were accidentally commented out --- dimos/perception/spatial_perception.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/dimos/perception/spatial_perception.py b/dimos/perception/spatial_perception.py index e229a903ea..38476f5a57 100644 --- a/dimos/perception/spatial_perception.py +++ b/dimos/perception/spatial_perception.py @@ -294,11 +294,11 @@ def _process_frame(self) -> None: self.last_record_time = current_time self.stored_frame_count += 1 - #logger.info( - # f"Stored frame at position ({current_pose.position.x:.2f}, {current_pose.position.y:.2f}, {current_pose.position.z:.2f}), " - # f"rotation ({euler.x:.2f}, {euler.y:.2f}, {euler.z:.2f}) " - # f"stored {self.stored_frame_count}/{self.frame_count} frames" - #) + logger.info( + f"Stored frame at position ({current_pose.position.x:.2f}, {current_pose.position.y:.2f}, {current_pose.position.z:.2f}), " + f"rotation ({euler.x:.2f}, {euler.y:.2f}, {euler.z:.2f}) " + f"stored {self.stored_frame_count}/{self.frame_count} frames" + ) # Periodically save visual memory to disk if self._visual_memory is not None and self.visual_memory_path is not None: From 4d10dd53b78cba2e6b104ce5863f049e060a0c0a Mon Sep 17 00:00:00 2001 From: Yash Sinha Date: Mon, 12 Jan 2026 16:09:20 -0800 Subject: [PATCH 3/5] grep suggestion fix --- dimos/perception/spatial_perception.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dimos/perception/spatial_perception.py b/dimos/perception/spatial_perception.py index 38476f5a57..85ca0c3b02 100644 --- a/dimos/perception/spatial_perception.py +++ b/dimos/perception/spatial_perception.py @@ -227,7 +227,8 @@ def _process_frame(self) -> None: return if tf is None: - logger.warning("No TF transform (map -> base_link) available") + logger.warning("No TF transform (world -> base_link) available") + return # Create Pose object with position and orientation current_pose = tf.to_pose() From 1ebb9b44e73238ae442d3e387c4ade2231bac9e3 Mon Sep 17 00:00:00 2001 From: Yash Sinha Date: Mon, 12 Jan 2026 21:16:40 -0800 Subject: [PATCH 4/5] Suggested changes -removed tf warm start --- dimos/perception/spatial_perception.py | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/dimos/perception/spatial_perception.py b/dimos/perception/spatial_perception.py index 85ca0c3b02..6139b4cf4f 100644 --- a/dimos/perception/spatial_perception.py +++ b/dimos/perception/spatial_perception.py @@ -187,10 +187,6 @@ def __init__( @rpc def start(self) -> None: super().start() - _ = self.tf - import time - - time.sleep(1.0) # Subscribe to LCM streams def set_video(image_msg: Image) -> None: @@ -222,14 +218,15 @@ def stop(self) -> None: def _process_frame(self) -> None: """Process the latest frame with pose data if available.""" tf = self.tf.get("world", "base_link") - if self._latest_video_frame is None: - logger.warning(" No video frame available") - return if tf is None: logger.warning("No TF transform (world -> base_link) available") return + if self._latest_video_frame is None: + logger.warning(" No video frame available") + return + # Create Pose object with position and orientation current_pose = tf.to_pose() From 3610d06fa0fb60699c7ac746dab044f3622cecce Mon Sep 17 00:00:00 2001 From: Yash Sinha Date: Mon, 12 Jan 2026 21:45:10 -0800 Subject: [PATCH 5/5] Removed logging --- dimos/perception/spatial_perception.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/dimos/perception/spatial_perception.py b/dimos/perception/spatial_perception.py index 6139b4cf4f..5463f08757 100644 --- a/dimos/perception/spatial_perception.py +++ b/dimos/perception/spatial_perception.py @@ -220,11 +220,9 @@ def _process_frame(self) -> None: tf = self.tf.get("world", "base_link") if tf is None: - logger.warning("No TF transform (world -> base_link) available") return if self._latest_video_frame is None: - logger.warning(" No video frame available") return # Create Pose object with position and orientation