Note: This tutorial assumes that you have completed the previous tutorials: Hello World (example Publisher).
(!) 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.

Advanced Configuration for NodeHandle and ArduinoHardware.

Description: This tutorial shows step by step how to configure NodeHandle and ArduinoHareware to better suit user needs.

Tutorial Level: ADVANCED

The rosserial_arduino package is designed to work out-of-box for many users, but users might need to change the ros.h and the ArduinoHardware.h to better suit their need.

In this tutorial, we will be changing both the file to demonstrate some of the advanced settings.

Prerequisite

Hardware

Objectives

Publisher/Subscriber Numbers and Input/Output Buffers

Arduino Mega has 8 kB of SRAM, much more than most of the other Arduino boards (Uno has 2 kB, Leonardo has 2.5 kB). This enable us to have more Publishers/Subscribers and use bigger messages.

While we might never need change those numbers for Arduino Mega as the default value is plenty enough, for smaller board that has limited SRAM, we would need to reduce the memory usage.

For the sake for demonstrating how to do it, we will decrease the number of publisher/subscriber from 25/25 to 10/15, and the input/output buffer size for 512/512 bytes to 128/256 bytes.

Change the Serial Port

Arduino Mega has 4 serial port in total:

Object Name

RX Pin

TX Pin

Serial

0

1

Serial1

19

18

Serial2

17

16

Serial3

15

14

Normally, we use Serial to communicate with Arduino Mega, as Serial is connected to the UART to USB bridge, and shows as the port we see on the computer. But for this tutorial, we will be using the Serial1. Because the Serial1 can not communicate directly with the computer, we use a UART to USB bridge (FTDI board).

Change the Baudate

By default the rosserial uses 57600 as the baudrate for communication. This rate is not the maximum that Arduino can achieve, and when there is a lot of information that need to sent back to ROS, the baudrate can become the bottleneck.

The Arduino Mega that operate at 16 Mhz can communicate wit baudrate up to 2M bits per seconds (bps) and other boards such as Teensy 3.2 can go up to 4.6 Mbps. The inefficiency of Arduino Serial library quickly become the bottleneck at high baudrate and we can't really benefit from the increased baudrate. (more information on this here)

For this tutorial, we will be changing the baudrate from 57.6 Kbps to 500 Kbps.

The code

We will have 3 source files for this example. The main sketch, the ros.h and the ArduinoHardware.h. Because many of our changes are under the hood and do not affect how we use rosserial, the hello world sketch is used with minimal modification. The original ros.h and ArduinoHardware.h can be found either in the src/ros_lib/ of rosserial_arduino package, or in the generated ros_lib in your Arduino library. You can modify them directly to achieve the same effect but it is recommended to add them to your sketch folder, to make the change per sketch and persistent over regeneration of ros_lib.

Main Sketch

We will first create a new sketch and copy the following code to it:

   1 /*
   2  * rosserial Publisher Example
   3  * Prints "hello world!"
   4  */
   5 
   6 #include "ros.h" 
   7 #include <std_msgs/String.h>
   8 
   9 ros::NodeHandle nh;
  10 
  11 std_msgs::String str_msg;
  12 ros::Publisher chatter("chatter", &str_msg);
  13 
  14 char hello[13] = "hello world!";
  15 
  16 void setup()
  17 {
  18   nh.initNode();
  19   nh.advertise(chatter);
  20 }
  21 
  22 void loop()
  23 {
  24   str_msg.data = hello;
  25   chatter.publish( &str_msg );
  26   nh.spinOnce();
  27   delay(1000);
  28 }

ros.h

ros.h is the NodeHandle definition, and we will create a new tab in Arduino by click on the down arrow icon below Serial Monitor, name it ros.h, and copy the following content to it:

   1 #ifndef _ROS_H_
   2 #define _ROS_H_
   3 
   4 #include "ros/node_handle.h"
   5 #include "ArduinoHardware.h"
   6 
   7 namespace ros
   8 {
   9   // default is 25, 25, 512, 512  
  10   typedef NodeHandle_<ArduinoHardware, 10, 15, 128, 256> NodeHandle;
  11 
  12   // This is legal too and will use the default 25, 25, 512, 512
  13   //typedef NodeHandle_<ArduinoHardware> NodeHandle;
  14 }
  15 
  16 #endif
  17 

ArduinoHardware.h

ArduinoHardware.h, hardware definition for Arduino platform, similarly to ros.h, we will create a new tab named ArduinoHardware.h and copy the following code to it:

   1 #ifndef ROS_ARDUINO_HARDWARE_H_
   2 #define ROS_ARDUINO_HARDWARE_H_
   3 
   4 #if ARDUINO>=100
   5   #include <Arduino.h>
   6 #else
   7   #include <WProgram.h>
   8 #endif
   9 
  10 #include <HardwareSerial.h>
  11 #define SERIAL_CLASS HardwareSerial
  12 
  13 class ArduinoHardware
  14 {
  15   public:
  16     ArduinoHardware()
  17     {
  18       iostream = &Serial1;
  19       baud_ = 500000;
  20     }
  21   
  22     void setBaud(long baud)
  23     {
  24       this->baud_= baud;
  25     }
  26   
  27     int getBaud()
  28     {
  29       return baud_;
  30     }
  31 
  32     void init()
  33     {
  34       iostream->begin(baud_);
  35     }
  36 
  37     int read()
  38     {
  39       return iostream->read();
  40     };
  41 
  42     void write(uint8_t* data, int length)
  43     {
  44       for(int i=0; i<length; i++)
  45       {
  46         iostream->write(data[i]);
  47       }
  48     }
  49 
  50     unsigned long time()
  51     {
  52       return millis();
  53     }
  54 
  55   protected:
  56     SERIAL_CLASS* iostream;
  57     long baud_;
  58 };
  59 
  60 #endif
  61 

The Code Explained

The Main Sketch

   5 #include "ros.h" 
   6 #include <std_msgs/String.h>
   7 

The only difference that we found it the main sketch is how ros.h is included, the brackets <ros.h> in the original hello world are replaced by quotation marks "ros.h". This tells the compiler to look for the file in local path before search it in other libraries. Everything else are the same as the hello world example.

ros.h

   9   // default is 25, 25, 512, 512  
  10   typedef NodeHandle_<ArduinoHardware, 10, 15, 128, 256> NodeHandle;

This set our NodeHandle to have 10 Publishers, 15 Subscriber, 128 bytes for input buffer and 256 bytes for output buffer.

ArduinoHardware.h

  10 #include <HardwareSerial.h>
  11 #define SERIAL_CLASS HardwareSerial
  12 

Here we include the header for the serial object we communicate with. The serial object class must have read(), write() and begin(baudrate) member function implemented. Serial, and Serial1 are both HardwareSerial objects.

  16     ArduinoHardware()
  17     {
  18       iostream = &Serial1;
  19       baud_ = 500000;
  20     }

Here we create the default constructor for ArduinoHardware and set the serial port we use to Serial1 and baudrate to 500 kbps.

Compile and Upload the Code

The code is compile and upload as usual. After compilation, you will be able to see a message of the code like this

Sketch uses 9,552 bytes (3%) of program storage space. Maximum is 253,952 bytes.
Global variables use 1,130 bytes (13%) of dynamic memory, leaving 7,062 bytes for local variables. Maximum is 8,192 bytes.

By varying the buffer sizes and Publishers/Subscribers numbers, the dynamic memory usage will change as well.

Running the Code

We will be using Serial1 to communicate with ROS now. Disconnect the USB cable from Arduino, Connect the FTDI breakout board to the Arduino Mega as following:

FTDI     Arduino Mega
VCC  ->  5V
TXO  ->  Pin 19
RXI  ->  Pin 18
GND  ->  GND

Connect the FTDI breakout board to the computer using USB cable.

Now, launch the roscore in a new terminal window:

roscore

Next, run the rosserial client application that forwards your Arduino messages to the rest of ROS. Make sure to use the correct serial port:

rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0 _baud:=500000

Finally, watch the greetings come in from your Arduino by launching a new terminal window and entering :

rostopic echo chatter

Extras

ros.h and ArduinoHardware.h license.

   1 /* 
   2  * Software License Agreement (BSD License)
   3  *
   4  * Copyright (c) 2011, Willow Garage, Inc.
   5  * All rights reserved.
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions
   9  * are met:
  10  *
  11  *  * Redistributions of source code must retain the above copyright
  12  *    notice, this list of conditions and the following disclaimer.
  13  *  * Redistributions in binary form must reproduce the above
  14  *    copyright notice, this list of conditions and the following
  15  *    disclaimer in the documentation and/or other materials provided
  16  *    with the distribution.
  17  *  * Neither the name of Willow Garage, Inc. nor the names of its
  18  *    contributors may be used to endorse or promote prducts derived
  19  *    from this software without specific prior written permission.
  20  *
  21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  32  * POSSIBILITY OF SUCH DAMAGE.
  33  */

Wiki: rosserial_arduino/Tutorials/NodeHandle and ArduinoHardware (last edited 2017-11-06 04:29:53 by BeiChenLiu)