Auto-Research/detect.py at main · AutoGolfCart/Auto-Research · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
import numpy as np

track_width = 2.0  # wheel center to center distance of car
forward_constant = 1.0  # multiplier for speed of car, adjust for proper braking distance
car_length = 6.0  # length of car from front to back wheels, center to center

graph = True

if graph:
    import matplotlib.pyplot as plt
    from matplotlib.patches import Rectangle
    from matplotlib.patches import Arc


def in_path(points: np.ndarray, speed: float, angle: float) -> bool:
    """
    Given an array of x, y points, the speed of the cart, and the front steering angle, returns whether or not the points are in the cart's predicted path.
    :param np.ndarray points: shape (n, 2) and dtype float64 - Array of x, y data points returned from the LIDAR scanner.
    :param float speed: Speed of the golf cart
    :param float angle: float - Steering angle of the golf cart, in degrees. 0 is straight, positive is left and negative is right
    :return: Boolean - whether there are any data points in the cart's predicted path
    """
    # workaround for angle of 0
    if angle < 1e-4:
        angle = 1e-4

    r_center = car_length * np.tan(np.radians(90 - angle))  # left turn = positive angle

    # transform points to match new origin at turn center
    points[:, 0] += r_center
    points[:, 1] += car_length

    r_cf = np.hypot(r_center, car_length)  # front center radius

    r_left = np.hypot(r_center - track_width / 2, car_length)  # left wheel turn radius
    r_right = np.hypot(r_center + track_width / 2, car_length)  # right wheel turn radius

    y_max = car_length + forward_constant * speed

    # check if y_max is past the turning circle
    y_large = np.minimum(np.fabs(r_left), np.fabs(r_right)) < y_max
    if y_large:
        x_min = r_center - track_width / 2 if angle < 0 else 0
        x_max = r_center + track_width / 2 if angle > 0 else 0
    else:
        x_min = r_center - track_width / 2 if angle < 0 else \
            np.sqrt(
                np.power(r_left, 2) - np.power(y_max, 2)
            )
        x_max = r_center + track_width / 2 if angle > 0 else \
            -np.sqrt(
                np.power(r_right, 2) - np.power(y_max, 2)
            )

    if graph:
        fig, ax = plt.subplots()
        # ax.plot(0, 0, label='Turn Center')
        ax.add_patch(Rectangle((r_center - track_width / 2, 0),
                               track_width, car_length))
        # check if y_max is past the turning circle
        if y_large:
            x1 = np.linspace(r_center - track_width / 2, 0, 100)  # left boundary
            x2 = np.linspace(r_center + track_width / 2, 0, 100)  # right boundary
        else:
            if angle < 0:
                x1 = np.linspace(r_center - track_width / 2,
                                 -np.sqrt(
                                     np.power(r_left, 2) - np.power(y_max, 2)
                                 ),
                                 100)  # left boundary
                x2 = np.linspace(r_center + track_width / 2,
                                 -np.sqrt(
                                     np.power(r_right, 2) - np.power(y_max, 2)
                                 ),
                                 100)  # right boundary
            else:
                x1 = np.linspace(r_center - track_width / 2,
                                 np.sqrt(
                                     np.power(r_left, 2) - np.power(y_max, 2)
                                 ),
                                 100)  # left boundary
                x2 = np.linspace(r_center + track_width / 2,
                                 np.sqrt(
                                     np.power(r_right, 2) - np.power(y_max, 2)
                                 ),
                                 100)  # right boundary
        y1 = np.sqrt(np.power(r_left, 2) - np.power(x1, 2))
        y2 = np.sqrt(np.power(r_right, 2) - np.power(x2, 2))
        ax.plot(x1, y1)
        ax.plot(x2, y2)
        if not y_large:
            if angle < 0:
                ax.plot(
                    (-np.sqrt(np.power(r_left, 2) - np.power(y_max, 2)), -np.sqrt(np.power(r_right, 2) - np.power(y_max, 2))),
                    (y_max, y_max))
            else:
                ax.plot(
                    (np.sqrt(np.power(r_left, 2) - np.power(y_max, 2)), np.sqrt(np.power(r_right, 2) - np.power(y_max, 2))),
                    (y_max, y_max))
        ax.scatter(points[:, 0], points[:, 1])
        plt.show()
        # plt.savefig('plt.png')

    # filter points to x range
    points = points[(points[:, 0] <= x_max) & (points[:, 0] >= x_min)]

    # check points
    if angle < 0:
        return np.any(points[
                          (np.hypot(points[:, 0], points[:, 1]) > r_right)
                          &
                          (np.hypot(points[:, 0], points[:, 1]) < r_left)
                          &
                          (points[:, 1] > car_length)
                          &
                          (points[:, 1] < y_max)
                          ])
    else:
        return np.any(points[
                          (np.hypot(points[:, 0], points[:, 1]) < r_right)
                          &
                          (np.hypot(points[:, 0], points[:, 1]) > r_left)
                          &
                          (points[:, 1] > car_length)
                          &
                          (points[:, 1] < y_max)
                          ])

# test

print(in_path(np.asarray([[2, .5], [1, 1]], dtype=np.float64), 5,30))