PID implementation in line following bot using turtlebot

Question

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()
            self.image_received = False
            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_received = True
            self.image = cv_image
    
        def take_picture(self, img_title):
            if self.image_received:
               
                cv2.imwrite(img_title, self.image)
                rospy.loginfo("received")
    
                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):
                rospy.loginfo("Saved image " + img_title)
            else:
                rospy.loginfo("No images received")
    
            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])
    mask = cv2.inRange(hsv, lb, ub)
    res = cv2.bitwise_and(frame, frame, mask=mask)

   

     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
    
    
    
    frame=cv2.imread("image.jpg",1)
    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])
    mask = cv2.inRange(hsv, lb, ub)
    res = cv2.bitwise_and(frame, frame, mask=mask)

    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():
            frame=cv2.imread("image.jpg",1)
            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):
        rospy.loginfo("Stop TurtleBot")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)



if _name_ == '_main_':
    try:
        GoForward()
    except:
        rospy.loginfo("GoForward node terminated.")

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

Answer 1

As a comment suggested, there's a lot to tackle here, so I'll just focus on what you asked in your title. When concering PIDs in the ROS ecosystem I'd almost always suggest using the PID package. From your description it should do everything you need.