github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

Fix the rewire() of rrt* path planning (#550) (#812)

Browse Source 
* Update rrt_star_reeds_shepp.py to fix #550

Add step_size attribute to RRTStarReedsShepp, and a method set_random_seed() to set the random seed, with two test cases.

* Update rewire() of rrt_star.py

Update rewire() of rrt_star.py to fix #550.  Since the old version didn't assign path_yaw of edge_node to near_node, the old rewire() doesn't fit rrt_star_reeds_shepp.py

* Update reeds_shepp_path_planning.py

Limit the range of phi for the sake of planning speed, and simplify the the calculation process in straight_left_straight().

* Update reeds_shepp_path_planning.py

* Remove unnecessary cost calculation

Cost of edge_node is calculated in line 221, and self.node_list[i] is replaced to edge_node, so no need to update.

* Update reeds_shepp_path_planning.py
This commit is contained in:
RyderCRD
2023-04-27 22:04:39 +08:00
committed by GitHub
parent 28004c7b9c
commit 80124f20e8
4 changed files with 56 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
PathPlanning/RRTStar/rrt_star.py
View File

@@ -225,13 +225,11 @@ class RRTStar(RRT):
improved_cost = near_node.cost > edge_node.cost
if no_collision and improved_cost:
near_node.x = edge_node.x
near_node.y = edge_node.y
near_node.cost = edge_node.cost
near_node.path_x = edge_node.path_x
near_node.path_y = edge_node.path_y
near_node.parent = edge_node.parent
self.propagate_cost_to_leaves(new_node)
for node in self.node_list:
if node.parent == self.node_list[i]:
node.parent = edge_node
self.node_list[i] = edge_node
self.propagate_cost_to_leaves(self.node_list[i])
def calc_new_cost(self, from_node, to_node):
d, _ = self.calc_distance_and_angle(from_node, to_node)

14
PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py
View File

@@ -36,7 +36,7 @@ class RRTStarReedsShepp(RRTStar):
self.path_yaw = []
def __init__(self, start, goal, obstacle_list, rand_area,
max_iter=200,
max_iter=200, step_size=0.2,
connect_circle_dist=50.0,
robot_radius=0.0
):
@@ -55,6 +55,7 @@ class RRTStarReedsShepp(RRTStar):
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.max_iter = max_iter
self.step_size = step_size
self.obstacle_list = obstacle_list
self.connect_circle_dist = connect_circle_dist
self.robot_radius = robot_radius
@@ -63,6 +64,9 @@ class RRTStarReedsShepp(RRTStar):
self.goal_yaw_th = np.deg2rad(1.0)
self.goal_xy_th = 0.5
def set_random_seed(self, seed):
random.seed(seed)
def planning(self, animation=True, search_until_max_iter=True):
"""
planning
@@ -147,8 +151,8 @@ class RRTStarReedsShepp(RRTStar):
def steer(self, from_node, to_node):
px, py, pyaw, mode, course_lengths = reeds_shepp_path_planning.reeds_shepp_path_planning(
from_node.x, from_node.y, from_node.yaw,
to_node.x, to_node.y, to_node.yaw, self.curvature)
from_node.x, from_node.y, from_node.yaw, to_node.x,
to_node.y, to_node.yaw, self.curvature, self.step_size)
if not px:
return None
@@ -169,8 +173,8 @@ class RRTStarReedsShepp(RRTStar):
def calc_new_cost(self, from_node, to_node):
_, _, _, _, course_lengths = reeds_shepp_path_planning.reeds_shepp_path_planning(
from_node.x, from_node.y, from_node.yaw,
to_node.x, to_node.y, to_node.yaw, self.curvature)
from_node.x, from_node.y, from_node.yaw, to_node.x,
to_node.y, to_node.yaw, self.curvature, self.step_size)
if not course_lengths:
return float("inf")

13
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View File

@@ -53,17 +53,14 @@ def mod2pi(x):
def straight_left_straight(x, y, phi):
phi = mod2pi(phi)
if y > 0.0 and 0.0 < phi < math.pi * 0.99:
# only take phi in (0.01*math.pi, 0.99*math.pi) for the sake of speed.
# phi in (0, 0.01*math.pi) will make test2() in test_rrt_star_reeds_shepp.py
# extremely time-consuming, since the value of xd, t will be very large.
if math.pi * 0.01 < phi < math.pi * 0.99 and y != 0:
xd = - y / math.tan(phi) + x
t = xd - math.tan(phi / 2.0)
u = phi
v = math.hypot(x - xd, y) - math.tan(phi / 2.0)
return True, t, u, v
elif y < 0.0 < phi < math.pi * 0.99:
xd = - y / math.tan(phi) + x
t = xd - math.tan(phi / 2.0)
u = phi
v = -math.hypot(x - xd, y) - math.tan(phi / 2.0)
v = np.sign(y) * math.hypot(x - xd, y) - math.tan(phi / 2.0)
return True, t, u, v
return False, 0.0, 0.0, 0.0

37
tests/test_rrt_star_reeds_shepp.py
View File

@@ -6,6 +6,43 @@ def test1():
m.show_animation = False
m.main(max_iter=5)
obstacleList = [
(5, 5, 1),
(4, 6, 1),
(4, 8, 1),
(4, 10, 1),
(6, 5, 1),
(7, 5, 1),
(8, 6, 1),
(8, 8, 1),
(8, 10, 1)
] # [x,y,size(radius)]
start = [0.0, 0.0, m.np.deg2rad(0.0)]
goal = [6.0, 7.0, m.np.deg2rad(90.0)]
def test2():
step_size = 0.2
rrt_star_reeds_shepp = m.RRTStarReedsShepp(start, goal,
obstacleList, [-2.0, 15.0],
max_iter=100, step_size=step_size)
rrt_star_reeds_shepp.set_random_seed(seed=8)
path = rrt_star_reeds_shepp.planning(animation=False)
for i in range(len(path)-1):
# + 0.00000000000001 for acceptable errors arising from the planning process
assert m.math.dist(path[i][0:2], path[i+1][0:2]) < step_size + 0.00000000000001
def test3():
step_size = 20
rrt_star_reeds_shepp = m.RRTStarReedsShepp(start, goal,
obstacleList, [-2.0, 15.0],
max_iter=100, step_size=step_size)
rrt_star_reeds_shepp.set_random_seed(seed=8)
path = rrt_star_reeds_shepp.planning(animation=False)
for i in range(len(path)-1):
# + 0.00000000000001 for acceptable errors arising from the planning process
assert m.math.dist(path[i][0:2], path[i+1][0:2]) < step_size + 0.00000000000001
if __name__ == '__main__':
conftest.run_this_test(__file__)