diff --git a/Mapping/rectangle_fitting/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py index 27c14594..6334178e 100644 --- a/Mapping/rectangle_fitting/rectangle_fitting.py +++ b/Mapping/rectangle_fitting/rectangle_fitting.py @@ -42,10 +42,10 @@ class VehicleSimulator(): plt.plot(gx, gy, "-xr") def calc_global_contour(self): - gx = [(ix * math.cos(self.yaw) + iy * math.sin(self.yaw)) + - self.x for (ix, iy) in zip(self.vc_x, self.vc_y)] - gy = [(ix * math.sin(self.yaw) - iy * math.cos(self.yaw)) + - self.y for (ix, iy) in zip(self.vc_x, self.vc_y)] + gx = [(ix * math.cos(self.yaw) + iy * math.sin(self.yaw)) + + self.x for (ix, iy) in zip(self.vc_x, self.vc_y)] + gy = [(ix * math.sin(self.yaw) - iy * math.cos(self.yaw)) + + self.y for (ix, iy) in zip(self.vc_x, self.vc_y)] return gx, gy @@ -131,6 +131,40 @@ def ray_casting_filter(xl, yl, thetal, rangel, angle_reso): return rx, ry +def adoptive_range_segmentation(ox, oy): + + S = [] + + checked = [False] * len(ox) + R = 5.0 + + for i, _ in enumerate(ox): + if checked[i]: + continue + C = [] + r = R + for j, _ in enumerate(ox): + d = math.sqrt((ox[i] - ox[j])**2 + (oy[i] - oy[j])**2) + if d <= r: + C.append(j) + checked[j] = True + S.append(C) + + # Merge claster + fS = [] + for k, _ in enumerate(S): + for l, _ in enumerate(S): + if k == l: + continue + + for k, _ in enumerate(S[k]): + + print(S) + input() + + return S + + def main(): # simulation parameters @@ -153,6 +187,9 @@ def main(): ox, oy = get_observation_points([v1, v2], angle_reso) + # step1: Adaptive Range Segmentation + ids = adoptive_range_segmentation(ox, oy) + if show_animation: # pragma: no cover plt.cla() plt.axis("equal") diff --git a/PathPlanning/RRT/simple_rrt.py b/PathPlanning/RRT/simple_rrt.py index 0d801b27..4e33524d 100644 --- a/PathPlanning/RRT/simple_rrt.py +++ b/PathPlanning/RRT/simple_rrt.py @@ -94,7 +94,7 @@ class RRT(): return path - def DrawGraph(self, rnd=None): + def DrawGraph(self, rnd=None): # pragma: no cover """ Draw Graph """ @@ -162,7 +162,7 @@ def main(): path = rrt.Planning(animation=show_animation) # Draw final path - if show_animation: + if show_animation: # pragma: no cover rrt.DrawGraph() plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') plt.grid(True) diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index ee677d3d..285d9404 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -1,6 +1,3 @@ -import cubic_spline_planner -import matplotlib.pyplot as plt -import math """ Path tracking simulation with rear wheel feedback steering control and PID speed control. @@ -8,9 +5,17 @@ Path tracking simulation with rear wheel feedback steering control and PID speed author: Atsushi Sakai(@Atsushi_twi) """ +import matplotlib.pyplot as plt +import math +import numpy as np import sys sys.path.append("../../PathPlanning/CubicSpline/") +try: + import cubic_spline_planner +except: + raise + Kp = 1.0 # speed propotional gain # steering control parameter