iterations=smoothing_iterations)
return smoothed_path
if __name__ == '__main__':
import numpy as np
import matplotlib.pyplot as plt
import robot_sim.robots.xybot.xybot as xyb
# ====Search Path with RRT====
obstacle_list = [((5, 5), 3), ((3, 6), 3), ((3, 8), 3), ((3, 10), 3),
((7, 5), 3), ((9, 5), 3), ((10, 5), 3), ((10, 0), 3),
((10, -2), 3), ((10, -4), 3), ((0, 12), 3), ((-1, 10), 3),
((-2, 8), 3)] # [x,y,size]
# Set Initial parameters
robot = xyb.XYBot()
rrtc = RRTConnect(robot)
# path = rrtc.plan(seed_jnt_values=np.array([0, 0]), end_conf=np.array([5, 10]), obstacle_list=obstacle_list,
# ext_dist=1, rand_rate=70, max_time=300, hnd_name=None, animation=True)
# plt.show()
# nx.draw(rrt.roadmap, with_labels=True, font_weight='bold')
# plt.show()
import time
total_t = 0
for i in range(100):
tic = time.time()
path = rrtc.plan(start_conf=np.array([0, 0]),
goal_conf=np.array([5, 10]),
obstacle_list=obstacle_list,
ext_dist=1,
rand_rate=70,
iterations=smoothing_iterations,
animation=animation)
return smoothed_path
else:
print("Reach to maximum iteration! Failed to find a path.")
return None
if __name__ == '__main__':
import robot_sim.robots.xybot.xybot as xyb
# ====Search Path with RRT====
obstacle_list = [((5, 5), 3), ((3, 6), 3), ((3, 8), 3), ((3, 10), 3),
((7, 5), 3), ((9, 5), 3), ((10, 5), 3)] # [x,y,size]
# Set Initial parameters
robot_s = xyb.XYBot()
rrtstar_s = RRTStar(robot_s, nearby_ratio=2)
path = rrtstar_s.plan(start_conf=np.array([0, 0]),
goal_conf=np.array([6, 9]),
obstacle_list=obstacle_list,
ext_dist=1,
rand_rate=70,
max_time=300,
component_name='all',
smoothing_iterations=0,
animation=True)
# plt.show()
# nx.draw(rrt.roadmap, with_labels=True, font_weight='bold')
# plt.show()
# import time
# total_t = 0