def mergeBlobs(blobs):
x = 0;
y = 0;
left = 0;
right = 0;
top = 0;
bottom = 0;
area = 0;
for b in blobs:
x = x + (b.x * b.area)
y = y + (b.y * b.area)
left = left + (b.left * b.area)
right = right + (b.right * b.area)
top = top + (b.top * b.area)
bottom = bottom + (b.bottom * b.area)
area = area + b.area
result = Blob()
result.x = x / area
result.y = y / area
result.left = left / area
result.right = right / area
result.top = top / area
result.bottom = bottom / area
result.area = x * y
return result
def localize(self, tags):
global arr_blob_pos
arr_blob_pos = []
for tag in tags.tags:
for color_rec in arr_color_codes:
if (color_rec[3]== tag.id):
#print "Got a tag of interest"
#This blob should be converted to a tag. Do it
blob_from_tag = Blob()
blob_from_tag.red = color_rec[0]
blob_from_tag.green = color_rec[1]
blob_from_tag.blue = color_rec[2]
blob_from_tag.x = tag.x
blob_from_tag.y = tag.y
arrx = [tag.cwCorners[0], tag.cwCorners[2], tag.cwCorners[4], tag.cwCorners[6]]
arry = [tag.cwCorners[1], tag.cwCorners[3], tag.cwCorners[5], tag.cwCorners[7]]
minx = min(arrx)
maxx = max(arrx)
miny = min(arry)
maxy = max(arry)
if (minx < 0):
minx = 0
if (maxx < 0):
maxx = 0
if (miny < 0):
miny = 0
if (maxy < 0):
maxy = 0
blob_from_tag.left = int(minx)
blob_from_tag.top = int(miny)
blob_from_tag.right = int(maxx)
blob_from_tag.bottom = int(maxy)
arr_blob_pos.append(blob_from_tag)
def localize(self, tags):
global arr_blob_pos
arr_blob_pos = []
for tag in tags.tags:
for color_rec in arr_color_codes:
if color_rec[3] == tag.id:
# print "Got a tag of interest"
# This blob should be converted to a tag. Do it
blob_from_tag = Blob()
blob_from_tag.red = color_rec[0]
blob_from_tag.green = color_rec[1]
blob_from_tag.blue = color_rec[2]
blob_from_tag.x = tag.x
blob_from_tag.y = tag.y
arrx = [tag.cwCorners[0], tag.cwCorners[2], tag.cwCorners[4], tag.cwCorners[6]]
arry = [tag.cwCorners[1], tag.cwCorners[3], tag.cwCorners[5], tag.cwCorners[7]]
minx = min(arrx)
maxx = max(arrx)
miny = min(arry)
maxy = max(arry)
if minx < 0:
minx = 0
if maxx < 0:
maxx = 0
if miny < 0:
miny = 0
if maxy < 0:
maxy = 0
blob_from_tag.left = int(minx)
blob_from_tag.top = int(miny)
blob_from_tag.right = int(maxx)
blob_from_tag.bottom = int(maxy)
arr_blob_pos.append(blob_from_tag)
def mergeBlobs(blobs):
global tempBlobs3
x = 0;
y = 0;
left = 0;
right = 0;
top = 0;
bottom = 0;
area = 0;
bigBlob = Blob()
result = Blob()
tempBlobs = copy.deepcopy(blobs)
tempBlobs = [x for x in tempBlobs if x.name == "Green"]
temp2Blobs = copy.deepcopy(tempBlobs)
tempBlobs3 = copy.deepcopy(tempBlobs)
endBlobs = []
xDistance = 0
yDistance = 0
maxXDistance = 0
maxYDistance = 0
while len(tempBlobs) > 0:
for b in tempBlobs:
if b.area > area:
area = b.area
bigBlob = copy.deepcopy(b)
area = 0
size = len(tempBlobs)
counter = 0
while counter < size:
counter = 0
size = len(tempBlobs)
for c in tempBlobs:
xDistance = abs(((bigBlob.right - bigBlob.left)/2 + bigBlob.left) - ((c.right - c.left)/2 + c.left))
yDistance = abs(((bigBlob.bottom - bigBlob.top)/2 + bigBlob.top) - ((c.bottom - c.top)/2 + c.top))
maxXDistance = abs(abs((bigBlob.right - bigBlob.left)/2) + abs((c.right - c.left)/2))
maxYDistance = abs(abs((bigBlob.bottom - bigBlob.top)/2) + abs((c.bottom - c.top)/2))
if (maxXDistance >= xDistance and maxYDistance >= yDistance) and bigBlob.left > c.left:
bigBlob.left = c.left
if (maxXDistance >= xDistance and maxYDistance >= yDistance) and bigBlob.right < c.right:
bigBlob.right = c.right
if (maxXDistance >= xDistance and maxYDistance >= yDistance) and bigBlob.top > c.top:
bigBlob.top = c.top
if (maxXDistance >= xDistance and maxYDistance >= yDistance) and bigBlob.bottom < c.bottom:
bigBlob.bottom = c.bottom
if (maxXDistance >= xDistance and maxYDistance >= yDistance):
temp2Blobs.remove(c)
else:
counter = counter + 1
bigBlob.area = abs((bigBlob.right - bigBlob.left)*(bigBlob.top - bigBlob.bottom))
bigBlob.x = (bigBlob.right - bigBlob.left)/2 + bigBlob.left
bigBlob.y = (bigBlob.bottom - bigBlob.top)/2 + bigBlob.top
tempBlobs = copy.deepcopy(temp2Blobs)
endBlobs.append(copy.deepcopy(bigBlob))
area = 0
for d in endBlobs:
if d.area > area:
area = d.area
result = copy.deepcopy(d)
return result
def mergeBlobs(blobsInfo):
x = 0
y = 0
left = 0
right = 0
top = 0
bottom = 0
area = 0
overlapsFound = 1
blobs = blobsInfo.blobs
for b in blobs:
blobs2 = copy.deepcopy(blobs)
for b2 in blobs2:
if (b.name == b2.name and b != b2):
overlapping = 0
blobx = b2.left
while blobx < b2.right and overlapping == 0:
bloby = b2.top
while bloby < b2.bottom and overlapping == 0:
if blobx >= b.left and blobx <= b.right and bloby >= b.top and bloby <= b.bottom:
overlapping = 1
bloby = bloby + 1
blobx = blobx + 1
if overlapping == 1:
x = b.x + (b2.x * b2.area)
y = b.y + (b2.y * b2.area)
left = b.left + (b2.left * b2.area)
right = b.right + (b2.right * b2.area)
top = b.top + (b2.top * b2.area)
bottom = b.bottom + (b2.bottom * b2.area)
area = b.area + b2.area
result = Blob()
result.x = x / area
result.y = y / area
result.left = left / area
result.right = right / area
result.top = top / area
result.bottom = bottom / area
result.area = x * y
b = result
blobs.remove(b2)
return blobs
def mergeBlobs(blobs):
global bigBlob, pub
x = 0
y = 0
left = 0
right = 0
top = 0
bottom = 0
area = 0
# Sort the blobs and find the big blob
bBA = 0
for blob in blobs:
if blob.area > bBA:
bBA = blob.area
bigBlob = blob
# Filter blobs, remove all blobs not within area
blobs = filter(filterBlob, blobs)
if len(blobs) == 0:
blobs.append(bigBlob)
firstBlob = blobs[0]
maxLeft = firstBlob.left
maxRight = firstBlob.right
maxTop = firstBlob.top
maxBottom = firstBlob.bottom
for blob in blobs:
maxLeft = blob.left if blob.left < maxLeft else maxLeft
maxRight = blob.right if blob.right > maxRight else maxRight
maxTop = blob.top if blob.top < maxTop else maxTop
maxBottom = blob.bottom if blob.bottom > maxBottom else maxBottom
# Create new blob based on prior blobs (i.e. merged)
result = Blob()
result.x = (maxLeft + maxRight) / 2
result.y = (maxTop + maxBottom) / 2
result.left = maxLeft
result.right = maxRight
result.top = maxTop
result.bottom = maxBottom
result.area = x * y
pub.publish(result.x)
print "Error: ", result.x - 320
return result
def mergeBlobs(blobs):
x = 0
y = 0
left = 0
right = 0
top = 0
bottom = 0
area = 0
bigBlob = Blob()
result = Blob()
blobCopy = copy.deepcopy(blobs)
tempBlobs = [x for x in blobCopy if x.name == "Out"]
temp2Blobs = copy.deepcopy(tempBlobs)
tempBlobs2 = [x for x in blobCopy if x.name == "In"]
temp2Blobs2 = copy.deepcopy(tempBlobs2)
endBlobs = []
endBlobs2 = []
endBlobs3 = []
xDistance = 0
yDistance = 0
maxXDistance = 0
maxYDistance = 0
while len(tempBlobs) > 0:
for b in tempBlobs:
if b.area > area:
area = b.area
bigBlob = copy.deepcopy(b)
area = 0
size = len(tempBlobs)
counter = 0
while counter < size:
counter = 0
size = len(tempBlobs)
for c in tempBlobs:
xDistance = abs((
(bigBlob.right - bigBlob.left) / 2 + bigBlob.left) -
((c.right - c.left) / 2 + c.left))
yDistance = abs((
(bigBlob.bottom - bigBlob.top) / 2 + bigBlob.top) -
((c.bottom - c.top) / 2 + c.top))
maxXDistance = abs(
abs((bigBlob.right - bigBlob.left) / 2) +
abs((c.right - c.left) / 2))
maxYDistance = abs(
abs((bigBlob.bottom - bigBlob.top) / 2) +
abs((c.bottom - c.top) / 2))
if (maxXDistance >= xDistance and
maxYDistance >= yDistance) and bigBlob.left > c.left:
bigBlob.left = c.left
if (maxXDistance >= xDistance and
maxYDistance >= yDistance) and bigBlob.right < c.right:
bigBlob.right = c.right
if (maxXDistance >= xDistance
and maxYDistance >= yDistance) and bigBlob.top > c.top:
bigBlob.top = c.top
if (maxXDistance >= xDistance and maxYDistance >= yDistance
) and bigBlob.bottom < c.bottom:
bigBlob.bottom = c.bottom
if (maxXDistance >= xDistance and maxYDistance >= yDistance):
temp2Blobs.remove(c)
else:
counter = counter + 1
bigBlob.area = abs((bigBlob.right - bigBlob.left) *
(bigBlob.top - bigBlob.bottom))
bigBlob.x = (bigBlob.right - bigBlob.left) / 2 + bigBlob.left
bigBlob.y = (bigBlob.bottom - bigBlob.top) / 2 + bigBlob.top
tempBlobs = copy.deepcopy(temp2Blobs)
endBlobs.append(copy.deepcopy(bigBlob))
while len(tempBlobs2) > 0:
for b in tempBlobs2:
if b.area > area:
area = b.area
bigBlob = copy.deepcopy(b)
area = 0
size = len(tempBlobs2)
counter = 0
while counter < size:
counter = 0
size = len(tempBlobs2)
for c in tempBlobs2:
xDistance = abs((
(bigBlob.right - bigBlob.left) / 2 + bigBlob.left) -
((c.right - c.left) / 2 + c.left))
yDistance = abs((
(bigBlob.bottom - bigBlob.top) / 2 + bigBlob.top) -
((c.bottom - c.top) / 2 + c.top))
maxXDistance = abs(
abs((bigBlob.right - bigBlob.left) / 2) +
abs((c.right - c.left) / 2))
maxYDistance = abs(
abs((bigBlob.bottom - bigBlob.top) / 2) +
abs((c.bottom - c.top) / 2))
if (maxXDistance >= xDistance and
maxYDistance >= yDistance) and bigBlob.left > c.left:
bigBlob.left = c.left
if (maxXDistance >= xDistance and
maxYDistance >= yDistance) and bigBlob.right < c.right:
bigBlob.right = c.right
if (maxXDistance >= xDistance
and maxYDistance >= yDistance) and bigBlob.top > c.top:
bigBlob.top = c.top
if (maxXDistance >= xDistance and maxYDistance >= yDistance
) and bigBlob.bottom < c.bottom:
bigBlob.bottom = c.bottom
if (maxXDistance >= xDistance and maxYDistance >= yDistance):
temp2Blobs2.remove(c)
else:
counter = counter + 1
bigBlob.area = abs((bigBlob.right - bigBlob.left) *
(bigBlob.top - bigBlob.bottom))
bigBlob.x = (bigBlob.right - bigBlob.left) / 2 + bigBlob.left
bigBlob.y = (bigBlob.bottom - bigBlob.top) / 2 + bigBlob.top
tempBlobs2 = copy.deepcopy(temp2Blobs2)
endBlobs2.append(copy.deepcopy(bigBlob))
area = 0
for d in endBlobs:
for e in endBlobs2:
if (d.left < e.left and d.right > e.right and d.top < e.top
and d.bottom > e.bottom):
endBlobs3.append(d)
area = 0
for f in endBlobs3:
if f.area > area:
area = f.area
result = copy.deepcopy(f)
return result
import cv2
import copy
import math
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from cmvision.msg import Blobs, Blob
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
from kobuki_msgs.msg import BumperEvent
pub = rospy.Publisher("kobuki_command", Twist, queue_size=10)
command = Twist()
oneBlob = Blob()
oneBlob.x = -10
oneBlob2 = Blob()
oneBlob2.x = -10
alpha1 = 0
beta1 = 0
alpha2 = 0
beta2 = 0
colorImage = Image()
isColorImageReady = False
blobsInfo = Blobs()
isBlobsInfoReady = False
areaBall = 0
areaGoal = 0
degree = 0
x = 0
def mergeBlobs(blobs):
global bigRedBlob,bigPinkBlob, pub
x = 0
y = 0
left = 0
right = 0
top = 0
bottom = 0
area = 0
pinkBlobs = []
redBlobs = []
for blob in blobs:
if blob.name == "BallRed":
redBlobs.append(blob)
elif blob.name == "GoalPink":
pinkBlobs.append(blob)
# Sort the blobs and find the big blob
bBA = 0
for blob in redBlobs:
if blob.area > bBA:
bBA = blob.area
bigRedBlob = blob
bBA = 0
for blob in pinkBlobs:
if blob.area > bBA:
bBA = blob.area
bigPinkBlob = blob
# Filter blobs, remove all blobs not within area
pBlobs = filter(filterBlob, pinkBlobs)
rBlobs = filter(filterBlob, redBlobs)
if len(pBlobs) == 0:
pBlobs.append(bigPinkBlob)
if len(rBlobs) == 0:
rBlobs.append(bigRedBlob)
firstPinkBlob = pBlobs[0]
maxLeft = firstPinkBlob.left
maxRight = firstPinkBlob.right
maxTop = firstPinkBlob.top
maxBottom = firstPinkBlob.bottom
for blob in pBlobs:
maxLeft = blob.left if blob.left < maxLeft else maxLeft
maxRight = blob.right if blob.right > maxRight else maxRight
maxTop = blob.top if blob.top < maxTop else maxTop
maxBottom = blob.bottom if blob.bottom > maxBottom else maxBottom
# Create new blob based on prior blobs (i.e. merged)
pResult = Blob()
pResult.x = (maxLeft + maxRight) / 2
pResult.y = (maxTop + maxBottom) / 2
pResult.left = maxLeft
pResult.right = maxRight
pResult.top = maxTop
pResult.bottom = maxBottom
pResult.area = x * y
pResult.name = firstPinkBlob.name
firstRedBlob = rBlobs[0]
maxLeft = firstRedBlob.left
maxRight = firstRedBlob.right
maxTop = firstRedBlob.top
maxBottom = firstRedBlob.bottom
for blob in rBlobs:
maxLeft = blob.left if blob.left < maxLeft else maxLeft
maxRight = blob.right if blob.right > maxRight else maxRight
maxTop = blob.top if blob.top < maxTop else maxTop
maxBottom = blob.bottom if blob.bottom > maxBottom else maxBottom
rResult = Blob()
rResult.x = (maxLeft + maxRight) / 2
rResult.y = (maxTop + maxBottom) / 2
rResult.left = maxLeft
rResult.right = maxRight
rResult.top = maxTop
rResult.bottom = maxBottom
rResult.area = x * y
rResult.name = firstRedBlob.name
blobArray = Blobs()
blobArray.blobs = [pResult, rResult]
pub.publish(blobArray)
return blobArray
def main():
global colorImage, isColorImageReady, blobsInfo, isBlobsInfoReady, pub, degree, x, y, tempBlobs3
rospy.init_node('showBlobs', anonymous=True)
rospy.Subscriber("/blobs", Blobs, updateBlobsInfo)
rospy.Subscriber('/odom', Odometry, odomCallback)
rospy.Subscriber("/v4l/camera/image_raw", Image, updateColorImage)
bridge = CvBridge()
cv2.namedWindow("Blob Location")
flagit = False
oneBlob = Blob()
oneBlob.x = -10
zeroCounter = 0
angArray = []
angArray.append(0)
angArray.append(0)
angIndex = 2
voice = True
while not rospy.is_shutdown() and (not isBlobsInfoReady or not isColorImageReady):
pass
flag = 0
while not rospy.is_shutdown():
try:
color_image = bridge.imgmsg_to_cv2(colorImage, "bgr8")
except CvBridgeError, e:
print e
print "colorImage"
blobsCopy = copy.deepcopy(blobsInfo)
tempBlobs3 = [x for blob in blobsCopy.blobs if blob.name == "Green"]
if len(blobsCopy.blobs) > 0:
oneBlob = mergeBlobs(blobsCopy.blobs)
cv2.rectangle(color_image, (oneBlob.left, oneBlob.top), (oneBlob.right, oneBlob.bottom), (0,255,0), 2)
flagit = True
cv2.imshow("Color Image", color_image)
cv2.waitKey(1)
while (len(tempBlobs3) > 0):
if len(tempBlobs3) > 0:
command.linear.x = .3
pub.publish(command)
if oneBlob.x <= 320 and oneBlob.x >= 0:
command.angular.z = (320 - oneBlob.x) * .0075 + (angArray[angIndex - 1] - angArray[angIndex - 2])*.001
if oneBlob.x > 320 and oneBlob.x <= 640:
command.angular.z = (oneBlob.x - 320) * -.0075 - (angArray[angIndex - 1] - angArray[angIndex - 2])*.001
angArray.append(command.angular.z)
angIndex = angIndex + 1
pub.publish(command)
blobsCopy = copy.deepcopy(blobsInfo)
oneBlob = mergeBlobs(blobsCopy.blobs)
# cv2.rectangle(color_image, (oneBlob.left, oneBlob.top), (oneBlob.right, oneBlob.bottom), (0,255,0), 2)
else:
command.linear.x = command.linear.x*.9
if command.linear.x > .05:
command.linear.x = command.linear.x - .005
if command.linear.x <= .05:
flag = 1
if flag == 1:
command.linear.x = 0
pub.publish(command)
os.system('spd-say \"show me the ball\"')
print("f")
flag = 0
command.angular.z = 0
# cv2.imshow("Color Image", color_image)
# cv2.waitKey(1)
pub.publish(command)
flagit = True
zeroCounter = 0