Skip to content

[TB3 / ROS2] Implement sonar using interrupts #343

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
limaanto wants to merge 1 commit into
base: master
Choose a base branch
from
Open

[TB3 / ROS2] Implement sonar using interrupts #343

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 1 addition & 3 deletions ...no/opencr_arduino/opencr/libraries/turtlebot3_ros2/include/turtlebot3/turtlebot3_sensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ class Turtlebot3Sensor

// Sonar sensor
void initSonar(void);
void updateSonar(uint32_t t);
void updateSonar(void);
float getSonarData(void);

// Illumination sensor
Expand All @@ -99,8 +99,6 @@ class Turtlebot3Sensor
LedPinArray led_pin_array_;
SonarPin sonar_pin_;

float sonar_data_;

bool is_melody_play_complete_;
uint16_t melody_note_[8];
uint8_t melody_duration_[8];
Expand Down
2 changes: 1 addition & 1 deletion arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -590,7 +590,7 @@ void TurtleBot3Core::run()
// Update the IMU unit
sensors.updateIMU();
// Update sonar data
// TODO: sensors.updateSonar(t);
sensors.updateSonar();
// Run buzzer if there is still melody to play.
sensors.onMelody();

Expand Down
106 changes: 82 additions & 24 deletions arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3_sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,40 @@

#include "../../include/turtlebot3/turtlebot3_sensor.h"

enum SonarState {
IDLE = 0,
SENDING_PING,
WAITING_FOR_MEASURE,
MEASURING,
COOLING_DOWN
};

volatile SonarState sonar_state_;
volatile float sonar_data_;
volatile uint32_t sonar_t_;
Comment on lines +27 to +29
Copy link

@gemini-code-assist gemini-code-assist bot Aug 20, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

high

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_;

static uint32_t sonar_echo_;

static void sonarEchoEnd(void)
{
if(sonar_state_ == MEASURING)
{
const uint32_t t = micros();
sonar_data_ = t - sonar_t_;
Copy link

@gemini-code-assist gemini-code-assist bot Aug 20, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

critical

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

sonar_state_ = COOLING_DOWN;
sonar_t_ = t;
}
}

static void sonarEchoStart(void)
{
if(sonar_state_ == WAITING_FOR_MEASURE)
{
attachInterrupt(digitalPinToInterrupt(sonar_echo_), sonarEchoEnd, FALLING);
sonar_t_ = micros();
sonar_state_ = MEASURING;
}
}

Turtlebot3Sensor::Turtlebot3Sensor()
{
}
Expand Down Expand Up @@ -287,46 +321,70 @@ float Turtlebot3Sensor::getIRsensorData(void)

void Turtlebot3Sensor::initSonar(void)
{
sonar_pin_.trig = BDPIN_GPIO_1;
sonar_pin_.echo = BDPIN_GPIO_2;
sonar_pin_.trig = 4;
sonar_pin_.echo = 2;
Comment on lines +324 to +325
Copy link

@gemini-code-assist gemini-code-assist bot Aug 20, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

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

sonar_echo_ = sonar_pin_.echo;

pinMode(sonar_pin_.trig, OUTPUT);
pinMode(sonar_pin_.echo, INPUT);
digitalWrite(sonar_pin_.trig, LOW);

sonar_data_ = -1.0;
sonar_state_ = IDLE;
}

void Turtlebot3Sensor::updateSonar(uint32_t t)
void Turtlebot3Sensor::updateSonar(void)
Copy link

@gemini-code-assist gemini-code-assist bot Aug 20, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

high

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();

{
static uint32_t t_time = 0;
static bool make_pulse = TRUE;
static bool get_duration = FALSE;
static const uint32_t ms = 1000;
static const uint32_t ping_timeout = 5*ms;
static const uint32_t pong_timeout = 20*ms;
static const uint32_t sonar_duration = 50*ms; // SensorState message is at 20Hz
static uint32_t sonar_start_;

float distance = 0.0, duration = 0.0;
const uint32_t t = micros();
const uint32_t duration = t - sonar_t_;

if (make_pulse == TRUE)
if (sonar_state_ == IDLE)
{
digitalWrite(sonar_pin_.trig, HIGH);

if (t - t_time >= 10)
sonar_state_ = SENDING_PING;
sonar_start_ = t;
sonar_t_ = t;
}
else if (sonar_state_ == SENDING_PING)
{
if (duration >= 10)
{
attachInterrupt(digitalPinToInterrupt(sonar_pin_.echo), sonarEchoStart, RISING);
sonar_state_ = WAITING_FOR_MEASURE;
digitalWrite(sonar_pin_.trig, LOW);

get_duration = TRUE;
make_pulse = FALSE;

t_time = t;
sonar_t_ = t;
}
}

if (get_duration == TRUE)
else if(sonar_state_ == WAITING_FOR_MEASURE)
{
duration = pulseIn(sonar_pin_.echo, HIGH);
distance = ((float)(340 * duration) / 10000) / 2;

make_pulse = TRUE;
get_duration = FALSE;
if (duration >= ping_timeout)
{
sonar_state_ = COOLING_DOWN;
sonar_data_ = -2.0;
sonar_t_ = t;
}
}
else if (sonar_state_ == MEASURING)
{
if (duration >= pong_timeout)
{
sonar_state_ = COOLING_DOWN;
sonar_data_ = -3.0;
}
}
else if(sonar_state_ == COOLING_DOWN)
{
if (t >= sonar_start_ + sonar_duration)
{
sonar_state_ = IDLE;
}
}

sonar_data_ = distance;
}

float Turtlebot3Sensor::getSonarData(void)
Expand Down