Local planner, Global Planner, Explore, SpatialMemory working via LCM/Dask Multiprocess

dimos/robot/frontier_exploration/wavefront_frontier_goal_selector.py

@@ -657,7 +657,9 @@ def explore(self, stop_event: Optional[threading.Event] = None) -> bool:
 
             # Navigate to the frontier
             logger.info(f"Navigating to frontier at {next_goal}")
-            navigation_successful = self.set_goal(next_goal, stop_event=stop_event)
+            navigation_successful = self.set_goal(
+                next_goal,
+            )
 
             if not navigation_successful:
                 logger.warning("Failed to navigate to frontier, continuing exploration")

dimos/robot/global_planner/planner.py

@@ -38,19 +38,19 @@ def __init__(self):
         Module.__init__(self)
         Visualizable.__init__(self)
 
-    # def set_goal(
-    #     self,
-    #     goal: VectorLike,
-    #     goal_theta: Optional[float] = None,
-    #     stop_event: Optional[threading.Event] = None,
-    # ):
-    #     path = self.plan(goal)
-    #     if not path:
-    #         logger.warning("No path found to the goal.")
-    #         return False
+    @rpc
+    def set_goal(
+        self,
+        goal: VectorLike,
+        goal_theta: Optional[float] = None,
+        stop_event: Optional[threading.Event] = None,
+    ):
+        path = self.plan(goal)
+        if not path:
+            logger.warning("No path found to the goal.")
+            return False
 
-    #     print("pathing success", path)
-    #     return self.set_local_nav(path, stop_event=stop_event, goal_theta=goal_theta)
+        print("pathing success", path)
 
 
 class AstarPlanner(Planner):
@@ -59,17 +59,20 @@ class AstarPlanner(Planner):
 
     get_costmap: Callable[[], Costmap]
     get_robot_pos: Callable[[], Vector3]
+    set_local_nav: Callable[[Path, Optional[threading.Event], Optional[float]], bool]
 
     conservativism: int = 8
 
     def __init__(
         self,
         get_costmap: Callable[[], Costmap],
         get_robot_pos: Callable[[], Vector3],
+        set_local_nav: Callable[[Path, Optional[threading.Event], Optional[float]], bool] = None,
     ):
         super().__init__()
         self.get_costmap = get_costmap
         self.get_robot_pos = get_robot_pos
+        self.set_local_nav = set_local_nav
 
     @rpc
     def start(self):
@@ -80,9 +83,10 @@ def plan(self, goal: VectorLike) -> Path:
         goal = to_vector(goal).to_2d()
         pos = self.get_robot_pos()
         print("current pos", pos)
-        costmap = self.get_costmap().smudge()
+        costmap = self.get_costmap()
 
         print("current costmap", costmap)
+
         self.vis("target", goal)
 
         print("ASTAR ", costmap, goal, pos)
@@ -92,5 +96,7 @@ def plan(self, goal: VectorLike) -> Path:
             path = path.resample(0.1)
             self.vis("a*", path)
             self.path.publish(path)
+            if hasattr(self, "set_local_nav"):
+                self.set_local_nav(path)
             return path
         logger.warning("No path found to the goal.")

dimos/robot/local_planner/__init__.py

@@ -1,7 +0,0 @@
-from dimos.robot.local_planner.local_planner import (
-    BaseLocalPlanner,
-    navigate_to_goal_local,
-    navigate_path_local,
-)
-
-from dimos.robot.local_planner.vfh_local_planner import VFHPurePursuitPlanner