From 2723a0a2eb3c8ccddf5810aa9380f1f9276caabd Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Thu, 19 Oct 2017 12:31:19 -0700 Subject: [PATCH] fix bug of reedsshepppath --- PathPlanning/CRRRTStar/cr_rrt_star_car.py | 2 +- PathPlanning/DubinsPath/dubins_path_planning.py | 4 ++-- .../RRTStarCar_reeds_sheep/reeds_shepp_path_planning.py | 8 ++++---- PathPlanning/RRTStarCar_reeds_sheep/rrt_star_car.py | 5 +++-- 4 files changed, 10 insertions(+), 9 deletions(-) diff --git a/PathPlanning/CRRRTStar/cr_rrt_star_car.py b/PathPlanning/CRRRTStar/cr_rrt_star_car.py index a4abe43c..35c435c5 100644 --- a/PathPlanning/CRRRTStar/cr_rrt_star_car.py +++ b/PathPlanning/CRRRTStar/cr_rrt_star_car.py @@ -349,7 +349,7 @@ class RRT(): for (ox, oy, size) in obstacleList: plt.plot(ox, oy, "ok", ms=30 * size) - reeds_shepp_path_planning.plot_arrow( + reeds_shepp_path_planning.plot_arro/Users/atsushisakai/Dropbox/Program/berkeley/RacerCourseOptimization/cvxpy/stanford/mpc_path_planner_common.pyw( self.start.x, self.start.y, self.start.yaw) reeds_shepp_path_planning.plot_arrow( self.end.x, self.end.y, self.end.yaw) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index a5bc8bbd..ba91360f 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -132,11 +132,11 @@ def LRL(alpha, beta, d): c_ab = math.cos(alpha - beta) mode = ["L", "R", "L"] - tmp_lrl = (6. - d * d + 2 * c_ab + 2 * d * (- sa + sb)) / 8. + tmp_lrl = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (- sa + sb)) / 8.0 if abs(tmp_lrl) > 1: return None, None, None, mode p = mod2pi(2 * math.pi - math.acos(tmp_lrl)) - t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.) + t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0) q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p)) return t, p, q, mode diff --git a/PathPlanning/RRTStarCar_reeds_sheep/reeds_shepp_path_planning.py b/PathPlanning/RRTStarCar_reeds_sheep/reeds_shepp_path_planning.py index d8463307..25ce47a3 100644 --- a/PathPlanning/RRTStarCar_reeds_sheep/reeds_shepp_path_planning.py +++ b/PathPlanning/RRTStarCar_reeds_sheep/reeds_shepp_path_planning.py @@ -30,10 +30,10 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): def reeds_shepp_path_planning(start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature): + step_size = 0.1 q0 = [start_x, start_y, start_yaw] q1 = [end_x, end_y, end_yaw] - step_size = 0.1 - qs = reeds_shepp.path_sample(q0, q1, curvature, step_size) + qs = reeds_shepp.path_sample(q0, q1, 1.0 / curvature, step_size) xs = [q[0] for q in qs] ys = [q[1] for q in qs] yaw = [q[2] for q in qs] @@ -42,8 +42,8 @@ def reeds_shepp_path_planning(start_x, start_y, start_yaw, ys.append(end_y) yaw.append(end_yaw) - clen = reeds_shepp.path_length(q0, q1, curvature) - pathtypeTuple = reeds_shepp.path_type(q0, q1, curvature) + clen = reeds_shepp.path_length(q0, q1, 1.0 / curvature) + pathtypeTuple = reeds_shepp.path_type(q0, q1, 1.0 / curvature) ptype = "" for t in pathtypeTuple: diff --git a/PathPlanning/RRTStarCar_reeds_sheep/rrt_star_car.py b/PathPlanning/RRTStarCar_reeds_sheep/rrt_star_car.py index bd0666a4..4dfc9322 100644 --- a/PathPlanning/RRTStarCar_reeds_sheep/rrt_star_car.py +++ b/PathPlanning/RRTStarCar_reeds_sheep/rrt_star_car.py @@ -22,7 +22,7 @@ class RRT(): """ def __init__(self, start, goal, obstacleList, randArea, - goalSampleRate=10, maxIter=2000): + goalSampleRate=10, maxIter=1000): u""" Setting Parameter @@ -48,6 +48,7 @@ class RRT(): self.nodeList = [self.start] for i in range(self.maxIter): + print(i) rnd = self.get_random_point() nind = self.GetNearestListIndex(self.nodeList, rnd) @@ -317,4 +318,4 @@ if __name__ == '__main__': plt.show() - matplotrecorder.save_movie("animation.gif", 0.1) + # matplotrecorder.save_movie("animation.gif", 0.1)