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)
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)
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)
