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

* Update rrt_star_reeds_shepp.py to fix AtsushiSakai#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 AtsushiSakai#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

Author: RyderCRD

PathPlanning/RRTStar/rrt_star.py

@@ -225,13 +225,11 @@ def rewire(self, new_node, near_inds):
             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)

PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py

@@ -36,7 +36,7 @@ def __init__(self, x, y, yaw):
             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 @@ def __init__(self, start, goal, obstacle_list, rand_area,
         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 @@ def __init__(self, start, goal, obstacle_list, rand_area,
         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 @@ def plot_start_goal_arrow(self):
     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 @@ def steer(self, from_node, to_node):
     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")
 

PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py

@@ -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

tests/test_rrt_star_reeds_shepp.py

@@ -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__)