Commit 1d766a01 authored by Zhang Xuan's avatar Zhang Xuan
Browse files

Add follower in distance based on the radius of the green ball

parent 26ea68cf
......@@ -21,12 +21,15 @@ def nothing(x):
pass
class FindColor:
def __init__(self, debug=True):
def __init__(self, debug=False):
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)
# Create the ball_radius topic where we will publish the radius of the ball
self.ball_radius_pub = rospy.Publisher('ball_radius', Float32, queue_size = 10)
# Dummy images
self.cv_image = np.zeros((100, 100, 3), np.uint8)
self.hsv = np.zeros((100, 100, 3), np.uint8)
......@@ -34,12 +37,12 @@ class FindColor:
self.h_min, self.s_min, self.v_min = GREEN_LOWER
self.h_max, self.s_max, self.v_max = GREEN_UPPER
self.debug = debug
if debug:
self.image_name, self.mask_name = "RGB", "Mask"
# Create Windows
cv2.namedWindow(self.image_name)
cv2.namedWindow(self.mask_name)
cv2.waitKey(1000)
# Creating track bar
for min_max in ['min', 'max']:
cv2.createTrackbar('h_{}'.format(min_max), self.mask_name, H_MIN, H_MAX, nothing)
......@@ -60,7 +63,6 @@ class FindColor:
except CvBridgeError as e:
print("Error converting the image: {}".format(e))
return
if self.debug:
# Get info from track bar and appy to result
self.h_min = cv2.getTrackbarPos('h_min', self.mask_name)
......@@ -101,7 +103,7 @@ class FindColor:
# 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 enclosing circle and center on the two images
......@@ -122,10 +124,14 @@ class FindColor:
# Publish the error to the corresponding topic
self.center_pub.publish(center_deviation)
# Publish the ball_radius
if radius > MIN_RADIUS_SIZE:
self.ball_radius_pub.publish(radius)
if self.debug:
# Show the images
cv2.imshow(self.image_name, self.cv_image)
cv2.imshow(self.mask_name, self.mask_image)
cv2.imshow(self.masked, self.mask_image)
# Wait 3 ms to show the images
cv2.waitKey(3)
......@@ -133,8 +139,8 @@ class FindColor:
if __name__ == '__main__':
print("Starting FindColor node")
ic = FindColor()
rospy.init_node('FindColor', anonymous=True)
ic = FindColor()
try:
rospy.spin()
except KeyboardInterrupt:
......
......@@ -2,28 +2,31 @@
from __future__ import print_function, division
import time
import message_filters
import rospy
import numpy as np
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32
vel_pub = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size=10)
Kp = 2
Kp_w = 2
Kd = 1
Ki = 0
last_time = None
Kp_x = 0.2
vel_x = 0
omega_z = 0
def turnCallback(data):
global errorD, errorI, last_time
global errorD, errorI, last_time, omega_z
# 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
vel_msg.angular.x, vel_msg.angular.y = 0, 0
if last_time is not None:
errorD = error - last_error
dt = time.time() - last_time
......@@ -32,18 +35,43 @@ def turnCallback(data):
errorI, errorD = 0, 0
dt = 1
# PID Control
vel_msg.angular.z = - (Kp * error + Kd * (errorD / dt) + Ki * dt * errorI)
omega_z = - (Kp_w * error + Kd * (errorD / dt) + Ki * dt * errorI)
# Update integral error
errorI += error
# Init twist message (speed + orientation command)
vel_msg = Twist()
vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = vel_x, 0, 0
vel_msg.angular.x, vel_msg.angular.y, vel_msg.angular.z = 0, 0, omega_z
# Publish the Twist message
vel_pub.publish(vel_msg)
def translateCallback(raw_data):
data=np.mean(raw_data.data)
global vel_x
# Error (deviation to the center rescaled to [-1, 1])
error = (data - 45)/20
# print("Error: {}".format(error))
# PID Control
vel_x = - Kp_x * error
# Init twist message (speed + orientation command)
vel_msg = Twist()
vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = vel_x, 0, 0
vel_msg.angular.x, vel_msg.angular.y, vel_msg.angular.z= 0, 0, omega_z
# Publish the Twist message
vel_pub.publish(vel_msg)
if __name__ == "__main__":
print("Starting follow_color node")
rospy.init_node('follow_color', anonymous=True)
turn_sub = rospy.Subscriber("center_deviation", Float32, turnCallback)
translate_sub = message_filters.Subscriber("ball_radius", Float32)
translate_cache = message_filters.Cache(translate_sub,10,allow_headerless=True)
translate_cache.registerCallback(translateCallback)
try:
rospy.spin()
except KeyboardInterrupt:
......
Markdown is supported
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