mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-12 20:48:01 -05:00
5392fcff4d
* Refactor module links and improve code documentation. Updated documentation to rename "API" sections to "Code Link" for clarity and consistency. Enhanced docstrings for `circle_fitting` and `kmeans_clustering` functions, improving parameter descriptions and adding return value details. Fixed typos in function and file names in the ray casting grid map module. * Fix import typo in ray casting grid map test module. Corrected the import statement in the test file by updating the module's name to `ray_casting_grid_map` for consistency with the source file. This ensures proper functionality of the test suite.
138 lines
3.4 KiB
Python
138 lines
3.4 KiB
Python
"""
|
|
|
|
Ray casting 2D grid map example
|
|
|
|
author: Atsushi Sakai (@Atsushi_twi)
|
|
|
|
"""
|
|
|
|
import math
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
|
|
EXTEND_AREA = 10.0
|
|
|
|
show_animation = True
|
|
|
|
|
|
def calc_grid_map_config(ox, oy, xyreso):
|
|
minx = round(min(ox) - EXTEND_AREA / 2.0)
|
|
miny = round(min(oy) - EXTEND_AREA / 2.0)
|
|
maxx = round(max(ox) + EXTEND_AREA / 2.0)
|
|
maxy = round(max(oy) + EXTEND_AREA / 2.0)
|
|
xw = int(round((maxx - minx) / xyreso))
|
|
yw = int(round((maxy - miny) / xyreso))
|
|
|
|
return minx, miny, maxx, maxy, xw, yw
|
|
|
|
|
|
class precastDB:
|
|
|
|
def __init__(self):
|
|
self.px = 0.0
|
|
self.py = 0.0
|
|
self.d = 0.0
|
|
self.angle = 0.0
|
|
self.ix = 0
|
|
self.iy = 0
|
|
|
|
def __str__(self):
|
|
return str(self.px) + "," + str(self.py) + "," + str(self.d) + "," + str(self.angle)
|
|
|
|
|
|
def atan_zero_to_twopi(y, x):
|
|
angle = math.atan2(y, x)
|
|
if angle < 0.0:
|
|
angle += math.pi * 2.0
|
|
|
|
return angle
|
|
|
|
|
|
def pre_casting(minx, miny, xw, yw, xyreso, yawreso):
|
|
|
|
precast = [[] for i in range(int(round((math.pi * 2.0) / yawreso)) + 1)]
|
|
|
|
for ix in range(xw):
|
|
for iy in range(yw):
|
|
px = ix * xyreso + minx
|
|
py = iy * xyreso + miny
|
|
|
|
d = math.hypot(px, py)
|
|
angle = atan_zero_to_twopi(py, px)
|
|
angleid = int(math.floor(angle / yawreso))
|
|
|
|
pc = precastDB()
|
|
|
|
pc.px = px
|
|
pc.py = py
|
|
pc.d = d
|
|
pc.ix = ix
|
|
pc.iy = iy
|
|
pc.angle = angle
|
|
|
|
precast[angleid].append(pc)
|
|
|
|
return precast
|
|
|
|
|
|
def generate_ray_casting_grid_map(ox, oy, xyreso, yawreso):
|
|
|
|
minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
|
|
|
|
pmap = [[0.0 for i in range(yw)] for i in range(xw)]
|
|
|
|
precast = pre_casting(minx, miny, xw, yw, xyreso, yawreso)
|
|
|
|
for (x, y) in zip(ox, oy):
|
|
|
|
d = math.hypot(x, y)
|
|
angle = atan_zero_to_twopi(y, x)
|
|
angleid = int(math.floor(angle / yawreso))
|
|
|
|
gridlist = precast[angleid]
|
|
|
|
ix = int(round((x - minx) / xyreso))
|
|
iy = int(round((y - miny) / xyreso))
|
|
|
|
for grid in gridlist:
|
|
if grid.d > d:
|
|
pmap[grid.ix][grid.iy] = 0.5
|
|
|
|
pmap[ix][iy] = 1.0
|
|
|
|
return pmap, minx, maxx, miny, maxy, xyreso
|
|
|
|
|
|
def draw_heatmap(data, minx, maxx, miny, maxy, xyreso):
|
|
x, y = np.mgrid[slice(minx - xyreso / 2.0, maxx + xyreso / 2.0, xyreso),
|
|
slice(miny - xyreso / 2.0, maxy + xyreso / 2.0, xyreso)]
|
|
plt.pcolor(x, y, data, vmax=1.0, cmap=plt.cm.Blues)
|
|
plt.axis("equal")
|
|
|
|
|
|
def main():
|
|
print(__file__ + " start!!")
|
|
|
|
xyreso = 0.25 # x-y grid resolution [m]
|
|
yawreso = np.deg2rad(10.0) # yaw angle resolution [rad]
|
|
|
|
for i in range(5):
|
|
ox = (np.random.rand(4) - 0.5) * 10.0
|
|
oy = (np.random.rand(4) - 0.5) * 10.0
|
|
pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(
|
|
ox, oy, xyreso, yawreso)
|
|
|
|
if show_animation: # pragma: no cover
|
|
plt.cla()
|
|
# for stopping simulation with the esc key.
|
|
plt.gcf().canvas.mpl_connect('key_release_event',
|
|
lambda event: [exit(0) if event.key == 'escape' else None])
|
|
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
|
|
plt.plot(ox, oy, "xr")
|
|
plt.plot(0.0, 0.0, "ob")
|
|
plt.pause(1.0)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|