User Tools

Site Tools


ardrone_follow_tag

This is an old revision of the document!


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 http://dasl.mem.drexel.edu/wiki/index.php/Tag_recognition_using_webcam .

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: <pre>roscd nolan3d </pre>

Step 2: Go to the bin and open up the file nolan3d.py and replace the code with the following code:

<source lang=“python”> #!/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()

</source>

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: <pre> roscore rosrun ardrone_brown ardrone_driver </pre>

Enter the commands in the same terminal:

<pre>

roscd ar_recog cd bin rosparam set aov .67 rosrun ar_recog ar_recog image:=/ardrone/image_raw

</pre>

Enter the commands ,each in a new terminal : <pre> rosrun drone_teleop drone_teleop.py </pre> Hit the button “t” for take off and close this window

Open up a new terminal and enter:

<pre> rosrun nolan3d nolan3d.py </pre>

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

<html> <iframe width=“853” height=“480” src=“http://www.youtube.com/embed/hh_w5F_MH8o” frameborder=“0” allowfullscreen></iframe> </html>

ardrone_follow_tag.1478023351.txt.gz · Last modified: 2016/11/01 11:02 by dwallace