Grid¶
src.python_motion_planning.common.env.map.grid.Grid
¶
Bases: BaseMap
Class for Grid Map. The shape of each dimension of the grid map is determined by the base world and resolution. For each dimension, the conversion equation is: shape_grid = shape_world * resolution + 1 For example, if the base world is (30, 40) and the resolution is 0.5, the grid map will be (30 * 0.5 + 1, 40 * 0.5 + 1) = (61, 81).
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
bounds
|
Iterable
|
The size of map in the world (shape: (n, 2) (n>=2)). bounds[i, 0] means the lower bound of the world in the i-th dimension. bounds[i, 1] means the upper bound of the world in the i-th dimension. |
[[0, 30], [0, 40]]
|
resolution
|
float
|
resolution of the grid map |
1.0
|
type_map
|
Union[GridTypeMap, ndarray]
|
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) |
None
|
inflation_radius
|
float
|
radius of the inflation |
0.0
|
strict_collision
|
bool
|
whether diagonal steps beside obstacles or inflation are collisions (default: True) |
True
|
Examples:
Python Console Session
>>> grid_map = Grid(bounds=[[0, 51], [0, 31]], resolution=0.5)
>>> grid_map
Grid(bounds=[[ 0. 51.]
[ 0. 31.]], resolution=0.5)
Python Console Session
>>> grid_map.bounds # bounds of the base world
array([[ 0., 51.],
[ 0., 31.]])
Python Console Session
>>> grid_map.type_map
GridTypeMap(data=
[[0 0 0 ... 0 0 0]
[0 0 0 ... 0 0 0]
[0 0 0 ... 0 0 0]
...
[0 0 0 ... 0 0 0]
[0 0 0 ... 0 0 0]
[0 0 0 ... 0 0 0]]
, shape=(102, 62), dtype=int8)
Python Console Session
>>> 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)]
Python Console Session
>>> grid_map.get_neighbors(Node((1, 2)), diagonal=False)
[Node((2, 2), (1, 2), 0, 0), Node((0, 2), (1, 2), 0, 0), Node((1, 3), (1, 2), 0, 0), Node((1, 1), (1, 2), 0, 0)]
Python Console Session
>>> grid_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)]
Python Console Session
>>> grid_map.get_neighbors(Node((grid_map.shape[0] - 1, grid_map.shape[1] - 1)), diagonal=False) # limited within the boundss
[Node((100, 61), (101, 61), 0, 0), Node((101, 60), (101, 61), 0, 0)]
Python Console Session
>>> grid_map.line_of_sight((1, 2), (3, 6))
[(1, 2), (1, 3), (2, 4), (2, 5), (3, 6)]
Python Console Session
>>> grid_map[1, 3] = TYPES.OBSTACLE
>>> grid_map.in_collision((1, 2), (3, 6))
True
Python Console Session
>>> grid_map = Grid(bounds=[[0, 3], [0, 3]])
>>> grid_map[1, 0] = TYPES.OBSTACLE
>>> grid_map.in_collision((0, 0), (1, 1))
True
>>> grid_map.strict_collision = False
>>> grid_map.in_collision((0, 0), (1, 1))
False
Python Console Session
>>> grid_map = Grid(bounds=[[0, 4], [0, 4]])
>>> grid_map[2, :] = TYPES.OBSTACLE
>>> grid_map.is_connected((0, 0), (3, 3))
False
>>> grid_map[2, 1] = TYPES.FREE
>>> grid_map.is_connected((0, 0), (3, 3))
True
Source code in src\python_motion_planning\common\env\map\grid.py
Python
class Grid(BaseMap):
"""
Class for Grid Map.
The shape of each dimension of the grid map is determined by the base world and resolution.
For each dimension, the conversion equation is: shape_grid = shape_world * resolution + 1
For example, if the base world is (30, 40) and the resolution is 0.5, the grid map will be (30 * 0.5 + 1, 40 * 0.5 + 1) = (61, 81).
Args:
bounds: The size of map in the world (shape: (n, 2) (n>=2)). bounds[i, 0] means the lower bound of the world in the i-th dimension. bounds[i, 1] means the upper bound of the world in the i-th dimension.
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)
inflation_radius: radius of the inflation
strict_collision: whether diagonal steps beside obstacles or inflation are collisions (default: True)
Examples:
>>> grid_map = Grid(bounds=[[0, 51], [0, 31]], resolution=0.5)
>>> grid_map
Grid(bounds=[[ 0. 51.]
[ 0. 31.]], resolution=0.5)
>>> grid_map.bounds # bounds of the base world
array([[ 0., 51.],
[ 0., 31.]])
>>> grid_map.dim
2
>>> grid_map.resolution
0.5
>>> grid_map.shape # shape of the grid map
(102, 62)
>>> grid_map.dtype
dtype('int8')
>>> grid_map.type_map
GridTypeMap(data=
[[0 0 0 ... 0 0 0]
[0 0 0 ... 0 0 0]
[0 0 0 ... 0 0 0]
...
[0 0 0 ... 0 0 0]
[0 0 0 ... 0 0 0]
[0 0 0 ... 0 0 0]]
, shape=(102, 62), dtype=int8)
>>> grid_map.map_to_world((1, 2))
(0.75, 1.25)
>>> grid_map.world_to_map((0.5, 1.0))
(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)]
>>> grid_map.get_neighbors(Node((1, 2)), diagonal=False)
[Node((2, 2), (1, 2), 0, 0), Node((0, 2), (1, 2), 0, 0), Node((1, 3), (1, 2), 0, 0), Node((1, 1), (1, 2), 0, 0)]
>>> grid_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)]
>>> grid_map.get_neighbors(Node((grid_map.shape[0] - 1, grid_map.shape[1] - 1)), diagonal=False) # limited within the boundss
[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)]
>>> grid_map.line_of_sight((1, 2), (1, 2))
[(1, 2)]
>>> grid_map.in_collision((1, 2), (3, 6))
False
>>> grid_map[1, 3] = TYPES.OBSTACLE
>>> grid_map.in_collision((1, 2), (3, 6))
True
>>> grid_map = Grid(bounds=[[0, 3], [0, 3]])
>>> grid_map[1, 0] = TYPES.OBSTACLE
>>> grid_map.in_collision((0, 0), (1, 1))
True
>>> grid_map.strict_collision = False
>>> grid_map.in_collision((0, 0), (1, 1))
False
>>> grid_map = Grid(bounds=[[0, 4], [0, 4]])
>>> grid_map[2, :] = TYPES.OBSTACLE
>>> grid_map.is_connected((0, 0), (3, 3))
False
>>> grid_map[2, 1] = TYPES.FREE
>>> grid_map.is_connected((0, 0), (3, 3))
True
"""
def __init__(self,
bounds: Iterable = [[0, 30], [0, 40]],
resolution: float = 1.0,
type_map: Union[GridTypeMap, np.ndarray] = None,
inflation_radius: float = 0.0,
strict_collision: bool = True,
) -> None:
super().__init__(bounds)
self._resolution = resolution
shape = tuple([int((self.bounds[i, 1] - self.bounds[i, 0]) / self.resolution) for i in range(self.dim)])
if type_map is None:
self._type_map = GridTypeMap(np.zeros(shape, dtype=np.int8))
else:
if type_map.shape != shape:
raise ValueError("Shape must be {} instead of {} with given bounds={} and resolution={}".format(shape, type_map.shape, self.bounds, self.resolution))
if isinstance(type_map, GridTypeMap):
self._type_map = type_map
elif isinstance(type_map, np.ndarray):
self._type_map = GridTypeMap(type_map)
else:
raise ValueError("Type map must be GridTypeMap or numpy.ndarray instead of {}".format(type(type_map)))
self._shape_array = np.asarray(self.shape, dtype=np.int64)
self._precompute_offsets()
self._esdf = np.zeros(self.shape, dtype=np.float32)
self._connectivity_map = np.zeros(self.shape, dtype=np.int32)
self.strict_collision = strict_collision
self.inflation_radius = inflation_radius
if self.inflation_radius >= 1:
self.inflate_obstacles(self.inflation_radius)
def __str__(self) -> str:
return "Grid(bounds={}, resolution={})".format(self.bounds, self.resolution)
def __repr__(self) -> str:
return self.__str__()
@property
def resolution(self) -> float:
return self._resolution
@property
def type_map(self) -> GridTypeMap:
return self._type_map
@property
def shape(self) -> tuple:
return self._type_map.shape
@property
def dtype(self) -> np.dtype:
return self._type_map.dtype
@property
def esdf(self) -> np.ndarray:
if self._esdf_lazy_flag_:
self.update_esdf()
self._esdf_lazy_flag_ = False
return self._esdf
@property
def connectivity_map(self) -> np.ndarray:
if self._connectivity_lazy_flag_:
self.update_connectivity()
self._connectivity_lazy_flag_ = False
return self._connectivity_map
@property
def data(self) -> np.ndarray:
return self._type_map.data
def __getitem__(self, idx):
return self._type_map[idx]
def __setitem__(self, idx, value):
self._type_map[idx] = value
@property
def _esdf_lazy_flag_(self) -> bool:
return bool(self._type_map._lazy_flags_ & 1)
@_esdf_lazy_flag_.setter
def _esdf_lazy_flag_(self, value: bool) -> None:
if value:
self._type_map._lazy_flags_ |= 1
else:
self._type_map._lazy_flags_ &= ~1
@property
def _connectivity_lazy_flag_(self) -> bool:
return bool(self._type_map._lazy_flags_ & 2)
@_connectivity_lazy_flag_.setter
def _connectivity_lazy_flag_(self, value: bool) -> None:
if value:
self._type_map._lazy_flags_ |= 2
else:
self._type_map._lazy_flags_ &= ~2
@property
def strict_collision(self) -> bool:
return self._strict_collision
@strict_collision.setter
def strict_collision(self, value: bool) -> None:
self._strict_collision = value
self._connectivity_lazy_flag_ = True
def _type_map_flat(self) -> np.ndarray:
return np.ravel(self._type_map.data)
def _esdf_flat(self) -> np.ndarray:
return np.ravel(self.esdf)
def map_to_world(self, point: tuple) -> Tuple[float, ...]:
"""
Convert map coordinates to world coordinates.
Args:
point: Point in map coordinates.
Returns:
point: Point in world coordinates.
"""
if len(point) != self.dim:
raise ValueError("Point dimension does not match map dimension.")
bounds = self.bounds
resolution = self.resolution
return tuple(float((point[d] + 0.5) * resolution + bounds[d, 0]) for d in range(self.dim))
def world_to_map(self, point: Tuple[float, ...], discrete: bool = True) -> tuple:
"""
Convert world coordinates to map coordinates.
Args:
point: Point in world coordinates.
discrete: Whether to round the coordinates to the nearest integer.
Returns:
point: Point in map coordinates.
"""
if len(point) != self.dim:
raise ValueError("Point dimension does not match map dimension.")
if discrete:
point_map = _grid_world_to_map_int(np.asarray(point, dtype=np.float64), self.bounds, self.resolution, self._shape_array)
return tuple(int(x) for x in point_map)
else:
inv_resolution = 1.0 / self.resolution
bounds = self.bounds
return tuple(float((point[d] - bounds[d, 0]) * inv_resolution - 0.5) for d in range(self.dim))
def get_distance(self, p1: Tuple[int, int], p2: Tuple[int, int]) -> float:
"""
Get the distance between two points.
Args:
p1: Start point.
p2: Goal point.
Returns:
dist: Distance between two points.
"""
return Geometry.dist(p1, p2, type='Euclidean')
def within_bounds(self, point: Tuple[int, ...]) -> bool:
"""
Check if a point is within the bounds of the grid map.
Args:
point: Point to check.
Returns:
bool: True if the point is within the bounds of the map, False otherwise.
"""
if len(point) != self.dim:
return False
shape = self.shape
for d in range(self.dim):
if point[d] < 0 or point[d] >= shape[d]:
return False
return True
def is_connected(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> bool:
"""
Check whether two points belong to the same free-space component.
Args:
p1: First point.
p2: Second point.
Returns:
connected: True if the two points are connected, False otherwise.
"""
if not self.within_bounds(p1) or not self.within_bounds(p2):
raise ValueError("Points are out of bounds or invalid.")
connectivity_map = self.connectivity_map
component = connectivity_map[p1]
return component != 0 and component == connectivity_map[p2]
def is_expandable(self, point: Tuple[int, ...], src_point: Tuple[int, ...] = None) -> bool:
"""
Check if a point is expandable.
Args:
point: Point to check.
src_point: Source point.
Returns:
expandable: True if the point is expandable, False otherwise.
"""
point_array = np.asarray(point, dtype=np.int64)
has_src_point = src_point is not None
src_array = point_array if src_point is None else np.asarray(src_point, dtype=np.int64)
return _grid_is_expandable(
point_array,
src_array,
has_src_point,
self._shape_array,
self._type_map_flat(),
self._esdf_flat(),
TYPES.OBSTACLE,
TYPES.INFLATION,
self.strict_collision,
)
def get_neighbors(self,
node: Node,
diagonal: bool = True
) -> list:
"""
Get neighbor nodes of a given node.
Args:
node: Node to get neighbor nodes.
diagonal: Whether to include diagonal neighbors.
Returns:
nodes: List of neighbor nodes.
"""
if node.dim != self.dim:
raise ValueError("Node dimension does not match map dimension.")
positions, mask = self._get_neighbor_arrays(node, diagonal)
return [
Node(tuple(positions[i].tolist()), node.current, node.g, node.h)
for i in range(positions.shape[0])
if mask[i]
]
def _get_neighbor_arrays(self, node: Node, diagonal: bool = True) -> Tuple[np.ndarray, np.ndarray]:
"""Get candidate neighbor positions and their expandable mask."""
offsets = self._diagonal_offsets_array if diagonal else self._orthogonal_offsets_array
positions, mask = _grid_neighbor_positions_and_mask(
np.asarray(node.current, dtype=np.int64),
offsets,
self._shape_array,
self._type_map_flat(),
self._esdf_flat(),
TYPES.OBSTACLE,
TYPES.INFLATION,
self.strict_collision,
)
return positions, mask
def line_of_sight(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> List[Tuple[int, ...]]:
"""
N-dimensional line of sight (Bresenham's line algorithm)
Args:
p1: Start point of the line.
p2: End point of the line.
Returns:
points: List of point on the line of sight.
"""
p1_array = np.asarray(p1, dtype=np.int64)
p2_array = np.asarray(p2, dtype=np.int64)
if p1_array.shape != p2_array.shape:
p2_array - p1_array
points = _grid_line_of_sight(p1_array, p2_array)
return [tuple(int(x) for x in points[i]) for i in range(points.shape[0])]
def in_collision(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> bool:
"""
Check if the line of sight between two points is in collision.
Args:
p1: Start point of the line.
p2: End point of the line.
Returns:
in_collision: True if the line of sight is in collision, False otherwise.
"""
p1_array = np.asarray(p1, dtype=np.int64)
p2_array = np.asarray(p2, dtype=np.int64)
if p1_array.shape != p2_array.shape:
p2_array - p1_array
return _grid_in_collision(
p1_array,
p2_array,
self._shape_array,
self._type_map_flat(),
self._esdf_flat(),
TYPES.OBSTACLE,
TYPES.INFLATION,
self.strict_collision,
)
def fill_boundary_with_obstacles(self) -> None:
"""
Fill the boundary of the map with obstacles.
"""
for d in range(self.dim):
# Create a tuple of slice objects to select boundary elements in current dimension
# First boundary (start index)
slices_start = [slice(None)] * self.dim
slices_start[d] = 0
self._type_map[tuple(slices_start)] = TYPES.OBSTACLE
# Last boundary (end index)
slices_end = [slice(None)] * self.dim
slices_end[d] = -1
self._type_map[tuple(slices_end)] = TYPES.OBSTACLE
def inflate_obstacles(self, radius: float = 1.0) -> None:
"""
Inflate the obstacles in the map.
Args:
radius: Radius of the inflation.
"""
mask = (self.esdf <= radius) & (self._type_map.data == TYPES.FREE)
self._type_map[mask] = TYPES.INFLATION
self.inflation_radius = radius
def fill_expands(self, expands: Dict[Tuple[int, ...], Node]) -> None:
"""
Fill the expands in the map.
Args:
expands: List of expands.
"""
for expand in expands.keys():
if self._type_map[expand] != TYPES.FREE:
continue
self._type_map[expand] = TYPES.EXPAND
def update_esdf(self) -> None:
"""
Update the ESDF (signed Euclidean Distance Field) based on the obstacles in the map.
- Obstacle grid ESDF = 0
- Free grid ESDF > 0. The value is the di/stance to the nearest obstacle
"""
obstacle_mask = (self._type_map.data == TYPES.OBSTACLE)
free_mask = ~obstacle_mask
# distance to obstacles
dist_outside = ndimage.distance_transform_edt(free_mask, sampling=self.resolution)
# distance to free space (internal distance of obstacles)
dist_inside = ndimage.distance_transform_edt(obstacle_mask, sampling=self.resolution)
self._esdf = dist_outside.astype(np.float32)
self._esdf[obstacle_mask] = -dist_inside[obstacle_mask]
self._esdf_lazy_flag_ = False
def update_connectivity(self) -> None:
"""Update the free-space connected component map."""
type_map = self._type_map.data
free_mask = (type_map != TYPES.OBSTACLE) & (type_map != TYPES.INFLATION)
connectivity = 1 if self.strict_collision else self.dim
structure = ndimage.generate_binary_structure(self.dim, connectivity)
ndimage.label(free_mask, structure=structure, output=self._connectivity_map)
self._connectivity_lazy_flag_ = False
def path_map_to_world(self, path: List[tuple]) -> List[Tuple[float, ...]]:
"""
Convert path from map coordinates to world coordinates
Args:
path: a list of map coordinates
Returns:
path: a list of world coordinates
"""
path = list(path)
if not path:
return []
points = np.asarray(path, dtype=np.float64)
if points.ndim != 2 or points.shape[1] != self.dim:
raise ValueError("Point dimension does not match map dimension.")
path_world = _grid_path_map_to_world(points, self.bounds, self.resolution)
return [tuple(float(x) for x in path_world[i]) for i in range(path_world.shape[0])]
def path_world_to_map(self, path: List[Tuple[float, ...]], discrete: bool = True) -> List[tuple]:
"""
Convert path from world coordinates to map coordinates
Args:
path: a list of world coordinates
discrete: whether to round the coordinates to the nearest integer
Returns:
path: a list of map coordinates
"""
path = list(path)
if not path:
return []
points = np.asarray(path, dtype=np.float64)
if points.ndim != 2 or points.shape[1] != self.dim:
raise ValueError("Point dimension does not match map dimension.")
if discrete:
path_map = _grid_path_world_to_map_int(points, self.bounds, self.resolution, self._shape_array)
return [tuple(int(x) for x in path_map[i]) for i in range(path_map.shape[0])]
else:
path_map = _grid_path_world_to_map_float(points, self.bounds, self.resolution)
return [tuple(float(x) for x in path_map[i]) for i in range(path_map.shape[0])]
def point_float_to_int(self, point: Tuple[float, ...]) -> Tuple[int, ...]:
"""
Convert a point from float to integer coordinates.
Args:
point: a point in float coordinates
Returns:
point: a point in integer coordinates
"""
shape = self.shape
point_int = []
for d in range(self.dim):
value = round(point[d])
if value < 0:
value = 0
elif value >= shape[d]:
value = shape[d] - 1
point_int.append(value)
return tuple(point_int)
def _precompute_offsets(self):
# Generate all possible offsets (-1, 0, +1) in each dimension
self._diagonal_offsets_array = np.array(np.meshgrid(*[[-1, 0, 1]]*self.dim), dtype=np.int64).T.reshape(-1, self.dim)
# Remove the zero offset (current node itself)
self._diagonal_offsets_array = self._diagonal_offsets_array[np.any(self._diagonal_offsets_array != 0, axis=1)]
# self._diagonal_offsets = [Node((offset.tolist(), dtype=self.dtype)) for offset in self._diagonal_offsets]
self._diagonal_offsets = [Node(tuple(offset.tolist())) for offset in self._diagonal_offsets_array]
# Generate only orthogonal offsets (one dimension changes by ±1)
self._orthogonal_offsets_array = np.zeros((2*self.dim, self.dim), dtype=np.int64)
for d in range(self.dim):
self._orthogonal_offsets_array[2*d, d] = 1
self._orthogonal_offsets_array[2*d+1, d] = -1
# self._orthogonal_offsets = [Node((offset.tolist(), dtype=self.dtype)) for offset in self._orthogonal_offsets]
self._orthogonal_offsets = [Node(tuple(offset.tolist())) for offset in self._orthogonal_offsets_array]
fill_boundary_with_obstacles()
¶
Fill the boundary of the map with obstacles.
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def fill_boundary_with_obstacles(self) -> None:
"""
Fill the boundary of the map with obstacles.
"""
for d in range(self.dim):
# Create a tuple of slice objects to select boundary elements in current dimension
# First boundary (start index)
slices_start = [slice(None)] * self.dim
slices_start[d] = 0
self._type_map[tuple(slices_start)] = TYPES.OBSTACLE
# Last boundary (end index)
slices_end = [slice(None)] * self.dim
slices_end[d] = -1
self._type_map[tuple(slices_end)] = TYPES.OBSTACLE
fill_expands(expands)
¶
Fill the expands in the map.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
expands
|
Dict[Tuple[int, ...], Node]
|
List of expands. |
required |
Source code in src\python_motion_planning\common\env\map\grid.py
get_distance(p1, p2)
¶
Get the distance between two points.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
p1
|
Tuple[int, int]
|
Start point. |
required |
p2
|
Tuple[int, int]
|
Goal point. |
required |
Returns:
| Name | Type | Description |
|---|---|---|
dist |
float
|
Distance between two points. |
Source code in src\python_motion_planning\common\env\map\grid.py
get_neighbors(node, diagonal=True)
¶
Get neighbor nodes of a given node.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
node
|
Node
|
Node to get neighbor nodes. |
required |
diagonal
|
bool
|
Whether to include diagonal neighbors. |
True
|
Returns:
| Name | Type | Description |
|---|---|---|
nodes |
list
|
List of neighbor nodes. |
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def get_neighbors(self,
node: Node,
diagonal: bool = True
) -> list:
"""
Get neighbor nodes of a given node.
Args:
node: Node to get neighbor nodes.
diagonal: Whether to include diagonal neighbors.
Returns:
nodes: List of neighbor nodes.
"""
if node.dim != self.dim:
raise ValueError("Node dimension does not match map dimension.")
positions, mask = self._get_neighbor_arrays(node, diagonal)
return [
Node(tuple(positions[i].tolist()), node.current, node.g, node.h)
for i in range(positions.shape[0])
if mask[i]
]
in_collision(p1, p2)
¶
Check if the line of sight between two points is in collision.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
p1
|
Tuple[int, ...]
|
Start point of the line. |
required |
p2
|
Tuple[int, ...]
|
End point of the line. |
required |
Returns:
| Name | Type | Description |
|---|---|---|
in_collision |
bool
|
True if the line of sight is in collision, False otherwise. |
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def in_collision(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> bool:
"""
Check if the line of sight between two points is in collision.
Args:
p1: Start point of the line.
p2: End point of the line.
Returns:
in_collision: True if the line of sight is in collision, False otherwise.
"""
p1_array = np.asarray(p1, dtype=np.int64)
p2_array = np.asarray(p2, dtype=np.int64)
if p1_array.shape != p2_array.shape:
p2_array - p1_array
return _grid_in_collision(
p1_array,
p2_array,
self._shape_array,
self._type_map_flat(),
self._esdf_flat(),
TYPES.OBSTACLE,
TYPES.INFLATION,
self.strict_collision,
)
inflate_obstacles(radius=1.0)
¶
Inflate the obstacles in the map.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
radius
|
float
|
Radius of the inflation. |
1.0
|
Source code in src\python_motion_planning\common\env\map\grid.py
is_connected(p1, p2)
¶
Check whether two points belong to the same free-space component.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
p1
|
Tuple[int, ...]
|
First point. |
required |
p2
|
Tuple[int, ...]
|
Second point. |
required |
Returns:
| Name | Type | Description |
|---|---|---|
connected |
bool
|
True if the two points are connected, False otherwise. |
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def is_connected(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> bool:
"""
Check whether two points belong to the same free-space component.
Args:
p1: First point.
p2: Second point.
Returns:
connected: True if the two points are connected, False otherwise.
"""
if not self.within_bounds(p1) or not self.within_bounds(p2):
raise ValueError("Points are out of bounds or invalid.")
connectivity_map = self.connectivity_map
component = connectivity_map[p1]
return component != 0 and component == connectivity_map[p2]
is_expandable(point, src_point=None)
¶
Check if a point is expandable.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
point
|
Tuple[int, ...]
|
Point to check. |
required |
src_point
|
Tuple[int, ...]
|
Source point. |
None
|
Returns:
| Name | Type | Description |
|---|---|---|
expandable |
bool
|
True if the point is expandable, False otherwise. |
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def is_expandable(self, point: Tuple[int, ...], src_point: Tuple[int, ...] = None) -> bool:
"""
Check if a point is expandable.
Args:
point: Point to check.
src_point: Source point.
Returns:
expandable: True if the point is expandable, False otherwise.
"""
point_array = np.asarray(point, dtype=np.int64)
has_src_point = src_point is not None
src_array = point_array if src_point is None else np.asarray(src_point, dtype=np.int64)
return _grid_is_expandable(
point_array,
src_array,
has_src_point,
self._shape_array,
self._type_map_flat(),
self._esdf_flat(),
TYPES.OBSTACLE,
TYPES.INFLATION,
self.strict_collision,
)
line_of_sight(p1, p2)
¶
N-dimensional line of sight (Bresenham's line algorithm)
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
p1
|
Tuple[int, ...]
|
Start point of the line. |
required |
p2
|
Tuple[int, ...]
|
End point of the line. |
required |
Returns:
| Name | Type | Description |
|---|---|---|
points |
List[Tuple[int, ...]]
|
List of point on the line of sight. |
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def line_of_sight(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> List[Tuple[int, ...]]:
"""
N-dimensional line of sight (Bresenham's line algorithm)
Args:
p1: Start point of the line.
p2: End point of the line.
Returns:
points: List of point on the line of sight.
"""
p1_array = np.asarray(p1, dtype=np.int64)
p2_array = np.asarray(p2, dtype=np.int64)
if p1_array.shape != p2_array.shape:
p2_array - p1_array
points = _grid_line_of_sight(p1_array, p2_array)
return [tuple(int(x) for x in points[i]) for i in range(points.shape[0])]
map_to_world(point)
¶
Convert map coordinates to world coordinates.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
point
|
tuple
|
Point in map coordinates. |
required |
Returns:
| Name | Type | Description |
|---|---|---|
point |
Tuple[float, ...]
|
Point in world coordinates. |
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def map_to_world(self, point: tuple) -> Tuple[float, ...]:
"""
Convert map coordinates to world coordinates.
Args:
point: Point in map coordinates.
Returns:
point: Point in world coordinates.
"""
if len(point) != self.dim:
raise ValueError("Point dimension does not match map dimension.")
bounds = self.bounds
resolution = self.resolution
return tuple(float((point[d] + 0.5) * resolution + bounds[d, 0]) for d in range(self.dim))
path_map_to_world(path)
¶
Convert path from map coordinates to world coordinates
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
path
|
List[tuple]
|
a list of map coordinates |
required |
Returns:
| Name | Type | Description |
|---|---|---|
path |
List[Tuple[float, ...]]
|
a list of world coordinates |
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def path_map_to_world(self, path: List[tuple]) -> List[Tuple[float, ...]]:
"""
Convert path from map coordinates to world coordinates
Args:
path: a list of map coordinates
Returns:
path: a list of world coordinates
"""
path = list(path)
if not path:
return []
points = np.asarray(path, dtype=np.float64)
if points.ndim != 2 or points.shape[1] != self.dim:
raise ValueError("Point dimension does not match map dimension.")
path_world = _grid_path_map_to_world(points, self.bounds, self.resolution)
return [tuple(float(x) for x in path_world[i]) for i in range(path_world.shape[0])]
path_world_to_map(path, discrete=True)
¶
Convert path from world coordinates to map coordinates
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
path
|
List[Tuple[float, ...]]
|
a list of world coordinates |
required |
discrete
|
bool
|
whether to round the coordinates to the nearest integer |
True
|
Returns:
| Name | Type | Description |
|---|---|---|
path |
List[tuple]
|
a list of map coordinates |
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def path_world_to_map(self, path: List[Tuple[float, ...]], discrete: bool = True) -> List[tuple]:
"""
Convert path from world coordinates to map coordinates
Args:
path: a list of world coordinates
discrete: whether to round the coordinates to the nearest integer
Returns:
path: a list of map coordinates
"""
path = list(path)
if not path:
return []
points = np.asarray(path, dtype=np.float64)
if points.ndim != 2 or points.shape[1] != self.dim:
raise ValueError("Point dimension does not match map dimension.")
if discrete:
path_map = _grid_path_world_to_map_int(points, self.bounds, self.resolution, self._shape_array)
return [tuple(int(x) for x in path_map[i]) for i in range(path_map.shape[0])]
else:
path_map = _grid_path_world_to_map_float(points, self.bounds, self.resolution)
return [tuple(float(x) for x in path_map[i]) for i in range(path_map.shape[0])]
point_float_to_int(point)
¶
Convert a point from float to integer coordinates.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
point
|
Tuple[float, ...]
|
a point in float coordinates |
required |
Returns:
| Name | Type | Description |
|---|---|---|
point |
Tuple[int, ...]
|
a point in integer coordinates |
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def point_float_to_int(self, point: Tuple[float, ...]) -> Tuple[int, ...]:
"""
Convert a point from float to integer coordinates.
Args:
point: a point in float coordinates
Returns:
point: a point in integer coordinates
"""
shape = self.shape
point_int = []
for d in range(self.dim):
value = round(point[d])
if value < 0:
value = 0
elif value >= shape[d]:
value = shape[d] - 1
point_int.append(value)
return tuple(point_int)
update_connectivity()
¶
Update the free-space connected component map.
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def update_connectivity(self) -> None:
"""Update the free-space connected component map."""
type_map = self._type_map.data
free_mask = (type_map != TYPES.OBSTACLE) & (type_map != TYPES.INFLATION)
connectivity = 1 if self.strict_collision else self.dim
structure = ndimage.generate_binary_structure(self.dim, connectivity)
ndimage.label(free_mask, structure=structure, output=self._connectivity_map)
self._connectivity_lazy_flag_ = False
update_esdf()
¶
Update the ESDF (signed Euclidean Distance Field) based on the obstacles in the map. - Obstacle grid ESDF = 0 - Free grid ESDF > 0. The value is the di/stance to the nearest obstacle
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def update_esdf(self) -> None:
"""
Update the ESDF (signed Euclidean Distance Field) based on the obstacles in the map.
- Obstacle grid ESDF = 0
- Free grid ESDF > 0. The value is the di/stance to the nearest obstacle
"""
obstacle_mask = (self._type_map.data == TYPES.OBSTACLE)
free_mask = ~obstacle_mask
# distance to obstacles
dist_outside = ndimage.distance_transform_edt(free_mask, sampling=self.resolution)
# distance to free space (internal distance of obstacles)
dist_inside = ndimage.distance_transform_edt(obstacle_mask, sampling=self.resolution)
self._esdf = dist_outside.astype(np.float32)
self._esdf[obstacle_mask] = -dist_inside[obstacle_mask]
self._esdf_lazy_flag_ = False
within_bounds(point)
¶
Check if a point is within the bounds of the grid map.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
point
|
Tuple[int, ...]
|
Point to check. |
required |
Returns:
| Name | Type | Description |
|---|---|---|
bool |
bool
|
True if the point is within the bounds of the map, False otherwise. |
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def within_bounds(self, point: Tuple[int, ...]) -> bool:
"""
Check if a point is within the bounds of the grid map.
Args:
point: Point to check.
Returns:
bool: True if the point is within the bounds of the map, False otherwise.
"""
if len(point) != self.dim:
return False
shape = self.shape
for d in range(self.dim):
if point[d] < 0 or point[d] >= shape[d]:
return False
return True
world_to_map(point, discrete=True)
¶
Convert world coordinates to map coordinates.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
point
|
Tuple[float, ...]
|
Point in world coordinates. |
required |
discrete
|
bool
|
Whether to round the coordinates to the nearest integer. |
True
|
Returns:
| Name | Type | Description |
|---|---|---|
point |
tuple
|
Point in map coordinates. |
Source code in src\python_motion_planning\common\env\map\grid.py
Python
def world_to_map(self, point: Tuple[float, ...], discrete: bool = True) -> tuple:
"""
Convert world coordinates to map coordinates.
Args:
point: Point in world coordinates.
discrete: Whether to round the coordinates to the nearest integer.
Returns:
point: Point in map coordinates.
"""
if len(point) != self.dim:
raise ValueError("Point dimension does not match map dimension.")
if discrete:
point_map = _grid_world_to_map_int(np.asarray(point, dtype=np.float64), self.bounds, self.resolution, self._shape_array)
return tuple(int(x) for x in point_map)
else:
inv_resolution = 1.0 / self.resolution
bounds = self.bounds
return tuple(float((point[d] - bounds[d, 0]) * inv_resolution - 0.5) for d in range(self.dim))