def neighbors(self, p):
#self.step = 3.0
# Path의 Resolution을 나타내는 Step --> 낮을수록 정확하고 느림 but 높을수록 빠름 but 발산 가능성 높아짐!!
paths = [['l', self.step / self._r], ['s', self.step], ['r', self.step / self._r],
['l', -self.step / self._r], ['s', -self.step], ['r', -self.step / self._r]]
for path in paths:
#section이 True면 커브와 직선을 나누어서 데이터를 받고 False면 한꺼번에 하나의 list로 받음
xs, ys, yaws = ReedsSheppPath.gen_path(p, [path], r=self._r, section=False)
if not self.is_collision_rs_car(xs, ys, yaws):
yield (round(xs[-1], 1), round(ys[-1], 1), round(yaws[-1], 1)), path, [xs, ys, yaws]
def steer(start, end, radius):
if local_planner_choice == 1:
lp = Dubins(radius, dubins_resolution)
# flag = 0
path_list = lp.all_options(
start, end
) # getting sorted list of all possible 6 dubins curves between start and end point
opt_path = path_list[
0] # choosing the shortest path that does not collide
path = lp.generate_points(
start, end, opt_path[1],
opt_path[2]) # Generating intermediate path points
# Collision check
for point in path:
if collision(point) == True:
return [], [], [], [], False
# flag += 1
# break
# if flag==0:
return path, opt_path[0], opt_path[1], opt_path[2], True
elif local_planner_choice == 2:
rspath = ReedsSheppPath(start, end, radius)
rspath.calc_paths()
opt1, length = rspath.get_shortest_path()
path = generate_pts(start, opt1, radius)
if path:
o1, o2 = make_opt(opt1)
# zip check / path check:
# Collision check
for point in path:
if collision(point) == True:
return [], [], [], [], False
# flag += 1
# break
# if flag==0:
return path, length, o1, o2, True
else:
return [], [], [], [], False
def neighbors(self, p):
step = 5.0
paths = [['l', step / self._r], ['s', step], ['r', step / self._r],
['l', -step / self._r], ['s', -step], ['r', -step / self._r]]
for path in paths:
xs, ys, yaws = ReedsSheppPath.gen_path(p, [path],
r=self._r,
section=False)
if not self.is_collision_rs_car(xs, ys, yaws):
yield (round(xs[-1],
2), round(ys[-1],
2), round(yaws[-1],
2)), path, [xs, ys, yaws]
def run(self, display=False):
d = self.h_cost(self._s)
self._openset[self._s] = {
'g': 0,
'h': d,
'f': d,
'camefrom': None,
'path': []
}
while self._openset:
x = min(self._openset, key=lambda key: self._openset[key]['f'])
self._closeset[x] = deepcopy(self._openset[x])
del self._openset[x]
if display:
self._map_info.close = (x[0], x[1])
rspath = ReedsSheppPath(x, self._e, self._r)
rspath.calc_paths()
path, _ = rspath.get_shortest_path()
xs, ys, yaws = ReedsSheppPath.gen_path(x,
path,
self._r,
section=False)
if not self.is_collision_rs_car(xs, ys, yaws):
self._closeset[self._e] = {
'camefrom': x,
'path': [path, [xs, ys, yaws]]
}
return True
for y, path, line in self.neighbors(x):
if y in self._closeset:
continue
if display:
self._map_info.open = (y[0], y[1])
tentative_g_score = self._closeset[x]['g'] + (abs(
path[1]) if path[0] == 's' else abs(path[1]) * self._r)
if y not in self._openset:
tentative_is_better = True
elif tentative_g_score < self._openset[y]['g']:
tentative_is_better = True
else:
tentative_is_better = False
if tentative_is_better:
d = self.h_cost(y)
self._openset[y] = {
'g': tentative_g_score,
'h': d,
'f': tentative_g_score + d,
'camefrom': x,
'path': [path, line]
}
return False
self._outline_x = [tail_l_x, tail_r_x, head_r_x, head_l_x, tail_l_x]
self._outline_y = [tail_l_y, tail_r_y, head_r_y, head_l_y, tail_l_y]
return self._outline_x, self._outline_y
def show(self):
self.get_outline()
draw_point(self._pos, self._l / 2.0)
plt.plot(self._outline_x, self._outline_y, color='black')
if __name__ == "__main__":
car = Car(2.0, 1.0)
plt.figure()
start = [1, 1, math.pi / 3]
end = [5, 10, -math.pi / 2]
r = 5.0
rspath = ReedsSheppPath(start, end, r)
rspath.calc_paths()
path, _ = rspath.get_shortest_path()
print path
xs, ys, yaws = ReedsSheppPath.gen_path(start, path, r, section=False)
for i in range(len(xs)):
plt.clf()
draw_point(start)
draw_point(end)
plt.axis("equal")
plt.plot(xs, ys)
car.set_position([xs[i], ys[i], yaws[i]])
car.show()
plt.pause(0.1)
plt.show()
def run(self, display=False):
st = time.time()
epsilon = 1.5
d = self.h_cost(self._s)
#openset ==> g: 현재노드 코스트 h : 휴리스틱 cost f = g+h camefrom = 부모노드 path = 부모노드에서 현재 노드로 어떻게 왔는지
#self._openset[self._s] = {'g': 0, 'h': d, 'f': d, 'camefrom':None, 'path': []}
self._openset[self._s] = {
'g': 0,
'h': d,
'f': d,
'camefrom': None,
'path': [],
'depth': 0
}
while self._openset:
#f 값이 가장 작은 openset 원소 == cost가 가장 낮은!
x = min(self._openset, key=lambda key: self._openset[key]['f'])
self._closeset[x] = self._openset.pop(x)
#x를 pop 한 이후 close set에 넣어서 visited node 확인
if display:
self._map_info.close = (x[0], x[1])
rspath = ReedsSheppPath(x, self._e, self._r)
#x(좌표)를 시작점으로 하는 reed 클래스를 생성
rspath.calc_paths()
path, _ = rspath.get_shortest_path()
#reed 클래스를 통해 가장 짧은 경로를 Dubins path 로 탐색
xs, ys, yaws = ReedsSheppPath.gen_path(x,
path,
self._r,
section=False,
step_size=self.step)
#현재 차의 방향과 도착지점의 방향이 같을때 직선 경로를 생성하는 코드 추가
#현재 좌표에서 goal까지 직선 이동이 가능할 때 직선으로 이동
if x[2] == self._e[2]:
length = self.distance(x, self._e)
pat = ['s', length]
xx, yy, yaws2 = ReedsSheppPath.gen_path(x, [pat],
self._r,
section=False)
if (xx[-1] == self._e[0] and yy[-1] == self._e[1]):
if not self.is_collision_rs_car(xx, yy, yaws2):
self._closeset[self._e] = {
'camefrom': x,
'path': [pat, [xx, yy, yaws2]]
}
print("time:", time.time() - st)
print("depth:", self._closeset[x]['depth'])
return True
# 가장 짧은 Dubins path의 경로를 좌표경로로 반환
if not self.is_collision_rs_car(xs, ys, yaws):
#xs,ys,yaws 의 path 좌표가 충돌을 안한다면 현재 path로 결정
self._closeset[self._e] = {
'camefrom': x,
'path': [path, [xs, ys, yaws]]
}
print("time:", time.time() - st)
print("depth:", self._closeset[x]['depth'])
return True
for y, path, line in self.neighbors(x):
# 골 지점에 도착하지 않았다면 현재 좌표 x에서 neighbors 를 통해 자식 노드를 Span
# STEP 사이즈에 따라 임의의 end point를 만들어서 진행 --> neighbor에서 총 6가지의 방법으로 span (l,s,r,-r,-s,-l)
if display:
self._map_info.open = (y[0], y[1])
#g = parent's g + x에서 현재까지 주행한 거리
tentative_g_score = self._closeset[x]['g'] + (abs(
path[1]) if path[0] == 's' else abs(path[1]) * self._r)
if y in self._closeset:
continue
if y not in self._openset:
tentative_is_better = True
elif tentative_g_score < self._openset[y]['g']:
tentative_is_better = True
else:
tentative_is_better = False
if tentative_is_better:
# epsilon * h --> high quality
d = epsilon * self.h_cost(y)
'''
if self._closeset[x]['depth'] < d:
w = 1 - self._closeset[x]['depth']/d
else:
w = 0
d = (1+epsilon*w)*d
'''
#self._openset[y] = {'g': tentative_g_score, 'h': d, 'f': tentative_g_score+d, 'camefrom': x, 'path': [path, line]}
self._openset[y] = {
'g': tentative_g_score,
'h': d,
'f': tentative_g_score + d,
'camefrom': x,
'path': [path, line],
'depth': self._closeset[x]['depth'] + 1
}
return False