import rospy
import cv2
import copy
import time
import math
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from cmvision.msg import Blobs, Blob
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
colorImage = Image()
isColorImageReady = False
blobsInfo = Blobs()
isBlobsInfoReady = False
command = Twist()
command.linear.x = 0
command.angular.z = 0
pub = rospy.Publisher("kobuki_command", Twist, queue_size=10)
odomReset = rospy.Publisher('/mobile_base/commands/reset_odometry',
Empty,
queue_size=10)
positionX = 0.0
positionY = 0.0
degree = 0.0
tempDeg = 0.0
velocity = 0.0
distOffset = 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
#!/usr/bin/python
import rospy
from std_msgs.msg import Empty
from geometry_msgs.msg import Twist
from cmvision.msg import Blobs, Blob
from sensor_msgs.msg import Image
import location
pub_command = rospy.Publisher("/kobuki_command", Twist,
queue_size=10) # publish command
pub_stop = rospy.Publisher("/emergency_stop", Empty,
queue_size=10) # publish stop
# initialization
rawBlobs = Blobs()
mergedBlobs = {}
width = 0
def init():
rospy.init_node("test") # initialize the node
location.init() # init the location
rospy.on_shutdown(cleanUp)
rospy.Subscriber("/blobs", Blobs, setRawBlobs) # subscribe to blobs
def track_blobs():
global rawBlobs, pub_command
Z_MAX = 0.5 # maximum speed
#!/usr/bin/env python
import rospy
from cmvision.msg import Blobs, Blob
mergedBlobs = Blobs()
colors = []
def blobsCallback(data):
b = Blob()
x = 0
y = 0
area = 0
if data.blob_count > 0:
for box in data.blobs:
area = area + box.area
x = x + (box.x * box.area)
y = y + (box.y * box.area)
x = x / area
y = y / area
print "(%i,%i)" % (x, y)
def detect_blob():
rospy.init_node('blob_tracker', anonymous=True)
rospy.Subscriber('/blobs', Blobs, blobsCallback)
rospy.spin()
if __name__ == '__main__':
