diff --git a/roboticstoolbox/mobile/drivers.py b/roboticstoolbox/mobile/drivers.py index 12caa399..1febf6a7 100644 --- a/roboticstoolbox/mobile/drivers.py +++ b/roboticstoolbox/mobile/drivers.py @@ -301,21 +301,91 @@ def _new_goal(self): class PurePursuit(VehicleDriverBase): - def __init__(self, speed=1, radius=1): - pass + def __init__(self, path, lookahead=1, linemarkerstyle=None, **kwargs): + """ + Driving agent for pure pursuit + + :param path: path as a set of waypoints + :type path: array_like(2,n) one column per point, optional + :param lookahead: lookahead distance, defaults to 1 + :type lookahead: float, optional + """ + super().__init__(**kwargs) + self._path = path + self._lookahead = lookahead + self._linemarkerstyle = linemarkerstyle def __str__(self): - pass + """Convert to string - def init(self): - pass + :return: driver parameters and state in in a compact human readable format + :rtype: str + """ + + s = "PurePursuit driver object\n" + s += ( + f" X {self._workspace[0]} : {self._workspace[1]}; Y {self._workspace[0]} :" + f" {self._workspace[1]}, lookahead={self._lookahead}\n" + ) + return s + + def init(self, ax=None): + if ax is not None: + h = plt.plot( + self._path[0, :], + self._path[1, :], + **self._linemarkerstyle, + label="waypoints", + )[0] + self._current_waypint = None def demand(self): - pass + + cp = self._veh._x[0:2] # current point + + # find closest point on the path to current point + d = np.linalg.norm(self._path.T - cp, axis=1) # rely on implicit expansion + i = np.argmin(d) + + # find all points on the path at least lookahead distance away + (k,) = np.where(d[i + 1 :] >= self._lookahead) # find all points beyond horizon + if len(k) == 0: + # no such points, we must be near the end, goal is the end + goal = self._path[:, -1] + k = -1 + else: + # many such points, take the first one + k = k[0] + 1 # first point beyond look ahead distance + goal = self._path[:, k + i] + k += i + + if self._waypoint_marker is not None: + pass + for marker in self._waypoint_marker: + marker.set_color("blue") + self._waypoint_marker[k].set_color("red") + + return self.driveto(goal) # ========================================================================= # if __name__ == "__main__": - import unittest + # import unittest + + import roboticstoolbox as rtb + + # veh = rtb.Bicycle(control=rtb.RandomPath(workspace=10), animation="car") + # print(veh.control) + # veh.run(20, animate=True) + + # create the path + path = np.array([[10.0, 10], [10, 60], [80, 80], [50, 10]]).T + veh = rtb.Bicycle( + control=rtb.PurePursuit(path, speed=4, lookahead=5, workspace=[0, 100]), + animation="car", + ) + print(veh) + print(veh.control) + veh.run(60, animate=True)