Commit 5fbb26d4 authored by Antonin Raffin's avatar Antonin Raffin
Browse files

Refactor the two nodes

parent 1542bdca
...@@ -6,102 +6,128 @@ import numpy as np ...@@ -6,102 +6,128 @@ import numpy as np
import rospy import rospy
from cv_bridge import CvBridge, CvBridgeError from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image from sensor_msgs.msg import Image
from std_msgs.msg import Int32 from std_msgs.msg import Float32
H_MIN, S_MIN, V_MIN = 0, 0, 0
H_MAX, S_MAX, V_MAX = 179, 255, 255
MIN_RADIUS_SIZE = 10
# Green Thresholds
GREEN_LOWER = [29, 86, 6]
GREEN_UPPER = [84, 255, 255]
# Dummy callback for trackbar initialization
def nothing(x):
pass
class FindColor: class FindColor:
def __init__(self): def __init__(self, debug=True):
self.bridge = CvBridge() self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback) self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback)
self.center_pub = rospy.Publisher('center_deviation', Int32, queue_size=10) self.center_pub = rospy.Publisher('center_deviation', Float32, queue_size=10)
# Dummy images
self.cv_image = np.zeros((100, 100, 3), np.uint8) self.cv_image = np.zeros((100, 100, 3), np.uint8)
self.hsv = np.zeros((100, 100, 3), np.uint8) self.hsv = np.zeros((100, 100, 3), np.uint8)
self.h_min = 0 # Init Thresholds
self.s_min = 0 self.h_min, self.s_min, self.v_min = GREEN_LOWER
self.v_min = 0 self.h_max, self.s_max, self.v_max = GREEN_UPPER
self.debug = debug
self.h_max = 179
self.s_max = 255 if debug:
self.v_max = 255 self.image_name, self.mask_name = "RGB", "Mask"
cv2.namedWindow('RGB') # Create Windows
cv2.namedWindow('Mask') cv2.namedWindow(self.image_name)
# Creating track bar cv2.namedWindow(self.mask_name)
cv2.createTrackbar('h_min', 'Mask', 0, 179, self.nothing) # Creating track bar
cv2.createTrackbar('s_min', 'Mask', 0, 255, self.nothing) for min_max in ['min', 'max']:
cv2.createTrackbar('v_min', 'Mask', 0, 255, self.nothing) cv2.createTrackbar('h_{}'.format(min_max), self.mask_name, H_MIN, H_MAX, nothing)
cv2.createTrackbar('h_max', 'Mask', 0, 179, self.nothing) cv2.createTrackbar('s_{}'.format(min_max), self.mask_name, S_MIN, S_MAX, nothing)
cv2.createTrackbar('s_max', 'Mask', 0, 255, self.nothing) cv2.createTrackbar('v_{}'.format(min_max), self.mask_name, V_MIN, V_MAX, nothing)
cv2.createTrackbar('v_max', 'Mask', 0, 255, self.nothing)
# Initial value, for green # Initial value, for green
cv2.setTrackbarPos('h_min', 'Mask', 29) cv2.setTrackbarPos('h_min', self.mask_name, self.h_min)
cv2.setTrackbarPos('s_min', 'Mask', 86) cv2.setTrackbarPos('s_min', self.mask_name, self.s_min)
cv2.setTrackbarPos('v_min', 'Mask', 6) cv2.setTrackbarPos('v_min', self.mask_name, self.v_min)
cv2.setTrackbarPos('h_max', 'Mask', 84) cv2.setTrackbarPos('h_max', self.mask_name, self.h_max)
cv2.setTrackbarPos('s_max', 'Mask', 255) cv2.setTrackbarPos('s_max', self.mask_name, self.s_max)
cv2.setTrackbarPos('v_max', 'Mask', 255) cv2.setTrackbarPos('v_max', self.mask_name, self.v_max)
def nothing(self, x):
pass
def callback(self, data): def callback(self, data):
try: try:
self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e: except CvBridgeError as e:
print(e) print("Error converting the image: {}".format(e))
return
# get info from track bar and appy to result
self.h_min = cv2.getTrackbarPos('h_min', 'Mask') # print("Image shape {}".format(self.cv_image.shape))
self.s_min = cv2.getTrackbarPos('s_min', 'Mask') image_width = self.cv_image.shape[1]
self.v_min = cv2.getTrackbarPos('v_min', 'Mask')
self.h_max = cv2.getTrackbarPos('h_max', 'Mask') if self.debug:
self.s_max = cv2.getTrackbarPos('s_max', 'Mask') # Get info from track bar and appy to result
self.v_max = cv2.getTrackbarPos('v_max', 'Mask') self.h_min = cv2.getTrackbarPos('h_min', self.mask_name)
self.s_min = cv2.getTrackbarPos('s_min', self.mask_name)
self.v_min = cv2.getTrackbarPos('v_min', self.mask_name)
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 # 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 # Normal masking algorithm
mask = None 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: 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])
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)
mask = cv2.erode(mask, kernel, iterations=2) mask = cv2.erode(mask, kernel, iterations=2)
mask = cv2.dilate(mask, kernel, iterations=2) mask = cv2.dilate(mask, kernel, iterations=2)
# Find contours in the mask # Find contours in the mask
cnts = 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]
center = None if len(contours) > 0:
radius = 0 # Find largest contour and compute the minimum eclosing circle
if len(cnts) > 0: c = max(contours, key=cv2.contourArea)
# Find largest contour and compute the minimum eclosing circle ((x, y), radius) = cv2.minEnclosingCircle(c)
c = max(cnts, key=cv2.contourArea) M = cv2.moments(c)
((x, y), radius) = cv2.minEnclosingCircle(c) center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
M = cv2.moments(c) else:
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) pass
# print("No center found")
else:
print("Invalid values for Thresholds")
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 > 10: if radius > MIN_RADIUS_SIZE and self.debug:
# Draw the circle # Draw the circle
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)
cv2.imshow("RGB", self.cv_image)
cv2.imshow("Mask", self.mask_image)
print(center)
# Publish horizon deviation from the middle # Publish horizon deviation from the middle
if center is not None: if center is not None:
center_deviation = int(center[0] - 320) # print("Center found: {}".format(center))
print(center_deviation) # center_deviation_px = int(center[0] - image_width // 2)
max_deviation = image_width // 2
# Rescale the error to [-1, 1]
center_deviation = (center[0] - max_deviation) / max_deviation
# print("Center deviation : {}".format(center_deviation))
self.center_pub.publish(center_deviation) self.center_pub.publish(center_deviation)
cv2.waitKey(3)
if self.debug:
cv2.imshow(self.image_name, self.cv_image)
cv2.imshow(self.mask_name, self.mask_image)
# Wait 3 ms to show the images
cv2.waitKey(3)
if __name__ == '__main__': if __name__ == '__main__':
ic = FindColor() ic = FindColor()
print("start") print("Starting FindColor node")
rospy.init_node('FindColor', anonymous=True) rospy.init_node('FindColor', anonymous=True)
try: try:
rospy.spin() rospy.spin()
......
#!/usr/bin/env python #!/usr/bin/env python
from __future__ import print_function, division from __future__ import print_function, division
import time
import rospy import rospy
import numpy as np
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
from std_msgs.msg import Int32 from std_msgs.msg import Float32
vel_pub = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size=10) vel_pub = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size=10)
MAX_DEVIATION = 320 Kp = 2
Kd = 1
Ki = 0
last_time = None
def turnCallback(data): def turnCallback(data):
deviation = data.data global errorD, errorI, last_time
kp = None # 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 = Twist()
vel_msg.linear.x = 0 vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = 0, 0, 0
vel_msg.linear.y = 0 vel_msg.angular.x, vel_msg.angular.y = 0, 0
vel_msg.linear.z = 0 if last_time is not None:
vel_msg.angular.x = 0 errorD = error - last_error
vel_msg.angular.y = 0 dt = time.time() - last_time
if abs(deviation) > MAX_DEVIATION / 2: last_time = time.time()
vel_msg.angular.z = - deviation / abs(deviation)
else: else:
vel_msg.angular.z = -deviation * 2.0 / 320.0 errorI, errorD = 0, 0
dt = 1
# PID Control
vel_msg.angular.z = - (Kp * error + Kd * (errorD / dt) + Ki * dt * errorI)
errorI += error
# Clip integral error
# errorI = np.clip(errorI, -1, 1)
vel_pub.publish(vel_msg) vel_pub.publish(vel_msg)
if __name__ == "__main__": if __name__ == "__main__":
print("Start") print("Starting follow_color node")
rospy.init_node('turnBot', anonymous=True) rospy.init_node('follow_color', anonymous=True)
turn_sub = rospy.Subscriber("center_deviation", Int32, turnCallback) turn_sub = rospy.Subscriber("center_deviation", Float32, turnCallback)
try: try:
rospy.spin() rospy.spin()
except KeyboardInterrupt: except KeyboardInterrupt:
......
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