How to Publish Wheel Encoder Tick Data Using ROS and Arduino

2021-05-11-12.48.52

In this tutorial, we will create a program using ROS and Arduino to publish the tick data from the encoders of each motor of a two-wheeled robot. 

The microcontroller we will use is the Nvidia Jetson Nano, but you can also use an Ubuntu-enabled Raspberry Pi.

We will interface Arduino with ROS so we can publish the tick counts for the left wheel and right wheel to ROS topics. The name of this node will be tick_publisher.

Why Publish Wheel Encoder Tick Data

Tick data from wheel encoders helps us determine how much each wheel (both the left and right wheels) has rotated. Since we know the radius of each wheel, we can use the tick data and the wheel radius data to determine the distance traveled by each wheel. Here is the equation for that: 

Distance a wheel has traveled = 2 * pi * radius of the wheel * (number of ticks / number of ticks per revolution)

Knowing the distance traveled by each wheel helps us determine where the robot is located in its environment relative to some starting location. This process is known as odometry

The first time you were exposed to the word “odometry” was likely when you began driving a car. Right behind your steering wheel is typically your odometer. The odometer of your car measures the distance traveled by the vehicle relative to some starting point where the car was first driven. Odometry in robotics works the same way.

odometer_car_dashboard
A typical odometer on a car’s dashboard

Real-World Applications

2021-05-13-20.47.13
You can see that a robotic vacuum cleaner has the same two-wheeled differential drive setup as the robot I’m currently working with in this tutorial.

This project has a number of real-world applications: 

  • Indoor Delivery Robots
  • Room Service Robots
  • Mapping of Underground Mines, Caves, and Hard-to-Reach Environments
  • Robot Vacuums
  • Order Fulfillment
  • Factories

Prerequisites

2021-05-11-19.38.06

You Will Need

  • Arduino Uno (Elegoo Uno works just fine and is cheaper than the regular Arduino)

Install Arduino on Your Jetson Nano

To get started, turn on your Jetson Nano.

Let’s download the Arduino IDE (you’ll have to download the Arduino IDE directly from the Arduino website if you are using Ubuntu on the Raspberry Pi).

Open a new terminal window, and type the following commands, one right after the other.

git clone https://github.com/JetsonHacksNano/installArduinoIDE.git
cd installArduinoIDE
./installArduinoIDE.sh

We are installing the Linux ARM 64 bits version.

Wait a few moments, and then reboot your computer.

sudo reboot

When you restart your computer, you will see an Arduino icon on the desktop of your Jetson Nano.

Open a new terminal window, and shut down your Jetson Nano.

sudo shutdown -h now

Plug the Arduino into the USB port of your Jetson Nano.

Turn on your Jetson Nano.

Open the Arduino IDE by double-clicking the icon on the desktop of your Jetson Nano.

Go to Tools -> Board, and select Arduino/Genuino Uno.

Go to Tools -> Port, and select the Serial port that is connected to your Arduino/Genuino Uno. In my case, it is /dev/ttyACM0.

Now open the Blink sketch again. Go to File – > Examples -> 01.Basics, and choose Blink.

Click the Upload button…the right arrow in the upper left of your screen.

The LED on your Arduino should be blinking! If it is not, go back to the beginning of this tutorial and follow the steps carefully.

To turn off the blinking light, open up a new sketch (Go to File -> New), and upload it to your board.

Integrate Arduino With ROS 

Getting Started

Now that we know Arduino is working, we need to integrate it with ROS. Here are the official instructions. I’ll walk you through the steps below.

Let’s install the necessary packages. 

Quit the Arduino IDE. Then type the following commands in a new terminal window (this software will take a while to download so be patient):

sudo apt-get install ros-melodic-rosserial-arduino
sudo apt-get install ros-melodic-rosserial

Open the IDE and go to File -> Preferences. Make a note of the Sketchbook location. Mine is:

/home/automaticaddison/Arduino

Quit the Arduino IDE.

Open a new terminal window, and go to the sketchbook location you noted above. I’ll type:

cd Arduino

Type the dir command to see the list of folders.

dir

Go to the libraries directory.

cd libraries
rm -rf ros_lib

Within that directory, run the following command to build the Arduino library that will be used by ROS (don’t leave out that period that comes at the end of the command):

rosrun rosserial_arduino make_libraries.py .

Type the dir command to see the list of folders. You should now see the ros_lib library.

dir

Make sure the Arduino IDE is closed. Now open it again.

Blink an LED on the Arduino Using ROS

In this example, Arduino is going to be considered a Subscriber node. It will subscribe to a topic called toggle_led. Publishing a message to that topic causes the LED to turn on. Publishing a message to the topic again causes the LED to turn off. 

Go to File -> Examples -> ros_lib and open the Blink sketch.

Before the nh.initNode(); line, add the following code:

nh.getHardware()->setBaud(115200);

Now we need to upload the code to Arduino. Make sure your Arduino is plugged into the USB port on your Jetson Nano (or whatever microcontroller you are using).

Upload the code to your Arduino using the right arrow button in the upper left of your screen. When you upload the code, your Arduino should flicker a little bit.

Note: I created a new Arduino sketch and saved it to /home/automaticaddison/Documents as “blink_ros_arduino_test”. This is not required.

Quit the Arduino IDE.

Open a new terminal window and type:

roscore

In a new terminal window, launch the ROS serial server. This command is explained here on the ROS website. It is necessary to complete the integration between ROS and Arduino:

cd ~/catkin_ws
rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200

Now let’s turn on the LED by publishing a single empty message to the /toggle_led topic. Open a new terminal window and type:

rostopic pub toggle_led std_msgs/Empty --once

The LED on the Arduino should turn on. 

Now press the Up arrow in the terminal and press ENTER to run this code again. You should see the LED turn off. You might also see a tiny yellow light blinking as well. Just ignore that one…you’re interested in the big yellow light that you’re able to turn off and on by publishing single messages to the /toggle_led topic.

2021-05-11-16.05.51
2021-05-11-16.06.02

You can see the active topics by typing.

rostopic list

Another good example to check out is a publisher and subscriber. You can see this by going to:

File -> Examples -> ros_lib -> pubsub

This ROS node will blink an LED and publish a “Hello World” message to a topic named chatter.

That’s it! You have now seen how you can integrate Arduino with ROS. To turn off your Arduino, all you need to do is disconnect it after you shut down your Jetson Nano.

Create a Tick Publisher Node

Now we want to write a tick publisher node. This ROS node will publish the accumulated tick count for the left and right motors of the robot at regular intervals.

Set Up the Hardware

Let’s revisit this tutorial to set up the hardware.

Here is the wiring diagram for the left motor:

jgb37_dc_motor_with_encoder-2
  • The Ground pin of the motor connects to GND of the Arduino. I’m using a 400-point solderless breadboard that I bought on Amazon.com to facilitate this connection.
  • Encoder A (sometimes labeled C1) of the motor connects to pin 2 of the Arduino. Pin 2 of the Arduino will record every time there is a rising digital signal from Encoder A.
  • Encoder B (sometimes labeled C2) of the motor connects to pin 4 of the Arduino. The signal that is read off pin 4 on the Arduino will determine if the motor is moving forward or in reverse. We’re not going to use this pin in this tutorial, but we will use it in a future tutorial.
  • The VCC pin of the motor connects to the 5V pin of the Arduino. This pin is responsible for providing power to the encoder.

For the right motor:

  • The Ground pin of the motor connects to GND of the Arduino.
  • Encoder A (sometimes labeled C1) of the motor connects to pin 3 of the Arduino. Pin 2 of the Arduino will record every time there is a rising digital signal from Encoder A.
  • Encoder B (sometimes labeled C2) of the motor connects to pin 11 of the Arduino. The signal that is read off pin 11 on the Arduino will determine if the motor is moving forward or in reverse. 
  • The VCC pin of the motor connects to the 5V pin of the Arduino. This pin is responsible for providing power to the encoder.

Write the Code

Now we’re ready to calculate the accumulated ticks for each wheel.

Open the Arduino IDE, and write the following program. The name of my program is tick_counter.ino.

/*
 * Author: Automatic Addison
 * Website: https://automaticaddison.com
 * Description: Calculate the accumulated ticks for each wheel using the 
 * built-in encoder (forward = positive; reverse = negative) 
 */

// Encoder output to Arduino Interrupt pin. Tracks the tick count.
#define ENC_IN_LEFT_A 2
#define ENC_IN_RIGHT_A 3

// Other encoder output to Arduino to keep track of wheel direction
// Tracks the direction of rotation.
#define ENC_IN_LEFT_B 4
#define ENC_IN_RIGHT_B 11

// True = Forward; False = Reverse
boolean Direction_left = true;
boolean Direction_right = true;

// Minumum and maximum values for 16-bit integers
const int encoder_minimum = -32768;
const int encoder_maximum = 32767;

// Keep track of the number of wheel ticks
volatile int left_wheel_tick_count = 0;
volatile int right_wheel_tick_count = 0;

// One-second interval for measurements
int interval = 1000;
long previousMillis = 0;
long currentMillis = 0;

void setup() {

  // Open the serial port at 9600 bps
  Serial.begin(9600); 

  // Set pin states of the encoder
  pinMode(ENC_IN_LEFT_A , INPUT_PULLUP);
  pinMode(ENC_IN_LEFT_B , INPUT);
  pinMode(ENC_IN_RIGHT_A , INPUT_PULLUP);
  pinMode(ENC_IN_RIGHT_B , INPUT);

  // Every time the pin goes high, this is a tick
  attachInterrupt(digitalPinToInterrupt(ENC_IN_LEFT_A), left_wheel_tick, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC_IN_RIGHT_A), right_wheel_tick, RISING);
}

void loop() {

  // Record the time
  currentMillis = millis();

  // If one second has passed, print the number of ticks
  if (currentMillis - previousMillis > interval) {
    
    previousMillis = currentMillis;

    Serial.println("Number of Ticks: ");
    Serial.println(right_wheel_tick_count);
    Serial.println(left_wheel_tick_count);
    Serial.println();
  }
}

// Increment the number of ticks
void right_wheel_tick() {
  
  // Read the value for the encoder for the right wheel
  int val = digitalRead(ENC_IN_RIGHT_B);

  if(val == LOW) {
    Direction_right = false; // Reverse
  }
  else {
    Direction_right = true; // Forward
  }
  
  if (Direction_right) {
    
    if (right_wheel_tick_count == encoder_maximum) {
      right_wheel_tick_count = encoder_minimum;
    }
    else {
      right_wheel_tick_count++;  
    }    
  }
  else {
    if (right_wheel_tick_count == encoder_minimum) {
      right_wheel_tick_count = encoder_maximum;
    }
    else {
      right_wheel_tick_count--;  
    }   
  }
}

// Increment the number of ticks
void left_wheel_tick() {
  
  // Read the value for the encoder for the left wheel
  int val = digitalRead(ENC_IN_LEFT_B);

  if(val == LOW) {
    Direction_left = true; // Reverse
  }
  else {
    Direction_left = false; // Forward
  }
  
  if (Direction_left) {
    if (left_wheel_tick_count == encoder_maximum) {
      left_wheel_tick_count = encoder_minimum;
    }
    else {
      left_wheel_tick_count++;  
    }  
  }
  else {
    if (left_wheel_tick_count == encoder_minimum) {
      left_wheel_tick_count = encoder_maximum;
    }
    else {
      left_wheel_tick_count--;  
    }   
  }
}

Upload the program to the Arduino.

If you open the serial monitor, you will see the accumulated tick count. Once the tick count gets outside the range of what a 16-bit integer can handle, it rolls over to either the minimum -32,768 or maximum 32,767 of the range.

1-tick-counter

Convert the Code to a ROS Node

Now that we see how to print tick counts for the right and left motors, let’s convert the code from the previous section into a ROS node. We want our node to publish tick counts for the right and left wheels of the robot to ROS topics named right_ticks and left_ticks.

Open the Arduino IDE, and write the following program. The name of my program is tick_publisher.ino.

/*
 * Author: Automatic Addison
 * Website: https://automaticaddison.com
 * Description: ROS node that publishes the accumulated ticks for each wheel
 * (right_ticks and left_ticks topics) using the built-in encoder 
 * (forward = positive; reverse = negative) 
 */

#include <ros.h>
#include <std_msgs/Int16.h>

// Handles startup and shutdown of ROS
ros::NodeHandle nh;

// Encoder output to Arduino Interrupt pin. Tracks the tick count.
#define ENC_IN_LEFT_A 2
#define ENC_IN_RIGHT_A 3

// Other encoder output to Arduino to keep track of wheel direction
// Tracks the direction of rotation.
#define ENC_IN_LEFT_B 4
#define ENC_IN_RIGHT_B 11

// True = Forward; False = Reverse
boolean Direction_left = true;
boolean Direction_right = true;

// Minumum and maximum values for 16-bit integers
const int encoder_minimum = -32768;
const int encoder_maximum = 32767;

// Keep track of the number of wheel ticks
std_msgs::Int16 right_wheel_tick_count;
ros::Publisher rightPub("right_ticks", &right_wheel_tick_count);

std_msgs::Int16 left_wheel_tick_count;
ros::Publisher leftPub("left_ticks", &left_wheel_tick_count);

// 100ms interval for measurements
const int interval = 100;
long previousMillis = 0;
long currentMillis = 0;

// Increment the number of ticks
void right_wheel_tick() {
  
  // Read the value for the encoder for the right wheel
  int val = digitalRead(ENC_IN_RIGHT_B);

  if(val == LOW) {
    Direction_right = false; // Reverse
  }
  else {
    Direction_right = true; // Forward
  }
  
  if (Direction_right) {
    
    if (right_wheel_tick_count.data == encoder_maximum) {
      right_wheel_tick_count.data = encoder_minimum;
    }
    else {
      right_wheel_tick_count.data++;  
    }    
  }
  else {
    if (right_wheel_tick_count.data == encoder_minimum) {
      right_wheel_tick_count.data = encoder_maximum;
    }
    else {
      right_wheel_tick_count.data--;  
    }   
  }
}

// Increment the number of ticks
void left_wheel_tick() {
  
  // Read the value for the encoder for the left wheel
  int val = digitalRead(ENC_IN_LEFT_B);

  if(val == LOW) {
    Direction_left = true; // Reverse
  }
  else {
    Direction_left = false; // Forward
  }
  
  if (Direction_left) {
    if (left_wheel_tick_count.data == encoder_maximum) {
      left_wheel_tick_count.data = encoder_minimum;
    }
    else {
      left_wheel_tick_count.data++;  
    }  
  }
  else {
    if (left_wheel_tick_count.data == encoder_minimum) {
      left_wheel_tick_count.data = encoder_maximum;
    }
    else {
      left_wheel_tick_count.data--;  
    }   
  }
}

void setup() {

  // Set pin states of the encoder
  pinMode(ENC_IN_LEFT_A , INPUT_PULLUP);
  pinMode(ENC_IN_LEFT_B , INPUT);
  pinMode(ENC_IN_RIGHT_A , INPUT_PULLUP);
  pinMode(ENC_IN_RIGHT_B , INPUT);

  // Every time the pin goes high, this is a tick
  attachInterrupt(digitalPinToInterrupt(ENC_IN_LEFT_A), left_wheel_tick, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC_IN_RIGHT_A), right_wheel_tick, RISING);

  // ROS Setup
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.advertise(rightPub);
  nh.advertise(leftPub);
}

void loop() {
  
  // Record the time
  currentMillis = millis();

  // If 100ms have passed, print the number of ticks
  if (currentMillis - previousMillis > interval) {
    
    previousMillis = currentMillis;
    
    rightPub.publish( &right_wheel_tick_count );
    leftPub.publish( &left_wheel_tick_count );
    nh.spinOnce();
  }
}

Upload the code to your Arduino using the right arrow button in the upper left of your screen. 

Quit the Arduino IDE.

Run the Tick Publisher

Open a new terminal window and type:

roscore

In a new terminal window, launch the ROS serial server. 

rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200
2021-05-11-22.36.41

Open a new terminal window, and check out the active ROS topics.

rostopic list
2021-05-11-22.34.09

You can see the right wheel tick count by typing the following command:

rostopic echo /right_ticks

You can see the left wheel tick count by typing the following command:

rostopic echo /left_ticks

Spin the wheels, and see the numbers go up and down.

2021-05-11-22.35.57
2021-05-11-22.35.50

Create a Launch File

Let’s start accumulating our ROS nodes in a launch file to make things easier for us down the road.

Open a new terminal window, and type the following command.

cd ~/catkin_ws/src/jetson_nano_bot

Create a package. 

catkin_create_pkg launch_jetson_nano_bot rospy roscpp std_msgs sensor_msgs geometry_msgs tf

Move to the package.

cd launch_jetson_nano_bot

Create a folder to contain launch files.

mkdir launch

Now open a new terminal and move to your catkin workspace.

cd ~/catkin_ws/

Compile the package.

catkin_make --only-pkg-with-deps  launch_jetson_nano_bot

Move inside the package.

roscd launch_jetson_nano_bot

Go inside the CMakeLists.txt file.

gedit CMakeLists.txt

Remove the hashtag on line 5 to make sure that C++11 support is enabled.

Save and close the file.

Move into the launch folder.

cd launch

Create a new launch file.

gedit jetson_nanobot.launch

Add the following code to a launch file.

<launch>
  <node pkg="rosserial_python" type="serial_node.py" name="serial_node">
    <param name="port" value="/dev/ttyACM0"/>
    <param name="baud" value="115200"/>
  </node>
</launch>

Save the file, and close it.

Change the permissions of the launch file we just created. It will ask you for your password.

sudo chmod +x jetson_nanobot.launch

Open a new terminal window, and go to the home directory.

cd

Launch the launch file.

roslaunch launch_jetson_nano_bot jetson_nanobot.launch

If you open a new terminal window, and type the following command, you will be able to see the active topics.

rostopic list