10. Counting encoder ticks via GPIO pins
You do not require ROS to be able to read a wheel encoder via GPIO pins, though ROS makes it easy to send (i.e., publish) the encoder data to other building blocks of your autonomous system. Hence this guide first builds up a stand-alone C++ code that counts the ticks of multiple wheel encoder channels; then as a final step, the code is put into a ROS node, i.e., an encoder counter node.
Requirements for this page:
Basic proficiency with C++
Familiarity with the the structure of a ROS node that spins at a fixed frequency.
Contents of this page
10.1. TL;DR (ROS encoder counter node)
Of course you did already read everything below, and the following TL;DR summary is just for convenience when you return to remind yourself of a small detail.
Take the stand-alone C++ encoder counter code from below (i.e., from the TL;DR (stand-alone C++ encoder counter) section), and follow the changes described in the Putting it into a ROS node section.
10.2. TL;DR (stand-alone C++ encoder counter)
Of course you did already read everything below, and the following TL;DR summary is just for convenience when you return to remind yourself of a small detail.
The full contents for a stand alone C++ encoder counter is just below, which you can compile and execute using the commands:
g++ -std=c++11 -o encoder_counter encoder_counter.cpp -lpthread -lgpiod
./encoder_counter
The executable will run for the few seconds during which time it is counting events on the specified GPIO lines.
// Includes required for threading
#include <mutex>
#include <thread>
#include <unistd.h>
#include <iostream>
#include <gpiod.h>
// Declare the member variables
// > For sharing the encoder counts between threads
int m_counts_for_m1a = 0;
int m_counts_for_m1b = 0;
int m_counts_for_m2a = 0;
int m_counts_for_m2b = 0;
// > Mutex for preventing multiple-access of shared variables
std::mutex m_counting_mutex;
// > Boolean flag for when to stop counting
bool m_threads_should_count = true;
// Declare the function prototypes
// > For the thread that reads (and displays) the current counts
void threadForReading();
// > For the thread that counts encoder events
void threadForCounting();
// Function implementation
void threadForCounting()
{
// Specify the gpio chip name of the GPIO interface
const char * gpio_chip_name = "/dev/gpiochip1";
// Specify the line numbers where the encoder channels are connected
int line_number_for_m1a = 105;
int line_number_for_m1b = 106;
int line_number_for_m2a = 84;
int line_number_for_m2b = 130;
// Initialise a GPIO chip, line, and event objects
struct gpiod_chip *chip;
struct gpiod_line *line_m1a;
struct gpiod_line *line_m1b;
struct gpiod_line *line_m2a;
struct gpiod_line *line_m2b;
struct gpiod_line_bulk line_bulk;
struct gpiod_line_event event;
struct gpiod_line_bulk event_bulk;
// Specify the timeout specifications
// > The first entry is seconds
// > The second entry is nano-seconds
struct timespec timeout_spec = { 0, 10000000 };
// Intialise a variable for the flags returned by GPIO calls
int returned_wait_flag;
int returned_read_flag;
// Open the GPIO chip
chip = gpiod_chip_open(gpio_chip_name);
// Retrieve the GPIO lines
line_m1a = gpiod_chip_get_line(chip,line_number_for_m1a);
line_m1b = gpiod_chip_get_line(chip,line_number_for_m1b);
line_m2a = gpiod_chip_get_line(chip,line_number_for_m2a);
line_m2b = gpiod_chip_get_line(chip,line_number_for_m2b);
// Initialise the line bulk
gpiod_line_bulk_init(&line_bulk);
// Add the lines to the line bulk
gpiod_line_bulk_add(&line_bulk, line_m1a);
gpiod_line_bulk_add(&line_bulk, line_m1b);
gpiod_line_bulk_add(&line_bulk, line_m2a);
gpiod_line_bulk_add(&line_bulk, line_m2b);
// Display the status
std::cout << "[ENCODER COUNTER] Chip " << gpio_chip_name << " opened and lines " << line_number_for_m1a << ", " << line_number_for_m1b << ", " << line_number_for_m2a << " and " << line_number_for_m2a << " retrieved";
// Request the line events to be monitored
// > Note: only one of these should be uncommented
gpiod_line_request_bulk_rising_edge_events(&line_bulk, "foobar");
//gpiod_line_request_bulk_falling_edge_events(&line_bulk, "foobar");
//gpiod_line_request_bulk_both_edges_events(&line_bulk, "foobar");
// Enter a loop that monitors the encoders
while (m_threads_should_count)
{
// Monitor for the requested events on the GPIO line bulk
// > Note: the function "gpiod_line_event_wait" returns:
// 0 if wait timed out
// -1 if an error occurred
// 1 if an event occurred.
returned_wait_flag = gpiod_line_event_wait_bulk(&line_bulk, &timeout_spec, &event_bulk);
// Lock the mutex before processing the events
m_counting_mutex.lock();
// Respond based on the the return flag
if (returned_wait_flag == 1)
{
// Get the number of events that occurred
int num_events_during_wait = gpiod_line_bulk_num_lines(&event_bulk);
// Iterate over the event
for (int i_event = 0; i_event < num_events_during_wait; i_event++)
{
// Get the line handle for this event
struct gpiod_line *line_handle = gpiod_line_bulk_get_line(&event_bulk, i_event);
// Get the number of this line
unsigned int this_line_number = gpiod_line_offset(line_handle);
// Read the event on the GPIO line
// > Note: the function "gpiod_line_event_read" returns:
// 0 if the event was read correctly
// -1 if an error occurred
returned_read_flag = gpiod_line_event_read(line_handle,&event);
// Respond based on the the return flag
if (returned_read_flag == 0)
{
// Increment the respective count
if (this_line_number == line_number_for_m1a)
m_counts_for_m1a++;
else if (this_line_number == line_number_for_m1b)
m_counts_for_m1b++;
else if (this_line_number == line_number_for_m2a)
m_counts_for_m2a++;
else if (this_line_number == line_number_for_m2b)
m_counts_for_m2b++;
} // END OF: "if (returned_read_flag == 0)"
} // END OF: "for (int i_event = 0; i_event < num_events_during_wait; i_event++)"
} // END OF: "if (returned_wait_flag == 1)"
// Get a local copy of the flag
bool should_count_local_copy = m_threads_should_count;
// Unlock the mutex
m_counting_mutex.unlock();
// Break if necessary
if (!should_count_local_copy)
break;
} // END OF: "while (true)"
// Release the lines
gpiod_line_release_bulk(&line_bulk);
// Close the GPIO chip
gpiod_chip_close(chip);
// Inform the user
std::cout << "[ENCODER COUNTER] Lines released and GPIO chip closed";
} // END OF: "void threadForCounting()"
// Function implementation
void threadForReading()
{
static int cumulative_counts_for_m1 = 0;
static int cumulative_counts_for_m2 = 0;
while(m_threads_should_count)
{
// Get a local copy of the variables
m_counting_mutex.lock();
int counts_for_m1a_local_copy = m_counts_for_m1a;
int counts_for_m1b_local_copy = m_counts_for_m1b;
int counts_for_m2a_local_copy = m_counts_for_m2a;
int counts_for_m2b_local_copy = m_counts_for_m2b;
bool should_count_local_copy = m_threads_should_count;
// Reset the shared counting variables to zero
m_counts_for_m1a = 0;
m_counts_for_m1b = 0;
m_counts_for_m2a = 0;
m_counts_for_m2b = 0;
m_counting_mutex.unlock();
// Add the counts to the cumulative total
cumulative_counts_for_m1 += (counts_for_m1a_local_copy + counts_for_m1b_local_copy);
cumulative_counts_for_m2 += (counts_for_m2a_local_copy + counts_for_m2b_local_copy);
// Display the cumulative counts
printf("cumulative counts = [%d, %d]\n", cumulative_counts_for_m1, cumulative_counts_for_m2);
// Break if necessary
if (!should_count_local_copy)
break;
// Sleep for a bit
usleep(100000);
}
}
// Main function
int main(int argc, char* argv[])
{
// Set the flag that threads should count
m_threads_should_count = true;
// Create the threads
std::thread counting_thread (threadForCounting);
std::thread reading_thread (threadForReading);
// Spin the main function for a bit
sleep(3);
// Set the flag to finish counting
m_counting_mutex.lock();
m_threads_should_count = false;
m_counting_mutex.unlock();
// Join back the threads
counting_thread.join();
reading_thread.join();
}
10.3. High level description
Under normal operating conditions, many wheel encoders generate pulses at up to 10 kHz; where the exact rate depends on the encoder resolution, gearbox ratio, and wheel speed. The key point is that we need to think carefully about the limitations and overheads of using a non real-time operating system (i.e., Ubuntu) and a user-space GPIO library (i.e., libgpiod or sysfs). To achieve the most reliable counting of encoder pulses at 10 kHz or more.
To achieve reliable counting we use a multi-threaded implementation where:
One thread monitors the GPIO pins connected to each encoder channel and counts all the rising and/or falling edges. This thread should be programmed in a fashion that has minimal over-head, i.e., no “fluff” in the code.
Another thread reads the the current value of the encoder counts at a fixed frequency and does something useful with the data, e.g., displays the counts value on the screen, publishes the data, performs odometry computations.
The variables that store the current value of the encoder counts are the only variables shared between the two threads, hence a mutex is used to serialise access to these variables.
10.4. Standard threads, boost threads or p threads - which one to use
There are three main libraries for multi-threading in C++ and an in depth discussion is beyond the scope of this wiki page. As a rule-of-thumb for any multi-threaded code, the standard threads option (std::thread ) should be your default choice. Only consider one of the other options if your use-case requires features not available with std::thread. As encoder counting with multiple threads is a simple use-case, we use std::thread.
To investigate the three options further, simply google it, and likely you will find the following summary and much much more:
std::threadIs available on most all platforms.
Requires C++11 or newer compiler.
Provides only basic multi-threading features.
boost::threadIs available on most all platforms.
Is supported by older compilers (i.e., older than C++11)
May require external dependencies, i.e., not install by default on some platforms.
Offers similar features to
std::thread
pthreadOffers more features that the other two options, for example, scheduling policies.
Only works on POSIX systems, i.e., does not work on Windows.
10.5. Prototypical multi-threaded code structure
We start from a “boiler plate” template that captures our key multi-threaded structure, i.e., separate threads for counting and reading, and a main thread that flags when to stop.
Piece together the following to build up the “boiler plate” template code:
#include <thread>), and for using mutex object (i.e., #include <mutex>).// Includes required for threading #include <mutex> #include <thread> // Other includes #include <unistd.h> #include <iostream>
m_ prefix to indicate “member” variables that are accessible from all functions.// Declare the member variables // > For sharing the encoder counts between threads int m_counts_for_m1a = 0; int m_counts_for_m1b = 0; int m_counts_for_m2a = 0; int m_counts_for_m2b = 0; // > Mutex for preventing multiple-access of shared variables std::mutex m_counting_mutex; // > Boolean flag for when to stop counting bool m_threads_should_count = true;
// Declare the function prototypes // > For the thread that reads (and displays) the current counts void threadForReading(); // > For the thread that counts encoder events void threadForCounting();
// Function implementation void threadForReading() { while(m_threads_should_count) { // Get a local copy of the variables m_counting_mutex.lock(); int counts_for_m1a_local_copy = m_counts_for_m1a; int counts_for_m1b_local_copy = m_counts_for_m1b; int counts_for_m2a_local_copy = m_counts_for_m2a; int counts_for_m2b_local_copy = m_counts_for_m2b; bool should_count_local_copy = m_threads_should_count; m_counting_mutex.unlock(); // Display the counts printf("counts = [%d, %d, %d, %d]\n", counts_for_m1a_local_copy, counts_for_m1b_local_copy, counts_for_m2a_local_copy, counts_for_m2b_local_copy); // Break if necessary if (!should_count_local_copy) break; // Sleep for a bit usleep(100000); } }Here it is important to understand that when the mutex is locked, then the threads are potentially serialised (i.e., the other thread pauses execution if it reaches its mutex lock line of code during this period). Hence only minimal operations are performed while the mutex is locked, i.e., quickly take a local copy of the variables.
// Function implementation void threadForCounting() { while(m_threads_should_count) { // Do some fake counting for the purpose of this template m_counting_mutex.lock(); m_counts_for_m1a++; m_counts_for_m1b++; m_counts_for_m2a++; m_counts_for_m2b++; bool should_count_local_copy = m_threads_should_count; m_counting_mutex.unlock(); // Break if necessary if (!should_count_local_copy) break; // Sleep for a bit for the purpose of this template usleep(10000); } } **Reiterating: it is important to understand that when the mutex is locked, then the threads are potentially serialised.**
// Main function int main(int argc, char* argv[]) { // Set the flag that threads should count m_threads_should_count = true; // Create the threads std::thread counting_thread (threadForCounting); std::thread reading_thread (threadForReading); // Spin the main function for a bit sleep(3); // Set the flag to finish counting m_counting_mutex.lock(); m_threads_should_count = false; m_counting_mutex.unlock(); // Join back the threads counting_thread.join(); reading_thread.join(); }
10.6. Compile and run (before adding more complexity)
To check that the multi-threaded development pipeline is working for you, it is a good idea to compile and run the prototypical starting point given in the section above.
Create a file with the contents given in the section above, for example with filename:
encoder_counter.cppCompile the file from command line using the command:
g++ -std=c++11 -o encoder_counter encoder_counter.cpp -lpthread
Run the executable that is generated:
./encoder_counter
The following (or similar) will be printed to the terminal:
counts = [1, 1, 1, 1]
counts = [10, 10, 10, 10]
counts = [20, 20, 20, 20]
counts = [30, 30, 30, 30]
counts = [40, 40, 40, 40]
...
10.7. Monitoring multiple GPIO pins for the encoder channels
In order to monitor events on multiple GPIO pins, we use The gpiod library. Documentation of the the C++ bindings for the gpiod library can be found at the following two locations:
Piece together the following to reimplement the threadForCounting() function to monitor the actual encoder channel and count all events that are detected:
// Function implementation void threadForCounting() { // Specify the gpio chip name of the GPIO interface const char * gpio_chip_name = "/dev/gpiochip0"; // Specify the line numbers where the encoder channels are connected int line_number_for_m1a = 105; int line_number_for_m1b = 106; int line_number_for_m2a = 84; int line_number_for_m2b = 130;Note
See the information for the single board computer that you are using to get some guidance on how to obtain the chip name and line numbers.
The values above are for an Nvidia Jetson Xavier NX running Jetpack 5.x with the encoder channels connected to physical pin numbers {29, 31, 33, 35} on the 40-pin header.
Step 2Initialise thegpiodlibary objects required for monitoring multiple line simultaneously, i.e., abulkof lines.// Initialise a GPIO chip, line, and event objects struct gpiod_chip *chip; struct gpiod_line *line_m1a; struct gpiod_line *line_m1b; struct gpiod_line *line_m2a; struct gpiod_line *line_m2b; struct gpiod_line_bulk line_bulk; struct gpiod_line_event event; struct gpiod_line_bulk event_bulk;Step 3Specify the timeout that is used as the wait time for monitoring, and initialise variables for various flags.// Specify the timeout specifications // > The first entry is seconds // > The second entry is nano-seconds struct timespec timeout_spec = { 0, 10000000 }; // Intialise a variable for the flags returned by GPIO calls int returned_wait_flag; int returned_read_flag;Step 4Open the GPIO chip and lines, and put the lines together into theline_bulkobject.// Open the GPIO chip chip = gpiod_chip_open(gpio_chip_name); // Retrieve the GPIO lines line_m1a = gpiod_chip_get_line(chip,line_number_for_m1a); line_m1b = gpiod_chip_get_line(chip,line_number_for_m1b); line_m2a = gpiod_chip_get_line(chip,line_number_for_m2a); line_m2b = gpiod_chip_get_line(chip,line_number_for_m2b); // Initialise the line bulk gpiod_line_bulk_init(&line_bulk); // Add the lines to the line bulk gpiod_line_bulk_add(&line_bulk, line_m1a); gpiod_line_bulk_add(&line_bulk, line_m1b); gpiod_line_bulk_add(&line_bulk, line_m2a); gpiod_line_bulk_add(&line_bulk, line_m2b); // Display the status std::cout << "[ENCODER COUNTER] Chip " << gpio_chip_name << " opened and lines " << line_number_for_m1a << ", " << line_number_for_m1b << ", " << line_number_for_m2a << " and " << line_number_for_m2a << " retrieved";Step 5Specify the type of line event that is to be monitored. The three options of rising edge, falling edge, or both are all shown, only one should be uncommented.// Request the line events to be monitored // > Note: only one of these should be uncommented gpiod_line_request_bulk_rising_edge_events(&line_bulk, "foobar"); //gpiod_line_request_bulk_falling_edge_events(&line_bulk, "foobar"); //gpiod_line_request_bulk_both_edges_events(&line_bulk, "foobar");Step 6Enter the while loop for monitoring the encoder channels while the flag indicates to do so.// Enter a loop that monitors the encoders while (m_threads_should_count) {Step 7Call thegpiodlibrary function that monitors for events on the all on the lines in the line bulk. This function stalls the execution here until either: an event is detected; or the timeout duration elapses.// Monitor for the requested events on the GPIO line bulk // > Note: the function "gpiod_line_event_wait" returns: // 0 if wait timed out // -1 if an error occurred // 1 if an event occurred. returned_wait_flag = gpiod_line_event_wait_bulk(&line_bulk, &timeout_spec, &event_bulk);Step 8Lock the mutex and process any events that occurred, incrementing the respective count variable for each event on a line.// Lock the mutex before processing the events m_counting_mutex.lock(); // Respond based on the the return flag if (returned_wait_flag == 1) { // Get the number of events that occurred int num_events_during_wait = gpiod_line_bulk_num_lines(&event_bulk); // Iterate over the event for (int i_event = 0; i_event < num_events_during_wait; i_event++) { // Get the line handle for this event struct gpiod_line *line_handle = gpiod_line_bulk_get_line(&event_bulk, i_event); // Get the number of this line unsigned int this_line_number = gpiod_line_offset(line_handle); // Read the event on the GPIO line // > Note: the function "gpiod_line_event_read" returns: // 0 if the event was read correctly // -1 if an error occurred returned_read_flag = gpiod_line_event_read(line_handle,&event); // Respond based on the the return flag if (returned_read_flag == 0) { // Increment the respective count if (this_line_number == line_number_for_m1a) m_counts_for_m1a++; else if (this_line_number == line_number_for_m1b) m_counts_for_m1b++; else if (this_line_number == line_number_for_m2a) m_counts_for_m2a++; else if (this_line_number == line_number_for_m2b) m_counts_for_m2b++; } // END OF: "if (returned_read_flag == 0)" } // END OF: "for (int i_event = 0; i_event < num_events_during_wait; i_event++)" } // END OF: "if (returned_wait_flag == 1)" // Get a local copy of the flag bool should_count_local_copy = m_threads_should_count; // Unlock the mutex m_counting_mutex.unlock();Step 9Break the monitoring loop if the flag indicates to do so, and release the GPIO lines so that other processes can access these GPIO lines if they need to.// Break if necessary if (!should_count_local_copy) break; } // END OF: "while (true)" // Release the lines gpiod_line_release_bulk(&line_bulk); // Close the GPIO chip gpiod_chip_close(chip); // Inform the user std::cout << "[ENCODER COUNTER] Lines released and GPIO chip closed"; } // END OF: "void threadForCounting()"
10.8. Reading and displaying the current encoder counts (and resetting)
The threadForReading() function in the boiler plate code above already makes a thread-safe copy of the counts variables and displays those value. Hence there is nothing more that need to be implemented for this step.
That said, such encoders are often referred to as “incremental” encoders. Hence we treat the threadForCounting() function as being responsible for keeping track of the increment in encoder event since that the previous read of the counts variables. This shifts the responsibility to the threadForReading() function for performing the desired computations with the counts, for example, keeping track of the cumulative counts or performing wheel odometry computations. The justification is that:
The
threadForCounting()function runs at a high frequency (i.e., the variable frequency of encoder events), hence we should minimise computations performed by this thread.The
threadForReading()functions runs at a lower frequency (i.e., the fixed frequency that we specify), hence it is more flexible to implement processing of the encoder counts in this thread.
threadForReading() function as follows to track cumulative counts and reset the shared variable to zero with each read:static variables for the cumulative counts.// Function implementation void threadForReading() { static int cumulative_counts_for_m1 = 0; static int cumulative_counts_for_m2 = 0; while(m_threads_should_count) { // Get a local copy of the variables m_counting_mutex.lock(); int counts_for_m1a_local_copy = m_counts_for_m1a; int counts_for_m1b_local_copy = m_counts_for_m1b; int counts_for_m2a_local_copy = m_counts_for_m2a; int counts_for_m2b_local_copy = m_counts_for_m2b; bool should_count_local_copy = m_threads_should_count; // Reset the shared counting variables to zero m_counts_for_m1a = 0; m_counts_for_m1b = 0; m_counts_for_m2a = 0; m_counts_for_m2b = 0; m_counting_mutex.unlock(); // Add the counts to the cumulative total cumulative_counts_for_m1 += (counts_for_m1a_local_copy + counts_for_m1b_local_copy); cumulative_counts_for_m2 += (counts_for_m2a_local_copy + counts_for_m2b_local_copy); // Display the cumulative counts printf("cumulative counts = [%d, %d]\n", cumulative_counts_for_m1, cumulative_counts_for_m2); // Break if necessary if (!should_count_local_copy) break; // Sleep for a bit usleep(100000); } }
10.9. Compile and run with the actual encoders
Add the gpiod library included to the top of your file:
#include <gpiod.h>
Compile and run the file from command line, this time adding the compile flag for the gpiod library, i.e.:
g++ -std=c++11 -o encoder_counter encoder_counter.cpp -lpthread -lgpiod
./encoder_counter
For the few seconds that the code is counting, inject encoder pulses into the respective pins.
10.10. Putting it into a ROS node
Putting the stand-alone C++ developed above into a ROS node is as simple as:
Start with a ROS node that spins at a fixed frequency.
Add the code of the main function in stand-alone C++ code to the main function of the ROS node.
Remove the
sleep(3);.The
ros::spin()performs the function of keeping the main function alive, and hence the encoder counting is on going until the node is shutdown.Hence, main function code before
sleep(3);goes beforeros::spin(), and code aftersleep(3);goes afterros::spin()
Add the code of the
threadForReadingfunction in stand-alone C++ code to thetimerCallback(...)function of the ROS node.Remove the displaying of the encoder counts and instead publish the encoder counts and instead display that data for other nodes to process.
Add the ROS node cpp file to the
CMakeList.txtas per the Make nodes executable (C++ and Python) page. * You need to add the-lgpiodflag at the end of thetarget_link_libraries(...)instruction, for example:target_link_libraries(encoder_counter ${catkin_LIBRARIES} -lgpiod)You can view an example implementation of such a ROS node in the asclinic-system respository at the location:
catkin_ws/src/asclinic_pkg/src/nodes/
in the file
encoder_read_multi_threaded.cpp.
10.11. Launch file with parameters
The line numbers of the GPIO pins to which the encoder channels are connected are likely to change depending on the situation, for example:
Due to an operating system update.
Due to running the code on different computers such as an Nvidia Jetson or a Raspberry Pi.
Hence, the line numbers are perfect candidates to input are parameters in a launch file. This allows launch the same node on different systems using the same C++ code.
Add the parameters to the launch file as follows:
<launch>
<group ns="asc">
<!-- LAUNCH AN ENCODER COUNTER NODE -->
<node
pkg = "asclinic_pkg"
name = "encoder_counter"
output = "screen"
type = "encoder_counter"
>
<param
name = "line_number_for_m1a"
value = "105"
/>
<param
name = "line_number_for_m1b"
value = "106"
/>
<param
name = "line_number_for_m2a"
value = "84"
/>
<param
name = "line_number_for_m2b"
value = "130"
/>
</node>
</group>
</launch>
For each parameter, add the following to the main function of the ROS node to get the parameter variable into the respective member variable:
// > For motor 1 channel A
if ( !nodeHandle.getParam("line_number_for_m1a", m_line_number_for_m1a) )
{
// Display an error message
ROS_INFO("[ENCODER COUNTER] FAILED to get \"line_number_for_m1a\" parameter. Using default value instead.");
}
Note
Two nodes can**NOT** access the same GPIO line. The first node to open a line blocks other processes from accessing that line.
