Friday, July 13, 2012

Custom ROS messages with rosserial_arduino

One of the cool things that the Willow Garage team has done is integrate ROS with the Arduino. This integration allows the Arduino to publish and subscribe ROS messages, making it easier to add custom hardware into ROS projects.

In this post, I'll outline how you can create a custom ROS message that works with rosserial_arduino. It wasn't perfectly clear to me how this should be done, and that was after going through the tutorials, so hopefully this will help some people.

I will assume that you already know what ROS is, have it installed, have completed the ROS tutorials (or at least a few), and know how to do basic things in Arduino. Also, I will assume that you're using Ubuntu 11.10, the ROS Electric distribution, and Arduino 1.0 (or higher). I haven't touched ROS Fuerte or Ubuntu 12.04 yet.


Setting up ROS and the Arduino IDE

First off, let's get rosserial installed. If you've installed ROS Electric according to the instructions on the ROS wiki, then this can be done with just one command.

> sudo apt-get install ros-electric-rosserial

After this completes, you will need to copy some files to the Arduino library folder. The first time I did this, I tried to put these where the rest of the Arduino libraries existed at /usr/share/arduino/libraries/. However, later I found this is not such a great idea. I ran into permission issues, and putting it in my home folder was much cleaner.

First, verify that you have a ~/sketchbook/libraries/ folder, and if not, create one. Then run the following set of commands:

> rospack profile
> roscd rosserial_arduino/libraries/
> cp -r ros_lib ~/sketchbook/libraries/

The "rospack profile" command tells ROS to update the cache of paths, so it can find everything using roscd.

After doing these steps, open the Arduino IDE and check for "ros_lib" under the Examples menu. You should see something similar to that at the bottom of the setup tutorial at this page ( http://www.ros.org/wiki/rosserial_arduino/Tutorials/Arduino IDE Setup ). I strongly recommend that you complete the next two rosserial_arduino tutorials, Hello World and Blink to make sure that everything is working so far.


Create a Custom Message

The rosserial_arduino tutorial for the Arduino Oscilloscope is a good example of using a custom message, but ideally an oscilloscope would return voltages rather than counts, so let's create a custom message where the voltage conversion is done for us. This will be useful for reading many types of sensors.

First, go to your ros_workspace folder (created in the ROS setup tutorials) and create a package called "adc_voltage". I'm going to put several dependencies on the package that I think are needed, but there may be extras or missing ones. Go ahead and execute these commands:

> cd ~/ros_workspace
> roscreate-pkg adc_voltage std_msgs rospy roscpp rosserial_arduino
> rospack profile
> roscd adc_voltage

Next, a message needs to be defined. The message in the Arduino Oscilloscope tutorial is a good starting place, but we'll make two changes. First off, most ROS nodes publish messages with a header that includes a sequence number, a timestamp, and the frame id. Second, we'll change from integers to floats.

Create a folder named "msg", and add a file "AdcVoltage.msg" with the contents below. This message simply defines a Header type at the beginning named "header", and 6 float values named "v0" and so on.

file: ~/ros_workspace/adc_voltage/msg/AdcVoltage.msg
Header header
float32 v0
float32 v1
float32 v2
float32 v3
float32 v4
float32 v5

Next is compiling the message. Go back to the "adc_voltage" folder, and edit the CMakeLists.txt file by uncommenting the"rosbuild_genmsg()" function.

file: ~/ros_workspace/adc_voltage/CMakeLists.txt
#uncomment if you have defined messages
rosbuild_genmsg()

Now there are two more minor steps, which are just command line. The first command will make the package. The second command, which is from the rosserial Adding Custom Messages tutorial, will generate the correct header files for Arduino and add them to the "ros_lib" folder under your local sketchbook/libraries folder.

> rosmake
> rosrun rosserial_client make_library.py ~/sketchbook/libraries adc_voltage


Using the message in Arduino

I will present the Arduino code in full first, and then explain it step by step.

/*
 * rosserial ADC Voltage Example
 *
 * This can be used to return analog voltage values from Arduino
 * as a ROS message.
 */

#include <ros.h>
#include <adc_voltage/AdcVoltage.h>

ros::NodeHandle nh;
adc_voltage::AdcVoltage voltage_msg;
ros::Publisher p("adc_voltage", &voltage_msg);
float Vcc;

// This code is from the SecretVoltmeter project. See
// http://code.google.com/p/tinkerit/wiki/SecretVoltmeter
float readVcc() {
  long result;
  // Read 1.1V reference against AVcc
  ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
  delay(2); // Wait for Vref to settle
  ADCSRA |= _BV(ADSC); // Convert
  while (bit_is_set(ADCSRA,ADSC));
  result = ADCL;
  result |= ADCH<<8;
  result = 1126400L / result; // Back-calculate AVcc in mV
  return (result/1000.0);
}

// Average 4 analog readings to elminate some of the noise
// and normalize the reading from 0.0 to 1.0
float averageAnalog(int pin){
  int v=0;
  for(int i=0; i<4; i++) v+= analogRead(pin);
  return (v/1023.0)/4.0;
}

void setup()
{
  // Initialize the ROS node and advertise the message
  nh.initNode();
  nh.advertise(p);
}

void loop()
{
  // Read the supply voltage
  Vcc = readVcc();
 
  // Add a time stamp for the message header
  voltage_msg.header.stamp = nh.now();
 
  // Assign the voltages to the message
  // Multiply the average by Vcc to get volts
  voltage_msg.v0 = Vcc*averageAnalog(0);
  voltage_msg.v1 = Vcc*averageAnalog(1);
  voltage_msg.v2 = Vcc*averageAnalog(2);
  voltage_msg.v3 = Vcc*averageAnalog(3);
  voltage_msg.v4 = Vcc*averageAnalog(4);
  voltage_msg.v5 = Vcc*averageAnalog(5);
 
  // Publish the ROS message 
  p.publish(&voltage_msg);
  nh.spinOnce();
 
  // Delay 1 second between messages
  delay(1000);
}


Section 1 : Includes

To use ROS with Arduino, we have to include "ros.h" for the ROS basic functionality. We also have to include the "AdcVoltage.h" file, which resides under the adc_voltage folder (in ~/sketchbook/libraries/ros_lib/) that was generated in the last section.

#include <ros.h>
#include <adc_voltage/AdcVoltage.h>

Section 2 : Global variables

The first global definition is to generate a handle to a ROS node, nh. Next we define a variable for our custom message. The custom message type has the form "package::MessageName", which in this case is "adc_voltage::AdcVoltage". I've named the variable "voltage_msg" for clarity. Next, we create a publisher object, where the arguments are the node name and a reference to the message. This is standard ROS syntax and can be looked up in the tutorials. Lastly, I've declared a variable for the supply voltage to the Arduino.

ros::NodeHandle nh;
adc_voltage::AdcVoltage voltage_msg;
ros::Publisher p("adc_voltage", &voltage_msg);
float Vcc;

Section 3 : Functions

To get accurate readings of the voltage, we're going to use a trick that I found at this website, which originally is posted on a project called SecretVoltmeter. This function returns the Arduino supply voltage using the internal ATMega 1.1V reference as a standard, instead of assuming that the voltage reference for the ADC is 5V. Some people question if this is very accurate, since the internal 1.1V reference may vary, so if you need real precise measurements you will need to obtain a precision voltage reference.

The second function is from the oscilloscope tutorial, but it has been modified to return the normalized voltage, where 1023 counts (based on a 10-bit ADC) are equal to Vcc.


// This code is from the SecretVoltmeter project. See
// http://code.google.com/p/tinkerit/wiki/SecretVoltmeter
float readVcc() {
  long result;
  // Read 1.1V reference against AVcc
  ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
  delay(2); // Wait for Vref to settle
  ADCSRA |= _BV(ADSC); // Convert
  while (bit_is_set(ADCSRA,ADSC));
  result = ADCL;
  result |= ADCH<<8;
  result = 1126400L / result; // Back-calculate AVcc in mV
  return (result/1000.0);
}

// Average 4 analog readings to elminate some of the noise
// and normalize the reading from 0.0 to 1.0
float averageAnalog(int pin){
  int v=0;
  for(int i=0; i<4; i++) v+= analogRead(pin);
  return (v/1023.0)/4.0;
}

Section 4 : Setup

The setup() function simply initialized the ROS node, and then advertises the message type that we'll be sending via the publisher.

void setup()
{
  // Initialize the ROS node and advertise the message
  nh.initNode();
  nh.advertise(p);
}

Section 5 : Loop

The loop() function is where we will be defining the contents of the message. After reading Vcc, we set the time stamp on our message header by calling <message>.header.stamp = nh.now(). Our message variable is named voltage_msg. The nh.now() call gets the current ROS system time and assigns it to the time stamp of the header.

The next six lines will get the normalized voltage on an ADC pin, scale it to correct value by multiplying with Vcc, and then assign it to the correct value in our message. The message fields are accessed by voltage_msg.<field>. As you can see from the first line, the first field in our message (aside from header) is v0, which is assigned the voltage from analog pin 0.

The last parts of the code will publish our message "voltage_msg" on the publisher that we previously defined, tell ROS to execute, and then delay 1 second before repeating the process.

void loop()
{
  // Read the supply voltage
  Vcc = readVcc();
 
  // Add a time stamp for the message header
  voltage_msg.header.stamp = nh.now();
 
  // Assign the voltages to the message
  // Multiply the average by Vcc to get volts
  voltage_msg.v0 = Vcc*averageAnalog(0);
  voltage_msg.v1 = Vcc*averageAnalog(1);
  voltage_msg.v2 = Vcc*averageAnalog(2);
  voltage_msg.v3 = Vcc*averageAnalog(3);
  voltage_msg.v4 = Vcc*averageAnalog(4);
  voltage_msg.v5 = Vcc*averageAnalog(5);
 
  // Publish the ROS message 
  p.publish(&voltage_msg);
  nh.spinOnce();
 
  // Delay 1 second between messages
  delay(1000);
}


Writing a Message Subscriber

Before we upload the above code to the Arduino, let's go ahead and create a node that subscribes to the message. For now this node will only print out a message, but it show how to access the different parts of the message. I've decided to write this node in Python, and it's based on the introductory ROS tutorials Writing a Publisher/Subscriber(python) and Examining a Publisher/Subscriber. I believe writing the node in C++ should be just as straightforward syntax aside.

Go back to the command line and type the commands below. This simply creates a "nodes" folder under the adc_voltage package, creates a Python file named "listener.py" in that folder, and sets it to be an executable.

> roscd adc_voltage
> mkdir nodes
> touch nodes/listener.py
> chmod +x nodes/listener.py

Now, open listener.py and paste in the following code:

file: ~/ros_workspace/adc_voltage/nodes/listener.py
#!/usr/bin/env python
import roslib; roslib.load_manifest('adc_voltage')
import rospy
from adc_voltage.msg import AdcVoltage

def msgCallback(data):
    msgString = "I read values (%.3f,%.3f,%.3f,%.3f,%.3f,%.3f)" % (data.v0,
        data.v1, data.v2, data.v3, data.v4, data.v5)
    rospy.loginfo(rospy.get_name()+msgString)

def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("adc_voltage", AdcVoltage, msgCallback)
    rospy.spin()

if __name__ == '__main__':
    listener()


The import statements, in addition to the stuff necessary for ROS (roslib and rospy), import the definition of our message. This is seen in "roslib.load_manifest('adc_voltage')" and "from adc_voltage.msg import AdcVoltage".

The listener() function will first initialize the listener node (that is defined in this file), and then subscribe to the "adc_voltage" node. This is done with the rospy.Subscriber() call. The first argument is the node to subscribe to ("adc_voltage"), the second is the message type (AdcVoltage), and third is the callback function that is called when a message is received, which we've named msgCallback().

In the msgCallback() function, the declaration requires the parameter "data", which contains the contents of the message in the form of a Python object. From there, the fields in the message can be accessed using the dot notation, i.e. data.v0, data.header.stamp, etc. Here, for illustation, I compose all of the fields into a string and then display it using the rospy.loginfo() call, as was shown in the tutorials.


Compiling and Testing

Now we can finally get down to the example of running the code. This will be done in several steps:

Step 1

Plug in your Arduino and upload the code. If you receive any errors about "WProgram.h" and are using Arduino 1.0, you will have to replace it with "Arduino.h".

Step 2

Next, let's start the ROS Master. Open a new terminal window and type

> roscore

Step 3

Now let's start the Python subscriber, so we can see new messages as soon as they come in. Open another terminal window and type

> rosrun adc_voltage listener.py

Step 4

The last step is to run a Python node that converts the Arduino messages into ROS messages. The last argument to this command is the USB/Serial port that your Arduino is connected to. It is most likely something like "/dev/ttyUSB0" or in my case "/dev/ttyACM0". The bottom right hand side of the Arduino IDE will display the port that you uploaded the code to. Now, open a third terminal and type

> rosrun rosserial_python serial_node.py /dev/ttyACM0

Once this last command is executed, you should (almost) immediately see messages like below start printing out in the second terminal window that "listener.py" was ran in.

[INFO] [WallTime: 1340852587.773491] /listener_567_1340852491897I read values (3.298,5.028,0.000,0.000,0.000,0.000)
[INFO] [WallTime: 1340852588.780613] /listener_567_1340852491897I read values (3.298,5.028,0.000,0.000,0.000,0.000)

Your values may differ. In these printouts, my A0 is connected to the 3.3V pin, A1 to the 5V pin, and A2-A5 to GND. Please note that ADC channels not connected to anything are floating, and may show values similar to the neighboring pins.

Step 5 - Optional

Let's take a closer look at what is happening on the message level. Open a fourth terminal and type

> rostopic echo /adc_voltage

You should see messages print, at a rate of 1 per second, similar to this:

---
header:
  seq: 108
  stamp:
    secs: 1340850432
    nsecs: 700189962
  frame_id:
v0: 3.29916405678
v1: 5.02799987793
v2: 0.0
v3: 0.0
v4: 0.0
v5: 0.0
---


Here you see the raw breakdown of the message, including the header, the time stamp, and the individual message values.

For a last fun piece, open another terminal and type

> rxgraph

You should see a window pop up with a node connection diagram, something like this. Here, the Arduino itself doesn't show up, but rather the "/serial_node" is what was executed in Step 4 above. From that node, we can see the "/adc_voltage" message being sent to the "/listener_..." node.


Closing Remarks

This is by no means a limit on what can be done. ROS has support for much more complicated (and nested) messages. If there's anything else you would like to see, post a comment. I'm more than willing to try to help. If you live in the Los Angeles area, I am also willing to help on your project. This is how cool things get started!

4 comments:

  1. Did you upgrade to precise/fuerte yet?
    Getting it to run with oneiric/electric was a lot easier than with precise/fuerte.
    http://answers.ros.org/question/38688/rosserial_python-fuerte/

    ReplyDelete
    Replies
    1. I haven't tried fuerte, but as you probably figured out I was the one that commented on your ROS Answers question. I never did see any follow up on it.. did you get it to work or you're still having problems?

      I was planning to try fuerte, but groovy is going to be out in another month or two, so I might just wait for that.

      Delete
  2. Hi Doug,

    Is it possible to publish ROS messages to arduino?

    I am given a task to make the led on arduino light up when the Hokuyo laser is very near the obstacle. How do I make use of custom messages to make it happen?

    ReplyDelete
    Replies
    1. Hi LTRYSO,

      I think if you follow the first 3 tutorials on the page below, you should have everything you need from the Arduino side.

      http://www.ros.org/wiki/rosserial_arduino/Tutorials

      Delete