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__':