# Implementing PID on a line following bot using turtlebot

I am using ROS melodic,turtlebot 2 on Ubuntu 18.04.

The idea is to create an environment consisting of lines as a path (slightly curved), and to program the turtlebot to follow the lines. Basically, a line following bot.

The idea is to click photos using the camera mounted on the turtlebot,process them to segment out the path which needs to be followed, and then control the bot's orientation, by using PID to control the angular velocity, so that it moves along the path.

I have created a program called take_photo_mod.py, which successfully keeps clicking photos and saving them as image.jpg, at a frequency that can be controlled.

 #!/usr/bin/env python
from __future__ import print_function
import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

class TakePhoto:
def __init__(self):

self.bridge = CvBridge()
img_topic = "/camera/rgb/image_raw"
self.image_sub = rospy.Subscriber(img_topic, Image, self.callback)
rospy.sleep(1)

def callback(self, data):

try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)

self.image = cv_image

def take_picture(self, img_title):

cv2.imwrite(img_title, self.image)

return True
else:
return False

if __name__ == '__main__':

rospy.init_node('take_photo', anonymous=False)
camera = TakePhoto()

while not rospy.is_shutdown():

img_title = rospy.get_param('~image_title', 'image'.jpg')
if camera.take_picture(img_title):
else:

rospy.sleep(1)


As for the next part, I plan to create a program go_forward.py, which incorporates two things:

Part 1. The openCV aspect: processing the image clicked to segment out the path. I have successfully managed to do this:

import cv2
import numpy as np
import math as m

def contor(lst):
if len(lst)>1:
m = list()
for i in lst:
for j in i:
m.append(j[0])
m = np.array(m)
else:
m=np.array([[w//2,h-150],[w//2,h-40]])
print("m=",m)
m = m[m[:, 1].argsort()]
return m

def aver(i, p):
a = p[i]
y = int(np.mean([a[:, 0]]))
x = int(np.mean([a[:, 1]]))
return (y, x)

def extract(frame):
theta=0
phi=0
print("h,w =",h,w)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
lb = np.array([0, 0, 0])
ub = np.array([255, 50, 160])

res2 = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
ret, thres = cv2.threshold(res2, 100, 255, cv2.THRESH_BINARY)
countours, hierarchy = cv2.findContours(thres, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
cv2.drawContours(frame, countours, -1, (255, 0, 255), 3)

points = contor(countours)
i = np.where((points[:, 1] > h - 80) & (points[:, 1] < h))
#print('i1', len(i[0]))
if len(i[0]) > 0:
end = aver(i, points) #coordinates (x2,y2)
else:
end = (w // 2, h)
i = np.where((points[:, 1] > h - 190) & (points[:, 1] < h - 110))
#print('i2', len(i[0]))
if len(i[0]) > 0:
strt = aver(i, points) #coordinates (x1,y1)
else:
strt = (w // 2, h - 200)

frame=cv2.line(frame,strt,end,(255,0,0),9)
cv2.imshow("aa",frame)
cv2.waitKey()
cv2.destroyAllWindows()

if (strt[0] - end[0]) == 0:
phi=m.pi/2 #angle with horizontal
theta=0 #angle with vertical
else:
slope = (strt[1] - end[1]) / (end[0] - strt[0])
phi=m.atan2(slope) #angle with horizontal
theta= (m.pi/2) - phi #angle with vertical

return theta

h, w, _ = frame.shape
i,d=0,0
error=extract(frame)
print(error)


image.jpg ( which I was using for testing the openCV aspect. When take_photo_mod.py runs , image.jpg will keep changing at the frequency with which the bot will take photos)

This draws a line that approximately gives the direction along the path. Further, this calculates the angle with the angle with the vertical.

Part 2. PID aspect: This is the part with which I am struggling. The camera is mounted on the bot, so whatever we are calculating on the image is in the bot's frame,and the orientation of the bot in the bot's frame is along the vertical. Hence the angle which we calculate with the vertical acts as the "error" term: the angle by which the bot should turn to be along the path. Now, I think it's the angular velocity which will serve as the control signal.

I already have a script that just makes the bot go forward till its terminated.It had angular velocity fixed at 0. So,the program "goforward.py" will basically be the combination of the openCV aspect ,and tweaking the "simply go forward" script to use PID to set it's angular velocity (instead of it being fixed at 0). Then, the two programs(nodes) take_photo_mod.py and goforward.py will be launched together using a launch file.

What I tried for goforward.py:

#!/usr/bin/env python

import rospy
import cv2
import numpy as np
import math as m
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Twist

#openCV aspect

def contor(lst):
if len(lst)>1:
m = list()
for i in lst:
for j in i:
m.append(j[0])
m = np.array(m)
else:
m=np.array([[w//2,h-150],[w//2,h-40]])
#print("m=",m)
m = m[m[:, 1].argsort()]
return m

def aver(i, p):
a = p[i]
y = int(np.mean([a[:, 0]]))
x = int(np.mean([a[:, 1]]))
return (y, x)

def extract(frame):

h, w, _ = frame.shape
#print(h,w)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
lb = np.array([0, 0, 0])
ub = np.array([255, 50, 160])

res2 = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
ret, thres = cv2.threshold(res2, 100, 255, cv2.THRESH_BINARY)
countours, hierarchy = cv2.findContours(thres, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cv2.drawContours(frame, countours, -1, (255, 0, 255), 3)

points = contor(countours)
i = np.where((points[:, 1] > h - 80) & (points[:, 1] < h))

if len(i[0]) > 0:
end = aver(i, points)
else:
end = (w // 2, h)
i = np.where((points[:, 1] > h - 190) & (points[:, 1] < h - 110))

if len(i[0]) > 0:
strt = aver(i, points)
else:
strt = (w // 2, h - 200)

if (strt[0]-end[0]) == 0:
phi=m.pi/2 #angle with horizontal
theta = 0 #angle with vertical
else:
slope = (strt[1] - end[1]) / (end[0] - strt[0])
phi=m.atan2(slope) #angle with horizontal
theta= (m.pi/2) - phi #angle with vertical

return theta

#PID

def PID (kp,ki,kd,dt,error):

#making the bot move

class GoForward():

def _init_(self):
rospy.init_node('GoForward', anonymous=False)
rospy.loginfo("To stop TurtleBot CTRL + C")
rospy.on_shutdown(self.shutdown)
self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
r = rospy.Rate(10)
move_cmd = Twist()
move_cmd.linear.x = 0.2
move_cmd.angular.z = 0

while not rospy.is_shutdown():
error=extract(frame)
w=0
kp=0.15
ki=0.02
kd=0.02
dt=0.01
w= PID(kp,ki,kd,dt,error)
velocity_control=0.5 #when the bot is turning, its velocity should be slowed down.
move_cmd.linear.x = 0.2-(veclocity_control *abs(w))
move_cmd.angular.z = -w  #-sign because we are controlling angular velocity in -z direction (clockwise),if the error is positive, and vice versa
self.cmd_vel.publish(move_cmd)
r.sleep()

def shutdown(self):
self.cmd_vel.publish(Twist())
rospy.sleep(1)

if _name_ == '_main_':
try:
GoForward()
except:


The problem I'm having is to implement the PID part properly. How exactly should I approach writing the PID (...) function?

Something I tried: (pseudocode):

def pid_controller (kp,ki,kd,dt,error):

e_prev = error
e_sum = 0

while  :

e=?
e_sum = e_sum + e *dt
dedt = ( e - e_prev ) / dt
w = kp * e + ki * e_sum + kd* dedt
e_prev = e

return w


I've got it in bits and pieces but I still don't know how the full thing should work exactly.... for instance when should the loop terminate? What exactly should I have in place of e and e_prev?

• Welcome to Robotics, satan 29. Could you please explain more about your PID problem? What are you having trouble with? Typically you have a reference and feedback, then take the difference as the reference error, and that's what is used by the PID controller. Are you having issues with getting the reference and feedback, or tuning, or general implementation? Sep 8, 2021 at 13:58
• General implementation. Ill edit it slightly to add something I tried Sep 8, 2021 at 16:58
• I have edited it. By the way, the reference error is "theta" calculated by the openCV part of the code. Sep 8, 2021 at 17:42

You're defining

def pid_controller (kp,ki,kd,dt,error)


... the error is your e. The alternative would be to define

def pid_controller (kp,ki,kd,dt,theta_reference,theta_feedback)


and have

error = theta_reference - theta_feedback
e = error

• what should I have as e_prev then ? and when does the loop terminate? Sep 8, 2021 at 18:09
• @satan 29 - e_prev is the previous error, so just set e_prev = error any time after you're done using e_prev for the current call (i.e. any time after defining dedt). The PID function itself doesn't loop, your program should be looping and calling the PID function. e_prev and e_sum will need to be static or persistent (can't remember what the correct term is in python) to not get reset between function calls. Your main code should check for a reference update, poll for feedback, calculate reference error, call the PID controller, then send the PID output to the motor/drive system, then loop. Sep 9, 2021 at 1:01