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)