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

Add comments

parent 5fbb26d4
......@@ -23,7 +23,9 @@ def nothing(x):
class FindColor:
def __init__(self, debug=True):
self.bridge = CvBridge()
# Subscribe to the camera
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)
# Dummy images
self.cv_image = np.zeros((100, 100, 3), np.uint8)
......@@ -59,9 +61,6 @@ class FindColor:
print("Error converting the image: {}".format(e))
return
# print("Image shape {}".format(self.cv_image.shape))
image_width = self.cv_image.shape[1]
if self.debug:
# Get info from track bar and appy to result
self.h_min = cv2.getTrackbarPos('h_min', self.mask_name)
......@@ -70,14 +69,16 @@ class FindColor:
self.h_max = cv2.getTrackbarPos('h_max', self.mask_name)
self.s_max = cv2.getTrackbarPos('s_max', self.mask_name)
self.v_max = cv2.getTrackbarPos('v_max', self.mask_name)
# Convert to HSV color space
self.hsv = cv2.cvtColor(self.cv_image, cv2.COLOR_RGB2HSV)
# Normal masking algorithm
mask, center, radius = None, None, 0
# 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:
lower = np.array([self.h_min, self.s_min, self.v_min])
upper = np.array([self.h_max, self.s_max, self.v_max])
# Create the binary mask
mask = cv2.inRange(self.hsv, lower, upper)
# Erosion and dilation to remove noise
kernel = np.ones((7, 7), np.uint8)
......@@ -86,9 +87,10 @@ class FindColor:
# Find contours in the mask
contours = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
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)
((x, y), radius) = cv2.minEnclosingCircle(c)
# Compute the centroid of the mask
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
else:
......@@ -97,27 +99,31 @@ class FindColor:
else:
print("Invalid values for Thresholds")
# Create the masked image
self.mask_image = cv2.bitwise_and(self.cv_image, self.cv_image, mask=mask)
# Only proceed if the radius meets a minimum size
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, 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, 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:
# print("Center found: {}".format(center))
# center_deviation_px = int(center[0] - image_width // 2)
max_deviation = image_width // 2
# 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
# print("Center deviation : {}".format(center_deviation))
# Publish the error to the corresponding topic
self.center_pub.publish(center_deviation)
if self.debug:
# Show the images
cv2.imshow(self.image_name, self.cv_image)
cv2.imshow(self.mask_name, self.mask_image)
# Wait 3 ms to show the images
......@@ -126,8 +132,8 @@ class FindColor:
if __name__ == '__main__':
ic = FindColor()
print("Starting FindColor node")
ic = FindColor()
rospy.init_node('FindColor', anonymous=True)
try:
rospy.spin()
......
......@@ -19,7 +19,7 @@ def turnCallback(data):
# Error (deviation to the center rescaled to [-1, 1])
error = data.data
# print("Error: {}".format(error))
# Init twist message (speed + orientation command)
vel_msg = Twist()
vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = 0, 0, 0
......@@ -33,10 +33,10 @@ def turnCallback(data):
dt = 1
# PID Control
vel_msg.angular.z = - (Kp * error + Kd * (errorD / dt) + Ki * dt * errorI)
# Update integral error
errorI += error
# Clip integral error
# errorI = np.clip(errorI, -1, 1)
# Publish the Twist message
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