The Code

View Pure Pursuit code on GitHub

Here we find the lookahead point using the intersection of the circle and the path. We use the "discriminant" to tell the amount of intersection points(if any) with the circle. 

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
     def find_lookahead_point(self): # finding the next lookahead point
        robot_position = self.jackal_robot.localize() # localizes properly
        for i in range(self.current_index, len(self.path) - 1):
            start, end = self.path[i], self.path[i + 1] # looking in the line between waypoints
            # Compute quadratic coefficients for lookahead
            dx, dy = end[0] - start[0], end[1] - start[1]
            fx, fy = start[0] - robot_position[0], start[1] - robot_position[1]
            a, b, c = dx**2 + dy**2, 2 * (fx * dx + fy * dy), fx**2 + fy**2 - self.lookahead**2
            discriminant = b**2 - 4 * a * c

            if discriminant >= 0: # finding and returning intersection/updating the points crossed to make sure it doesn't go backwards
                discriminant_sqrt = math.sqrt(discriminant)
                t1 = (-b + discriminant_sqrt) / (2 * a)
                t2 = (-b - discriminant_sqrt) / (2 * a)

                if 0 <= t1 <= 1:
                    self.current_index = i  # Update current index
                    return (start[0] + t1 * dx, start[1] + t1 * dy)
                if 0 <= t2 <= 1:
                    self.current_index = i  # Update current index
                    return (start[0] + t2 * dx, start[1] + t2 * dy)

        return self.path[-1] # returns last point if nothing found(shouldnt ever occur but fault tolerance)

View More Pure Pursuit code (simulation) on GitHub

In this file we find the lookahead point and utilize PID to make the robot head toward the lookahead point found using the previous code. 

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
  goal=self.follower.find_lookahead_point()
                self.x_goal=goal[0]
                self.y_goal=goal[1]


                # showing the lookahead intersection point
                p.addUserDebugLine([self.x_goal,self.y_goal-0.1,0.05],
                                    [self.x_goal,self.y_goal+0.1 , 0.05],
                                    lineColorRGB=[1, 0, 0], 
                                    lineWidth=300)

                self.x_error=self.x_goal-self.current_x
                self.y_error=self.y_goal-self.current_y
                self.h_error=self.angle_wrap(math.atan2(self.y_error,self.x_error)-self.current_h)

                # calculate with pid for the velocity to go to the lookahead
                self.linear_velocity=self.linear_pid.calculateVelocity(self.x_error)
                self.angular_velocity=self.angular_pid.calculateVelocity(self.h_error)

                # setting the velocities
                self.jackal_robot.inverse_kinematics(self.linear_velocity,self.angular_velocity)
                self.jackal_robot.setVelocity()
Previous