mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
80124f20e8
* 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
49 lines
1.6 KiB
Python
49 lines
1.6 KiB
Python
import conftest # Add root path to sys.path
|
|
from PathPlanning.RRTStarReedsShepp import rrt_star_reeds_shepp as m
|
|
|
|
|
|
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__)
|