Proper implementation of `pure_pursuit` for ground robots using waypoints

Question

I am trying to understand and implement pure-pursuit waypoint following in 2D map. My questions are:

1. What exactly do I need to record in my waypoint.csv file?

To get some idea, I was looking at this waypoint_logger example where a single waypoint is defined in the form of

{  data.pose.pose.position.x,
   data.pose.pose.position.y,
   euler[2],
   speed  }

where the first two components are 2D pose from robot odometry, the third component includes 3rd euler component from euler_from_quaternion conversion of odometry orientation and last one represents the velocity (from odometry twist component).

2. How to implement pure_pursuit based on recorded waypoints?

Again looking at examples, this implementation (if I understood correctly) requires waypoints to be in the form of

{   data.pose.pose.position.x
    data.pose.pose.position.y  
    data.pose.pose.orientation.z
    data.pose.pose.orientation.w  }

Another implementation of pure_pursuit requires nav_msgs::Odometry odom and nav_msgs::Path path from the robot in order to construct ackermann_msgs. Neither use velocity/speed, instead need other information.

Any reference/guide is appreciated.

Answer 1

Is pure pursuit something like this?

Here my agent is chasing a moving target that has 60-70% of its velocity. The agent is given only the information of the present position of the robot. So if you are pursuing a string of recorded waypoints, maybe my method can help. At every instant, the robot looks for its heading correction. If the heading is within a threshold, then nothing happens. Otherwise, the agent corrects itself and proceeds along that heading till it reaches an endpoint. So the controller looks something like this.

You can publish your recorded waypoints to a topic that gets updated at an interval then your agent can pursue that target until it gets a new target and pursues it. You can define a target threshold that whenever the agent reaches within the target position, it stops. You will not need velocity and yaw for this. You will just be needing the position setpoints.

Here's the code for the above implementation.

#!/usr/bin/env python

# This Node Controls and Navigates the robot to a given goal position.  

# import needed libraries
import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64
import matplotlib.pyplot as plt
import numpy as np
from tf.transformations import euler_from_quaternion
import math
import time

# define some variables
pose = [0] * 3
yaw = 0
gx = 10
gy = 0

def goalx_callback(data):
    global gx
    gx = data.data


def goaly_callback(data):
    global gy
    gy = data.data


def odom_callback(data):
    global pose, yaw

    x = data.pose.pose.orientation.x
    y = data.pose.pose.orientation.y
    z = data.pose.pose.orientation.z
    w = data.pose.pose.orientation.w
    pose = [data.pose.pose.position.x, data.pose.pose.position.y,
            euler_from_quaternion([x, y, z, w])[2]]

    yaw = pose[2]


# main Control Loop
def control_loop():
    # initialize node name
    rospy.init_node('rover1_controller')
    # initialize velocity publisher
    pub = rospy.Publisher('rover1/cmd_vel', Twist, queue_size=10)
    global pose, yaw
    global gx, gy
    # subscribe to topics required(odometry and goal positions)
    rospy.Subscriber('rover1/odom', Odometry, odom_callback)
    rospy.Subscriber('/goal_x', Float64, goalx_callback)
    rospy.Subscriber('/goal_y', Float64, goaly_callback)
    # define ros rate
    rate = rospy.Rate(10)
    # define velocity msg and publish a 0 initially
    velocity_msg = Twist()
    velocity_msg.linear.x = 0
    velocity_msg.angular.z = 0

    pub.publish(velocity_msg)

    # Loop which runs until target is reached

    while not rospy.is_shutdown():
        # Present Coordinates
        present_point = [pose[0], pose[1]]
        y = gy - pose[1]
        x = gx - pose[0]
        # angle at which robot should move
        steering_angle = math.atan2(y, x)

        angle = steering_angle - yaw
        print(gx, gy)
        # calculate required yaw
        yaw_precision = (math.pi / 180) * 2  # +- 2 degree allowed

        yaw_error = math.fabs(angle) - yaw_precision
        # define angular velocity
        if angle != 0:
            angular_vel = 1 * math.fabs(angle) / angle
        else:
            angular_vel = 1

        # define euclidean distance between robot and target
        dist = math.sqrt(math.pow(pose[1] - gy, 2) +
                         (math.pow(pose[0] - gx, 2)))

        dist_precision = 0.1
        dist_error = dist - dist_precision

        # control loop
        # adjust yaw if needed
        if yaw_error > 0:

            # angle adjustment using yaw
            velocity_msg.angular.x = 0
            velocity_msg.angular.y = 0
            velocity_msg.angular.z = angular_vel

            pub.publish(velocity_msg)



        elif yaw_error <= 0:

            velocity_msg.angular.x = 0
            velocity_msg.angular.y = 0
            velocity_msg.angular.z = 0

            pub.publish(velocity_msg)

            # adjust distance after yaw
            # break the loop if the distance between robot and target is less than 1
            if dist_error > 1:

                velocity_msg.linear.x = 1
                pub.publish(velocity_msg)



            elif dist_error <= 1:

                velocity_msg.linear.x = 0
                pub.publish(velocity_msg)
                break
        rate.sleep()

    # send a 0 vel to stop the robot
    velocity_msg.linear.x = 0
    velocity_msg.angular.z = 0

    pub.publish(velocity_msg)


# Call the controller function
if __name__ == '__main__':
    try:
        print('start')
        control_loop()
    except rospy.ROSInterruptException:
        pass

Hope this helps.