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?
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?
