(!) 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.

Hello World

Description: The avr_bridge hello world using an arduino

Keywords: arduino, avr

Tutorial Level: BEGINNER

avr_bridge allows avr microcontrollers to function as full fledged ros components. For a full introduction to the parts of avr_bridge, see its pkg description. In this tutorial, we will be doing a hello world program with an Arduino Uno.

The source code for this tutorial can be found in the avr_bridge_call response repository. You can grab the code by using

git clone git@github.com:rutgers/avr_bridge_callresponse.git

In this tutorial, we will be making a hello world by having the Arduino recieve a message on the the topic "call" and and it will respond "You have sent: " the message data on the topic "response".

Making an avr_bridge Project

First, make a ros package which depends on avr_bridge.

roscreate-pkg avr_bridge_callResponse avr_bridge

Next, modify the CMakeList.txt so that it uses the avrbuild.cmake script. This script automates the crosscompiling of the avr code for the arduino. It also adds the Arduino library to your project and generates the avr_ros communication code.

To your CMakeList.txt add

set(AVR_BRIDGE_CONFIG callResponse.yaml)
set(ARDUINO_TYPE "arduino_diecimila")
set(ADUINO_PORT "/dev/ttyUSB0")
rosbuild_find_ros_package(avr_bridge)
include(${avr_bridge_PACKAGE_PATH}/arduino_cmake/avrbuild.cmake)

set(PROJECT_SRC src/main.cpp src/avr_ros_user.cpp)

add_executable(firmware ${PROJECT_SRC} ${AVR_ROS_SRC} ${ARDUINO_SRC})

AVR_BRIDGE_CONFIG defines the location of the avr_bridge configuration yaml file. This file describes every topic that your microcontroller is subscribing to and publishing. It is described in the next section.

ARDUINO_TYPE defines the type of arduino board that you are using. This is important because it sets the default clock frequency, avr type (atmega328p, atmega2560, ...) , and the type of programmer to use.

rosbuild_find_ros_package(avr_bridge)
include(${avr_bridge_PACKAGE_PATH}/arduino_cmake/avrbuild.cmake)

These two lines find the avr_bridge package and include the avrbuild.cmake script.

Finally, we define the firmware for this avr project. It is important that you name your executable firmware. This will allow avrbuild.cmake to post process your executable to make the hex file and to allow you to run make flash to program your arduino .

Configuration File

After the CMakeList.txt is written, you need to write your yaml configuration file. In the example project, this file is called callResponse.yaml.

port: /dev/ttyUSB0
name: callResponse
publish: #list of published topics
 response: #topic name
  type: std_msgs/String  #msg type
subscribe:
 call:
  type: std_msgs/String

This configuration specifies the arduino's usb-serial port (/dev/ttyUSB0), the program's name, and the board's subscriptions and publications. In this case, we are publishing one topic, "response", of type String from the std_msgs pkg. Similarly, we are subscribing to the topic "call" which is of type std_msgs/String. We have also named the board callResponse. You must always name the board. By naming the board, you can use avr_bridge getID.py to query the board's name and set up a udev rule based on the board's name.

The avr_bridge system works with all ROS messages, but it does not handle all ros primitives correctly. AVR processors have no float64. As a result, message fields which are float64 are not serialized, or deserialized correctly on the avr. Do not use them.

Generating the Avr Code and Makefiles

Inside the avr_bridge_callResponse folder, execute:

cmake .

This will not only generate your typical make files and it will generate the avr_ros communication code in your src/avr_ros folder.

The avr_ros folder contains all of the avr ros files needed to use avr_bridge in your project. The command automatically generates the c++ implementation for the ros msgs along with the standard communication stack implementation.

avr_ros_user.cpp

The avr_ros_user.cpp file contains the implementation for board specific hooks used to run the avr_bridge communication stack. Every program needs to define how to write a byte. In this case, we are using the uart of the arduino to communicate.

   1 namespace ros {
   2     int fputc(char c, FILE *stream) {
   3         Serial.write(c);
   4         return 0;
   5     }
   6     int fgetc(FILE * stream){
   7                 return Serial.read();
   8         }
   9 
  10 
  11 }

main.cpp

The main.cpp is is copied below for convenience.

   1 #include "WProgram.h" //include the Arduino library
   2 #include <stdio.h>
   3 #include "avr_ros/ros.h" //main avrRos library
   4 #include "avr_ros/String.h" //std_msg/String library
   5 
   6 //Define global message objects to use in
   7 //the callback functions and throughout the program
   8 ros::Publisher resp;
   9 std_msgs::String call_msg;
  10 std_msgs::String response_msg;
  11 
  12 void toggle()
  13 { //toggle an led to debug the program
  14     static char t=0;
  15     if (!t ) {
  16         digitalWrite(13, HIGH);   // set the LED on
  17         t = 1;
  18     } else {
  19         digitalWrite(13, LOW);    // set the LED off
  20         t = 0;
  21     }
  22 }
  23 
  24 void response(Msg *msg){
  25         toggle();
  26 
  27         //make sure that if you are manipulating the raw string,
  28         //there is enough space in it
  29         //to handle all of the data
  30         sprintf(response_msg.data.getRawString(),
  31                 "You sent : %s", call_msg.data.getRawString());
  32         node.publish(resp, &response_msg);
  33 }
  34 
  35 
  36 // Since we are hooking into a standard
  37 // arduino sketch, we must define our program in
  38 // terms of the arduino setup and loop functions.
  39 
  40 void setup()
  41 {
  42     Serial.begin(57600);
  43 
  44     pinMode(13, OUTPUT); //set up the LED
  45     resp = node.advertise("response");
  46     node.subscribe("call",response, &call_msg);
  47 
  48     call_msg.data.setMaxLength(30);
  49     response_msg.data.setMaxLength(60);
  50 }
  51 
  52 void loop()
  53 {
  54     for(;;) {
  55         int c = Serial.read();
  56         if (c == EOF)
  57             break;
  58         node.spin(c);
  59     }
  60 
  61     /* Do other work */
  62     delay(10);
  63 }

The main program follows the standard Arduino template, but it has a few complexities added because we are using the library outside of the Arduino IDE and using the avr_ros library.

As with the Arduino IDE, there is no main function, instead we are using the setup() and loop() hooks.

   1 void setup()
   2 {
   3     Serial.begin(57600);
   4 
   5     pinMode(13, OUTPUT); //set up the LED
   6     resp = node.advertise("response");
   7     node.subscribe("call",response, &call_msg);
   8 
   9     call_msg.data.setMaxLength(30);
  10     response_msg.data.setMaxLength(60);
  11 }
  12 
  13 void loop()
  14 {
  15     for(;;) {
  16         int c = Serial.read();
  17         if (c == EOF)
  18             break;
  19         node.spin(c);
  20     }
  21 
  22     /* Do other work */
  23     delay(10);
  24 }

Inside the setup, we can see an important feature of the avrRos system. avrRos declares a global Ros object called "node" which is automatically initialized. This node object uses the functions defined in avr_ros_user.cpp to communicate with a full fledged computer running ros.

In the avrRos system, the ros publishers are created by calling void node.advertise(char const *topicName). This returns a publisher object that is then used in publishing. To publish, call : node.publisher(Publisher pub, Msg *msg_object).

Next, to subscribe to a topic, node.subscribe(char const *topicName, RosCb *cb_function, Msg *msg). In this example, we subscribe to the "response" topic register the response callback, and use std_msg::String response_msg as the msg object. This is slightly different from the standard ROS semantics. For avrRos, our call back function must be of type RosCb:

   1 typedef void (RosCb)(Msg const *msg);

Additionally, for every subscription, you must register a Msg object of the appropriate type. This msg object is used to deserialize and store raw binary data sent by the PC. This interface was chosen in order to handle large ROS messages on the memory limited avr processors. This way, there can be one global msg object that holds the communication results rather than the processor dynamically generating a new msg object for every transmission. These msg objects should by exist for the entire life of the program.

In the last portion of setup(), the maximum lengths of call_msg and response_msg are set. These maximum lengths are used to allocate memory for the msg's variable length string. Every variable length message field must be initialized with a maximum length. Again, this implementation is a compromise with the limited memory of the avr processor.

Next, in loop, node.spin must be called every iteration. As in the rest of ROS, node.spin handles the subscription callbacks and other communication work. Typically, you have node.spin at the head of your loop and the rest of your loop handles the work of the program.

Compiling and Programming

To compile and program your Arduino simply run :

make
make flash

Running Hello World

First, compile the example and program your arduino.

In a new terminal run :

roscore

In another terminal, move into the example project directory and start up the bridge node

rosrun avr_bridge bridge_node.py callResponse.yaml

Finally, we are going to communicate with the Arduino. First open a terminal and run

rostopic echo response

Finally, run in a new terminal

rostopic pub call std_msg/String -r1 -- "Hello World"

The Arduino is receiving hello world and responding on the response topic. You should see:

data: You sent : Hello World

Wiki: avr_bridge/Tutorials/Hello World (last edited 2011-05-25 21:12:42 by MichaelFerguson)