mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 02:28:03 -05:00
This commit is contained in:
Andrew Tu
3
.github/FUNDING.yml
vendored
Normal file
3
.github/FUNDING.yml
vendored
Normal file
@@ -0,0 +1,3 @@
|
||||
# These are supported funding model platforms
|
||||
patreon: myenigma
|
||||
custom: https://www.paypal.me/myenigmapay/
|
||||
1
.gitignore
vendored
1
.gitignore
vendored
@@ -1,5 +1,6 @@
|
||||
*.csv
|
||||
*.gif
|
||||
*.g2o
|
||||
|
||||
# Byte-compiled / optimized / DLL files
|
||||
__pycache__/
|
||||
|
||||
@@ -6,17 +6,22 @@ author: Atsushi Sakai (@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import math
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# Estimation parameter of EKF
|
||||
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # predict state covariance
|
||||
# Covariance for EKF simulation
|
||||
Q = np.diag([
|
||||
0.1, # variance of location on x-axis
|
||||
0.1, # variance of location on y-axis
|
||||
np.deg2rad(1.0), # variance of yaw angle
|
||||
1.0 # variance of velocity
|
||||
])**2 # predict state covariance
|
||||
R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
|
||||
|
||||
# Simulation parameter
|
||||
Qsim = np.diag([1.0, np.deg2rad(30.0)])**2
|
||||
Rsim = np.diag([0.5, 0.5])**2
|
||||
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)])**2
|
||||
GPS_NOISE = np.diag([0.5, 0.5])**2
|
||||
|
||||
DT = 0.1 # time tick [s]
|
||||
SIM_TIME = 50.0 # simulation time [s]
|
||||
@@ -27,7 +32,7 @@ show_animation = True
|
||||
def calc_input():
|
||||
v = 1.0 # [m/s]
|
||||
yawrate = 0.1 # [rad/s]
|
||||
u = np.array([[v, yawrate]]).T
|
||||
u = np.array([[v], [yawrate]])
|
||||
return u
|
||||
|
||||
|
||||
@@ -36,14 +41,10 @@ def observation(xTrue, xd, u):
|
||||
xTrue = motion_model(xTrue, u)
|
||||
|
||||
# add noise to gps x-y
|
||||
zx = xTrue[0, 0] + np.random.randn() * Rsim[0, 0]
|
||||
zy = xTrue[1, 0] + np.random.randn() * Rsim[1, 1]
|
||||
z = np.array([[zx, zy]]).T
|
||||
z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
|
||||
|
||||
# add noise to input
|
||||
ud1 = u[0, 0] + np.random.randn() * Qsim[0, 0]
|
||||
ud2 = u[1, 0] + np.random.randn() * Qsim[1, 1]
|
||||
ud = np.array([[ud1, ud2]]).T
|
||||
ud = u + INPUT_NOISE @ np.random.randn(2, 1)
|
||||
|
||||
xd = motion_model(xd, ud)
|
||||
|
||||
@@ -62,19 +63,18 @@ def motion_model(x, u):
|
||||
[0.0, DT],
|
||||
[1.0, 0.0]])
|
||||
|
||||
x = F@x + B@u
|
||||
x = F @ x + B @ u
|
||||
|
||||
return x
|
||||
|
||||
|
||||
def observation_model(x):
|
||||
# Observation Model
|
||||
H = np.array([
|
||||
[1, 0, 0, 0],
|
||||
[0, 1, 0, 0]
|
||||
])
|
||||
|
||||
z = H@x
|
||||
z = H @ x
|
||||
|
||||
return z
|
||||
|
||||
|
||||
@@ -11,13 +11,18 @@ import scipy.linalg
|
||||
import math
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# Estimation parameter of UKF
|
||||
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2
|
||||
R = np.diag([1.0, np.deg2rad(40.0)])**2
|
||||
# Covariance for UKF simulation
|
||||
Q = np.diag([
|
||||
0.1, # variance of location on x-axis
|
||||
0.1, # variance of location on y-axis
|
||||
np.deg2rad(1.0), # variance of yaw angle
|
||||
1.0 # variance of velocity
|
||||
])**2 # predict state covariance
|
||||
R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
|
||||
|
||||
# Simulation parameter
|
||||
Qsim = np.diag([0.5, 0.5])**2
|
||||
Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
|
||||
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)])**2
|
||||
GPS_NOISE = np.diag([0.5, 0.5])**2
|
||||
|
||||
DT = 0.1 # time tick [s]
|
||||
SIM_TIME = 50.0 # simulation time [s]
|
||||
@@ -42,14 +47,10 @@ def observation(xTrue, xd, u):
|
||||
xTrue = motion_model(xTrue, u)
|
||||
|
||||
# add noise to gps x-y
|
||||
zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
|
||||
zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
|
||||
z = np.array([[zx, zy]]).T
|
||||
z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
|
||||
|
||||
# add noise to input
|
||||
ud1 = u[0] + np.random.randn() * Rsim[0, 0]
|
||||
ud2 = u[1] + np.random.randn() * Rsim[1, 1]
|
||||
ud = np.array([ud1, ud2])
|
||||
ud = u + INPUT_NOISE @ np.random.randn(2, 1)
|
||||
|
||||
xd = motion_model(xd, ud)
|
||||
|
||||
@@ -100,7 +101,9 @@ def generate_sigma_points(xEst, PEst, gamma):
|
||||
|
||||
|
||||
def predict_sigma_motion(sigma, u):
|
||||
# Sigma Points prediction with motion model
|
||||
"""
|
||||
Sigma Points prediction with motion model
|
||||
"""
|
||||
for i in range(sigma.shape[1]):
|
||||
sigma[:, i:i + 1] = motion_model(sigma[:, i:i + 1], u)
|
||||
|
||||
@@ -108,7 +111,9 @@ def predict_sigma_motion(sigma, u):
|
||||
|
||||
|
||||
def predict_sigma_observation(sigma):
|
||||
# Sigma Points prediction with observation model
|
||||
"""
|
||||
Sigma Points prediction with observation model
|
||||
"""
|
||||
for i in range(sigma.shape[1]):
|
||||
sigma[0:2, i] = observation_model(sigma[:, i])
|
||||
|
||||
|
||||
@@ -73,7 +73,7 @@ def update_clusters(clusters):
|
||||
mind = min(dlist)
|
||||
min_id = dlist.index(mind)
|
||||
clusters.labels[ip] = min_id
|
||||
cost += min_id
|
||||
cost += mind
|
||||
|
||||
return clusters, cost
|
||||
|
||||
|
||||
BIN
Mapping/lidar_to_grid_map/animation.gif
Normal file
BIN
Mapping/lidar_to_grid_map/animation.gif
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 279 KiB |
BIN
Mapping/lidar_to_grid_map/grid_map_example.png
Normal file
BIN
Mapping/lidar_to_grid_map/grid_map_example.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 372 KiB |
154
Mapping/lidar_to_grid_map/lidar01.csv
Normal file
154
Mapping/lidar_to_grid_map/lidar01.csv
Normal file
@@ -0,0 +1,154 @@
|
||||
0.008450416037156572,0.5335
|
||||
0.046902201120156306,0.5345
|
||||
0.08508127850753233,0.537
|
||||
0.1979822644959155,0.2605
|
||||
0.21189035697274505,0.2625
|
||||
0.2587960806200922,0.26475
|
||||
0.3043382657893199,0.2675
|
||||
0.34660795861105775,0.27075
|
||||
0.43632879047139106,0.59
|
||||
0.4739624524675188,0.60025
|
||||
0.5137777760286397,0.611
|
||||
0.5492297764597742,0.6265
|
||||
0.5895905154121426,0.64
|
||||
0.6253152235389017,0.6565
|
||||
0.6645851317087743,0.676
|
||||
0.6997644244442851,0.6975
|
||||
0.7785769484796541,0.3345
|
||||
0.7772134100015329,0.74575
|
||||
0.8652979956881222,0.3315
|
||||
0.8996591653367609,0.31775
|
||||
0.9397471965935056,0.31275
|
||||
0.9847439663714841,0.31125
|
||||
1.0283771976713423,0.31325
|
||||
1.0641019057981014,0.31975
|
||||
1.1009174447073562,0.3335
|
||||
1.2012738766970301,0.92275
|
||||
1.2397256617800307,0.95325
|
||||
1.2779047391674068,0.9865
|
||||
1.316629231946031,1.01775
|
||||
1.3561718478115274,1.011
|
||||
1.3948963405901518,1.0055
|
||||
1.4330754179775278,1.00225
|
||||
1.4731634492342724,0.99975
|
||||
1.5113425266216485,0.9975
|
||||
1.5517032655740168,1.001
|
||||
1.5896096352657691,1.00275
|
||||
1.6288795434356418,1.008
|
||||
1.6684221593011381,1.0135
|
||||
1.7066012366885142,1.022
|
||||
1.7453257294671385,1.02875
|
||||
1.7862318838107551,0.9935
|
||||
1.8257744996762515,1.0025
|
||||
1.8661352386286207,0.96075
|
||||
1.9045870237116205,0.92125
|
||||
1.9465840088377355,0.8855
|
||||
1.986944747790103,0.85725
|
||||
2.025669240568728,0.832
|
||||
2.065757271825472,0.80675
|
||||
2.1066634261690886,0.78875
|
||||
2.1464787497302105,0.7705
|
||||
2.1865667809869542,0.75625
|
||||
2.2261093968524506,0.74475
|
||||
2.2683790896741876,0.68275
|
||||
2.3090125363221823,0.6375
|
||||
2.3510095214482956,0.59925
|
||||
2.3916429680962885,0.5665
|
||||
2.4330945378311526,0.538
|
||||
2.4783640153047557,0.50825
|
||||
2.5203610004308707,0.4875
|
||||
2.562903400948233,0.46825
|
||||
2.599173524466238,0.45
|
||||
2.642806755766097,0.4355
|
||||
2.685076448587836,0.42275
|
||||
2.722437402888339,0.4125
|
||||
2.766888757275069,0.40125
|
||||
2.8007045115324587,0.39525
|
||||
2.841883373571701,0.385
|
||||
2.8819714048284446,0.3805
|
||||
2.922332143780814,0.38575
|
||||
2.9637837135156797,0.38425
|
||||
3.0005992524249336,0.36575
|
||||
3.0401418682904318,0.3765
|
||||
3.079957191851552,0.3915
|
||||
3.115409192282687,0.408
|
||||
3.154679100452558,0.4265
|
||||
3.184949654666836,0.447
|
||||
3.2242195628367085,0.4715
|
||||
3.2574899017028507,0.49875
|
||||
3.2959416867858504,0.52875
|
||||
3.3292120256519926,0.564
|
||||
3.3665729799524957,0.6055
|
||||
3.4031158111661277,0.6515
|
||||
3.438022396206014,0.70675
|
||||
3.4756560582021407,0.771
|
||||
3.513562427893893,0.77075
|
||||
3.5522869206725183,0.7785
|
||||
3.621827383056667,0.79575
|
||||
3.65918833735717,0.8045
|
||||
3.697367414744546,0.81725
|
||||
3.7377281536969154,0.83325
|
||||
3.775634523388667,0.8415
|
||||
3.8135408930804187,0.85575
|
||||
3.8522653858590425,0.87325
|
||||
3.8898990478551703,0.88725
|
||||
3.9299870791119154,0.906
|
||||
3.9665299103255465,0.9265
|
||||
4.006072526191043,0.94575
|
||||
4.043978895882795,0.97175
|
||||
4.081885265574547,1.02075
|
||||
4.1206097583531704,1.046
|
||||
4.1587888357405465,1.07025
|
||||
4.196422497736674,1.097
|
||||
4.234874282819675,1.12575
|
||||
4.286688744988257,0.73475
|
||||
4.324322406984384,0.72225
|
||||
4.364410438241129,0.731
|
||||
4.405862007975994,0.7405
|
||||
4.44267754688525,0.749
|
||||
4.484129116620115,0.76025
|
||||
4.522853609398739,0.76825
|
||||
4.560759979090491,0.77125
|
||||
4.5989390564778665,0.77725
|
||||
4.640117918517108,0.782
|
||||
4.679115118991357,0.78425
|
||||
4.717294196378733,0.789
|
||||
4.757109519939853,0.78825
|
||||
4.796652135805349,0.7855
|
||||
4.8337403824102285,0.786
|
||||
4.871646752101981,0.78275
|
||||
4.9109166602718535,0.7785
|
||||
4.950186568441726,0.7635
|
||||
4.990274599698471,0.74725
|
||||
5.028180969390222,0.737
|
||||
5.0677235852557185,0.72575
|
||||
5.109720570381833,0.71525
|
||||
5.149263186247329,0.70625
|
||||
5.1863514328522085,0.69975
|
||||
5.230530079543315,0.693
|
||||
5.269799987713188,0.68925
|
||||
5.307979065100563,0.68425
|
||||
5.347248973270435,0.68275
|
||||
5.386518881440308,0.68075
|
||||
5.426606912697053,0.68175
|
||||
5.465604113171301,0.67825
|
||||
5.502419652080556,0.6835
|
||||
5.543871221815422,0.6885
|
||||
5.580959468420302,0.67925
|
||||
5.624319992024535,0.6555
|
||||
5.660044700151294,0.639
|
||||
5.700950854494911,0.623
|
||||
5.740220762664784,0.6075
|
||||
5.783581286269018,0.59475
|
||||
5.820124117482649,0.58475
|
||||
5.861848394913139,0.57325
|
||||
5.899209349213642,0.565
|
||||
5.938751965079138,0.55525
|
||||
5.9782945809446355,0.55175
|
||||
6.017564489114507,0.546
|
||||
6.059288766544997,0.5405
|
||||
6.097467843932373,0.53825
|
||||
6.139464829058487,0.534
|
||||
6.176825783358991,0.5325
|
||||
6.215822983833238,0.53125
|
||||
6.252911230438118,0.53075
|
||||
|
220
Mapping/lidar_to_grid_map/lidar_to_grid_map.py
Normal file
220
Mapping/lidar_to_grid_map/lidar_to_grid_map.py
Normal file
@@ -0,0 +1,220 @@
|
||||
"""
|
||||
|
||||
LIDAR to 2D grid map example
|
||||
|
||||
author: Erno Horvath, Csaba Hajdu based on Atsushi Sakai's scripts (@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import math
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from collections import deque
|
||||
|
||||
EXTEND_AREA = 1.0
|
||||
|
||||
|
||||
def file_read(f):
|
||||
"""
|
||||
Reading LIDAR laser beams (angles and corresponding distance data)
|
||||
"""
|
||||
measures = [line.split(",") for line in open(f)]
|
||||
angles = []
|
||||
distances = []
|
||||
for measure in measures:
|
||||
angles.append(float(measure[0]))
|
||||
distances.append(float(measure[1]))
|
||||
angles = np.array(angles)
|
||||
distances = np.array(distances)
|
||||
return angles, distances
|
||||
|
||||
|
||||
def bresenham(start, end):
|
||||
"""
|
||||
Implementation of Bresenham's line drawing algorithm
|
||||
See en.wikipedia.org/wiki/Bresenham's_line_algorithm
|
||||
Bresenham's Line Algorithm
|
||||
Produces a np.array from start and end (original from roguebasin.com)
|
||||
>>> points1 = bresenham((4, 4), (6, 10))
|
||||
>>> print(points1)
|
||||
np.array([[4,4], [4,5], [5,6], [5,7], [5,8], [6,9], [6,10]])
|
||||
"""
|
||||
# setup initial conditions
|
||||
x1, y1 = start
|
||||
x2, y2 = end
|
||||
dx = x2 - x1
|
||||
dy = y2 - y1
|
||||
is_steep = abs(dy) > abs(dx) # determine how steep the line is
|
||||
if is_steep: # rotate line
|
||||
x1, y1 = y1, x1
|
||||
x2, y2 = y2, x2
|
||||
swapped = False # swap start and end points if necessary and store swap state
|
||||
if x1 > x2:
|
||||
x1, x2 = x2, x1
|
||||
y1, y2 = y2, y1
|
||||
swapped = True
|
||||
dx = x2 - x1 # recalculate differentials
|
||||
dy = y2 - y1 # recalculate differentials
|
||||
error = int(dx / 2.0) # calculate error
|
||||
ystep = 1 if y1 < y2 else -1
|
||||
# iterate over bounding box generating points between start and end
|
||||
y = y1
|
||||
points = []
|
||||
for x in range(x1, x2 + 1):
|
||||
coord = [y, x] if is_steep else (x, y)
|
||||
points.append(coord)
|
||||
error -= abs(dy)
|
||||
if error < 0:
|
||||
y += ystep
|
||||
error += dx
|
||||
if swapped: # reverse the list if the coordinates were swapped
|
||||
points.reverse()
|
||||
points = np.array(points)
|
||||
return points
|
||||
|
||||
|
||||
def calc_grid_map_config(ox, oy, xyreso):
|
||||
"""
|
||||
Calculates the size, and the maximum distances according to the the measurement center
|
||||
"""
|
||||
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))
|
||||
print("The grid map is ", xw, "x", yw, ".")
|
||||
return minx, miny, maxx, maxy, xw, yw
|
||||
|
||||
|
||||
def atan_zero_to_twopi(y, x):
|
||||
angle = math.atan2(y, x)
|
||||
if angle < 0.0:
|
||||
angle += math.pi * 2.0
|
||||
return angle
|
||||
|
||||
|
||||
def init_floodfill(cpoint, opoints, xypoints, mincoord, xyreso):
|
||||
"""
|
||||
cpoint: center point
|
||||
opoints: detected obstacles points (x,y)
|
||||
xypoints: (x,y) point pairs
|
||||
"""
|
||||
centix, centiy = cpoint
|
||||
prev_ix, prev_iy = centix - 1, centiy
|
||||
ox, oy = opoints
|
||||
xw, yw = xypoints
|
||||
minx, miny = mincoord
|
||||
pmap = (np.ones((xw, yw))) * 0.5
|
||||
for (x, y) in zip(ox, oy):
|
||||
ix = int(round((x - minx) / xyreso)) # x coordinate of the the occupied area
|
||||
iy = int(round((y - miny) / xyreso)) # y coordinate of the the occupied area
|
||||
free_area = bresenham((prev_ix, prev_iy), (ix, iy))
|
||||
for fa in free_area:
|
||||
pmap[fa[0]][fa[1]] = 0 # free area 0.0
|
||||
prev_ix = ix
|
||||
prev_iy = iy
|
||||
return pmap
|
||||
|
||||
|
||||
def flood_fill(cpoint, pmap):
|
||||
"""
|
||||
cpoint: starting point (x,y) of fill
|
||||
pmap: occupancy map generated from Bresenham ray-tracing
|
||||
"""
|
||||
# Fill empty areas with queue method
|
||||
sx, sy = pmap.shape
|
||||
fringe = deque()
|
||||
fringe.appendleft(cpoint)
|
||||
while fringe:
|
||||
n = fringe.pop()
|
||||
nx, ny = n
|
||||
# West
|
||||
if nx > 0:
|
||||
if pmap[nx - 1, ny] == 0.5:
|
||||
pmap[nx - 1, ny] = 0.0
|
||||
fringe.appendleft((nx - 1, ny))
|
||||
# East
|
||||
if nx < sx - 1:
|
||||
if pmap[nx + 1, ny] == 0.5:
|
||||
pmap[nx + 1, ny] = 0.0
|
||||
fringe.appendleft((nx + 1, ny))
|
||||
# North
|
||||
if ny > 0:
|
||||
if pmap[nx, ny - 1] == 0.5:
|
||||
pmap[nx, ny - 1] = 0.0
|
||||
fringe.appendleft((nx, ny - 1))
|
||||
# South
|
||||
if ny < sy - 1:
|
||||
if pmap[nx, ny + 1] == 0.5:
|
||||
pmap[nx, ny + 1] = 0.0
|
||||
fringe.appendleft((nx, ny + 1))
|
||||
|
||||
|
||||
def generate_ray_casting_grid_map(ox, oy, xyreso, breshen=True):
|
||||
"""
|
||||
The breshen boolean tells if it's computed with bresenham ray casting (True) or with flood fill (False)
|
||||
"""
|
||||
minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
|
||||
pmap = np.ones((xw, yw))/2 # default 0.5 -- [[0.5 for i in range(yw)] for i in range(xw)]
|
||||
centix = int(round(-minx / xyreso)) # center x coordinate of the grid map
|
||||
centiy = int(round(-miny / xyreso)) # center y coordinate of the grid map
|
||||
# occupancy grid computed with bresenham ray casting
|
||||
if breshen:
|
||||
for (x, y) in zip(ox, oy):
|
||||
ix = int(round((x - minx) / xyreso)) # x coordinate of the the occupied area
|
||||
iy = int(round((y - miny) / xyreso)) # y coordinate of the the occupied area
|
||||
laser_beams = bresenham((centix, centiy), (ix, iy)) # line form the lidar to the cooupied point
|
||||
for laser_beam in laser_beams:
|
||||
pmap[laser_beam[0]][laser_beam[1]] = 0.0 # free area 0.0
|
||||
pmap[ix][iy] = 1.0 # occupied area 1.0
|
||||
pmap[ix+1][iy] = 1.0 # extend the occupied area
|
||||
pmap[ix][iy+1] = 1.0 # extend the occupied area
|
||||
pmap[ix+1][iy+1] = 1.0 # extend the occupied area
|
||||
# occupancy grid computed with with flood fill
|
||||
else:
|
||||
pmap = init_floodfill((centix, centiy), (ox, oy), (xw, yw), (minx, miny), xyreso)
|
||||
flood_fill((centix, centiy), pmap)
|
||||
pmap = np.array(pmap, dtype=np.float)
|
||||
for (x, y) in zip(ox, oy):
|
||||
ix = int(round((x - minx) / xyreso))
|
||||
iy = int(round((y - miny) / xyreso))
|
||||
pmap[ix][iy] = 1.0 # occupied area 1.0
|
||||
pmap[ix+1][iy] = 1.0 # extend the occupied area
|
||||
pmap[ix][iy+1] = 1.0 # extend the occupied area
|
||||
pmap[ix+1][iy+1] = 1.0 # extend the occupied area
|
||||
return pmap, minx, maxx, miny, maxy, xyreso
|
||||
|
||||
|
||||
def main():
|
||||
"""
|
||||
Example usage
|
||||
"""
|
||||
print(__file__, "start")
|
||||
xyreso = 0.02 # x-y grid resolution
|
||||
ang, dist = file_read("lidar01.csv")
|
||||
ox = np.sin(ang) * dist
|
||||
oy = np.cos(ang) * dist
|
||||
pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(ox, oy, xyreso, True)
|
||||
xyres = np.array(pmap).shape
|
||||
plt.figure(1, figsize=(10,4))
|
||||
plt.subplot(122)
|
||||
plt.imshow(pmap, cmap="PiYG_r") # cmap = "binary" "PiYG_r" "PiYG_r" "bone" "bone_r" "RdYlGn_r"
|
||||
plt.clim(-0.4, 1.4)
|
||||
plt.gca().set_xticks(np.arange(-.5, xyres[1], 1), minor=True)
|
||||
plt.gca().set_yticks(np.arange(-.5, xyres[0], 1), minor=True)
|
||||
plt.grid(True, which="minor", color="w", linewidth=0.6, alpha=0.5)
|
||||
plt.colorbar()
|
||||
plt.subplot(121)
|
||||
plt.plot([oy, np.zeros(np.size(oy))], [ox, np.zeros(np.size(oy))], "ro-")
|
||||
plt.axis("equal")
|
||||
plt.plot(0.0, 0.0, "ob")
|
||||
plt.gca().set_aspect("equal", "box")
|
||||
bottom, top = plt.ylim() # return the current ylim
|
||||
plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation
|
||||
plt.grid(True)
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
318
Mapping/lidar_to_grid_map/lidar_to_grid_map_tutorial.ipynb
Normal file
318
Mapping/lidar_to_grid_map/lidar_to_grid_map_tutorial.ipynb
Normal file
File diff suppressed because one or more lines are too long
@@ -14,172 +14,191 @@ import math
|
||||
|
||||
show_animation = True
|
||||
|
||||
class AStarPlanner:
|
||||
|
||||
class Node:
|
||||
def __init__(self, ox, oy, reso, rr):
|
||||
"""
|
||||
Intialize map for a star planning
|
||||
|
||||
def __init__(self, x, y, cost, pind):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.cost = cost
|
||||
self.pind = pind
|
||||
ox: x position list of Obstacles [m]
|
||||
oy: y position list of Obstacles [m]
|
||||
reso: grid resolution [m]
|
||||
rr: robot radius[m]
|
||||
"""
|
||||
|
||||
def __str__(self):
|
||||
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
|
||||
self.reso = reso
|
||||
self.rr = rr
|
||||
self.calc_obstacle_map(ox, oy)
|
||||
self.motion = self.get_motion_model()
|
||||
|
||||
class Node:
|
||||
def __init__(self, x, y, cost, pind):
|
||||
self.x = x # index of grid
|
||||
self.y = y # index of grid
|
||||
self.cost = cost
|
||||
self.pind = pind
|
||||
|
||||
def calc_final_path(ngoal, closedset, reso):
|
||||
# generate final course
|
||||
rx, ry = [ngoal.x * reso], [ngoal.y * reso]
|
||||
pind = ngoal.pind
|
||||
while pind != -1:
|
||||
n = closedset[pind]
|
||||
rx.append(n.x * reso)
|
||||
ry.append(n.y * reso)
|
||||
pind = n.pind
|
||||
def __str__(self):
|
||||
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
|
||||
|
||||
return rx, ry
|
||||
def planning(self, sx, sy, gx, gy):
|
||||
"""
|
||||
A star path search
|
||||
|
||||
input:
|
||||
sx: start x position [m]
|
||||
sy: start y position [m]
|
||||
gx: goal x position [m]
|
||||
gx: goal x position [m]
|
||||
|
||||
def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
|
||||
"""
|
||||
gx: goal x position [m]
|
||||
gx: goal x position [m]
|
||||
ox: x position list of Obstacles [m]
|
||||
oy: y position list of Obstacles [m]
|
||||
reso: grid resolution [m]
|
||||
rr: robot radius[m]
|
||||
"""
|
||||
output:
|
||||
rx: x position list of the final path
|
||||
ry: y position list of the final path
|
||||
"""
|
||||
|
||||
nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)
|
||||
ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
|
||||
ox = [iox / reso for iox in ox]
|
||||
oy = [ioy / reso for ioy in oy]
|
||||
nstart = self.Node(self.calc_xyindex(sx, self.minx),
|
||||
self.calc_xyindex(sy, self.miny), 0.0, -1)
|
||||
ngoal = self.Node(self.calc_xyindex(gx, self.minx),
|
||||
self.calc_xyindex(gy, self.miny), 0.0, -1)
|
||||
|
||||
obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr)
|
||||
openset, closedset = dict(), dict()
|
||||
openset[self.calc_index(nstart)] = nstart
|
||||
|
||||
motion = get_motion_model()
|
||||
while 1:
|
||||
c_id = min(
|
||||
openset, key=lambda o: openset[o].cost + self.calc_heuristic(ngoal, openset[o]))
|
||||
current = openset[c_id]
|
||||
|
||||
openset, closedset = dict(), dict()
|
||||
openset[calc_index(nstart, xw, minx, miny)] = nstart
|
||||
# show graph
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(self.calc_position(current.x, self.minx),
|
||||
self.calc_position(current.y, self.miny), "xc")
|
||||
if len(closedset.keys()) % 10 == 0:
|
||||
plt.pause(0.001)
|
||||
|
||||
while 1:
|
||||
c_id = min(
|
||||
openset, key=lambda o: openset[o].cost + calc_heuristic(ngoal, openset[o]))
|
||||
current = openset[c_id]
|
||||
if current.x == ngoal.x and current.y == ngoal.y:
|
||||
print("Find goal")
|
||||
ngoal.pind = current.pind
|
||||
ngoal.cost = current.cost
|
||||
break
|
||||
|
||||
# show graph
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(current.x * reso, current.y * reso, "xc")
|
||||
if len(closedset.keys()) % 10 == 0:
|
||||
plt.pause(0.001)
|
||||
# Remove the item from the open set
|
||||
del openset[c_id]
|
||||
|
||||
if current.x == ngoal.x and current.y == ngoal.y:
|
||||
print("Find goal")
|
||||
ngoal.pind = current.pind
|
||||
ngoal.cost = current.cost
|
||||
break
|
||||
# Add it to the closed set
|
||||
closedset[c_id] = current
|
||||
|
||||
# Remove the item from the open set
|
||||
del openset[c_id]
|
||||
# Add it to the closed set
|
||||
closedset[c_id] = current
|
||||
# expand search grid based on motion model
|
||||
for i, _ in enumerate(self.motion):
|
||||
node = self.Node(current.x + self.motion[i][0],
|
||||
current.y + self.motion[i][1],
|
||||
current.cost + self.motion[i][2], c_id)
|
||||
n_id = self.calc_index(node)
|
||||
|
||||
# expand search grid based on motion model
|
||||
for i, _ in enumerate(motion):
|
||||
node = Node(current.x + motion[i][0],
|
||||
current.y + motion[i][1],
|
||||
current.cost + motion[i][2], c_id)
|
||||
n_id = calc_index(node, xw, minx, miny)
|
||||
if n_id in closedset:
|
||||
continue
|
||||
|
||||
if n_id in closedset:
|
||||
continue
|
||||
if not self.verify_node(node):
|
||||
continue
|
||||
|
||||
if not verify_node(node, obmap, minx, miny, maxx, maxy):
|
||||
continue
|
||||
if n_id not in openset:
|
||||
openset[n_id] = node # Discover a new node
|
||||
else:
|
||||
if openset[n_id].cost >= node.cost:
|
||||
# This path is the best until now. record it!
|
||||
openset[n_id] = node
|
||||
|
||||
if n_id not in openset:
|
||||
openset[n_id] = node # Discover a new node
|
||||
else:
|
||||
if openset[n_id].cost >= node.cost:
|
||||
# This path is the best until now. record it!
|
||||
openset[n_id] = node
|
||||
rx, ry = self.calc_final_path(ngoal, closedset)
|
||||
|
||||
rx, ry = calc_final_path(ngoal, closedset, reso)
|
||||
return rx, ry
|
||||
|
||||
return rx, ry
|
||||
def calc_final_path(self, ngoal, closedset):
|
||||
# generate final course
|
||||
rx, ry = [self.calc_position(ngoal.x, self.minx)], [
|
||||
self.calc_position(ngoal.y, self.miny)]
|
||||
pind = ngoal.pind
|
||||
while pind != -1:
|
||||
n = closedset[pind]
|
||||
rx.append(self.calc_position(n.x, self.minx))
|
||||
ry.append(self.calc_position(n.y, self.miny))
|
||||
pind = n.pind
|
||||
|
||||
return rx, ry
|
||||
|
||||
def calc_heuristic(n1, n2):
|
||||
w = 1.0 # weight of heuristic
|
||||
d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
|
||||
return d
|
||||
def calc_heuristic(self, n1, n2):
|
||||
w = 1.0 # weight of heuristic
|
||||
d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
|
||||
return d
|
||||
|
||||
def calc_position(self, index, minp):
|
||||
pos = index*self.reso+minp
|
||||
return pos
|
||||
|
||||
def verify_node(node, obmap, minx, miny, maxx, maxy):
|
||||
def calc_xyindex(self, position, minp):
|
||||
return round((position - minp)/self.reso)
|
||||
|
||||
if node.x < minx:
|
||||
return False
|
||||
elif node.y < miny:
|
||||
return False
|
||||
elif node.x >= maxx:
|
||||
return False
|
||||
elif node.y >= maxy:
|
||||
return False
|
||||
def calc_index(self, node):
|
||||
return (node.y - self.miny) * self.xwidth + (node.x - self.minx)
|
||||
|
||||
if obmap[node.x][node.y]:
|
||||
return False
|
||||
def verify_node(self, node):
|
||||
px = self.calc_position(node.x, self.minx)
|
||||
py = self.calc_position(node.y, self.miny)
|
||||
|
||||
return True
|
||||
if px < self.minx:
|
||||
return False
|
||||
elif py < self.miny:
|
||||
return False
|
||||
elif px >= self.maxx:
|
||||
return False
|
||||
elif py >= self.maxy:
|
||||
return False
|
||||
|
||||
if self.obmap[node.x][node.y]:
|
||||
return False
|
||||
|
||||
def calc_obstacle_map(ox, oy, reso, vr):
|
||||
return True
|
||||
|
||||
minx = round(min(ox))
|
||||
miny = round(min(oy))
|
||||
maxx = round(max(ox))
|
||||
maxy = round(max(oy))
|
||||
# print("minx:", minx)
|
||||
# print("miny:", miny)
|
||||
# print("maxx:", maxx)
|
||||
# print("maxy:", maxy)
|
||||
def calc_obstacle_map(self, ox, oy):
|
||||
|
||||
xwidth = round(maxx - minx)
|
||||
ywidth = round(maxy - miny)
|
||||
# print("xwidth:", xwidth)
|
||||
# print("ywidth:", ywidth)
|
||||
self.minx = round(min(ox))
|
||||
self.miny = round(min(oy))
|
||||
self.maxx = round(max(ox))
|
||||
self.maxy = round(max(oy))
|
||||
print("minx:", self.minx)
|
||||
print("miny:", self.miny)
|
||||
print("maxx:", self.maxx)
|
||||
print("maxy:", self.maxy)
|
||||
|
||||
# obstacle map generation
|
||||
obmap = [[False for i in range(ywidth)] for i in range(xwidth)]
|
||||
for ix in range(xwidth):
|
||||
x = ix + minx
|
||||
for iy in range(ywidth):
|
||||
y = iy + miny
|
||||
# print(x, y)
|
||||
for iox, ioy in zip(ox, oy):
|
||||
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
|
||||
if d <= vr / reso:
|
||||
obmap[ix][iy] = True
|
||||
break
|
||||
self.xwidth = round((self.maxx - self.minx)/self.reso)
|
||||
self.ywidth = round((self.maxy - self.miny)/self.reso)
|
||||
print("xwidth:", self.xwidth)
|
||||
print("ywidth:", self.ywidth)
|
||||
|
||||
return obmap, minx, miny, maxx, maxy, xwidth, ywidth
|
||||
# obstacle map generation
|
||||
self.obmap = [[False for i in range(self.ywidth)]
|
||||
for i in range(self.xwidth)]
|
||||
for ix in range(self.xwidth):
|
||||
x = self.calc_position(ix, self.minx)
|
||||
for iy in range(self.ywidth):
|
||||
y = self.calc_position(iy, self.miny)
|
||||
for iox, ioy in zip(ox, oy):
|
||||
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
|
||||
if d <= self.rr:
|
||||
self.obmap[ix][iy] = True
|
||||
break
|
||||
|
||||
def get_motion_model(self):
|
||||
# dx, dy, cost
|
||||
motion = [[1, 0, 1],
|
||||
[0, 1, 1],
|
||||
[-1, 0, 1],
|
||||
[0, -1, 1],
|
||||
[-1, -1, math.sqrt(2)],
|
||||
[-1, 1, math.sqrt(2)],
|
||||
[1, -1, math.sqrt(2)],
|
||||
[1, 1, math.sqrt(2)]]
|
||||
|
||||
def calc_index(node, xwidth, xmin, ymin):
|
||||
return (node.y - ymin) * xwidth + (node.x - xmin)
|
||||
|
||||
|
||||
def get_motion_model():
|
||||
# dx, dy, cost
|
||||
motion = [[1, 0, 1],
|
||||
[0, 1, 1],
|
||||
[-1, 0, 1],
|
||||
[0, -1, 1],
|
||||
[-1, -1, math.sqrt(2)],
|
||||
[-1, 1, math.sqrt(2)],
|
||||
[1, -1, math.sqrt(2)],
|
||||
[1, 1, math.sqrt(2)]]
|
||||
|
||||
return motion
|
||||
return motion
|
||||
|
||||
|
||||
def main():
|
||||
@@ -190,38 +209,39 @@ def main():
|
||||
sy = 10.0 # [m]
|
||||
gx = 50.0 # [m]
|
||||
gy = 50.0 # [m]
|
||||
grid_size = 1.0 # [m]
|
||||
robot_size = 1.0 # [m]
|
||||
grid_size = 2.0 # [m]
|
||||
robot_radius = 1.0 # [m]
|
||||
|
||||
# set obstable positions
|
||||
ox, oy = [], []
|
||||
|
||||
for i in range(60):
|
||||
for i in range(-10, 60):
|
||||
ox.append(i)
|
||||
oy.append(0.0)
|
||||
for i in range(60):
|
||||
oy.append(-10.0)
|
||||
for i in range(-10, 60):
|
||||
ox.append(60.0)
|
||||
oy.append(i)
|
||||
for i in range(61):
|
||||
for i in range(-10, 61):
|
||||
ox.append(i)
|
||||
oy.append(60.0)
|
||||
for i in range(61):
|
||||
ox.append(0.0)
|
||||
for i in range(-10, 61):
|
||||
ox.append(-10.0)
|
||||
oy.append(i)
|
||||
for i in range(40):
|
||||
for i in range(-10, 40):
|
||||
ox.append(20.0)
|
||||
oy.append(i)
|
||||
for i in range(40):
|
||||
for i in range(0, 40):
|
||||
ox.append(40.0)
|
||||
oy.append(60.0 - i)
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(ox, oy, ".k")
|
||||
plt.plot(sx, sy, "xr")
|
||||
plt.plot(sx, sy, "og")
|
||||
plt.plot(gx, gy, "xb")
|
||||
plt.grid(True)
|
||||
plt.axis("equal")
|
||||
|
||||
rx, ry = a_star_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
|
||||
a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
|
||||
rx, ry = a_star.planning(sx, sy, gx, gy)
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(rx, ry, "-r")
|
||||
|
||||
@@ -1,7 +1,9 @@
|
||||
"""
|
||||
Dijkstra grid based planning
|
||||
|
||||
Grid based Dijkstra planning
|
||||
|
||||
author: Atsushi Sakai(@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
@@ -9,210 +11,235 @@ import math
|
||||
|
||||
show_animation = True
|
||||
|
||||
class Dijkstra:
|
||||
|
||||
class Node:
|
||||
def __init__(self, ox, oy, reso, rr):
|
||||
"""
|
||||
Initialize map for a star planning
|
||||
|
||||
def __init__(self, x, y, cost, pind):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.cost = cost
|
||||
self.pind = pind
|
||||
ox: x position list of Obstacles [m]
|
||||
oy: y position list of Obstacles [m]
|
||||
reso: grid resolution [m]
|
||||
rr: robot radius[m]
|
||||
"""
|
||||
|
||||
def __str__(self):
|
||||
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
|
||||
self.reso = reso
|
||||
self.rr = rr
|
||||
self.calc_obstacle_map(ox, oy)
|
||||
self.motion = self.get_motion_model()
|
||||
|
||||
class Node:
|
||||
def __init__(self, x, y, cost, pind):
|
||||
self.x = x # index of grid
|
||||
self.y = y # index of grid
|
||||
self.cost = cost
|
||||
self.pind = pind
|
||||
|
||||
def dijkstra_planning(sx, sy, gx, gy, ox, oy, reso, rr):
|
||||
"""
|
||||
gx: goal x position [m]
|
||||
gx: goal x position [m]
|
||||
ox: x position list of Obstacles [m]
|
||||
oy: y position list of Obstacles [m]
|
||||
reso: grid resolution [m]
|
||||
rr: robot radius[m]
|
||||
"""
|
||||
def __str__(self):
|
||||
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
|
||||
|
||||
nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)
|
||||
ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
|
||||
ox = [iox / reso for iox in ox]
|
||||
oy = [ioy / reso for ioy in oy]
|
||||
def planning(self, sx, sy, gx, gy):
|
||||
"""
|
||||
dijkstra path search
|
||||
|
||||
obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr)
|
||||
input:
|
||||
sx: start x position [m]
|
||||
sy: start y position [m]
|
||||
gx: goal x position [m]
|
||||
gx: goal x position [m]
|
||||
|
||||
motion = get_motion_model()
|
||||
output:
|
||||
rx: x position list of the final path
|
||||
ry: y position list of the final path
|
||||
"""
|
||||
|
||||
openset, closedset = dict(), dict()
|
||||
openset[calc_index(nstart, xw, minx, miny)] = nstart
|
||||
nstart = self.Node(self.calc_xyindex(sx, self.minx),
|
||||
self.calc_xyindex(sy, self.miny), 0.0, -1)
|
||||
ngoal = self.Node(self.calc_xyindex(gx, self.minx),
|
||||
self.calc_xyindex(gy, self.miny), 0.0, -1)
|
||||
|
||||
while 1:
|
||||
c_id = min(openset, key=lambda o: openset[o].cost)
|
||||
current = openset[c_id]
|
||||
# print("current", current)
|
||||
openset, closedset = dict(), dict()
|
||||
openset[self.calc_index(nstart)] = nstart
|
||||
|
||||
# show graph
|
||||
if show_animation:
|
||||
plt.plot(current.x * reso, current.y * reso, "xc")
|
||||
if len(closedset.keys()) % 10 == 0:
|
||||
plt.pause(0.001)
|
||||
while 1:
|
||||
c_id = min(openset, key=lambda o: openset[o].cost)
|
||||
current = openset[c_id]
|
||||
|
||||
if current.x == ngoal.x and current.y == ngoal.y:
|
||||
print("Find goal")
|
||||
ngoal.pind = current.pind
|
||||
ngoal.cost = current.cost
|
||||
break
|
||||
# show graph
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(self.calc_position(current.x, self.minx),
|
||||
self.calc_position(current.y, self.miny), "xc")
|
||||
if len(closedset.keys()) % 10 == 0:
|
||||
plt.pause(0.001)
|
||||
|
||||
# Remove the item from the open set
|
||||
del openset[c_id]
|
||||
# Add it to the closed set
|
||||
closedset[c_id] = current
|
||||
if current.x == ngoal.x and current.y == ngoal.y:
|
||||
print("Find goal")
|
||||
ngoal.pind = current.pind
|
||||
ngoal.cost = current.cost
|
||||
break
|
||||
|
||||
# expand search grid based on motion model
|
||||
for i, _ in enumerate(motion):
|
||||
node = Node(current.x + motion[i][0], current.y + motion[i][1],
|
||||
current.cost + motion[i][2], c_id)
|
||||
n_id = calc_index(node, xw, minx, miny)
|
||||
# Remove the item from the open set
|
||||
del openset[c_id]
|
||||
|
||||
if not verify_node(node, obmap, minx, miny, maxx, maxy):
|
||||
continue
|
||||
# Add it to the closed set
|
||||
closedset[c_id] = current
|
||||
|
||||
if n_id in closedset:
|
||||
continue
|
||||
# Otherwise if it is already in the open set
|
||||
if n_id in openset:
|
||||
if openset[n_id].cost > node.cost:
|
||||
openset[n_id].cost = node.cost
|
||||
openset[n_id].pind = c_id
|
||||
else:
|
||||
openset[n_id] = node
|
||||
# expand search grid based on motion model
|
||||
for i, _ in enumerate(self.motion):
|
||||
node = self.Node(current.x + self.motion[i][0],
|
||||
current.y + self.motion[i][1],
|
||||
current.cost + self.motion[i][2], c_id)
|
||||
n_id = self.calc_index(node)
|
||||
|
||||
rx, ry = calc_final_path(ngoal, closedset, reso)
|
||||
if n_id in closedset:
|
||||
continue
|
||||
|
||||
return rx, ry
|
||||
if not self.verify_node(node):
|
||||
continue
|
||||
|
||||
if n_id not in openset:
|
||||
openset[n_id] = node # Discover a new node
|
||||
else:
|
||||
if openset[n_id].cost >= node.cost:
|
||||
# This path is the best until now. record it!
|
||||
openset[n_id] = node
|
||||
|
||||
def calc_final_path(ngoal, closedset, reso):
|
||||
# generate final course
|
||||
rx, ry = [ngoal.x * reso], [ngoal.y * reso]
|
||||
pind = ngoal.pind
|
||||
while pind != -1:
|
||||
n = closedset[pind]
|
||||
rx.append(n.x * reso)
|
||||
ry.append(n.y * reso)
|
||||
pind = n.pind
|
||||
rx, ry = self.calc_final_path(ngoal, closedset)
|
||||
|
||||
return rx, ry
|
||||
return rx, ry
|
||||
|
||||
def calc_final_path(self, ngoal, closedset):
|
||||
# generate final course
|
||||
rx, ry = [self.calc_position(ngoal.x, self.minx)], [
|
||||
self.calc_position(ngoal.y, self.miny)]
|
||||
pind = ngoal.pind
|
||||
while pind != -1:
|
||||
n = closedset[pind]
|
||||
rx.append(self.calc_position(n.x, self.minx))
|
||||
ry.append(self.calc_position(n.y, self.miny))
|
||||
pind = n.pind
|
||||
|
||||
def verify_node(node, obmap, minx, miny, maxx, maxy):
|
||||
return rx, ry
|
||||
|
||||
if obmap[node.x][node.y]:
|
||||
return False
|
||||
def calc_heuristic(self, n1, n2):
|
||||
w = 1.0 # weight of heuristic
|
||||
d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
|
||||
return d
|
||||
|
||||
if node.x < minx:
|
||||
return False
|
||||
elif node.y < miny:
|
||||
return False
|
||||
elif node.x > maxx:
|
||||
return False
|
||||
elif node.y > maxy:
|
||||
return False
|
||||
def calc_position(self, index, minp):
|
||||
pos = index*self.reso+minp
|
||||
return pos
|
||||
|
||||
return True
|
||||
def calc_xyindex(self, position, minp):
|
||||
return round((position - minp)/self.reso)
|
||||
|
||||
def calc_index(self, node):
|
||||
return (node.y - self.miny) * self.xwidth + (node.x - self.minx)
|
||||
|
||||
def calc_obstacle_map(ox, oy, reso, vr):
|
||||
def verify_node(self, node):
|
||||
px = self.calc_position(node.x, self.minx)
|
||||
py = self.calc_position(node.y, self.miny)
|
||||
|
||||
minx = round(min(ox))
|
||||
miny = round(min(oy))
|
||||
maxx = round(max(ox))
|
||||
maxy = round(max(oy))
|
||||
# print("minx:", minx)
|
||||
# print("miny:", miny)
|
||||
# print("maxx:", maxx)
|
||||
# print("maxy:", maxy)
|
||||
if px < self.minx:
|
||||
return False
|
||||
elif py < self.miny:
|
||||
return False
|
||||
elif px >= self.maxx:
|
||||
return False
|
||||
elif py >= self.maxy:
|
||||
return False
|
||||
|
||||
xwidth = round(maxx - minx)
|
||||
ywidth = round(maxy - miny)
|
||||
# print("xwidth:", xwidth)
|
||||
# print("ywidth:", ywidth)
|
||||
if self.obmap[node.x][node.y]:
|
||||
return False
|
||||
|
||||
# obstacle map generation
|
||||
obmap = [[False for i in range(ywidth)] for i in range(xwidth)]
|
||||
for ix in range(xwidth):
|
||||
x = ix + minx
|
||||
for iy in range(ywidth):
|
||||
y = iy + miny
|
||||
# print(x, y)
|
||||
for iox, ioy in zip(ox, oy):
|
||||
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
|
||||
if d <= vr / reso:
|
||||
obmap[ix][iy] = True
|
||||
break
|
||||
return True
|
||||
|
||||
return obmap, minx, miny, maxx, maxy, xwidth, ywidth
|
||||
def calc_obstacle_map(self, ox, oy):
|
||||
|
||||
self.minx = round(min(ox))
|
||||
self.miny = round(min(oy))
|
||||
self.maxx = round(max(ox))
|
||||
self.maxy = round(max(oy))
|
||||
print("minx:", self.minx)
|
||||
print("miny:", self.miny)
|
||||
print("maxx:", self.maxx)
|
||||
print("maxy:", self.maxy)
|
||||
|
||||
def calc_index(node, xwidth, xmin, ymin):
|
||||
return (node.y - ymin) * xwidth + (node.x - xmin)
|
||||
self.xwidth = round((self.maxx - self.minx)/self.reso)
|
||||
self.ywidth = round((self.maxy - self.miny)/self.reso)
|
||||
print("xwidth:", self.xwidth)
|
||||
print("ywidth:", self.ywidth)
|
||||
|
||||
# obstacle map generation
|
||||
self.obmap = [[False for i in range(self.ywidth)]
|
||||
for i in range(self.xwidth)]
|
||||
for ix in range(self.xwidth):
|
||||
x = self.calc_position(ix, self.minx)
|
||||
for iy in range(self.ywidth):
|
||||
y = self.calc_position(iy, self.miny)
|
||||
for iox, ioy in zip(ox, oy):
|
||||
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
|
||||
if d <= self.rr:
|
||||
self.obmap[ix][iy] = True
|
||||
break
|
||||
|
||||
def get_motion_model():
|
||||
# dx, dy, cost
|
||||
motion = [[1, 0, 1],
|
||||
[0, 1, 1],
|
||||
[-1, 0, 1],
|
||||
[0, -1, 1],
|
||||
[-1, -1, math.sqrt(2)],
|
||||
[-1, 1, math.sqrt(2)],
|
||||
[1, -1, math.sqrt(2)],
|
||||
[1, 1, math.sqrt(2)]]
|
||||
def get_motion_model(self):
|
||||
# dx, dy, cost
|
||||
motion = [[1, 0, 1],
|
||||
[0, 1, 1],
|
||||
[-1, 0, 1],
|
||||
[0, -1, 1],
|
||||
[-1, -1, math.sqrt(2)],
|
||||
[-1, 1, math.sqrt(2)],
|
||||
[1, -1, math.sqrt(2)],
|
||||
[1, 1, math.sqrt(2)]]
|
||||
|
||||
return motion
|
||||
return motion
|
||||
|
||||
|
||||
def main():
|
||||
print(__file__ + " start!!")
|
||||
|
||||
# start and goal position
|
||||
sx = 10.0 # [m]
|
||||
sy = 10.0 # [m]
|
||||
sx = -5.0 # [m]
|
||||
sy = -5.0 # [m]
|
||||
gx = 50.0 # [m]
|
||||
gy = 50.0 # [m]
|
||||
grid_size = 1.0 # [m]
|
||||
robot_size = 1.0 # [m]
|
||||
grid_size = 2.0 # [m]
|
||||
robot_radius = 1.0 # [m]
|
||||
|
||||
ox = []
|
||||
oy = []
|
||||
|
||||
for i in range(60):
|
||||
# set obstacle positions
|
||||
ox, oy = [], []
|
||||
for i in range(-10, 60):
|
||||
ox.append(i)
|
||||
oy.append(0.0)
|
||||
for i in range(60):
|
||||
oy.append(-10.0)
|
||||
for i in range(-10, 60):
|
||||
ox.append(60.0)
|
||||
oy.append(i)
|
||||
for i in range(61):
|
||||
for i in range(-10, 61):
|
||||
ox.append(i)
|
||||
oy.append(60.0)
|
||||
for i in range(61):
|
||||
ox.append(0.0)
|
||||
for i in range(-10, 61):
|
||||
ox.append(-10.0)
|
||||
oy.append(i)
|
||||
for i in range(40):
|
||||
for i in range(-10, 40):
|
||||
ox.append(20.0)
|
||||
oy.append(i)
|
||||
for i in range(40):
|
||||
for i in range(0, 40):
|
||||
ox.append(40.0)
|
||||
oy.append(60.0 - i)
|
||||
|
||||
if show_animation:
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(ox, oy, ".k")
|
||||
plt.plot(sx, sy, "xr")
|
||||
plt.plot(sx, sy, "og")
|
||||
plt.plot(gx, gy, "xb")
|
||||
plt.grid(True)
|
||||
plt.axis("equal")
|
||||
|
||||
rx, ry = dijkstra_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
|
||||
dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)
|
||||
rx, ry = dijkstra.planning(sx, sy, gx, gy)
|
||||
|
||||
if show_animation:
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(rx, ry, "-r")
|
||||
plt.show()
|
||||
|
||||
|
||||
@@ -10,15 +10,26 @@ import math
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
import sys
|
||||
sys.path.append("../../")
|
||||
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
def dwa_control(x, u, config, goal, ob):
|
||||
"""
|
||||
Dynamic Window Approach control
|
||||
"""
|
||||
|
||||
dw = calc_dynamic_window(x, config)
|
||||
|
||||
u, traj = calc_final_input(x, u, dw, config, goal, ob)
|
||||
|
||||
return u, traj
|
||||
|
||||
|
||||
class Config():
|
||||
# simulation parameters
|
||||
"""
|
||||
simulation parameter class
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
# robot parameter
|
||||
@@ -29,15 +40,19 @@ class Config():
|
||||
self.max_dyawrate = 40.0 * math.pi / 180.0 # [rad/ss]
|
||||
self.v_reso = 0.01 # [m/s]
|
||||
self.yawrate_reso = 0.1 * math.pi / 180.0 # [rad/s]
|
||||
self.dt = 0.1 # [s]
|
||||
self.dt = 0.1 # [s] Time tick for motion prediction
|
||||
self.predict_time = 3.0 # [s]
|
||||
self.to_goal_cost_gain = 1.0
|
||||
self.to_goal_cost_gain = 0.15
|
||||
self.speed_cost_gain = 1.0
|
||||
self.robot_radius = 1.0 # [m]
|
||||
self.obstacle_cost_gain = 1.0
|
||||
self.robot_radius = 1.0 # [m] for collision check
|
||||
|
||||
|
||||
|
||||
def motion(x, u, dt):
|
||||
# motion model
|
||||
"""
|
||||
motion model
|
||||
"""
|
||||
|
||||
x[2] += u[1] * dt
|
||||
x[0] += u[0] * math.cos(x[2]) * dt
|
||||
@@ -49,6 +64,9 @@ def motion(x, u, dt):
|
||||
|
||||
|
||||
def calc_dynamic_window(x, config):
|
||||
"""
|
||||
calculation dynamic window based on current state x
|
||||
"""
|
||||
|
||||
# Dynamic window from robot specification
|
||||
Vs = [config.min_speed, config.max_speed,
|
||||
@@ -67,9 +85,12 @@ def calc_dynamic_window(x, config):
|
||||
return dw
|
||||
|
||||
|
||||
def calc_trajectory(xinit, v, y, config):
|
||||
def predict_trajectory(x_init, v, y, config):
|
||||
"""
|
||||
predict trajectory with an input
|
||||
"""
|
||||
|
||||
x = np.array(xinit)
|
||||
x = np.array(x_init)
|
||||
traj = np.array(x)
|
||||
time = 0
|
||||
while time <= config.predict_time:
|
||||
@@ -81,42 +102,44 @@ def calc_trajectory(xinit, v, y, config):
|
||||
|
||||
|
||||
def calc_final_input(x, u, dw, config, goal, ob):
|
||||
"""
|
||||
calculation final input with dinamic window
|
||||
"""
|
||||
|
||||
xinit = x[:]
|
||||
min_cost = 10000.0
|
||||
min_u = u
|
||||
min_u[0] = 0.0
|
||||
x_init = x[:]
|
||||
min_cost = float("inf")
|
||||
best_u = [0.0, 0.0]
|
||||
best_traj = np.array([x])
|
||||
|
||||
# evalucate all trajectory with sampled input in dynamic window
|
||||
for v in np.arange(dw[0], dw[1], config.v_reso):
|
||||
for y in np.arange(dw[2], dw[3], config.yawrate_reso):
|
||||
traj = calc_trajectory(xinit, v, y, config)
|
||||
|
||||
traj = predict_trajectory(x_init, v, y, config)
|
||||
|
||||
# calc cost
|
||||
to_goal_cost = calc_to_goal_cost(traj, goal, config)
|
||||
to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(traj, goal, config)
|
||||
speed_cost = config.speed_cost_gain * \
|
||||
(config.max_speed - traj[-1, 3])
|
||||
ob_cost = calc_obstacle_cost(traj, ob, config)
|
||||
# print(ob_cost)
|
||||
ob_cost = config.obstacle_cost_gain*calc_obstacle_cost(traj, ob, config)
|
||||
|
||||
final_cost = to_goal_cost + speed_cost + ob_cost
|
||||
|
||||
#print (final_cost)
|
||||
|
||||
# search minimum trajectory
|
||||
if min_cost >= final_cost:
|
||||
min_cost = final_cost
|
||||
min_u = [v, y]
|
||||
best_u = [v, y]
|
||||
best_traj = traj
|
||||
|
||||
return min_u, best_traj
|
||||
return best_u, best_traj
|
||||
|
||||
|
||||
def calc_obstacle_cost(traj, ob, config):
|
||||
# calc obstacle cost inf: collistion, 0:free
|
||||
"""
|
||||
calc obstacle cost inf: collision
|
||||
"""
|
||||
|
||||
skip_n = 2
|
||||
skip_n = 2 # for speed up
|
||||
minr = float("inf")
|
||||
|
||||
for ii in range(0, len(traj[:, 1]), skip_n):
|
||||
@@ -137,28 +160,18 @@ def calc_obstacle_cost(traj, ob, config):
|
||||
|
||||
|
||||
def calc_to_goal_cost(traj, goal, config):
|
||||
# calc to goal cost. It is 2D norm.
|
||||
"""
|
||||
calc to goal cost with angle difference
|
||||
"""
|
||||
|
||||
goal_magnitude = math.sqrt(goal[0]**2 + goal[1]**2)
|
||||
traj_magnitude = math.sqrt(traj[-1, 0]**2 + traj[-1, 1]**2)
|
||||
dot_product = (goal[0] * traj[-1, 0]) + (goal[1] * traj[-1, 1])
|
||||
error = dot_product / (goal_magnitude * traj_magnitude)
|
||||
error_angle = math.acos(error)
|
||||
cost = config.to_goal_cost_gain * error_angle
|
||||
dx = goal[0] - traj[-1, 0]
|
||||
dy = goal[1] - traj[-1, 1]
|
||||
error_angle = math.atan2(dy, dx)
|
||||
cost = abs(error_angle - traj[-1, 2])
|
||||
|
||||
return cost
|
||||
|
||||
|
||||
def dwa_control(x, u, config, goal, ob):
|
||||
# Dynamic Window control
|
||||
|
||||
dw = calc_dynamic_window(x, config)
|
||||
|
||||
u, traj = calc_final_input(x, u, dw, config, goal, ob)
|
||||
|
||||
return u, traj
|
||||
|
||||
|
||||
def plot_arrow(x, y, yaw, length=0.5, width=0.1): # pragma: no cover
|
||||
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
|
||||
head_length=width, head_width=width)
|
||||
@@ -184,21 +197,20 @@ def main(gx=10, gy=10):
|
||||
[12.0, 12.0]
|
||||
])
|
||||
|
||||
# input [forward speed, yawrate]
|
||||
u = np.array([0.0, 0.0])
|
||||
config = Config()
|
||||
traj = np.array(x)
|
||||
|
||||
for i in range(1000):
|
||||
u, ltraj = dwa_control(x, u, config, goal, ob)
|
||||
while True:
|
||||
u, ptraj = dwa_control(x, u, config, goal, ob)
|
||||
|
||||
x = motion(x, u, config.dt)
|
||||
x = motion(x, u, config.dt) # simulate robot
|
||||
traj = np.vstack((traj, x)) # store state history
|
||||
|
||||
# print(traj)
|
||||
|
||||
if show_animation:
|
||||
plt.cla()
|
||||
plt.plot(ltraj[:, 0], ltraj[:, 1], "-g")
|
||||
plt.plot(ptraj[:, 0], ptraj[:, 1], "-g")
|
||||
plt.plot(x[0], x[1], "xr")
|
||||
plt.plot(goal[0], goal[1], "xb")
|
||||
plt.plot(ob[:, 0], ob[:, 1], "ok")
|
||||
@@ -207,8 +219,9 @@ def main(gx=10, gy=10):
|
||||
plt.grid(True)
|
||||
plt.pause(0.0001)
|
||||
|
||||
# check goal
|
||||
if math.sqrt((x[0] - goal[0])**2 + (x[1] - goal[1])**2) <= config.robot_radius:
|
||||
# check reaching goal
|
||||
dist_to_goal = math.sqrt((x[0] - goal[0])**2 + (x[1] - goal[1])**2)
|
||||
if dist_to_goal <= config.robot_radius:
|
||||
print("Goal!!")
|
||||
break
|
||||
|
||||
|
||||
@@ -161,12 +161,18 @@ def generate_roadmap(sample_x, sample_y, rr, obkdtree):
|
||||
|
||||
def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
|
||||
"""
|
||||
sx: start x position [m]
|
||||
sy: start y position [m]
|
||||
gx: goal x position [m]
|
||||
gx: goal x position [m]
|
||||
gy: goal y position [m]
|
||||
ox: x position list of Obstacles [m]
|
||||
oy: y position list of Obstacles [m]
|
||||
reso: grid resolution [m]
|
||||
rr: robot radius[m]
|
||||
rr: robot radius [m]
|
||||
road_map: ??? [m]
|
||||
sample_x: ??? [m]
|
||||
sample_y: ??? [m]
|
||||
|
||||
@return: Two lists of path coordinates ([x1, x2, ...], [y1, y2, ...]), empty list when no path was found
|
||||
"""
|
||||
|
||||
nstart = Node(sx, sy, 0.0, -1)
|
||||
@@ -175,9 +181,12 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
|
||||
openset, closedset = dict(), dict()
|
||||
openset[len(road_map) - 2] = nstart
|
||||
|
||||
path_found = True
|
||||
|
||||
while True:
|
||||
if not openset:
|
||||
print("Cannot find path")
|
||||
path_found = False
|
||||
break
|
||||
|
||||
c_id = min(openset, key=lambda o: openset[o].cost)
|
||||
@@ -217,6 +226,9 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
|
||||
openset[n_id].pind = c_id
|
||||
else:
|
||||
openset[n_id] = node
|
||||
|
||||
if path_found is False:
|
||||
return [], []
|
||||
|
||||
# generate final course
|
||||
rx, ry = [ngoal.x], [ngoal.y]
|
||||
@@ -249,8 +261,8 @@ def sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree):
|
||||
sample_x, sample_y = [], []
|
||||
|
||||
while len(sample_x) <= N_SAMPLE:
|
||||
tx = (random.random() - minx) * (maxx - minx)
|
||||
ty = (random.random() - miny) * (maxy - miny)
|
||||
tx = (random.random() * (maxx - minx)) + minx
|
||||
ty = (random.random() * (maxy - miny)) + miny
|
||||
|
||||
index, dist = obkdtree.search(np.array([tx, ty]).reshape(2, 1))
|
||||
|
||||
|
||||
@@ -9,13 +9,15 @@ import numpy as np
|
||||
import math
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
|
||||
k = 0.1 # look forward gain
|
||||
Lfc = 1.0 # look-ahead distance
|
||||
Lfc = 2.0 # look-ahead distance
|
||||
Kp = 1.0 # speed proportional gain
|
||||
dt = 0.1 # [s]
|
||||
L = 2.9 # [m] wheel base of vehicle
|
||||
|
||||
|
||||
old_nearest_point_index = None
|
||||
show_animation = True
|
||||
|
||||
|
||||
@@ -26,6 +28,8 @@ class State:
|
||||
self.y = y
|
||||
self.yaw = yaw
|
||||
self.v = v
|
||||
self.rear_x = self.x - ((L / 2) * math.cos(self.yaw))
|
||||
self.rear_y = self.y - ((L / 2) * math.sin(self.yaw))
|
||||
|
||||
|
||||
def update(state, a, delta):
|
||||
@@ -34,6 +38,8 @@ def update(state, a, delta):
|
||||
state.y = state.y + state.v * math.sin(state.yaw) * dt
|
||||
state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
|
||||
state.v = state.v + a * dt
|
||||
state.rear_x = state.x - ((L / 2) * math.cos(state.yaw))
|
||||
state.rear_y = state.y - ((L / 2) * math.sin(state.yaw))
|
||||
|
||||
return state
|
||||
|
||||
@@ -59,7 +65,7 @@ def pure_pursuit_control(state, cx, cy, pind):
|
||||
ty = cy[-1]
|
||||
ind = len(cx) - 1
|
||||
|
||||
alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
|
||||
alpha = math.atan2(ty - state.rear_y, tx - state.rear_x) - state.yaw
|
||||
|
||||
Lf = k * state.v + Lfc
|
||||
|
||||
@@ -67,22 +73,43 @@ def pure_pursuit_control(state, cx, cy, pind):
|
||||
|
||||
return delta, ind
|
||||
|
||||
def calc_distance(state, point_x, point_y):
|
||||
|
||||
dx = state.rear_x - point_x
|
||||
dy = state.rear_y - point_y
|
||||
return math.sqrt(dx ** 2 + dy ** 2)
|
||||
|
||||
|
||||
def calc_target_index(state, cx, cy):
|
||||
|
||||
# search nearest point index
|
||||
dx = [state.x - icx for icx in cx]
|
||||
dy = [state.y - icy for icy in cy]
|
||||
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
|
||||
ind = d.index(min(d))
|
||||
global old_nearest_point_index
|
||||
|
||||
if old_nearest_point_index is None:
|
||||
# search nearest point index
|
||||
dx = [state.rear_x - icx for icx in cx]
|
||||
dy = [state.rear_y - icy for icy in cy]
|
||||
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
|
||||
ind = d.index(min(d))
|
||||
old_nearest_point_index = ind
|
||||
else:
|
||||
ind = old_nearest_point_index
|
||||
distance_this_index = calc_distance(state, cx[ind], cy[ind])
|
||||
while True:
|
||||
ind = ind + 1 if (ind + 1) < len(cx) else ind
|
||||
distance_next_index = calc_distance(state, cx[ind], cy[ind])
|
||||
if distance_this_index < distance_next_index:
|
||||
break
|
||||
distance_this_index = distance_next_index
|
||||
old_nearest_point_index = ind
|
||||
|
||||
L = 0.0
|
||||
|
||||
Lf = k * state.v + Lfc
|
||||
|
||||
# search look ahead target point index
|
||||
while Lf > L and (ind + 1) < len(cx):
|
||||
dx = cx[ind] - state.x
|
||||
dy = cy[ind] - state.y
|
||||
dx = cx[ind] - state.rear_x
|
||||
dy = cy[ind] - state.rear_y
|
||||
L = math.sqrt(dx ** 2 + dy ** 2)
|
||||
ind += 1
|
||||
|
||||
|
||||
36
README.md
36
README.md
@@ -14,6 +14,7 @@ Python codes for robotics algorithm.
|
||||
|
||||
|
||||
|
||||
|
||||
# Table of Contents
|
||||
* [What is this?](#what-is-this)
|
||||
* [Requirements](#requirements)
|
||||
@@ -26,12 +27,13 @@ Python codes for robotics algorithm.
|
||||
* [Mapping](#mapping)
|
||||
* [Gaussian grid map](#gaussian-grid-map)
|
||||
* [Ray casting grid map](#ray-casting-grid-map)
|
||||
* [Lidar to grid map](#lidar-to-grid-map)
|
||||
* [k-means object clustering](#k-means-object-clustering)
|
||||
* [Rectangle fitting](#rectangle-fitting)
|
||||
* [SLAM](#slam)
|
||||
* [Iterative Closest Point (ICP) Matching](#iterative-closest-point-icp-matching)
|
||||
* [FastSLAM 1.0](#fastslam-10)
|
||||
* [Graph based SLAM](#graph-based-slam)
|
||||
* [Pose Optimization SLAM](#pose-optimization-slam)
|
||||
* [Path Planning](#path-planning)
|
||||
* [Dynamic Window Approach](#dynamic-window-approach)
|
||||
* [Grid based search](#grid-based-search)
|
||||
@@ -191,6 +193,12 @@ This is a 2D ray casting grid mapping example.
|
||||
|
||||
|
||||
|
||||
## Lidar to grid map
|
||||
|
||||
This example shows how to convert a 2D range measurement to a grid map.
|
||||
|
||||
|
||||
|
||||
## k-means object clustering
|
||||
|
||||
This is a 2D object clustering with k-means algorithm.
|
||||
@@ -242,24 +250,11 @@ Ref:
|
||||
- [SLAM simulations by Tim Bailey](http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm)
|
||||
|
||||
|
||||
## Graph based SLAM
|
||||
## Pose Optimization SLAM
|
||||
|
||||
This is a graph based SLAM example.
|
||||
|
||||
The blue line is ground truth.
|
||||
|
||||
The black line is dead reckoning.
|
||||
|
||||
The red line is the estimated trajectory with Graph based SLAM.
|
||||
|
||||
The black stars are landmarks for graph edge generation.
|
||||
|
||||
|
||||
|
||||
Ref:
|
||||
|
||||
- [A Tutorial on Graph-Based SLAM](http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf)
|
||||
This is a graph based pose optimization SLAM example.
|
||||
|
||||
|
||||
|
||||
# Path Planning
|
||||
|
||||
@@ -557,7 +552,11 @@ MIT
|
||||
|
||||
# Use-case
|
||||
|
||||
See: [users\_comments](https://github.com/AtsushiSakai/PythonRobotics/blob/master/users_comments.md)
|
||||
If this project helps your robotics project, please let me know with [](https://saythanks.io/to/AtsushiSakai).
|
||||
|
||||
Your robot's video, which is using PythonRobotics, is very welcome!!
|
||||
|
||||
This is a list of other user's comment and references:[users\_comments](https://github.com/AtsushiSakai/PythonRobotics/blob/master/users_comments.md)
|
||||
|
||||
# Contribution
|
||||
|
||||
@@ -609,3 +608,4 @@ This is a list: [Users comments](https://github.com/AtsushiSakai/PythonRobotics/
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
9
SLAM/PoseOptimizationSLAM/README.md
Normal file
9
SLAM/PoseOptimizationSLAM/README.md
Normal file
@@ -0,0 +1,9 @@
|
||||
# How to use
|
||||
|
||||
1. Download data
|
||||
|
||||
>$ python data_downloader.py
|
||||
|
||||
2. run SLAM
|
||||
|
||||
>$ python pose_optimization_slam.py
|
||||
28
SLAM/PoseOptimizationSLAM/data_downloader.py
Normal file
28
SLAM/PoseOptimizationSLAM/data_downloader.py
Normal file
@@ -0,0 +1,28 @@
|
||||
"""
|
||||
|
||||
Data down loader for pose optimization
|
||||
|
||||
author: Atsushi Sakai
|
||||
|
||||
"""
|
||||
|
||||
|
||||
import subprocess
|
||||
def main():
|
||||
print("start!!")
|
||||
|
||||
cmd = "wget https://www.dropbox.com/s/vcz8cag7bo0zlaj/input_INTEL_g2o.g2o?dl=0 -O intel.g2o -nc"
|
||||
subprocess.call(cmd, shell=True)
|
||||
|
||||
cmd = "wget https://www.dropbox.com/s/d8fcn1jg1mebx8f/input_MITb_g2o.g2o?dl=0 -O mit_killian.g2o -nc"
|
||||
|
||||
subprocess.call(cmd, shell=True)
|
||||
cmd = "wget https://www.dropbox.com/s/gmdzo74b3tzvbrw/input_M3500_g2o.g2o?dl=0 -O manhattan3500.g2o -nc"
|
||||
subprocess.call(cmd, shell=True)
|
||||
|
||||
print("done!!")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
290
SLAM/PoseOptimizationSLAM/pose_optimization_slam.py
Normal file
290
SLAM/PoseOptimizationSLAM/pose_optimization_slam.py
Normal file
@@ -0,0 +1,290 @@
|
||||
"""
|
||||
|
||||
2D (x, y, yaw) pose optimization SLAM
|
||||
|
||||
author: Atsushi Sakai
|
||||
|
||||
Ref:
|
||||
- [A Compact and Portable Implementation of Graph\-based SLAM](https://www.researchgate.net/publication/321287640_A_Compact_and_Portable_Implementation_of_Graph-based_SLAM)
|
||||
|
||||
- [GitHub \- furo\-org/p2o: Single header 2D/3D graph\-based SLAM library](https://github.com/furo-org/p2o)
|
||||
|
||||
"""
|
||||
|
||||
import sys
|
||||
import time
|
||||
import math
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from scipy import sparse
|
||||
from scipy.sparse import linalg
|
||||
|
||||
|
||||
class Optimizer2D:
|
||||
|
||||
def __init__(self):
|
||||
self.verbose = False
|
||||
self.animation = False
|
||||
self.p_lambda = 0.0
|
||||
self.init_w = 1e10
|
||||
self.stop_thre = 1e-3
|
||||
self.dim = 3 # state dimension
|
||||
|
||||
def optimize_path(self, nodes, consts, max_iter, min_iter):
|
||||
|
||||
graph_nodes = nodes[:]
|
||||
prev_cost = sys.float_info.max
|
||||
|
||||
for i in range(max_iter):
|
||||
start = time.time()
|
||||
cost, graph_nodes = self.optimize_path_one_step(
|
||||
graph_nodes, consts)
|
||||
elapsed = time.time() - start
|
||||
if self.verbose:
|
||||
print("step ", i, " cost: ", cost, " time:", elapsed, "s")
|
||||
|
||||
# check convergence
|
||||
if (i > min_iter) and (prev_cost - cost < self.stop_thre):
|
||||
if self.verbose:
|
||||
print("converged:", prev_cost
|
||||
- cost, " < ", self.stop_thre)
|
||||
break
|
||||
prev_cost = cost
|
||||
|
||||
if self.animation:
|
||||
plt.cla()
|
||||
plot_nodes(nodes, color="-b")
|
||||
plot_nodes(graph_nodes)
|
||||
plt.axis("equal")
|
||||
plt.pause(1.0)
|
||||
|
||||
return graph_nodes
|
||||
|
||||
def optimize_path_one_step(self, graph_nodes, constraints):
|
||||
|
||||
indlist = [i for i in range(self.dim)]
|
||||
numnodes = len(graph_nodes)
|
||||
bf = np.zeros(numnodes * self.dim)
|
||||
tripletList = TripletList()
|
||||
|
||||
for con in constraints:
|
||||
ida = con.id1
|
||||
idb = con.id2
|
||||
assert 0 <= ida and ida < numnodes, "ida is invalid"
|
||||
assert 0 <= idb and idb < numnodes, "idb is invalid"
|
||||
r, Ja, Jb = self.calc_error(
|
||||
graph_nodes[ida], graph_nodes[idb], con.t)
|
||||
|
||||
trJaInfo = Ja.transpose() @ con.info_mat
|
||||
trJaInfoJa = trJaInfo @ Ja
|
||||
trJbInfo = Jb.transpose() @ con.info_mat
|
||||
trJbInfoJb = trJbInfo @ Jb
|
||||
trJaInfoJb = trJaInfo @ Jb
|
||||
|
||||
for k in indlist:
|
||||
for m in indlist:
|
||||
tripletList.push_back(
|
||||
ida * self.dim + k, ida * self.dim + m, trJaInfoJa[k, m])
|
||||
tripletList.push_back(
|
||||
idb * self.dim + k, idb * self.dim + m, trJbInfoJb[k, m])
|
||||
tripletList.push_back(
|
||||
ida * self.dim + k, idb * self.dim + m, trJaInfoJb[k, m])
|
||||
tripletList.push_back(
|
||||
idb * self.dim + k, ida * self.dim + m, trJaInfoJb[m, k])
|
||||
|
||||
bf[ida * self.dim: ida * self.dim + 3] += trJaInfo @ r
|
||||
bf[idb * self.dim: idb * self.dim + 3] += trJbInfo @ r
|
||||
|
||||
for k in indlist:
|
||||
tripletList.push_back(k, k, self.init_w)
|
||||
|
||||
for i in range(self.dim * numnodes):
|
||||
tripletList.push_back(i, i, self.p_lambda)
|
||||
|
||||
mat = sparse.coo_matrix((tripletList.data, (tripletList.row, tripletList.col)),
|
||||
shape=(numnodes * self.dim, numnodes * self.dim))
|
||||
|
||||
x = linalg.spsolve(mat.tocsr(), -bf)
|
||||
|
||||
out_nodes = []
|
||||
for i in range(len(graph_nodes)):
|
||||
u_i = i * self.dim
|
||||
pos = Pose2D(
|
||||
graph_nodes[i].x + x[u_i],
|
||||
graph_nodes[i].y + x[u_i + 1],
|
||||
graph_nodes[i].theta + x[u_i + 2]
|
||||
)
|
||||
out_nodes.append(pos)
|
||||
|
||||
cost = self.calc_global_cost(out_nodes, constraints)
|
||||
|
||||
return cost, out_nodes
|
||||
|
||||
def calc_global_cost(self, nodes, constraints):
|
||||
cost = 0.0
|
||||
for c in constraints:
|
||||
diff = self.error_func(nodes[c.id1], nodes[c.id2], c.t)
|
||||
cost += diff.transpose() @ c.info_mat @ diff
|
||||
|
||||
return cost
|
||||
|
||||
def error_func(self, pa, pb, t):
|
||||
ba = self.calc_constraint_pose(pb, pa)
|
||||
error = np.array([ba.x - t.x,
|
||||
ba.y - t.y,
|
||||
self.pi2pi(ba.theta - t.theta)])
|
||||
return error
|
||||
|
||||
def calc_constraint_pose(self, l, r):
|
||||
diff = np.array([l.x - r.x, l.y - r.y, l.theta - r.theta])
|
||||
v = self.rot_mat_2d(-r.theta) @ diff
|
||||
v[2] = self.pi2pi(l.theta - r.theta)
|
||||
return Pose2D(v[0], v[1], v[2])
|
||||
|
||||
def rot_mat_2d(self, theta):
|
||||
return np.array([[math.cos(theta), -math.sin(theta), 0.0],
|
||||
[math.sin(theta), math.cos(theta), 0.0],
|
||||
[0.0, 0.0, 1.0]
|
||||
])
|
||||
|
||||
def calc_error(self, pa, pb, t):
|
||||
e0 = self.error_func(pa, pb, t)
|
||||
dx = pb.x - pa.x
|
||||
dy = pb.y - pa.y
|
||||
dxdt = -math.sin(pa.theta) * dx + math.cos(pa.theta) * dy
|
||||
dydt = -math.cos(pa.theta) * dx - math.sin(pa.theta) * dy
|
||||
Ja = np.array([[-math.cos(pa.theta), -math.sin(pa.theta), dxdt],
|
||||
[math.sin(pa.theta), -math.cos(pa.theta), dydt],
|
||||
[0.0, 0.0, -1.0]
|
||||
])
|
||||
Jb = np.array([[math.cos(pa.theta), math.sin(pa.theta), 0.0],
|
||||
[-math.sin(pa.theta), math.cos(pa.theta), 0.0],
|
||||
[0.0, 0.0, 1.0]
|
||||
])
|
||||
return e0, Ja, Jb
|
||||
|
||||
def pi2pi(self, rad):
|
||||
|
||||
val = math.fmod(rad, 2.0 * math.pi)
|
||||
if val > math.pi:
|
||||
val -= 2.0 * math.pi
|
||||
elif val < -math.pi:
|
||||
val += 2.0 * math.pi
|
||||
|
||||
return val
|
||||
|
||||
|
||||
class TripletList:
|
||||
|
||||
def __init__(self):
|
||||
self.row = []
|
||||
self.col = []
|
||||
self.data = []
|
||||
|
||||
def push_back(self, irow, icol, idata):
|
||||
self.row.append(irow)
|
||||
self.col.append(icol)
|
||||
self.data.append(idata)
|
||||
|
||||
|
||||
class Pose2D:
|
||||
|
||||
def __init__(self, x, y, theta):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.theta = theta
|
||||
|
||||
|
||||
class Constrant2D:
|
||||
|
||||
def __init__(self, id1, id2, t, info_mat):
|
||||
self.id1 = id1
|
||||
self.id2 = id2
|
||||
self.t = t
|
||||
self.info_mat = info_mat
|
||||
|
||||
|
||||
def plot_nodes(nodes, color ="-r", label = ""):
|
||||
x, y = [], []
|
||||
for n in nodes:
|
||||
x.append(n.x)
|
||||
y.append(n.y)
|
||||
plt.plot(x, y, color, label=label)
|
||||
|
||||
|
||||
def load_data(fname):
|
||||
nodes, consts = [], []
|
||||
|
||||
for line in open(fname):
|
||||
sline = line.split()
|
||||
tag = sline[0]
|
||||
|
||||
if tag == "VERTEX_SE2":
|
||||
data_id = int(sline[1])
|
||||
x = float(sline[2])
|
||||
y = float(sline[3])
|
||||
theta = float(sline[4])
|
||||
nodes.append(Pose2D(x, y, theta))
|
||||
elif tag == "EDGE_SE2":
|
||||
id1 = int(sline[1])
|
||||
id2 = int(sline[2])
|
||||
x = float(sline[3])
|
||||
y = float(sline[4])
|
||||
th = float(sline[5])
|
||||
c1 = float(sline[6])
|
||||
c2 = float(sline[7])
|
||||
c3 = float(sline[8])
|
||||
c4 = float(sline[9])
|
||||
c5 = float(sline[10])
|
||||
c6 = float(sline[11])
|
||||
t = Pose2D(x, y, th)
|
||||
info_mat = np.array([[c1, c2, c3],
|
||||
[c2, c4, c5],
|
||||
[c3, c5, c6]
|
||||
])
|
||||
consts.append(Constrant2D(id1, id2, t, info_mat))
|
||||
|
||||
print("n_nodes:", len(nodes))
|
||||
print("n_consts:", len(consts))
|
||||
|
||||
return nodes, consts
|
||||
|
||||
|
||||
def main():
|
||||
print("start!!")
|
||||
|
||||
fnames = ["intel.g2o",
|
||||
"manhattan3500.g2o",
|
||||
"mit_killian.g2o"
|
||||
]
|
||||
|
||||
max_iter = 20
|
||||
min_iter = 3
|
||||
|
||||
# parameter setting
|
||||
optimizer = Optimizer2D()
|
||||
optimizer.p_lambda = 1e-6
|
||||
optimizer.verbose = True
|
||||
optimizer.animation = True
|
||||
|
||||
for f in fnames:
|
||||
nodes, consts = load_data(f)
|
||||
|
||||
start = time.time()
|
||||
final_nodes = optimizer.optimize_path(nodes, consts, max_iter, min_iter)
|
||||
print("elapsed_time", time.time() - start, "sec")
|
||||
|
||||
# plotting
|
||||
plt.cla()
|
||||
plot_nodes(nodes, color="-b", label="before")
|
||||
plot_nodes(final_nodes, label="after")
|
||||
plt.axis("equal")
|
||||
plt.grid(True)
|
||||
plt.legend()
|
||||
plt.pause(3.0)
|
||||
|
||||
print("done!!")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -13,6 +13,13 @@ You can check stargazer's locations of this project from:
|
||||
|
||||
- [Stargazers location map](https://drive.google.com/open?id=1bBXS9IQmZ3Tfe1wNGvJbObRt9Htt4PGC&usp=sharing)
|
||||
|
||||
|
||||
# Related projects
|
||||
|
||||
This is a list of related projects.
|
||||
|
||||
- [onlytailei/CppRobotics: cpp implementation of robotics algorithms including localization, mapping, SLAM, path planning and control](https://github.com/onlytailei/CppRobotics)
|
||||
|
||||
# Individual users comments
|
||||
|
||||
These are comments from user's using [](https://saythanks.io/to/AtsushiSakai)
|
||||
@@ -196,6 +203,42 @@ Dear AtsushiSakai, <br>Thank you for this great resource! I very much appreciate
|
||||
|
||||
--Mridul Aanjaneya
|
||||
|
||||
---
|
||||
|
||||
Thank you for the python robotics sample code and visualizations!! This is my new favorite thing on the internet.
|
||||
|
||||
--selamg@mit.edu
|
||||
|
||||
---
|
||||
|
||||
Mr AtsushiSakai .. <br>Your work and teaching is light for me <br>thank you very much for giving time and effort to make it public for us
|
||||
|
||||
--Karim Anass
|
||||
|
||||
---
|
||||
|
||||
Thank You
|
||||
|
||||
--Anonymous
|
||||
|
||||
---
|
||||
|
||||
I've learned the robotics from the traditional way of solving problem through finding packages and reading papers. This amazing project is unbelievably helpful for new-comers to learn and get familiar with the algorithms. Gods know how many hours you've taken to sort all the materials and written everything into one single coherent project. I'm truly amazed and deepest appreciation.
|
||||
|
||||
--Ewing Kang
|
||||
|
||||
---
|
||||
|
||||
Hey, I'm a student and I just recently got into robotics, and visited your repository multiple times. Today I was super happy to find the link to Patreon! I am impressed by didactic quality of the repo, keep up the good work!
|
||||
|
||||
--Carolina Bianchi
|
||||
|
||||
---
|
||||
|
||||
Dear AtsushiSakai, thanks a lot for the PythonRobotics git repository. I'm a college student who is trying to make a mini autonomous robot for my final project submission, I still haven't started using your code but by the look of it I'm sure it will be of greater use. Will let you know how my project is coming up.
|
||||
|
||||
--Pragdheesh
|
||||
|
||||
=========
|
||||
|
||||
# Citations
|
||||
@@ -212,3 +255,5 @@ URL: http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8516589&isnumber=85
|
||||
|
||||
- Autonomous Vehicle Readings https://richardkelley.io/readings
|
||||
|
||||
- 36 Amazing Python Open Source Projects (v.2019) – Mybridge for Professionals https://medium.mybridge.co/36-amazing-python-open-source-projects-v-2019-2fe058d79450
|
||||
|
||||
|
||||
Reference in New Issue
Block a user