Hello,
I got the error as below when I run this code(rosrun my_following_line_package follow_line_step_hsv.py) which is right above the Exercise 2.4.1
How can I fix this? I am sure I followed all the instructions correctly.
Thank you.
Hi,
You should have something similar to this:
#!/usr/bin/env python
import rospy
import cv2
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Image
from move_robot import MoveKobuki
class LineFollower(object):
def __init__(self):
self.bridge_object = CvBridge()
self.image_sub = rospy.Subscriber("/camera/rgb/image_raw",Image,self.camera_callback)
self.movekobuki_object = MoveKobuki()
def camera_callback(self,data):
try:
# We select bgr8 because its the OpneCV encoding by default
cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding="bgr8")
except CvBridgeError as e:
print(e)
# We get image dimensions and crop the parts of the image we don't need
# Bear in mind that because the first value of the image matrix is start and second value is down limit.
# Select the limits so that it gets the line not too close and not too far, and the minimum portion possible
# To make process faster.
height, width, channels = cv_image.shape
# Noetic integer conversion
height = int(height)
width = int(width)
channels = int(channels)
descentre = 160
rows_to_watch = 20
aux1 = int(((height)/2)+descentre)
aux2 = int((height)/2+(descentre+rows_to_watch))
crop_img = cv_image[aux1:aux2][1:width]
# Convert from RGB to HSV
hsv = cv2.cvtColor(crop_img, cv2.COLOR_BGR2HSV)
# Define the Yellow Colour in HSV
#RGB
#[[[222,255,0]]]
#BGR
#[[[0,255,222]]]
"""
To know which color to track in HSV, Put in BGR. Use ColorZilla to get the color registered by the camera
>>> yellow = np.uint8([[[B,G,R ]]])
>>> hsv_yellow = cv2.cvtColor(yellow,cv2.COLOR_BGR2HSV)
>>> print( hsv_yellow )
[[[ 34 255 255]]
"""
lower_yellow = np.array([20,100,100])
upper_yellow = np.array([50,255,255])
# Threshold the HSV image to get only yellow colors
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
# Calculate centroid of the blob of binary image using ImageMoments
m = cv2.moments(mask, False)
try:
cx, cy = m['m10']/m['m00'], m['m01']/m['m00']
except ZeroDivisionError:
cy, cx = height/2, width/2
# Bitwise-AND mask and original image
res = cv2.bitwise_and(crop_img,crop_img, mask= mask)
# Draw the centroid in the resultut image
# cv2.circle(img, center, radius, color[, thickness[, lineType[, shift]]])
cv2.circle(res,(int(cx), int(cy)), 10,(0,0,255),-1)
cv2.imshow("Original", cv_image)
cv2.imshow("HSV", hsv)
cv2.imshow("MASK", mask)
cv2.imshow("RES", res)
cv2.waitKey(1)
error_x = cx - width / 2
twist_object = Twist()
twist_object.linear.x = 0.2
twist_object.angular.z = -error_x / 100
rospy.loginfo("ANGULAR VALUE SENT===>"+str(twist_object.angular.z))
# Make it start turning
self.movekobuki_object.move_robot(twist_object)
def clean_up(self):
self.movekobuki_object.clean_class()
cv2.destroyAllWindows()
def main():
rospy.init_node('line_following_node', anonymous=True)
line_follower_object = LineFollower()
rate = rospy.Rate(5)
ctrl_c = False
def shutdownhook():
# works better than the rospy.is_shut_down()
line_follower_object.clean_up()
rospy.loginfo("shutdown time!")
ctrl_c = True
rospy.on_shutdown(shutdownhook)
while not ctrl_c:
rate.sleep()
if __name__ == '__main__':
main()
And then just launch it. The first time it might fail due t some GTK XWindow issue. Just relaunch it again an dit should work perfectly. Please tell us if is not so. Please compare it with your code