annotations

Author: omigeft

src/python_motion_planning/common/env/map/base_map.py

@@ -1,8 +1,7 @@
 """
 @file: map.py
-@breif: Map for Path Planning
 @author: Wu Maojia
-@update: 2025.9.5
+@update: 2025.10.3
 """
 from typing import Iterable, Union
 from abc import ABC, abstractmethod

src/python_motion_planning/common/env/map/grid.py

@@ -1,8 +1,7 @@
 """
 @file: grid.py
-@breif: Grid Map for Path Planning
 @author: Wu Maojia
-@update: 2025.9.5
+@update: 2025.10.3
 """
 from itertools import product
 from typing import Iterable, Union, Tuple, Callable, List, Dict
@@ -11,7 +10,7 @@
 import numpy as np
 from scipy import ndimage
 
-from .base_map import BaseMap
+from python_motion_planning.common.env.map.base_map import BaseMap
 from python_motion_planning.common.env import Node, TYPES
 from python_motion_planning.common.utils.geometry import Geometry
 
@@ -109,18 +108,17 @@ class Grid(BaseMap):
         resolution: resolution of the grid map
         type_map: initial type map of the grid map (its shape must be the same as the converted grid map shape, and its dtype must be int)
         dtype: data type of coordinates (must be int)
+        inflation_radius: radius of the inflation
 
     Examples:
-        >>> bounds = [[0, 30], [0, 40]]
-        >>> type_map = np.zeros((61, 81), dtype=np.int8)
-        >>> grid_map = Grid(bounds=bounds, resolution=0.5, type_map=type_map)
+        >>> grid_map = Grid(bounds=[[0, 51], [0, 31]], resolution=0.5)
         >>> grid_map
-        Grid(bounds=[[ 0. 30.]
-         [ 0. 40.]], resolution=0.5)
+        Grid(bounds=[[ 0. 51.]
+         [ 0. 31.]], resolution=0.5)
 
         >>> grid_map.bounds    # bounds of the base world
-        array([[ 0., 30.],
-               [ 0., 40.]])
+        array([[ 0., 51.],
+               [ 0., 31.]])
 
         >>> grid_map.dim
         2
@@ -129,7 +127,7 @@ class Grid(BaseMap):
         0.5
 
         >>> grid_map.shape   # shape of the grid map
-        (61, 81)
+        (102, 62)
 
         >>> grid_map.dtype
         <class 'numpy.int32'>
@@ -143,13 +141,13 @@ class Grid(BaseMap):
          [0 0 0 ... 0 0 0]
          [0 0 0 ... 0 0 0]
          [0 0 0 ... 0 0 0]]
-        ), shape=(61, 81), dtype=int8)
+        ), shape=(102, 62), dtype=int8)
 
         >>> grid_map.map_to_world((1, 2))
-        (0.5, 1.0)
+        (0.75, 1.25)
 
         >>> grid_map.world_to_map((0.5, 1.0))
-        (1, 2)
+        (0, 2)
 
         >>> grid_map.get_neighbors(Node((1, 2)))
         [Node((0, 1), (1, 2), 0, 0), Node((0, 2), (1, 2), 0, 0), Node((0, 3), (1, 2), 0, 0), Node((1, 1), (1, 2), 0, 0), Node((1, 3), (1, 2), 0, 0), Node((2, 1), (1, 2), 0, 0), Node((2, 2), (1, 2), 0, 0), Node((2, 3), (1, 2), 0, 0)]
@@ -159,10 +157,10 @@ class Grid(BaseMap):
 
         >>> grid_map.type_map[1, 0] = TYPES.OBSTACLE     # place an obstacle
         >>> grid_map.get_neighbors(Node((0, 0)))    # limited within the bounds
-        [Node((0, 1), (0, 0), 0, 0), Node((1, 1), (0, 0), 0, 0)]
+        [Node((0, 1), (0, 0), 0, 0), Node((1, 0), (0, 0), 0, 0), Node((1, 1), (0, 0), 0, 0)]
 
         >>> grid_map.get_neighbors(Node((grid_map.shape[0] - 1, grid_map.shape[1] - 1)), diagonal=False)  # limited within the boundss
-        [Node((59, 80), (60, 80), 0, 0), Node((60, 79), (60, 80), 0, 0)]
+        [Node((100, 61), (101, 61), 0, 0), Node((101, 60), (101, 61), 0, 0)]
 
         >>> grid_map.line_of_sight((1, 2), (3, 6))
         [(1, 2), (1, 3), (2, 4), (2, 5), (3, 6)]
@@ -174,6 +172,7 @@ class Grid(BaseMap):
         False
 
         >>> grid_map.type_map[1, 3] = TYPES.OBSTACLE
+        >>> grid_map.update_esdf()
         >>> grid_map.in_collision((1, 2), (3, 6))
         True
     """
@@ -364,7 +363,7 @@ def get_neighbors(self,
 
         return filtered_neighbors
 
-    def line_of_sight(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> list:
+    def line_of_sight(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> List[Tuple[int, ...]]:
         """
         N-dimensional line of sight (Bresenham's line algorithm)
         
@@ -396,7 +395,7 @@ def line_of_sight(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> list:
 
         # Allocate the result array
         result = []
-        result.append(tuple(current))
+        result.append(tuple(int(x) for x in current))
 
         for i in range(1, steps + 1):
             current[primary_axis] += primary_step
@@ -411,7 +410,7 @@ def line_of_sight(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> list:
                     current[d] += 1 if delta[d] > 0 else -1
                     error[d] -= delta2[primary_axis]
 
-            result.append(tuple(current))
+            result.append(tuple(int(x) for x in current))
 
         return result
 
@@ -452,10 +451,6 @@ def in_collision(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> bool:
         steps = abs_delta[primary_axis]
         current = p1
 
-        # # Check the start point
-        # if not self.is_expandable(tuple(current)):
-        #     return True
-        
         for _ in range(steps):
             last_point = current.copy()
             current[primary_axis] += primary_step
@@ -469,7 +464,7 @@ def in_collision(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> bool:
                 if error[d] > abs_delta[primary_axis]:
                     current[d] += 1 if delta[d] > 0 else -1
                     error[d] -= delta2[primary_axis]
-            
+
             # Check the current point
             if not self.is_expandable(tuple(current), tuple(last_point)):
                 return True

src/python_motion_planning/common/env/node.py

@@ -1,8 +1,7 @@
 """
 @file: node.py
-@breif: map node data stucture
 @author: Wu Maojia, Yang Haodong 
-@update: 2025.9.5
+@update: 2025.10.3
 """
 from __future__ import annotations
 

src/python_motion_planning/common/env/robot/base_robot.py

@@ -1,3 +1,8 @@
+"""
+@file: base_robot.py
+@author: Wu Maojia
+@update: 2025.10.3
+"""
 from typing import List, Dict, Tuple, Optional
 import numpy as np
 

src/python_motion_planning/common/env/robot/circular_robot.py

@@ -1,8 +1,7 @@
 """
 @file: circular_robot.py
-@breif: CircularRobot class for the environment.
 @author: Wu Maojia
-@update: 2025.9.20
+@update: 2025.10.3
 """
 from typing import List, Dict, Tuple, Optional
 import numpy as np

src/python_motion_planning/common/env/robot/diff_drive_robot.py

@@ -1,3 +1,8 @@
+"""
+@file: diff_drive_robot.py
+@author: Wu Maojia
+@update: 2025.10.3
+"""
 from typing import List, Dict, Tuple, Optional
 
 import numpy as np

src/python_motion_planning/common/env/types.py

@@ -1,8 +1,7 @@
 """
 @file: types.py
-@breif: macro definition of types of plots in maps
-@author: Yang Haodong, Wu Maojia
-@update: 2025.3.29
+@author: Wu Maojia, Yang Haodong
+@update: 2025.10.3
 """
 # from enum import IntEnum
 

src/python_motion_planning/common/env/world/base_world.py

@@ -1,3 +1,8 @@
+"""
+@file: base_world.py
+@author: Wu Maojia
+@update: 2025.10.3
+"""
 from typing import List, Dict, Tuple, Optional
 import numpy as np
 import gymnasium as gym

src/python_motion_planning/common/env/world/toy_simulator.py

@@ -1,3 +1,8 @@
+"""
+@file: toy_simulator.py
+@author: Wu Maojia
+@update: 2025.10.3
+"""
 from typing import List, Dict, Tuple, Optional
 import numpy as np
 import gymnasium as gym

src/python_motion_planning/common/utils/frame_transformer.py

@@ -1,3 +1,8 @@
+"""
+@file: frame_transformer.py
+@author: Wu Maojia
+@update: 2025.10.3
+"""
 from typing import List, Tuple
 
 import numpy as np
@@ -111,15 +116,15 @@ def pos_world_to_robot(dim: int, pos_world: np.ndarray, robot_pose: np.ndarray)
         if dim != 2:
             raise NotImplementedError("Only 2D pos transform is implemented.")
 
-        # 机器人在世界坐标系中的位置和姿态
+        # pose in world frame
         rx, ry, theta = robot_pose
         c, s = np.cos(theta), np.sin(theta)
 
-        # 平移到原点
+        # translate to the origin
         tx = pos_world[0] - rx
         ty = pos_world[1] - ry
 
-        # 旋转（使用逆旋转矩阵）
+        # rotate (use inverse rotation matrix)
         pos_robot = np.array([
             c * tx + s * ty,
             -s * tx + c * ty
@@ -143,14 +148,13 @@ def pose_world_to_robot(dim: int, pose_world: np.ndarray, robot_pose: np.ndarray
         if dim != 2:
             raise NotImplementedError("Only 2D pose transform is implemented.")
 
-        # 转换位置
+        # transform position
         position_robot = FrameTransformer.pos_world_to_robot(
             dim, pose_world[:2], robot_pose
         )
 
-        # 转换姿态（角度差）
+        # transform orientation
         orientation_robot = pose_world[2] - robot_pose[2]
-        # 归一化角度到[-π, π]
         orientation_robot = Geometry.regularize_orient(orientation_robot)
 
         return np.concatenate([position_robot, [orientation_robot]])
@@ -171,15 +175,15 @@ def pos_robot_to_world(dim: int, pos_robot: np.ndarray, robot_pose: np.ndarray)
         if dim != 2:
             raise NotImplementedError("Only 2D pos transform is implemented.")
 
-        # 机器人在世界坐标系中的位置和姿态
+        # pose in world frame
         rx, ry, theta = robot_pose
         c, s = np.cos(theta), np.sin(theta)
 
-        # 旋转
+        # rotate to the origin
         tx = c * pos_robot[0] - s * pos_robot[1]
         ty = s * pos_robot[0] + c * pos_robot[1]
 
-        # 平移
+        # translate
         pos_world = np.array([
             tx + rx,
             ty + ry
@@ -202,14 +206,13 @@ def pose_robot_to_world(dim: int, pose_robot: np.ndarray, robot_pose: np.ndarray
         if dim != 2:
             raise NotImplementedError("Only 2D pose transform is implemented.")
 
-        # 转换位置
+        # transform position
         position_world = FrameTransformer.pos_robot_to_world(
             dim, pose_robot[:2], robot_pose
         )
 
-        # 转换姿态（角度和）
+        # transform orientation
         orientation_world = pose_robot[2] + robot_pose[2]
-        # 归一化角度到[-π, π]
         orientation_world = Geometry.regularize_orient(orientation_world)
 
         return np.concatenate([position_world, [orientation_world]])