Exemple #1
0
def c(data):
	global directionz, currentTarget, snelheid
	if img is None:
		return
	
	print currentTarget.__name__
	targets = [x for x in data.blobs if currentTarget(getat(img, x))]
	heatmap.cooldown(target_heatmap)
	heatmap.draw(target_heatmap, targets)
	hm2 = 1 * target_heatmap
	hm2[hm2 < 80] = 0
	
	f = cv.fromarray(hm2)
	hm_pub.publish(bridge.cv_to_imgmsg(f))
	hm_enc_pub.publish("16SC1")
	twist = Twist()
	# When a dot in the activation matrix is high enough we start to fly towards this place
	if hm2.max() > 16:
		snelheid = 0
		weights = np.apply_along_axis(np.sum, 0, hm2)
		if np.sum(hm2) > 30000:
			# found!
			target_heatmap[:] = 0
			if currentTarget in nextTarget:
				currentTarget = nextTarget[currentTarget]
			else:
				# Start landing
				twist.linear.x = -.1
				action.publish(twist)
				l = rospy.Publisher("/ardrone/land", Empty)
				l.publish(Empty())
				print "found"
				currentTarget = finished
		loc = np.average(indexes, weights=weights)/len(indexes) # When the target if farther to the left or right, we turn faster
		# Print what direction we are going to fly towards
		if loc < .5:
			print "left (new)"
		else:
			print "right (new)"
		twist.linear.x = .05
		twist.angular.z = .5 - loc
	else:
		snelheid = max(snelheid + .001, .05)
		twist.angular.z = snelheid
	action.publish(twist)
Exemple #2
0
def c(data):
	global directionz, currentTarget, snelheid
	if img is None:
		return
	lights = [x for x in data.blobs if getat(img, x)[0] > 100]
	if lights:
		naar = min(lights, key=lambda x: x.y)
		twist = Twist()
		twist.linear.x = .1
		if naar.x < 50:
			twist.angular.z = .7
			directionz = 1
			print "hard left"
		elif naar.y > WIDTH - 50:
			twist.angular.z = -.7
			directionz = -1
			print "hard right"
		elif naar.x < MIDPOINTX - 20:
			twist.angular.z = .3
			directionz = 1
			print "left"
		elif naar.x > MIDPOINTX + 20:
			twist.angular.z = -.3
			directionz = -1
			print "right"
		else:
			#twist.linear.x = .2
			print "straight on"
		action.publish(twist)
	else:
		twist = Twist()
		twist.angular.z = directionz
		#action.publish(twist)
		#print "n/a"
	print currentTarget.__name__
	targets = [x for x in data.blobs if currentTarget(getat(img, x))]
	heatmap.cooldown(target_heatmap)
	heatmap.draw(target_heatmap, targets)
	hm2 = 1 * target_heatmap
	hm2[hm2 < 80] = 0
	
	f = cv.fromarray(hm2)
	#print f, dir(f), f.step, f.channels, f.cols, f.rows, f.width, f.height
	hm_pub.publish(bridge.cv_to_imgmsg(f))
	hm_enc_pub.publish("16SC1")
	if targets:
		target = max(targets, key=lambda x: x.area)
		print target.area
		if target.area > 2500:
			#found!
			target_heatmap[:] = 0

			# If we have a new target after this one, take the next target. Otherwise we start landing
			if currentTarget in nextTarget:
				currentTarget = nextTarget[currentTarget]
			else:
				# Start landing
				twist = Twist()
				twist.linear.x = -.1
				action.publish(twist)
				l = rospy.Publisher("/ardrone/land", Empty)
				l.publish(Empty())
				print "found!"
				currentTarget = finished
	twist = Twist()
	if hm2.max() > 16:
		weights = np.apply_along_axis(np.sum, 0, hm2)
		loc = np.average(indexes, weights=weights)/len(indexes)
		if loc < .5:
			print "left (new)"
		else:
			print "right (new)"
		twist.linear.x = .1
		twist.angular.z = .5 - loc# - .5
	action.publish(twist)
Exemple #3
0
def c(data):
    global directionz, currentTarget, snelheid
    if img is None:
        return
    lights = [x for x in data.blobs if getat(img, x)[0] > 100]
    if lights:
        naar = min(lights, key=lambda x: x.y)
        twist = Twist()
        twist.linear.x = .1
        if naar.x < 50:
            twist.angular.z = .7
            directionz = 1
            print "hard left"
        elif naar.y > WIDTH - 50:
            twist.angular.z = -.7
            directionz = -1
            print "hard right"
        elif naar.x < MIDPOINTX - 20:
            twist.angular.z = .3
            directionz = 1
            print "left"
        elif naar.x > MIDPOINTX + 20:
            twist.angular.z = -.3
            directionz = -1
            print "right"
        else:
            #twist.linear.x = .2
            print "straight on"
        action.publish(twist)
    else:
        twist = Twist()
        twist.angular.z = directionz
        #action.publish(twist)
        #print "n/a"
    print currentTarget.__name__
    targets = [x for x in data.blobs if currentTarget(getat(img, x))]
    heatmap.cooldown(target_heatmap)
    heatmap.draw(target_heatmap, targets)
    hm2 = 1 * target_heatmap
    hm2[hm2 < 80] = 0

    f = cv.fromarray(hm2)
    #print f, dir(f), f.step, f.channels, f.cols, f.rows, f.width, f.height
    hm_pub.publish(bridge.cv_to_imgmsg(f))
    hm_enc_pub.publish("16SC1")
    if targets:
        target = max(targets, key=lambda x: x.area)
        print target.area
        if target.area > 2500:
            #found!
            target_heatmap[:] = 0

            # If we have a new target after this one, take the next target. Otherwise we start landing
            if currentTarget in nextTarget:
                currentTarget = nextTarget[currentTarget]
            else:
                # Start landing
                twist = Twist()
                twist.linear.x = -.1
                action.publish(twist)
                l = rospy.Publisher("/ardrone/land", Empty)
                l.publish(Empty())
                print "found!"
                currentTarget = finished
    twist = Twist()
    if hm2.max() > 16:
        weights = np.apply_along_axis(np.sum, 0, hm2)
        loc = np.average(indexes, weights=weights) / len(indexes)
        if loc < .5:
            print "left (new)"
        else:
            print "right (new)"
        twist.linear.x = .1
        twist.angular.z = .5 - loc  # - .5
    action.publish(twist)