Up and flying with the AR.Drone and ROS: Handling feedback

This is the third tutorial in the Up and flying with the AR.Drone and ROS series.

In this tutorial we will:

1. Learn about the AR.Drone’s state feedback (and how it is handled by ROS)
2. Learn about the AR.Drone’s tag detection
3. Program our first ROS nodes: A subscriber and a publisher

In the previous tutorials we:

1. Installed ROS, the AR.Drone driver and AR.Drone keyboard controller then flew the AR.Drone using the provided keyboard controller (link)
2. Learned about ROS communication, setup and then flew the AR.Drone using a joystick (link)

In the next tutorials, we will:

• Write a controller to enable tag following
• Write a controller for the drone which allows us to control drone velocity, rather than body angle

What you’ll need for the tutorial

1. An AR.Drone!
2. A computer or laptop with WiFi
3. Linux, either the ARDroneUbuntu virtual machine, or a custom installation
4. Either the keyboard or joystick controller from the previous tutorials

I also assume that you’ve completed either the keyboard or joystick tutorial, which detail how to setup the software, connect to the drone, etc, and provide a method of controlling the AR.Drone from your computer.

Updating

If you have not completed the joystick tutorial:

Please go back and update your installation according to the instructions listed there.

If you downloaded the ARDroneUbuntu virtual machine:

To update your installation for this tutorial, click the “Update Tutorials” button in the ARDroneUbuntu Virtual Machine

If you use a custom installation:

At the terminal, enter the following:

$ roscd ardrone_autonomy
$ git pull
$ roscd ardrone_tutorials
$ git pull
$ roscd
$ rosmake ardrone_tutorials

AR.Drone State Feedback – A First Look

Preparation:

1. Find the Orange-[Green|Yellow|Blue]-Orange tag that came with the AR.Drone.
2. In your ARDroneUbunutu virtual machine (or custom installation), open a new terminal window and type:

   $ roscd ardrone_tutorials/launch

3. If you intend to fly with the keyboard, type:

   $ geany keyboard_controller_with_tags.launch

   Otherwise, if you intend to fly with the joystick:

   $ geany joystick_controller.launch

4. Now find the line:

   <param name="enemy_colors" value="3" />

5. And change the value to “1”, “2”, or “3”, depending on whether your tags are Orange-Green-Orange, Orange-Yellow-Orange or Orange-Blue-Orange respectively.

Tag Detection and Feedback Data

1. Run the system (but don’t takeoff with the drone!) using either the keyboard_controller:

   $ roslaunch ardrone_tutorials keyboard_controller_with_tags.launch

   or the joystick_controller:

   $ roslaunch ardrone_tutorials joystick_controller.launch

2. Open a new terminal window and type:

   $ rostopic list

   • This list shows all the active ROS “topics” used for communication to and from the AR.Drone
3. In the same terminal window, type:

   $ rostopic echo /ardrone/navdata

   • This echoes the AR.Drone’s feedback data into the terminal window.
   • Note that many items will be zero because the AR.Drone is not yet flying
4. Move the Orange-Color-Orange tag in front of the AR.Drone’s front camera. It should be detected, and the console output (from the previous step), should update to reflect this detection.
   • Note the tags_xc and tags_yc values. These represent the x/y location of the tag within the image.
   • The video display window will show a dot at these coordinates, and the distance measurement above the dot
5. Now place the tag somewhere that can be seen by the flying drone. After this is done, take off with the drone.
6. Watch the console output. What happens if you fly forwards? sideways? What variable changes if you rotate the drone?

Plotting the AR.Drone’s State

1. A more intuitive way of representing this feedback data is using a plot. PressCtrl-C to terminate the echo process from the last stage, and then type:

   $ rxplot /ardrone/navdata/rotX,/ardrone/navdata/rotY

   • This launches a plotting window showing the AR.Drone’s estimated roll (rotX) and pitch (rotY).
   • Similar plots could, for example, be generated for yaw (rotZ), x and y velocity (vx and vy), altitude (altd), etc.
2. Similarly to displaying the AR.Drone’s feedback data, we can use the plotting method to investigate how we are controlling the drone:

   $ rxplot /cmd_vel/linear/y,/cmd_vel/linear/x

   • This plots the roll (linear/y) and pitch (linear/x) commands.
   • Note that these are scaled with a value of $\pm1$ being the maximum commandable angle (set by the euler_angle_max in the launch file).
   • Note also that the direction of AR.Drone movement is commanded (ie. move along the x-axis = /cmd_vel/linear/x = commanded pitch), whereas the AR.Drone’s feedback data is named with reference to axis of rotation (ie. tilt around y-axis = /ardrone/navdata/rotY = feedback pitch).

Logging Data

1. To log data, we can use the inbuilt ROS command rosbag:

   $ rosbag record -O ARDroneFlight.bag /ardrone/navdata /cmd_vel

   • This will record the topics /ardrone/navdata and /cmd_vel to a “bag-file”, stamped with the current date and time.
   • Leave this process running for a few seconds, fly the AR.Drone around, and when finished press Ctrl-C to terminate.
   • Recording all topics is possible with rosbag record -a, however this also records the AR.Drone’s HD video stream, thus bag-files will quickly become very large
   • The advantage of using rosbag to log data, is that bag-files can be replayed at a later time, and interact very nicely with the existing ROS infrastructure.
2. Land the AR.Drone and close any windows / terminate any processes relating to the controller. Now, into the terminal window that ran the AR.Drone controller, type:

   $ roscore

   Now open a new terminal window and type:

   $ rxplot /cmd_vel/linear/y,/cmd_vel/linear/x

   Now finally (ignoring the warning from the previous step), switch back to the terminal window that you used to run the rosbag command and type:

   $ rosbag play ARDroneFlight.bag

   Does the flight look familiar? It should! This is the flight you recorded in the last step! Use Ctrl-C to terminate the above processes.

Custom Data Handling

In the last section, we investigated the AR.Drone’s feedback data by using ROS’s inbuilt plotting (rxplot) and logging (rosbag) commands. In this section, you will learn how we get this feedback data into your own application by writing a custom ROS Node. This node will be used to compare the commanded roll and pitch angles with the AR.Drone’s roll and pitch estimates, thus laying the foundations for the next tutorial, where we will use this feedback data to programmatically control the AR.Drone.

Messages, Topics and Nodes

In ROS terminology, a program is known as a Node. To interact with other nodes (for example the AR.Drone driver), each node will have some subscribers, which listen for information messages on a specific topic (communications channel); and some publishers, which send messages to a specific topic.

These messages can be anything. An example that we saw in the previous section was the Navdata message, which is sent over the /ardrone/navdata topic and contains feedback data from the AR.Drone.

We will now investigate this message a little further. Open up a new terminal window and type

rosmsg show ardrone_autonomy/Navdata

You should be presented with the following output:

$ rosmsg show ardrone_autonomy/Navdata
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 batteryPercent
uint32 state
int32 magX
int32 magY
int32 magZ
int32 pressure
int32 temp
float32 wind_speed
float32 wind_angle
float32 wind_comp_angle
float32 rotX
float32 rotY
float32 rotZ
int32 altd
float32 vx
float32 vy
float32 vz
float32 ax
float32 ay
float32 az
uint32 tags_count
uint32[] tags_type
uint32[] tags_xc
uint32[] tags_yc
uint32[] tags_width
uint32[] tags_height
float32[] tags_orientation
float32[] tags_distance
float32 tm

This listing shows us the various members of the Navdata message, including their datatypes. Note that the Navdata message has a header, which includes the time the message is sent.

Minimum Viable Subscriber

Open up a new terminal window and type:

$ roscd ardrone_tutorials
$ geany src/minimum_viable_subscriber.py

Up will pop a geany window, ready for your to edit the new file. Enter the following into the new geany window, then save and close the file:

import roslib
import rospy

roslib.load_manifest('ardrone_tutorials')

from ardrone_autonomy.msg import Navdata

def ReceiveData(data):
  print '[{0:.3f}] Pitch: {1:.3f}'.format(data.header.stamp.to_sec(), data.rotY)

rospy.init_node('minimum_viable_subscriber')
sub_Navdata = rospy.Subscriber('/ardrone/navdata', Navdata, ReceiveData)

while not rospy.is_shutdown():
  pass

We will now use the previously recorded bag-file to test the node that we’ve just written.

1. In one terminal window, enter:

   $ roscore

   Which begins the roscore process, which coordinates the interactions between nodes.

2. Now in a new terminal window type:

   $ roscd ardrone_tutorials
   $ python src/minimum_viable_subscriber.py

   This will launch the node that we’ve just programmed.

3. Finally, in a third terminal window, begin the bag-file playback with:

   $ rosbag play ARDroneFlight.bag

   • Note you must be in the same directory as the bag-file for the above command to work
4. If you switch back to the terminal window in which our new node is running, you should see a stream of time-stamped printouts with the drone’s pitch, for example:

   [1353596639.491] Pitch: 4.375
   [1353596639.511] Pitch: 4.128
   [1353596639.531] Pitch: 3.750
   [1353596639.552] Pitch: 3.304
   [1353596639.571] Pitch: 3.063

Minimum Viable Subscriber: A Closer Look

If you made it through the previous section, congratulations, you have successfully written and tested a ROS node, which listens for Navdata measurements from the AR.Drone (or equivalently: a recorded flight) and outputs a time-stamped pitch measurement. But how does it all work?

Lets take a look at the source-code from the previous section, line by line:

import roslib
import rospy

With these two lines, we import the standard ROS packages, required for every ROS node.

roslib.load_manifest('ardrone_tutorials')

Once the required packages are imported, load the manifest file. This file details the dependencies of the current project (for example, that ardrone_tutorials is dependent on the AR.Drone driver, ardrone_autonomy), which through the above command are loaded into the path.

from ardrone_autonomy.msg import Navdata

Once the project dependencies have been loaded, we can import those which are needed by the current node. In the above case, we load the Navdata message definition from the ardrone_autonomy package.

def ReceiveData(data):
  print '[{0:.3f}] Pitch: {1:.3f}'.format(data.header.stamp.to_sec(), data.rotY)

Here we define a callback function to handle the Navdata messages. This function prints a formatted string containing the message’s timestamp and the AR.Drone’s pitch angle. This callback function will be used later in the program.

rospy.init_node('minimum_viable_subscriber')

Before issuing any commands to the rospy package, we need to initialize a ROS node.

sub_Navdata = rospy.Subscriber('/ardrone/navdata', Navdata, ReceiveData)

We can now subscribe the node to various topics. Above we subscribe the node to the /ardrone/navdata topic, which carries Navdata messages. Received messages should then be handled by the previously-defined ReceiveData function.

while not rospy.is_shutdown():
  pass

Because our program is relying on callbacks for data processing, we need to stop the program from terminating. The above defines a loop that runs until the node is shut down (for example with Ctrl-C). In most situations the above loop would perform some sort of processing, however in our example, we simply pass (do nothing).

Publishing Data

In this section we will introduce the concept of publishing data to a topic (the opposite of subscribing). We will be using this more extensively in the next tutorial, where we will be programmatically controlling the AR.Drone by publishing messages to the /cmd_vel topic.

Minimum Viable Publisher

The goal of this section is to create a minimum_viable_publisher.py file such that Navdata messages are averaged over a second and the average pitch published to the (new) /ardrone/average_pitch topic. Modify the file as follows:

import roslib
import rospy

roslib.load_manifest('ardrone_tutorials')

from ardrone_autonomy.msg import Navdata
from std_msgs.msg import String

messages = []

def ReceiveData(data):
  messages.append(data)

rospy.init_node('minimum_viable_publisher')
sub_Navdata = rospy.Subscriber('/ardrone/navdata', Navdata, ReceiveData)
pub_Average = rospy.Publisher('/ardrone/average_pitch', String)

r = rospy.Rate(1)
while not rospy.is_shutdown():
  if len(messages)>0:
    avg = sum([m.rotY for m in messages])/len(messages)
    messages = []

    avgmsg = String()
    avgmsg.data = 'Average Pitch: {0:.3f}'.format(avg)
    pub_Average.publish(avgmsg)
  r.sleep()

You can run the program using the following series of commands:

1. In one terminal, run:

   $ roscore

2. In the next terminal, run the new program

   $ roscd ardrone_tutorials
   $ python src/minimum_viable_publisher.py

3. We now echo the published data to the topic we’ve created:

   $ rostopic echo /ardrone/average_pitch

4. And finally, like we’ve done twice before, we begin the playback of our recorded flight

   $ rosbag play ARDroneFlight.bag

5. Switching back to the terminal in which we ranrostopic echo, you should see a new message with the average pitch appearing once a second:

   data: Average Pitch: 0.649
   ---
   data: Average Pitch: 1.078
   ---
   data: Average Pitch: 3.781
   ---
   data: Average Pitch: 3.285
   ---

Minimum Viable Publisher: A Closer Look

Comparing minimum_viable_publisher.py to minimum_viable_subscriber.py from the last section, we can see many similarities:

from ardrone_autonomy.msg import Navdata
from std_msgs.msg import String

We additionally import the String message, required for our new topic.

messages = []
def ReceiveData(data):
  messages.append(data)

We have updated the navdata callback function to save the messages in a queue, rather than process them straight away.

Note: because we’re accessing the messages list in both a callback function and the main loop, we should be using Python’s threading.Lock to control access to the list and prevent race conditions. See ardrone_tutorials/src/drone_video_display.py for an example.

pub_Average = rospy.Publisher('/ardrone/average_pitch', String)

We create a new publisher, responsible for publishing messages of the String type to the /ardrone/average_pitch topic. Note that we do not need to explicitly initialize this topic, it will be initialized automatically on first usage.

The biggest change is in the program’s main loop, which I will now cover in a little more detail:

r = rospy.Rate(1)

Firstly we define a Rate object. We use this to control the frequency of the main processing loop. In our case, we want to print a message every second, thus we want a frequency of $1 \text{Hz}$.

while not rospy.is_shutdown():
  if len(messages)>0:
    avg = sum([m.rotY for m in messages])/len(messages)
    messages = []

    avgmsg = String()
    avgmsg.data = 'Average Pitch: {0:.3f}'.format(avg)
    pub_Average.publish(avgmsg)

If messages have been received, we calculate the average pitch using a python list comprehension to extract the pitch (.rotY) from every Navdata message. We then empty the messages queue.

We then construct a new String message and set its data property, before publishing it to the topic we previously created.

r.sleep()

Finally, we tell the rate object to sleep. This command will calculate how long it needs to sleep, such that the desired loop frequency is achieved.

Conclusions

I hope you’ve enjoyed working through this tutorial and learning about data flow in the ROS environment. In the next tutorial, which I will release in a month’s time, we will use what we have learned today to programmatically control the AR.Drone, while at the same time learning about PID control.

If you have any questions about the tutorial, please leave me a comment below and I will get back to you as soon as possible!

You can see all the tutorials in here: Up and flying with the AR.Drone and ROS.

If you liked this article, you may also be interested in:

• Parrot AR.Drone app harnesses crowd power to fast-track vision learning in robotic spacecraft
• Taking your first steps in robotics with the Thymio II
• Discovering social robotics with the Aisoy1
• Classroom robotics: Motivating independent learning and discovery

See all the latest robotics news on Robohub, or sign up for our weekly newsletter.

Mike Hamer

Mike Hamer