Table of Contents
Make the AR.Drone Follow the Image Tag
Introduction
This tutorial you will learn how to do tag following using the ardrone. Before you continue this tutorial make sure you have gone through this tutorial.
Create the tag following file
We already have the tag recognition code working and its all meant to identify image tags. This tutorial helps you to have tag recognition and also make the ardrone follow the tag.
Step 1:Open up a terminal and enter:
roscd nolan3d
Step 2: Go to the bin and open up the file nolan3d.py and replace the code with the following code:
- nolan3d.py
#!/usr/bin/env python import roslib; roslib.load_manifest('nolan') import rospy from ar_recog.msg import Tag, Tags from geometry_msgs.msg import Twist from std_msgs.msg import Empty from time import time class Delta: def __init__(self): self.old_value = 0 self.old_time = 0 self.old_vel = 0 def get_velocity(self, new_value): new_time = time() # velocity = distance / time vel = (new_value - self.old_value) / (new_time - self.old_time) # smooth velocity self.old_vel = self.old_vel + (vel - self.old_vel) * 0.1 # save values for next time self.old_time = new_time self.old_value = new_value return self.old_vel distance = Delta() yRot = Delta() def handleTags(msg): global pub global lastDir global lastSeen width = msg.image_width height = msg.image_height biggest = Tag() for tag in msg.tags: if (tag.diameter > biggest.diameter): biggest = tag if biggest.diameter == 0: twist = Twist() twist.linear.x = 0 if (time() - lastSeen > .5): twist.angular.z = .5*lastDir pub.publish(twist) # reset velocity and distance distance.get_velocity(0) yRot.get_velocity(0) return lastSeen = time() cx = 0; cy = 0 for i in [0,2,4,6]: cx = cx + biggest.cwCorners[i] cy = cy + biggest.cwCorners[i+1] cx = cx / 4. / width cy = cy / 4. / height twist = Twist() # move foward and backward, trying to stop at stopping_dist stopping_dist = 2000. dist = (biggest.distance - stopping_dist) / stopping_dist print biggest.distance print dist dist_vel = distance.get_velocity(dist) if abs(dist) < 0.25: # if we are close enough to the stopping distance, just try to stop twist.linear.x = dist_vel * 0.25 else: # otherwise try to move within stopping_dist twist.linear.x = dist * 0.25 print twist.linear.x twist.linear.x = max(0.03, min(0.05, twist.linear.x)) #change done here for stopping at a optimal distance after seeing the tag by karthik # try to face perpendicular to the tag yRot_velocity = yRot.get_velocity(biggest.yRot) if abs(biggest.yRot) < 0.5: # if we are mostly facing perpendicular, just try to stay still twist.linear.y = yRot_velocity * 0.25 else: # otherwise, rotate towards being in front of the tag twist.linear.y = biggest.yRot * 0.25 twist.linear.y = max(-0.05, min(0.05, twist.linear.y)) # rotate to face the tag twist.angular.z = (-(cx - .5)/.5) if (twist.angular.z < 0): lastDir = -1 else: lastDir = 1 pub.publish(twist) print twist if __name__ == "__main__": global pub global lastDir global lastSeen lastSeen = 0 lastDir = -1 rospy.init_node('nolan', anonymous=True) pub = rospy.Publisher('cmd_vel', Twist) rospy.Subscriber("tags", Tags, handleTags) takeoff_pub =rospy.Publisher('/ardrone/takeoff', Empty) rospy.spin()
Save and close the file.
Set the drone to see the image tag
Make sure the ardrone is on and connected with your computer. Enter the following lines each in a new terminal:
roscore rosrun ardrone_brown ardrone_driver
Enter the commands in the same terminal:
roscd ar_recog cd bin rosparam set aov .67 rosrun ar_recog ar_recog image:=/ardrone/image_raw
Enter the commands ,each in a new terminal :
rosrun drone_teleop drone_teleop.py
Hit the button “t” for take off and close this window
Open up a new terminal and enter:
rosrun nolan3d nolan3d.py
Following the tag
Now the drone would be in air. Make sure you hold the image tag at a optimal distance(not too close). Please have a look at this video.
Video