I'm making a USV (Unmanned Surface Vehicle) for my bachelor project. The track it needs to keep is made by having coloured buoys on the left/right side of the track and for obstacles. So I need to track the depth of these objects and give all that information on to my navigation program.
And I have made a Python code using ROS and OpenCV to track these buoys with a ZED2 camera. But I'm having CPU and memory issues. Where the ubuntu desktop starts to lag. Using a Nvidia Jetson Xavier NX and I’m using 85% of the CPU and 5,5+/7.59Gb Memory.
Anyone interested in looking over my code and see if I'm doing something stupid. That would explain my issues.
from __future__ import print_function
import roslib
import sys
import rospy
import cv2
from main.msg import VarRed, VarGreen, VarYellow, RedHSV, GreenHSV, YellowHSV, MidPoint
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
import imutils
import time
from collections import deque
import math
class image_converter:
def __init__(self):
self.image_subd = rospy.Subscriber("/zed2/zed_node/depth/depth_registered",Image,self.callbackDepth)
self.image_sub = rospy.Subscriber("/zed2/zed_node/rgb_raw/image_raw_color",Image,self.callbackVideo)
self.image_pub = rospy.Publisher("/Tracking/RG_image", Image, queue_size = 1)
self.RedHSV_sub = rospy.Subscriber("/Tracking/Red_HSV", RedHSV, self.redHSV)
self.GreenHSV_sub = rospy.Subscriber("/Tracking/Green_HSV", GreenHSV, self.greenHSV)
self.YellowHSV_sub = rospy.Subscriber("/Tracking/Yellow_HSV", YellowHSV, self.yellowHSV)
self.MidPoint_pub = rospy.Publisher("/Tracking/MidPoint", MidPoint, queue_size = 1)
self.red_bridge = CvBridge()
self.red_publisher = rospy.Publisher("/Tracking/red", VarRed, queue_size = 1)
self.green_bridge = CvBridge()
self.green_publisher = rospy.Publisher("/Tracking/green", VarGreen, queue_size = 1)
self.yellow_bridge = CvBridge()
self.yellow_publisher = rospy.Publisher("/Tracking/yellow", VarYellow, queue_size = 1)
self.RedLower = (0, 101, 68) # Declaring the red-specter
self.RedUpper = (15, 255, 255)
self.GreenLower = (75, 145, 48) # Declaring the green-specter
self.GreenUpper = (96, 255, 75)
self.YellowLower = (28, 56, 91) # Declaring the yellow-specter
self.YellowUpper = (51, 152, 150)
self.red_pts = deque(maxlen=14)
self.currentDepthImg=0
self.red_counter = 0
self.red_x = 0
self.red_y = 0
self.red_radius = 30
self.green_pts = deque(maxlen=14)
self.green_currentDepthImg=0
self.green_counter = 0
self.green_x = 0
self.green_y = 0
self.green_radius = 30
self.yellow_pts = deque(maxlen=14)
self.yellow_currentDepthImg=0
self.yellow_counter = 0
self.yellow_x = 0
self.yellow_y = 0
self.yellow_radius = 30
def redHSV(self,msg):
self.RedLower = (msg.r_h_low-10, msg.r_s_low-10, msg.r_v_low-10)
self.RedUpper = (msg.r_h_high+10, msg.r_s_high+10, msg.r_v_high+10)
def greenHSV(self,msg):
self.GreenLower = (msg.g_h_low-10, msg.g_s_low-10, msg.g_v_low-10)
self.GreenUpper = (msg.g_h_high+10, msg.g_s_high+10, msg.g_v_high+10)
def yellowHSV(self,msg):
self.YellowLower = (msg.y_h_low-10, msg.y_s_low-10, msg.y_v_low-10)
self.YellowUpper = (msg.y_h_high+10, msg.y_s_high+10, msg.y_v_high+10)
def callbackDepth(self,msg_depth):
try:
cv_image_depth = self.red_bridge.imgmsg_to_cv2(msg_depth, "32FC1") # CV_Bridge Depth
except CvBridgeError as e:
print(e)
return
self.currentDepthImg=cv_image_depth
try:
a=1
except CvBridgeError as e:
print(e)
return
def callbackVideo(self,data):
try:
cv_image = self.red_bridge.imgmsg_to_cv2(data, "bgr8") # CV_Bridge Video
except CvBridgeError as e:
print(e)
return
(rows,cols,channels) = cv_image.shape
frame = cv_image
blurred = cv2.GaussianBlur(frame, (21, 21), 0) # resize the frame, blur it, and convert it to the HSV (11,11), 0
hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV) # color space.
red_mask = cv2.inRange(hsv, self.RedLower, self.RedUpper) # Construct a mask for the color "red", then perform
red_mask = cv2.erode(red_mask, None, iterations=2) # a series of dilations and erosions to remove any small
red_mask = cv2.dilate(red_mask, None, iterations=2) # blobs thats left in the mask.
green_mask = cv2.inRange(hsv, self.GreenLower, self.GreenUpper) # construct a mask for the color "green", then perform
green_mask = cv2.erode(green_mask, None, iterations=2) # a series of dilations and erosions to remove any small
green_mask = cv2.dilate(green_mask, None, iterations=2) # blobs thats left in the mask.
yellow_mask = cv2.inRange(hsv, self.YellowLower, self.YellowUpper) # construct a mask for the color "yellow", then perform
yellow_mask = cv2.erode(yellow_mask, None, iterations=2) # a series of dilations and erosions to remove any small
yellow_mask = cv2.dilate(yellow_mask, None, iterations=2) # blobs thats left in the mask.
red_cnts = cv2.findContours(red_mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) # find contours in the mask and initialize the current
red_cnts = imutils.grab_contours(red_cnts)
red_center = None
self.red_radius = 0
green_cnts = cv2.findContours(green_mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) # find contours in the mask and initialize the current
green_cnts = imutils.grab_contours(green_cnts)
green_center = None
self.green_radius = 0
yellow_cnts = cv2.findContours(yellow_mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) # find contours in the mask and initialize the current
yellow_cnts = imutils.grab_contours(yellow_cnts)
yellow_center = None
self.yellow_radius = 0
cv_imaged=self.currentDepthImg
#-----------------------------------------RED_START------------------------------------------------------
if len(red_cnts) > 0: # only proceed if at least one contour was found
red_c = max(red_cnts, key=cv2.contourArea) # find the largest contour in the red_mask, then use
((self.red_x, self.red_y), self.red_radius) = cv2.minEnclosingCircle(red_c) # it to compute the minimum enclosing circle and
red_M = cv2.moments(red_c) # centroid
red_center = (int(red_M["m10"] / red_M["m00"]), int(red_M["m01"] / red_M["m00"]))
if self.red_radius > 5: # only proceed if the radius meets a minimum size
cv2.circle(frame, (int(self.red_x), int(self.red_y)), int(self.red_radius), (0, 255, 255), 2) # draw the circle and centroid on the red_frame,
cv2.circle(frame, red_center, 5, (0, 255, 255), -1) # then update the list of tracked points
msg = VarRed()
msg.r_visible = True
#if self.red_y == self.red_y and self.red_x == self.red_x:
r_length = cv_imaged[int(self.red_y),int(self.red_x)] # length to object
msg.r_x = self.red_x
msg.r_y = self.red_y
msg.r_rad = self.red_radius
ToRad = 2*np.pi/360 # = 0.01745329252
ToDeg = 360/(2*np.pi) # = 57.29577951308
# Printing pixel values
cv2.rectangle(frame, (0, 0), (200, 190), (0,0,0), -1)
cv2.putText(frame, str("L: %.3f" %r_length), ((int(self.red_x)),int(self.red_y)), cv2.FONT_HERSHEY_COMPLEX, 1, (0,255,255), 2)
cv2.putText(frame, str("RX: %.1f" %msg.r_x +" px"), (10,30), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("RY: %.1f" %msg.r_y + " px"), (10,60), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
# For X-direction
red_l_cm = (r_length*100) # Converting to Centimeters
start_x_r = 960/(math.tan((55*ToRad))) # finding start x-length in px
ang_x_r = (math.atan((self.red_x-960)/start_x_r))*ToDeg # finding horizontal angle
red_x_cm = (red_l_cm*math.sin((ang_x_r)*ToRad))
cv2.putText(frame, str("RXC: %.1f" %red_x_cm + " cm"), (10,90), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("X Ang: %.1f" %ang_x_r), (10,150), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
# For Y-direction
start_y_r = 540/(math.tan((35*ToRad))) # finding start y-length in px
ang_y_r = ((math.atan((self.red_y-540)/start_y_r))*ToDeg)*-1 # finding vertical angle
red_y_cm = (red_l_cm/math.tan((ang_y_r*ToRad)+(math.pi/2)))*-1 # finding the y-length
cv2.putText(frame, str("RYC: %.1f" %red_y_cm + " cm"), (10,120), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("Y Ang: %.1f" %ang_y_r), (10,180), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
red_z = (math.cos(abs(ang_x_r)*ToRad))*red_l_cm
self.red_pts.appendleft(red_center)
msg.r_length = red_l_cm
msg.r_xc = red_x_cm
msg.r_yc = red_y_cm
msg.r_angle = ang_x_r # update the points queue
msg.r_z = red_z
self.red_publisher.publish(msg)
for i in range(1, len(self.red_pts)): # loop over the set of points
if self.red_pts[i - 1] is None or self.red_pts[i] is None: # if either of the tracked points
continue # are None, ignore them.
thickness = int(np.sqrt(64 / float(i + 1)) * 2.5) # otherwise, compute the thickness of the line and
cv2.line(frame, self.red_pts[i - 1], self.red_pts[i], (0, 255, 255), thickness) # draw the connecting lines
if self.red_radius < 5:
msg = VarRed()
msg.r_visible = False
self.red_publisher.publish(msg)
#-----------------------------------------RED_END------------------------------------------------------
#-----------------------------------------GREEN_START------------------------------------------------------
if len(green_cnts) > 0: # same as in red, but for green
green_c = max(green_cnts, key=cv2.contourArea)
((self.green_x, self.green_y), self.green_radius) = cv2.minEnclosingCircle(green_c)
green_M = cv2.moments(green_c)
green_center = (int(green_M["m10"] / green_M["m00"]), int(green_M["m01"] / green_M["m00"]))
if self.green_radius > 5:
cv2.circle(frame, (int(self.green_x), int(self.green_y)), int(self.green_radius), (0, 255, 255), 2)
cv2.circle(frame, green_center, 5, (0, 255, 255), -1)
ToRad = 2*np.pi/360 # = 0.01745329252
ToDeg = 360/(2*np.pi) # = 57.29577951308
msg1 = VarGreen()
msg1.g_visible = True
g_length = cv_imaged[int(self.green_y),int(self.green_x)]
msg1.g_x = self.green_x
msg1.g_y = self.green_y
msg1.g_rad = self.green_radius
# Printing pixel values
cv2.rectangle(frame, (1740, 0), (1920, 200), (0,0,0), -1)
cv2.putText(frame, str("L: %.3f" %g_length), ((int(self.green_x)),int(self.green_y)), cv2.FONT_HERSHEY_COMPLEX, 1, (0,255,255), 2)
cv2.putText(frame, str("GX: %.1f" %msg1.g_x +"px"), (1740,30), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("GY: %.1f" %msg1.g_y + "px"), (1740,60), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
# For X-direction
green_l_cm = (g_length*100)
start_x_g = 960/(math.tan((55*2*np.pi/360)))
ang_x_g = (math.atan((self.green_x-960)/start_x_g))*57.295779513
green_x_cm = (green_l_cm*math.sin((ang_x_g)*ToRad))
# For Y-direction
start_y_g = 540/(math.tan((35*2*np.pi/360)))
ang_y_g = ((math.atan((self.green_y-540)/start_y_g))*57.295779513)*-1
green_y_cm = green_l_cm/math.tan(ang_y_g*ToRad+(math.pi/2))*-1
cv2.putText(frame, str("GXC: %.1f" %green_x_cm + "cm"), (1740,90), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("X Ang: %.1f" %ang_x_g), (1740,150), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("GYC: %.1f" %green_y_cm + "cm"), (1740,120), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.putText(frame, str("Y Ang: %.1f" %ang_y_g), (1740,180), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
green_z = (math.cos(abs(ang_x_g)*ToRad))*green_l_cm
self.green_pts.appendleft(green_center)
msg1.g_length = green_l_cm
msg1.g_xc = green_x_cm
msg1.g_yc = green_y_cm
msg1.g_angle = ang_x_g
msg1.g_z = green_z
self.green_publisher.publish(msg1)
for i in range(1, len(self.green_pts)):
if self.green_pts[i - 1] is None or self.green_pts[i] is None:
continue
thickness = int(np.sqrt(64 / float(i + 1)) * 2.5)
cv2.line(frame, self.green_pts[i - 1], self.green_pts[i], (0, 255, 255), thickness)
if self.green_radius < 5:
msg1 = VarGreen()
msg1.g_visible = False
self.green_publisher.publish(msg1)
#-----------------------------------------GREEN_END------------------------------------------------------
#-----------------------------------------YELLOW_START------------------------------------------------------
if len(yellow_cnts) > 0: # only proceed if at least one contour was found
yellow_c = max(yellow_cnts, key=cv2.contourArea) # find the largest contour in the yellow_mask, then use
((self.yellow_x, self.yellow_y), self.yellow_radius) = cv2.minEnclosingCircle(yellow_c) # it to compute the minimum enclosing circle and
yellow_M = cv2.moments(yellow_c) # centroid
yellow_center = (int(yellow_M["m10"] / yellow_M["m00"]), int(yellow_M["m01"] / yellow_M["m00"]))
if self.yellow_radius > 5: # only proceed if the radius meets a minimum size
cv2.circle(frame, (int(self.yellow_x), int(self.yellow_y)), int(self.yellow_radius), (0, 0, 200), 2) # draw the circle and centroid on the yellow_frame,
cv2.circle(frame, yellow_center, 5, (0, 0, 200), -1) # then update the list of tracked points
ToRad = 2*np.pi/360 # = 0.01745329252
ToDeg = 360/(2*np.pi) # = 57.29577951308
msg2 = VarYellow()
msg2.y_visible = True
y_length = cv_imaged[int(self.yellow_y),int(self.yellow_x)] # length to object
msg2.y_x = self.yellow_x
msg2.y_y = self.yellow_y
msg2.y_rad = self.yellow_radius
cv2.putText(frame, str("L: %.3f" %y_length), ((int(self.yellow_x)),int(self.yellow_y)), cv2.FONT_HERSHEY_COMPLEX, 1, (0,0,200), 2)
# For X-direction
yellow_l_cm = y_length*100 # Converting to Centimeters
start_x_y = 960/(math.tan((55*2*np.pi/360))) # finding start x-length in px
ang_x_y = (math.atan((self.yellow_x-960)/start_x_y))*57.295779513 # finding horizontal angle
#yellow_x = yellow_l_cm/math.tan((ang_x_y/57.295779513)) # finding the x-length
yellow_x_cm = (yellow_l_cm*math.sin((ang_x_y)*ToRad))
# For Y-direction
start_y_y = 540/(math.tan((35*2*np.pi/360))) # finding start y-length in px
ang_y_y = ((math.atan((self.yellow_y-540)/start_y_y))*57.295779513)*-1 # finding vertical angle
#yellow_y = yellow_l_cm/math.tan((ang_y_y/57.295779513)) # finding the y-length
yellow_y_cm = yellow_l_cm/math.tan(ang_y_y*ToRad+(math.pi/2))*-1
yellow_z = (math.cos(abs(ang_x_y)*ToRad))*yellow_l_cm
self.yellow_pts.appendleft(yellow_center)
msg2.y_length = yellow_l_cm
msg2.y_xc = yellow_x_cm
msg2.y_yc = yellow_y_cm
msg2.y_angle = ang_x_y # update the points queue
msg2.y_z = yellow_z
self.yellow_publisher.publish(msg2)
for i in range(1, len(self.yellow_pts)): # loop over the set of points
if self.yellow_pts[i - 1] is None or self.yellow_pts[i] is None: # if either of the tracked points
continue # are None, ignore them.
thickness = int(np.sqrt(64 / float(i + 1)) * 2.5) # otherwise, compute the thickness of the line and
cv2.line(frame, self.yellow_pts[i - 1], self.yellow_pts[i], (0, 0, 255), thickness) # draw the connecting lines
if self.yellow_radius < 5:
msg2 = VarYellow()
msg2.y_visible = False
self.yellow_publisher.publish(msg2)
#-----------------------------------------YELLOW_END------------------------------------------------------
try:
if (self.green_radius > 5) & (self.red_radius > 5): # if you can see both colors, proceed
ToRad = 2*np.pi/360 # = 0.01745329252
ToDeg = 360/(2*np.pi) # = 57.29577951308
red_z = (math.cos(abs(ang_x_r)*ToRad))*red_l_cm
green_z = (math.cos(abs(ang_x_g)*ToRad))*green_l_cm
Delta_z = abs(red_z-green_z)
Tot_x = abs(green_x_cm) + abs(red_x_cm)
if Delta_z == Delta_z and Tot_x == Tot_x:
red_green_angle = (math.atan(Delta_z/Tot_x))*ToDeg
normal_angle = red_green_angle
if green_l_cm >= red_l_cm:
normal_angle = red_green_angle*-1
if green_l_cm < red_l_cm:
normal_angle = red_green_angle
MidPoint_data = MidPoint()
MidPoint_data.angle = normal_angle
self.MidPoint_pub.publish(MidPoint_data)
length_between_x = math.sqrt((Tot_x*Tot_x)+(Delta_z*Delta_z))
Delta_y = abs(red_y_cm-green_y_cm)
length_between = math.sqrt((length_between_x*length_between_x)+(Delta_y*Delta_y))
#dx = green_x_cm - red_x_cm # Finding the space between the colors in x-direction
#dy = green_y_cm - red_y_cm # Finding the space between the colors in y-direction
# Calculating the direct length between the colors in cm
#cv2.rectangle(frame, (500, 0), (680, 160), (0,0,0), -1)
#cv2.putText(frame, str("Dist: %.1f" %length_between + " cm"), (500,30), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
#Middle_x = dx
#Middle_y = dy
MP_X = (msg1.g_x + msg.r_x)/2
MP_Y = (msg1.g_y + msg.r_y)/2
#Middle_Point_Angle = (math.atan((MP_X-960)/start_x_g))*57.295779513
#Middle_Point_Angle = ang_x_g - ang_x_r
#Middle_Point_Length =(red_x_cm-abs(Middle_x))/(math.sin((math.pi/2)-(Middle_Point_Angle*((2*math.pi)/720))))
#Middle_Point_Length =((red_x_cm-abs(Middle_x))/(math.cos(Middle_Point_Angle*((2*math.pi)/720))))
#cv2.putText(frame, str("MX: %.1f" %Middle_x + " cm"), (500,60), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
#cv2.putText(frame, str("MY: %.1f" %Middle_y + " cm"), (500,90), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
if MP_X == MP_X and MP_Y == MP_Y:
cv2.circle(frame, (int(MP_X), int(MP_Y)), 8, (0, 0, 255), -1)
#cv2.putText(frame, str("M_L: %.1f" %Middle_Point_Length + " cm"), (500,120), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
#cv2.putText(frame, str("M_ang: %.1f" %Middle_Point_Angle), (500,150), cv2.FONT_HERSHEY_COMPLEX, 0.8, (255,255,255), 1)
cv2.line(frame, (int(self.red_x),int(self.red_y)), (int(self.green_x),int(self.green_y)), (0, 0, 0), 2)
#MidPoint_data = MidPoint()
#MidPoint_data.z = Middle_Point_Length
#self.MidPoint_pub.publish(MidPoint_data)
cv2.line(frame, (960, 1280), (960, 0), (0, 255, 0), 1)
cv2.line(frame, (0, 540), (1920, 540), (0, 255, 0), 1)
self.image_pub.publish(self.red_bridge.cv2_to_imgmsg(frame, "bgr8"))
except CvBridgeError as e:
print(e)
return
def main(args):
ic = image_converter()
rospy.init_node('Color_Tracker', anonymous=True)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
if __name__ == '__main__':
main(sys.argv)
