mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-12 23:58:04 -05:00
code clean up
This commit is contained in:
Atsushi Sakai
@@ -2,23 +2,19 @@
|
||||
|
||||
A* grid based planning
|
||||
|
||||
author: Atsushi Sakai(@Atsushi_twi)
|
||||
Nikos Kanargias (nkana@tee.gr)
|
||||
author: Nikos Kanargias (nkana@tee.gr)
|
||||
|
||||
See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
|
||||
|
||||
"""
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import math
|
||||
import heapq
|
||||
|
||||
# _round = round
|
||||
# def round(x):
|
||||
# return int(_round(x))
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
show_animation = False
|
||||
|
||||
|
||||
class Node:
|
||||
|
||||
def __init__(self, x, y, cost, pind):
|
||||
@@ -67,7 +63,6 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr):
|
||||
openset[calc_index(ngoal, xw, minx, miny)] = ngoal
|
||||
pq = []
|
||||
pq.append((0, calc_index(ngoal, xw, minx, miny)))
|
||||
|
||||
|
||||
while 1:
|
||||
if not pq:
|
||||
@@ -87,7 +82,6 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr):
|
||||
plt.pause(0.001)
|
||||
|
||||
# Remove the item from the open set
|
||||
|
||||
|
||||
# expand search grid based on motion model
|
||||
for i, _ in enumerate(motion):
|
||||
@@ -104,15 +98,17 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr):
|
||||
|
||||
if n_id not in openset:
|
||||
openset[n_id] = node # Discover a new node
|
||||
heapq.heappush(pq, (node.cost, calc_index(node, xw, minx, miny)))
|
||||
heapq.heappush(
|
||||
pq, (node.cost, calc_index(node, xw, minx, miny)))
|
||||
else:
|
||||
if openset[n_id].cost >= node.cost:
|
||||
# This path is the best until now. record it!
|
||||
openset[n_id] = node
|
||||
heapq.heappush(pq, (node.cost, calc_index(node, xw, minx, miny)))
|
||||
heapq.heappush(
|
||||
pq, (node.cost, calc_index(node, xw, minx, miny)))
|
||||
|
||||
|
||||
rx, ry = calc_final_path(closedset[calc_index(nstart, xw, minx, miny)], closedset, reso)
|
||||
rx, ry = calc_final_path(closedset[calc_index(
|
||||
nstart, xw, minx, miny)], closedset, reso)
|
||||
|
||||
return rx, ry, closedset
|
||||
|
||||
|
||||
@@ -1,36 +1,45 @@
|
||||
"""
|
||||
|
||||
Car model for Hybrid A* path planning
|
||||
|
||||
author: Zheng Zh (@Zhengzh)
|
||||
|
||||
"""
|
||||
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
from math import sqrt, cos, sin, tan, pi
|
||||
|
||||
WB = 3. # rear to front wheel
|
||||
W = 2. # width of car
|
||||
LF = 3.3 # distance from rear to vehicle front end
|
||||
LB = 1.0 # distance from rear to vehicle back end
|
||||
MAX_STEER = 0.6 #[rad] maximum steering angle
|
||||
WB = 3. # rear to front wheel
|
||||
W = 2. # width of car
|
||||
LF = 3.3 # distance from rear to vehicle front end
|
||||
LB = 1.0 # distance from rear to vehicle back end
|
||||
MAX_STEER = 0.6 # [rad] maximum steering angle
|
||||
|
||||
WBUBBLE_DIST = (LF-LB)/2.0
|
||||
WBUBBLE_R = sqrt(((LF+LB)/2.0)**2+1)
|
||||
WBUBBLE_DIST = (LF - LB) / 2.0
|
||||
WBUBBLE_R = sqrt(((LF + LB) / 2.0)**2 + 1)
|
||||
|
||||
# vehicle rectangle verticles
|
||||
VRX = [LF, LF, -LB, -LB, LF]
|
||||
VRY = [W/2,-W/2,-W/2,W/2,W/2]
|
||||
VRY = [W / 2, -W / 2, -W / 2, W / 2, W / 2]
|
||||
|
||||
|
||||
def check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
|
||||
for x, y, yaw in zip(xlist, ylist, yawlist):
|
||||
cx = x + WBUBBLE_DIST*cos(yaw)
|
||||
cy = y + WBUBBLE_DIST*sin(yaw)
|
||||
cx = x + WBUBBLE_DIST * cos(yaw)
|
||||
cy = y + WBUBBLE_DIST * sin(yaw)
|
||||
|
||||
ids = kdtree.search_in_distance([cx, cy], WBUBBLE_R)
|
||||
|
||||
if not ids:
|
||||
continue
|
||||
|
||||
|
||||
if not rectangle_check(x, y, yaw,
|
||||
[ox[i] for i in ids], [oy[i] for i in ids]):
|
||||
return False # collision
|
||||
|
||||
return True # no collision
|
||||
[ox[i] for i in ids], [oy[i] for i in ids]):
|
||||
return False # collision
|
||||
|
||||
return True # no collision
|
||||
|
||||
|
||||
def rectangle_check(x, y, yaw, ox, oy):
|
||||
# transform obstacles to base link frame
|
||||
@@ -38,14 +47,14 @@ def rectangle_check(x, y, yaw, ox, oy):
|
||||
for iox, ioy in zip(ox, oy):
|
||||
tx = iox - x
|
||||
ty = ioy - y
|
||||
rx = c*tx - s*ty
|
||||
ry = s*tx + c*ty
|
||||
rx = c * tx - s * ty
|
||||
ry = s * tx + c * ty
|
||||
|
||||
if not (rx > LF or rx < -LB or ry > W/2.0 or ry < -W/2.0):
|
||||
if not (rx > LF or rx < -LB or ry > W / 2.0 or ry < -W / 2.0):
|
||||
# print (x, y, yaw, iox, ioy, c, s, rx, ry)
|
||||
return False # no collision
|
||||
|
||||
return True # collision
|
||||
return False # no collision
|
||||
|
||||
return True # collision
|
||||
|
||||
|
||||
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
|
||||
@@ -58,35 +67,38 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
|
||||
fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4)
|
||||
# plt.plot(x, y)
|
||||
|
||||
|
||||
def plot_car(x, y, yaw):
|
||||
car_color = '-k'
|
||||
c, s = cos(yaw), sin(yaw)
|
||||
|
||||
|
||||
car_outline_x, car_outline_y = [], []
|
||||
for rx, ry in zip(VRX, VRY):
|
||||
tx = c*rx-s*ry + x
|
||||
ty = s*rx+c*ry + y
|
||||
tx = c * rx - s * ry + x
|
||||
ty = s * rx + c * ry + y
|
||||
car_outline_x.append(tx)
|
||||
car_outline_y.append(ty)
|
||||
|
||||
arrow_x, arrow_y, arrow_yaw = c*1.5+x, s*1.5+y, yaw
|
||||
|
||||
arrow_x, arrow_y, arrow_yaw = c * 1.5 + x, s * 1.5 + y, yaw
|
||||
plot_arrow(arrow_x, arrow_y, arrow_yaw)
|
||||
|
||||
|
||||
plt.plot(car_outline_x, car_outline_y, car_color)
|
||||
|
||||
|
||||
def pi_2_pi(angle):
|
||||
return (angle + pi) % (2 * pi) - pi
|
||||
|
||||
|
||||
def move(x, y, yaw, distance, steer, L=WB):
|
||||
x += distance * cos(yaw)
|
||||
y += distance * sin(yaw)
|
||||
yaw += pi_2_pi(distance * tan(steer) / L) # distance/2
|
||||
yaw += pi_2_pi(distance * tan(steer) / L) # distance/2
|
||||
|
||||
return x, y, yaw
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
x, y, yaw = 0., 0., 1.
|
||||
plt.axis('equal')
|
||||
plot_car(x, y, yaw)
|
||||
plt.show()
|
||||
|
||||
plt.show()
|
||||
@@ -2,40 +2,43 @@
|
||||
|
||||
Hybrid A* path planning
|
||||
|
||||
author: Atsushi Sakai (@Atsushi_twi)
|
||||
Zheng Zh (@Zhengzh)
|
||||
author: Zheng Zh (@Zhengzh)
|
||||
|
||||
"""
|
||||
|
||||
import heapq
|
||||
import matplotlib.pyplot as plt
|
||||
import scipy.spatial
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
import sys
|
||||
sys.path.append("../ReedsSheppPath/")
|
||||
try:
|
||||
from a_star import dp_planning # , calc_obstacle_map
|
||||
import reeds_shepp_path_planning as rs
|
||||
from car import move, check_car_collision, MAX_STEER, WB, plot_car
|
||||
except:
|
||||
raise
|
||||
|
||||
import math
|
||||
import numpy as np
|
||||
import scipy.spatial
|
||||
import matplotlib.pyplot as plt
|
||||
import reeds_shepp_path_planning as rs
|
||||
import heapq
|
||||
from car import move, check_car_collision, MAX_STEER, WB, plot_car
|
||||
from a_star import dp_planning #, calc_obstacle_map
|
||||
|
||||
XY_GRID_RESOLUTION = 2.0 #[m]
|
||||
YAW_GRID_RESOLUTION = np.deg2rad(15.0) #[rad]
|
||||
GOAL_TYAW_TH = np.deg2rad(5.0) #[rad]
|
||||
MOTION_RESOLUTION = 0.1 #[m] path interporate resolution
|
||||
N_STEER = 20.0 # number of steer command
|
||||
XY_GRID_RESOLUTION = 2.0 # [m]
|
||||
YAW_GRID_RESOLUTION = np.deg2rad(15.0) # [rad]
|
||||
GOAL_TYAW_TH = np.deg2rad(5.0) # [rad]
|
||||
MOTION_RESOLUTION = 0.1 # [m] path interporate resolution
|
||||
N_STEER = 20.0 # number of steer command
|
||||
EXTEND_AREA = 0.0 # [m]
|
||||
H_COST = 1.0
|
||||
MOTION_RESOLUTION = 0.1
|
||||
SKIP_COLLISION_CHECK = 4
|
||||
VR = 1.0 # robot radius
|
||||
VR = 1.0 # robot radius
|
||||
|
||||
SB_COST = 100.0 # switch back penalty cost
|
||||
BACK_COST = 5.0 # backward penalty cost
|
||||
STEER_CHANGE_COST = 5.0 # steer angle change penalty cost
|
||||
STEER_COST = 1.0 # steer angle change penalty cost
|
||||
SB_COST = 100.0 # switch back penalty cost
|
||||
BACK_COST = 5.0 # backward penalty cost
|
||||
STEER_CHANGE_COST = 5.0 # steer angle change penalty cost
|
||||
STEER_COST = 1.0 # steer angle change penalty cost
|
||||
# JACKKNIF_COST= 200.0 # Jackknif cost
|
||||
H_COST = 5.0 # Heuristic cost
|
||||
H_COST = 5.0 # Heuristic cost
|
||||
|
||||
show_animation = True
|
||||
|
||||
@@ -43,6 +46,7 @@ show_animation = True
|
||||
# def round(x):
|
||||
# return int(_round(x))
|
||||
|
||||
|
||||
class Node:
|
||||
|
||||
def __init__(self, xind, yind, yawind, direction, xlist, ylist, yawlist, directions, steer=0.0, pind=None, cost=None):
|
||||
@@ -58,6 +62,7 @@ class Node:
|
||||
self.pind = pind
|
||||
self.cost = cost
|
||||
|
||||
|
||||
class Path:
|
||||
|
||||
def __init__(self, xlist, ylist, yawlist, directionlist, cost):
|
||||
@@ -93,7 +98,7 @@ class KDTree:
|
||||
dist.append(idist)
|
||||
|
||||
return index, dist
|
||||
|
||||
|
||||
dist, index = self.tree.query(inp, k=k)
|
||||
return index, dist
|
||||
|
||||
@@ -109,10 +114,10 @@ class KDTree:
|
||||
class Config:
|
||||
|
||||
def __init__(self, ox, oy, xyreso, yawreso):
|
||||
min_x_m = min(ox) #- EXTEND_AREA
|
||||
min_y_m = min(oy) #- EXTEND_AREA
|
||||
max_x_m = max(ox) #+ EXTEND_AREA
|
||||
max_y_m = max(oy) #+ EXTEND_AREA
|
||||
min_x_m = min(ox) # - EXTEND_AREA
|
||||
min_y_m = min(oy) # - EXTEND_AREA
|
||||
max_x_m = max(ox) # + EXTEND_AREA
|
||||
max_y_m = max(oy) # + EXTEND_AREA
|
||||
|
||||
ox.append(min_x_m)
|
||||
oy.append(min_y_m)
|
||||
@@ -137,10 +142,10 @@ def show_expansion_tree():
|
||||
|
||||
|
||||
def calc_motion_inputs():
|
||||
|
||||
|
||||
for steer in np.linspace(-MAX_STEER, MAX_STEER, N_STEER):
|
||||
for d in [1, -1]:
|
||||
yield [steer, d]
|
||||
yield [steer, d]
|
||||
|
||||
|
||||
def get_neighbors(current, config, ox, oy, kdtree):
|
||||
@@ -150,39 +155,40 @@ def get_neighbors(current, config, ox, oy, kdtree):
|
||||
if node and verify_index(node, config):
|
||||
yield node
|
||||
|
||||
|
||||
def calc_next_node(current, steer, direction, config, ox, oy, kdtree):
|
||||
|
||||
|
||||
x, y, yaw = current.xlist[-1], current.ylist[-1], current.yawlist[-1]
|
||||
|
||||
arc_l = XY_GRID_RESOLUTION * 1.5
|
||||
xlist, ylist, yawlist = [], [], []
|
||||
for dist in np.arange(0, arc_l, MOTION_RESOLUTION):
|
||||
x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION*direction, steer)
|
||||
x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer)
|
||||
xlist.append(x)
|
||||
ylist.append(y)
|
||||
yawlist.append(yaw)
|
||||
|
||||
|
||||
# plt.plot(xlist, ylist)
|
||||
if not check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
|
||||
return None
|
||||
|
||||
d = direction==1
|
||||
xind = round(x/XY_GRID_RESOLUTION)
|
||||
yind = round(y/XY_GRID_RESOLUTION)
|
||||
yawind = round(yaw/YAW_GRID_RESOLUTION)
|
||||
|
||||
d = direction == 1
|
||||
xind = round(x / XY_GRID_RESOLUTION)
|
||||
yind = round(y / XY_GRID_RESOLUTION)
|
||||
yawind = round(yaw / YAW_GRID_RESOLUTION)
|
||||
|
||||
addedcost = 0.0
|
||||
|
||||
if d != current.direction:
|
||||
addedcost += SB_COST
|
||||
|
||||
|
||||
# steer penalty
|
||||
addedcost += STEER_COST*abs(steer)
|
||||
addedcost += STEER_COST * abs(steer)
|
||||
|
||||
# steer change penalty
|
||||
addedcost += STEER_CHANGE_COST*abs(current.steer-steer)
|
||||
addedcost += STEER_CHANGE_COST * abs(current.steer - steer)
|
||||
|
||||
cost = current.cost + addedcost + arc_l
|
||||
cost = current.cost + addedcost + arc_l
|
||||
|
||||
node = Node(xind, yind, yawind, d, xlist,
|
||||
ylist, yawlist, [d],
|
||||
@@ -193,10 +199,11 @@ def calc_next_node(current, steer, direction, config, ox, oy, kdtree):
|
||||
|
||||
|
||||
def is_same_grid(n1, n2):
|
||||
if n1.xind==n2.xind and n1.yind==n2.yind and n1.yawind==n2.yawind:
|
||||
if n1.xind == n2.xind and n1.yind == n2.yind and n1.yawind == n2.yawind:
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def analytic_expantion(current, goal, c, ox, oy, kdtree):
|
||||
|
||||
sx = current.xlist[-1]
|
||||
@@ -207,9 +214,9 @@ def analytic_expantion(current, goal, c, ox, oy, kdtree):
|
||||
gy = goal.ylist[-1]
|
||||
gyaw = goal.yawlist[-1]
|
||||
|
||||
max_curvature = math.tan(MAX_STEER)/WB
|
||||
paths = rs.calc_paths(sx,sy,syaw,gx, gy, gyaw,
|
||||
max_curvature, step_size=MOTION_RESOLUTION)
|
||||
max_curvature = math.tan(MAX_STEER) / WB
|
||||
paths = rs.calc_paths(sx, sy, syaw, gx, gy, gyaw,
|
||||
max_curvature, step_size=MOTION_RESOLUTION)
|
||||
|
||||
if not paths:
|
||||
return None
|
||||
@@ -222,13 +229,13 @@ def analytic_expantion(current, goal, c, ox, oy, kdtree):
|
||||
if not best or best > cost:
|
||||
best = cost
|
||||
best_path = path
|
||||
|
||||
|
||||
return best_path
|
||||
|
||||
|
||||
def update_node_with_analystic_expantion(current, goal,
|
||||
c, ox, oy, kdtree):
|
||||
apath = analytic_expantion(current, goal, c, ox,oy, kdtree)
|
||||
def update_node_with_analystic_expantion(current, goal,
|
||||
c, ox, oy, kdtree):
|
||||
apath = analytic_expantion(current, goal, c, ox, oy, kdtree)
|
||||
|
||||
if apath:
|
||||
plt.plot(apath.x, apath.y)
|
||||
@@ -241,11 +248,11 @@ def update_node_with_analystic_expantion(current, goal,
|
||||
|
||||
fd = []
|
||||
for d in apath.directions[1:]:
|
||||
fd.append(d>=0)
|
||||
|
||||
fd.append(d >= 0)
|
||||
|
||||
fsteer = 0.0
|
||||
fpath = Node(current.xind, current.yind, current.yawind,
|
||||
current.direction, fx, fy, fyaw, fd, cost=fcost, pind=fpind, steer=fsteer)
|
||||
fpath = Node(current.xind, current.yind, current.yawind,
|
||||
current.direction, fx, fy, fyaw, fd, cost=fcost, pind=fpind, steer=fsteer)
|
||||
return True, fpath
|
||||
|
||||
return False, None
|
||||
@@ -255,33 +262,33 @@ def calc_rs_path_cost(rspath):
|
||||
|
||||
cost = 0.0
|
||||
for l in rspath.lengths:
|
||||
if l >= 0: # forward
|
||||
if l >= 0: # forward
|
||||
cost += l
|
||||
else: # back
|
||||
else: # back
|
||||
cost += abs(l) * BACK_COST
|
||||
|
||||
# swich back penalty
|
||||
for i in range(len(rspath.lengths)-1):
|
||||
if rspath.lengths[i] * rspath.lengths[i+1] < 0.0: # switch back
|
||||
for i in range(len(rspath.lengths) - 1):
|
||||
if rspath.lengths[i] * rspath.lengths[i + 1] < 0.0: # switch back
|
||||
cost += SB_COST
|
||||
|
||||
# steer penalyty
|
||||
for ctype in rspath.ctypes:
|
||||
if ctype != "S": # curve
|
||||
cost += STEER_COST*abs(MAX_STEER)
|
||||
if ctype != "S": # curve
|
||||
cost += STEER_COST * abs(MAX_STEER)
|
||||
|
||||
# ==steer change penalty
|
||||
# calc steer profile
|
||||
nctypes = len(rspath.ctypes)
|
||||
ulist = [0.0]*nctypes
|
||||
ulist = [0.0] * nctypes
|
||||
for i in range(nctypes):
|
||||
if rspath.ctypes[i] == "R" :
|
||||
if rspath.ctypes[i] == "R":
|
||||
ulist[i] = - MAX_STEER
|
||||
elif rspath.ctypes[i] == "L":
|
||||
ulist[i] = MAX_STEER
|
||||
|
||||
for i in range(len(rspath.ctypes)-1):
|
||||
cost += STEER_CHANGE_COST*abs(ulist[i+1] - ulist[i])
|
||||
|
||||
for i in range(len(rspath.ctypes) - 1):
|
||||
cost += STEER_CHANGE_COST * abs(ulist[i + 1] - ulist[i])
|
||||
|
||||
return cost
|
||||
|
||||
@@ -311,11 +318,12 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
|
||||
openList, closedList = {}, {}
|
||||
|
||||
_, _, h_dp = dp_planning(nstart.xlist[-1], nstart.ylist[-1],
|
||||
ngoal.xlist[-1], ngoal.ylist[-1], ox, oy, xyreso, VR)
|
||||
ngoal.xlist[-1], ngoal.ylist[-1], ox, oy, xyreso, VR)
|
||||
|
||||
pq = []
|
||||
openList[calc_index(nstart, config)] = nstart
|
||||
heapq.heappush(pq, (calc_cost(nstart, h_dp, ngoal, config), calc_index(nstart, config)))
|
||||
heapq.heappush(pq, (calc_cost(nstart, h_dp, ngoal, config),
|
||||
calc_index(nstart, config)))
|
||||
|
||||
while True:
|
||||
if not openList:
|
||||
@@ -340,29 +348,32 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
|
||||
|
||||
if isupdated:
|
||||
break
|
||||
|
||||
|
||||
for neighbor in get_neighbors(current, config, ox, oy, obkdtree):
|
||||
neighbor_index = calc_index(neighbor, config)
|
||||
# if closedList.has_key(neighbor_index):
|
||||
if neighbor_index in closedList:
|
||||
continue
|
||||
if not neighbor in openList \
|
||||
or openList[neighbor_index].cost > neighbor.cost:
|
||||
heapq.heappush(pq, (calc_cost(neighbor, h_dp, ngoal, config), neighbor_index))
|
||||
or openList[neighbor_index].cost > neighbor.cost:
|
||||
heapq.heappush(
|
||||
pq, (calc_cost(neighbor, h_dp, ngoal, config), neighbor_index))
|
||||
openList[neighbor_index] = neighbor
|
||||
|
||||
|
||||
path = get_final_path(closedList, fpath, nstart, config)
|
||||
return path
|
||||
|
||||
|
||||
def calc_cost(n, h_dp, goal, c):
|
||||
ind = (n.yind - c.miny) * c.xw + (n.xind - c.minx)
|
||||
if not ind in h_dp:
|
||||
return (n.cost + 999999999) # collision cost
|
||||
return (n.cost + H_COST*h_dp[ind].cost)
|
||||
return (n.cost + 999999999) # collision cost
|
||||
return (n.cost + H_COST * h_dp[ind].cost)
|
||||
|
||||
|
||||
def get_final_path(closed, ngoal, nstart, config):
|
||||
rx, ry, ryaw = list(reversed(ngoal.xlist)), list(reversed(ngoal.ylist)), list(reversed(ngoal.yawlist))
|
||||
rx, ry, ryaw = list(reversed(ngoal.xlist)), list(
|
||||
reversed(ngoal.ylist)), list(reversed(ngoal.yawlist))
|
||||
direction = list(reversed(ngoal.directions))
|
||||
nid = ngoal.pind
|
||||
finalcost = ngoal.cost
|
||||
@@ -373,7 +384,7 @@ def get_final_path(closed, ngoal, nstart, config):
|
||||
ry.extend(list(reversed(n.ylist)))
|
||||
ryaw.extend(list(reversed(n.yawlist)))
|
||||
direction.extend(list(reversed(n.directions)))
|
||||
|
||||
|
||||
nid = n.pind
|
||||
|
||||
rx = list(reversed(rx))
|
||||
@@ -391,10 +402,10 @@ def get_final_path(closed, ngoal, nstart, config):
|
||||
|
||||
def verify_index(node, c):
|
||||
xind, yind = node.xind, node.yind
|
||||
if xind>=c.minx and xind<=c.maxx and yind >=c.miny \
|
||||
and yind<=c.maxy:
|
||||
if xind >= c.minx and xind <= c.maxx and yind >= c.miny \
|
||||
and yind <= c.maxy:
|
||||
return True
|
||||
|
||||
|
||||
return False
|
||||
|
||||
|
||||
@@ -445,7 +456,7 @@ def main():
|
||||
|
||||
path = hybrid_a_star_planning(
|
||||
start, goal, ox, oy, XY_GRID_RESOLUTION, YAW_GRID_RESOLUTION)
|
||||
|
||||
|
||||
# plot_car(*start)
|
||||
# plot_car(*goal)
|
||||
x = path.xlist
|
||||
|
||||
Reference in New Issue
Block a user