# Proper implementation of pure_pursuit for ground robots using waypoints

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.

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
if yaw_error > 0:

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)

# 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.