[TB3 / ROS2] Implement sonar using interrupts

[limaanto]

Hi everyone!
I've been extensively using Turtlebot3 with HC-SR04 sonars under ROS 2 lately and have been frustrated with the unintuitive latencies caused by pulseIn as pointed out in #233.
As an alternative, I have implemented interrupt-based time measurement.

Basically, it uses a state machine to

• Trigger
• Wait for the echo to start
• Wait for the echo to end
• Cooldown (waiting for the next SensorState message to be ready)

The two "wait" states are implemented using attachInterrupt.
Everything is timed-out if something wrong happens and this is reflected in sonar_data_ by negative numbers.

Viewed from the outside, two things have changed:

• The updateSonar is now actually called
• The sonar pins unfortunately had to change to be interrupt-compatible

The whole process should be pretty lightweight on the CPU and produces accurate measurement.
The following graph is the distance error obtained by comparing measured timings and known distances

Cheers

[gemini-code-assist]

Summary of Changes

Hello @limaanto, I'm Gemini Code Assist¹! I'm currently reviewing this pull request and will post my feedback shortly. In the meantime, here's a summary to help you and other reviewers quickly get up to speed!

This pull request significantly refactors the sonar sensor implementation for the Turtlebot3 in a ROS2 environment. The primary goal is to address and resolve latency issues associated with the previous pulseIn method by transitioning to a more efficient and accurate interrupt-driven time measurement system. The changes involve implementing a state machine to manage the sonar's lifecycle, utilizing hardware interrupts for precise timing, and updating pin configurations for compatibility. This leads to more reliable and responsive distance measurements from the HC-SR04 sonar.

Highlights

• Interrupt-Driven Sonar Implementation: The core of the sonar sensor implementation has been re-engineered to use hardware interrupts for time measurement, moving away from the blocking pulseIn function. This change aims to significantly reduce latency and improve the accuracy of sonar readings.
• State Machine for Sonar Operation: A robust state machine has been introduced to manage the sonar's operational flow, encompassing phases such as triggering the ping, waiting for the echo to start, measuring the echo duration, and a cooldown period before the next measurement. This ensures precise control over the sonar cycle.
• Updated Sonar Pin Configuration: The sonar's trigger and echo pins have been reconfigured to be compatible with the microcontroller's interrupt capabilities, which was a necessary step for the new interrupt-based measurement approach.
• Enhanced Error Handling with Timeouts: Timeout mechanisms have been integrated into the sonar's measurement process. If the echo signal is not detected within expected timeframes, the sonar_data_ will report negative values, indicating a measurement failure.
• Activation of Sonar Update Function: The updateSonar function is now actively called within the main TurtleBot3Core::run() loop, ensuring continuous and real-time updates of sonar data, which was previously commented out or not fully utilized.

Using Gemini Code Assist

The full guide for Gemini Code Assist can be found on our documentation page, here are some quick tips.

Invoking Gemini

You can request assistance from Gemini at any point in your pull request via creating an issue comment (i.e. comment on the pull request page) using either /gemini <command> or @gemini-code-assist <command>. Below is a summary of the supported commands.

Feature	Command	Description
Code Review	/gemini review	Performs a code review for the current pull request in its current state.
Pull Request Summary	/gemini summary	Provides a summary of the current pull request in its current state.
Comment	@gemini-code-assist	Responds in comments when explicitly tagged, both in issue comments and review comments.
Help	/gemini help	Displays a list of available commands.

Customization

To customize Gemini Code Assist for GitHub experience, repository maintainers can create a configuration file and/or provide a custom code review style guide (such as PEP-8 for Python) by creating and adding files to a .gemini/ folder in the base of the repository. Detailed instructions can be found here.

Limitations & Feedback

Gemini Code Assist may make mistakes. Please leave feedback on any instances where its feedback is incorrect or counter productive. You can react with 👍 and 👎 on @gemini-code-assist comments. If you're interested in giving your feedback about your experience with Gemini Code Assist for Github and other Google products, sign up here.

You can also get AI-powered code generation, chat, as well as code reviews directly in the IDE at no cost with the Gemini Code Assist IDE Extension.

Footnotes

1. Review the Privacy Notices, Generative AI Prohibited Use Policy, Terms of Service, and learn how to configure Gemini Code Assist in GitHub here. Gemini can make mistakes, so double check it and use code with caution. ↩

[gemini-code-assist]

Code Review

This pull request implements an interrupt-based approach for the sonar sensor, which is a great improvement over the blocking pulseIn function, enhancing the robot's responsiveness. The implementation uses a state machine and is well-structured with timeouts for robustness.

My review focuses on a few key areas to improve the implementation further:

• A critical issue where the sonar pulse duration is not converted to distance.
• Potential race conditions due to unprotected access to shared variables between the main loop and interrupt service routines.
• Improving code quality by avoiding global variables with external linkage and hardcoded pin numbers.

Addressing these points will make the new implementation more robust and maintainable.

[gemini-code-assist]

This calculation stores the raw echo pulse duration (in microseconds) in sonar_data_. However, getSonarData() returns this value directly, and the rest of the system (e.g., the ROS node) expects a distance value in meters. The conversion from duration to distance is missing. This is a critical bug that will result in incorrect sensor readings being published.

The distance can be calculated using the speed of sound (~343 m/s). The formula is: distance_in_meters = (duration_in_microseconds * 343.0 / 1000000.0) / 2.0.

    sonar_data_ = (t - sonar_t_) * 0.0001715f; // Convert duration to distance in meters

[gemini-code-assist]

These file-scope variables are missing the static keyword. Without it, they have external linkage, which pollutes the global namespace and can lead to linking errors or hard-to-debug issues if other files define variables with the same names. They should be declared static to limit their scope to this translation unit.

static volatile SonarState sonar_state_;
static volatile float sonar_data_;
static volatile uint32_t sonar_t_;

[gemini-code-assist]

This function reads and writes several volatile variables (sonar_state_, sonar_t_, sonar_data_) that are shared with interrupt service routines. This can lead to race conditions. For example, an interrupt could modify sonar_t_ and sonar_state_ after updateSonar has read sonar_t_ but before it has read sonar_state_, causing the function to operate on an inconsistent state.

To ensure thread safety, all accesses (reads and writes) to these shared variables from non-ISR code should be protected within a critical section. You can achieve this by disabling interrupts before the access and re-enabling them immediately after.

Example of safe access:

// Safe read
SonarState local_state;
noInterrupts();
local_state = sonar_state_;
interrupts();
if (local_state == ...)

// Safe write
noInterrupts();
sonar_state_ = NEW_STATE;
sonar_t_ = micros();
interrupts();

[gemini-code-assist]

The sonar pin numbers are hardcoded as magic numbers (4 and 2). This reduces code readability and maintainability. It's better to use the symbolic BDPIN_ constants defined for the board, as was done in the previous version of the code. This makes it clear which physical pins are being used and makes the code easier to adapt to hardware changes.

  sonar_pin_.trig = BDPIN_GPIO_4;      // Pin 4
  sonar_pin_.echo = BDPIN_PUSH_SW_1; // Pin 2