Note: This tutorial assumes that you have completed the previous tutorials: Creating a Package, Writing a Simple Publisher and Subscriber, Blinking an Arduino LED, Connecting an Android Device to a ROS Master.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Driving an Arduino Board LED from an Android Device (Python)

Description: This tutorial covers how to write a node in Python to connect an Android device with an Arduino board.

Tutorial Level: BEGINNER

Introduction

Four nodes will be used to connect the Arduino board with the Android device: a Master node, an Arduino node, an Android Sensors Driver node and finally an Android Sensors Driver to Arduino node. This last node ensures the communication between both Android and Arduino nodes through the Master one.

This tutorial covers the creation of the Android Sensors Driver to Arduino node and the system setup.

Creating a Package

The easier way of ensuring all the dependencies of your node are met is creating a package and declaring them in its manifest.xml file.

Now go into the ~/ros_workspace directory then create your package (the manifest.xml file will be automatically created):

$ cd ~/ros_workspace
$ roscreate-pkg android_to_arduino rospy roscpp std_msgs sensor_msgs

You can now change directory directly to your package:

$ roscd android_to_arduino

Setting the Arduino node up

To set your Arduino board up, you can follow this tutorial until uploading the code to your Arduino. Once the sketch uploaded, come back here. We will run it later.

Setting the Android device up

The Android Sensors Driver application should be installed on you Android device. You don't need to run it for now, as we will do it later.

Writing the Android Sensors Driver to Arduino Node

Change directory into the android_to_arduino package you created earlier in this tutorial:

$ roscd android_to_arduino

The code

Create the nodes/android_sensors_driver_to_arduino.py file within the android_to_arduino package and paste the following inside it:

   1 #!/usr/bin/env python
   2 import roslib; roslib.load_manifest('android_to_arduino')
   3 import rospy
   4 
   5 # Input messages type
   6 from sensor_msgs.msg import Imu
   7 # Output messages type
   8 from std_msgs.msg import Empty
   9 
  10 class AndroidSensorsDriverToArduino(object):
  11   
  12     def __init__(self):
  13         """Toggle an Arduino board LED from Android Sensors Driver.
  14 
  15         This node links an android_sensors_driver publisher [1]
  16         and an Arduino subscriber [2].
  17 
  18         [1] http://www.ros.org/wiki/android_sensors_driver/Tutorials/Connecting%20to%20a%20ROS%20Master
  19         [2] http://www.ros.org/wiki/rosserial_arduino/Tutorials/Blink
  20 
  21         """
  22         # Create a publisher for the topic the Arduino node listens to.
  23         self.pub = rospy.Publisher('toggle_led', Empty)
  24         # Intitialize variables
  25         self.last_status = 0
  26         self.status = 0
  27 
  28     def callback(self, data):
  29         """A sensor_msgs/Imu callback method.
  30 
  31         This method is called each time a sensor_msgs/Imu message
  32         is received from the /android/imu topic.
  33 
  34         """
  35         if data.linear_acceleration.x > 0:
  36             self.status = 0
  37         else:
  38             self.status = 1
  39         if self.status != self.last_status:
  40             # Send toggle_led message
  41             self.pub.publish(Empty())
  42             self.last_status = self.status
  43 
  44     def run(self):
  45         while not rospy.is_shutdown():
  46             rospy.init_node('android_sensors_driver_to_arduino')
  47             # Subscribe the node to the /android/Imu topic
  48             rospy.Subscriber('android/imu', Imu, self.callback)
  49             rospy.loginfo("Welcome to the android_sensors_driver_to_arduino node!")
  50             # Wait for incoming messages
  51             rospy.spin()
  52 
  53 if __name__ == '__main__':
  54     try:
  55         node = AndroidSensorsDriverToArduino()
  56         node.run()
  57     except rospy.ROSInterruptException: pass

Don't forget to make the node executable:

$ chmod +x nodes/android_sensors_driver_to_arduino.py

The generic code explained

   1 #!/usr/bin/env python
   2 import roslib; roslib.load_manifest('android_to_arduino')

These lines declare your node as executable and tell roslib to read in your manifest.xml file and add all the dependencies listed there to your PYTHONPATH. If you omit this step, or if your manifest.xml file is uncomplete (any dependency is missing), you won't be able to import the libraries in the next section of code.

   3 import rospy
   4 
   5 # Input messages type
   6 from sensor_msgs.msg import Imu
   7 # Output messages type
   8 from std_msgs.msg import Empty

Importing rospy is required to write a ROS node. We will also use both sensor_msgs/Imu and std_msgs/Empty messages types. The first one is emitted by the Android Sensors Driver node, while the second one is expected by the Arduino node to blink the LED.

  53 if __name__ == '__main__':

When the node will be run using rosrun, the code will be executed starting here.

  55         node = AndroidSensorsDriverToArduino()
  56         node.run()

The first line tells python to create a node instance from the AndroidSensorsDriverToArduino class. While doing this, the __init__() method will be called. (This behavior belongs to pyhton and is not specific to ROS.)

The second line calls the run() method of the newly created node instance. It runs the node.

  54     try:
  55         node = AndroidSensorsDriverToArduino()
  56         node.run()
  57     except rospy.ROSInterruptException: pass

Enclosing the past two lines into a try ... except construct ensure that if anything goes wrong while your node is running, rospy will be warned and will provide you some feedback.

  44     def run(self):
  45         while not rospy.is_shutdown():
  46             rospy.init_node('android_sensors_driver_to_arduino')

The run() method is a classical ROS construct: it first check that rospy is running, then it initializes the node (i.e. it assigns a name to the node and connects it with the Master node).

  50             # Wait for incoming messages
  51             rospy.spin()

After performing some actions (we'll see them in detail later), it finally keeps the node running.

The subscriber code explained

  47             # Subscribe the node to the /android/Imu topic
  48             rospy.Subscriber('android/imu', Imu, self.callback)
  49             rospy.loginfo("Welcome to the android_sensors_driver_to_arduino node!")

Subscriptions should be performed once the node is connected to the Master node, it's why they belong to the run() method. Once the ROS part of the initialization is finished, we subscribe to the /android/Imu topic by providing the message type we are interested in (sensor_msgs/Imu) and a callback method, callback().

Each time a new message of the correct type will be posted to the topic by the Android Sensors Driver node, ROS will invoke the callback method.

Once the subscribtion has been performed, our node initialization is complete and we can inform the user logging a welcome message.

  28     def callback(self, data):
  29         """A sensor_msgs/Imu callback method.
  30 
  31         This method is called each time a sensor_msgs/Imu message
  32         is received from the /android/imu topic.
  33 
  34         """
  35         if data.linear_acceleration.x > 0:
  36             self.status = 0
  37         else:
  38             self.status = 1
  39         if self.status != self.last_status:
  40             # Send toggle_led message
  41             self.pub.publish(Empty())
  42             self.last_status = self.status

An extra argument data should be used when calling the callback() method. This argument, the message which triggered the method call, will each time be automatically provided by ROS.

We know which type of message will be received this way (remember we specified it when subscribing to the topic), so we can use its contents to determine the Android device orientation.

In this example, we know the device will always be accelerated by the gravity, and we use the value of the linear_acceleration among the x axis to determine how the device is oriented relatively to the Earth. (Yes, this code would work on the International Space Station.) If the device orientation has changed since we last read it, we should toggle the LED.

The publisher code explained

  22         # Create a publisher for the topic the Arduino node listens to.
  23         self.pub = rospy.Publisher('toggle_led', Empty)

We may declare on which topics our node will publish before it gets connected to the Master node. It's why the Pubisher instance creation belongs to the __init__() method.

Unlike with the subscriber, we need to keep a reference to the created Publisher instance because we will call its publish() method to emit messages, so we give a name to it (pub) and store it inside our node.

  40             # Send toggle_led message
  41             self.pub.publish(Empty())

When required, we can use the Publisher instance we stored to emit a message. We must pass the message we want to publish as an argument of its publish() method. In this example, the Arduino node expects a std_msgs/Empty message, which can be easily created calling the Empty() function without arguments.

Usage

Running the Master, Arduino and Android Sensors Driver To Arduino Nodes

First of all, the Android Sensors Driver node requires the IP address of the Master node to be known. Check which IP uses the computer where you will run roscore (you can read it from ifconfig for example), then make this information available to roscore (192.168.42.103 is an example):

$ export ROS_MASTER_URI=http://192.168.42.103:11311/

Once done, run the Master node:

$ roscore

Let's run your Arduino node. (Your Arduino board should be connected. It has been set up in the introduction of this tutorial.) Open a new terminal, and run the node (see this section for details):

$ rosrun rosserial_python serial_node.py /dev/ttyUSB0

You can test toggling the Arduino board LED with rostopic:

$ rostopic pub toggle_led std_msgs/Empty --once

To run the Arduino Sensors Driver to Arduino node you have created, open a third terminal and run:

$ rosrun android_to_arduino android_sensors_driver_to_arduino.py

Running the Android Sensors Driver Node

Finally, launch the Android Sensors Driver on you Android device and provide it the IP of the Master node to connect with, as described here (192.168.42.103 in our example).

You can check if the Android Sensors Driver node is working by opening a fourth terminal and displaying the messages published on the android/Imu topic:

$ rostopic echo /android/imu

This topic should be filled with changing sensor_msgs/Imu type messages.

If everything is going OK, move you Android device, you should be able to control the way the Arduino board LED blinks.

Of course, you can check your Android Sensors Driver to Arduino node is correctly emitting the toggle messages when the orientation of you Android device changes, by displaying the /toggle_led topic contents in a fith terminal:

$ rostopic echo /toggle_led

Tips

This configuration works well with my Android device (I move it from one landscape orientation to another). Depending on the way the axes system are attached to yours, you may obtain better results by seeking the linear_acceleration over the y ot z axes. Display the /android/Imu topic to see how the linear acceleration evolves when you change your device orientation.

Because the Android Sensors Driver emits continuously a lot of messages, I also find putting the Android device in a stable position before launching the application helps to understand what is happening.

Further reading

Please see rosserial/Overview for more information on publishers and subscribers. Also see limitations for information about how your Arduino board handles data types.

Wiki: android_sensors_driver/Tutorials/DrivingAnArduinoBoardFromAnAndroidDevice (last edited 2012-03-18 14:53:30 by GonzaloBulnes)