Commit 26ea68cf authored by Antonin Raffin's avatar Antonin Raffin
Browse files

Add comments

parent 5fbb26d4
...@@ -23,7 +23,9 @@ def nothing(x): ...@@ -23,7 +23,9 @@ def nothing(x):
class FindColor: class FindColor:
def __init__(self, debug=True): def __init__(self, debug=True):
self.bridge = CvBridge() self.bridge = CvBridge()
# Subscribe to the camera
self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback) self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback)
# Create the center_deviation topic where we will publish the error
self.center_pub = rospy.Publisher('center_deviation', Float32, queue_size=10) self.center_pub = rospy.Publisher('center_deviation', Float32, queue_size=10)
# Dummy images # Dummy images
self.cv_image = np.zeros((100, 100, 3), np.uint8) self.cv_image = np.zeros((100, 100, 3), np.uint8)
...@@ -59,9 +61,6 @@ class FindColor: ...@@ -59,9 +61,6 @@ class FindColor:
print("Error converting the image: {}".format(e)) print("Error converting the image: {}".format(e))
return return
# print("Image shape {}".format(self.cv_image.shape))
image_width = self.cv_image.shape[1]
if self.debug: if self.debug:
# Get info from track bar and appy to result # Get info from track bar and appy to result
self.h_min = cv2.getTrackbarPos('h_min', self.mask_name) self.h_min = cv2.getTrackbarPos('h_min', self.mask_name)
...@@ -70,14 +69,16 @@ class FindColor: ...@@ -70,14 +69,16 @@ class FindColor:
self.h_max = cv2.getTrackbarPos('h_max', self.mask_name) self.h_max = cv2.getTrackbarPos('h_max', self.mask_name)
self.s_max = cv2.getTrackbarPos('s_max', self.mask_name) self.s_max = cv2.getTrackbarPos('s_max', self.mask_name)
self.v_max = cv2.getTrackbarPos('v_max', self.mask_name) self.v_max = cv2.getTrackbarPos('v_max', self.mask_name)
# Convert to HSV color space # Convert to HSV color space
self.hsv = cv2.cvtColor(self.cv_image, cv2.COLOR_RGB2HSV) self.hsv = cv2.cvtColor(self.cv_image, cv2.COLOR_RGB2HSV)
# Normal masking algorithm
mask, center, radius = None, None, 0 mask, center, radius = None, None, 0
# If valid values update the Thresholds # If valid values update the Thresholds
if self.h_min <= self.h_max and self.s_min <= self.s_max and self.v_min <= self.v_max: if self.h_min <= self.h_max and self.s_min <= self.s_max and self.v_min <= self.v_max:
lower = np.array([self.h_min, self.s_min, self.v_min]) lower = np.array([self.h_min, self.s_min, self.v_min])
upper = np.array([self.h_max, self.s_max, self.v_max]) upper = np.array([self.h_max, self.s_max, self.v_max])
# Create the binary mask
mask = cv2.inRange(self.hsv, lower, upper) mask = cv2.inRange(self.hsv, lower, upper)
# Erosion and dilation to remove noise # Erosion and dilation to remove noise
kernel = np.ones((7, 7), np.uint8) kernel = np.ones((7, 7), np.uint8)
...@@ -86,9 +87,10 @@ class FindColor: ...@@ -86,9 +87,10 @@ class FindColor:
# Find contours in the mask # Find contours in the mask
contours = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] contours = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
if len(contours) > 0: if len(contours) > 0:
# Find largest contour and compute the minimum eclosing circle # Find largest contour and compute the minimum enclosing circle
c = max(contours, key=cv2.contourArea) c = max(contours, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c) ((x, y), radius) = cv2.minEnclosingCircle(c)
# Compute the centroid of the mask
M = cv2.moments(c) M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
else: else:
...@@ -97,27 +99,31 @@ class FindColor: ...@@ -97,27 +99,31 @@ class FindColor:
else: else:
print("Invalid values for Thresholds") print("Invalid values for Thresholds")
# Create the masked image
self.mask_image = cv2.bitwise_and(self.cv_image, self.cv_image, mask=mask) self.mask_image = cv2.bitwise_and(self.cv_image, self.cv_image, mask=mask)
# Only proceed if the radius meets a minimum size # Only proceed if the radius meets a minimum size
if radius > MIN_RADIUS_SIZE and self.debug: if radius > MIN_RADIUS_SIZE and self.debug:
# Draw the circle # Draw the enclosing circle and center on the two images
cv2.circle(self.mask_image, (int(x), int(y)), int(radius), (0, 255, 255), 2) cv2.circle(self.mask_image, (int(x), int(y)), int(radius), (0, 255, 255), 2)
cv2.circle(self.mask_image, center, 5, (0, 0, 255), -1) cv2.circle(self.mask_image, center, 5, (0, 0, 255), -1)
cv2.circle(self.cv_image, (int(x), int(y)), int(radius), (0, 255, 255), 2) cv2.circle(self.cv_image, (int(x), int(y)), int(radius), (0, 255, 255), 2)
cv2.circle(self.cv_image, center, 5, (0, 0, 255), -1) cv2.circle(self.cv_image, center, 5, (0, 0, 255), -1)
# Publish horizon deviation from the middle # Publish deviation from the middle of the image (= the error)
if center is not None: if center is not None:
# print("Center found: {}".format(center)) # print("Center found: {}".format(center))
# center_deviation_px = int(center[0] - image_width // 2) # center_deviation_px = int(center[0] - image_width // 2)
max_deviation = image_width // 2
# Rescale the error to [-1, 1] # Rescale the error to [-1, 1]
image_width = self.cv_image.shape[1]
max_deviation = image_width // 2
center_deviation = (center[0] - max_deviation) / max_deviation center_deviation = (center[0] - max_deviation) / max_deviation
# print("Center deviation : {}".format(center_deviation)) # print("Center deviation : {}".format(center_deviation))
# Publish the error to the corresponding topic
self.center_pub.publish(center_deviation) self.center_pub.publish(center_deviation)
if self.debug: if self.debug:
# Show the images
cv2.imshow(self.image_name, self.cv_image) cv2.imshow(self.image_name, self.cv_image)
cv2.imshow(self.mask_name, self.mask_image) cv2.imshow(self.mask_name, self.mask_image)
# Wait 3 ms to show the images # Wait 3 ms to show the images
...@@ -126,8 +132,8 @@ class FindColor: ...@@ -126,8 +132,8 @@ class FindColor:
if __name__ == '__main__': if __name__ == '__main__':
ic = FindColor()
print("Starting FindColor node") print("Starting FindColor node")
ic = FindColor()
rospy.init_node('FindColor', anonymous=True) rospy.init_node('FindColor', anonymous=True)
try: try:
rospy.spin() rospy.spin()
......
...@@ -19,7 +19,7 @@ def turnCallback(data): ...@@ -19,7 +19,7 @@ def turnCallback(data):
# Error (deviation to the center rescaled to [-1, 1]) # Error (deviation to the center rescaled to [-1, 1])
error = data.data error = data.data
# print("Error: {}".format(error)) # print("Error: {}".format(error))
# Init twist message (speed + orientation command) # Init twist message (speed + orientation command)
vel_msg = Twist() vel_msg = Twist()
vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = 0, 0, 0 vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = 0, 0, 0
...@@ -33,10 +33,10 @@ def turnCallback(data): ...@@ -33,10 +33,10 @@ def turnCallback(data):
dt = 1 dt = 1
# PID Control # PID Control
vel_msg.angular.z = - (Kp * error + Kd * (errorD / dt) + Ki * dt * errorI) vel_msg.angular.z = - (Kp * error + Kd * (errorD / dt) + Ki * dt * errorI)
# Update integral error
errorI += error errorI += error
# Clip integral error
# errorI = np.clip(errorI, -1, 1)
# Publish the Twist message
vel_pub.publish(vel_msg) vel_pub.publish(vel_msg)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment