Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
ENSTAR
formation_ros_2020
Commits
1d766a01
Commit
1d766a01
authored
Oct 09, 2017
by
Zhang Xuan
Browse files
Add follower in distance based on the radius of the green ball
parent
26ea68cf
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/find_color.py
View file @
1d766a01
...
...
@@ -21,12 +21,15 @@ def nothing(x):
pass
class
FindColor
:
def
__init__
(
self
,
debug
=
Tru
e
):
def
__init__
(
self
,
debug
=
Fals
e
):
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
_nam
e
,
self
.
mask_image
)
cv2
.
imshow
(
self
.
maske
d
,
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
:
...
...
src/follow_color.py
View file @
1d766a01
...
...
@@ -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
:
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment