fix move skill

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
 

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:

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,13 @@ 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

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)