Why is Sonar disabled in the code?

[OleIdole]

Hi, I need to use a HC-SR04 ultrasonic sensor on Turtlebot3 with ROS. The problem is that the sensor_state node reads sonar distance as 0 constantly. Upon further inspection, I noticed that sensor_state_msg.sonar = sensors.getSonarData() is disabled in turtlebot3_core, it is also disabled in several other sections in the code and marked with // TODO.

I am confused why it is disabled as there are examples in ROS which uses sonar. Is there any old version of the code that has Sonar implemented or a way I can enable it? Or is it simply incomplete?

[ROBOTIS-Will]

@OleIdole
The default TurtleBot3 platform does not use a ultrasonic sensor so it has been disabled and not implemented to save the data bandwidth.
In order to implement your own sensor, please refer to Example > OpenCR / ROS > 02. Sensors > c_Ultrasonic Arduino example.
Thank you.

[OleIdole]

@ROBOTIS-Will
Thank you for clarifying! I've looked into that example but it is only designed to serialprint the information. It seems like the code regarding sonar, although disabled, is already made in the Turtlebot3 platform. Do you know if I can make it include the sonar information in the sensor_state msg by enabling the currently disabled code that is written? Or was it only partially implemented and not fully functional?

Thanks in advance.

[OleIdole]

I continued fault-finding on the issue and managed to read the sonar data. Turns out I got it working a couple of days ago by enabling the code that was disabled for sonar in the Turtlebot3 code. But I didn't see that it worked because when I launched the node, nothing was in the range of the ultrasonic sensor, which resulted in no echo. And with the current code, this seems to wait a full second before the signal is counted as not returned/lost. The problem is that the sonar is requested to ping many times every second, and with no object in range, this will clog up a buffer where messages are sent only once a second.

It works fine if I launch the node with something within distance. So the issue I started is considered solved! My next focus now will be to find some way of handling the buffer where it doesn't store old information and doesn't wait a full second for each signal that isn't returned.

Thank you for helping.

[TThetier]

Hi @OleIdole.
I'm currently trying to achieve the same thing, ie adding a sonar on the turtlebot3. I noticed the same thing as you (sonar functions disabled).
How did you manage to modify the openCR firmware to enable those functions please? And have you solved your following issues?
Thanks!

[OleIdole]

@TThetier
I did manage to fix it! The problem was mainly that the Turtlebot3 library implemented sonar with the pulseIn() function. This function records the time-of-flight from when a pulse is sent (ping) until it is received (echo). The problem is that it can wait up to 1 second if the echo is not returned, which clogs up everything. Imagine getting heaps of ping requests in one second, but being stuck on each request for a full second.

I solved it by replacing the ping implementation with functionality from the NewPing library, which only waits for as long as the sensor can possibly take to return a ping. E.g. if the sensor is capable of measuring up to 2 meters, then the roundtrip time is approximately 0.12 seconds for max distance. We don't need to wait longer after that, because the sensor is not capable of further distances. That is what the NewPing library uses to determine its max wait time. Implementing it solved all issues of measurements clogging up.

To implement it, download the NewPing library, include it in turtlebot3_sensor.cpp and replace existing sonar ping function with the one from NewPing. You also need to uncomment the sonar functions in turtlebot3_core.ino as the code will not call and include sonar in its range scans otherwise.

[lslabon]

Hey :)

I had the same problem with no sensor output oft he sonar either and was able to fix it just at the moment, but on a different way than Oleldole did.
I decided to let you guys know how I got rid of the problem, because I could not find a soltion that worked out for me..

1. I (obviously) enabled every function containing the sonar in the Arduino Turtlebot3_core code

2. I changed the turtlebot3_sensor.cpp the following way (contained in the package of opencr):

I set every make_pulse and get_duration == TRUE.
I deleted the = 0.0 when defining the floats
Then I deleted the time part in the code ( I didn't get that one, so I decided to change it) and replaced it with the Arduino c_Ultrasonic part: digital_write ..... (turn off - turn on - turn off) to get the sensor data

So this is how my code looks like: (the part with the Sonar)

---

void Turtlebot3Sensor::initSonar(void)
{
sonar_pin_.trig = BDPIN_GPIO_1;
sonar_pin_.echo = BDPIN_GPIO_2;

pinMode(sonar_pin_.trig, OUTPUT);
pinMode(sonar_pin_.echo, INPUT);
}

void Turtlebot3Sensor::updateSonar(uint32_t t)
{
static uint32_t t_time;
static bool make_pulse = TRUE;
static bool get_duration = TRUE;

float distance, duration;

if (make_pulse == TRUE)
{
digitalWrite(sonar_pin_.trig, LOW);
delayMicroseconds(2);
digitalWrite(sonar_pin_.trig, HIGH);
delayMicroseconds(10);
digitalWrite(sonar_pin_.trig, LOW);
}

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

make_pulse = TRUE;
get_duration = TRUE;

}

sonar_data_ = distance;
}

float Turtlebot3Sensor::getSonarData(void)
{
float distance1;

distance1 = sonar_data_;

return distance1;

---

I am kind of new to this whole ros thing and programming at all - so feel free to give some advice how the time part did work :) I just decided to replace it by some code I understand and it works out fine for me:p

Now I am going to add a couple more sonars to my robot. When I figured out how to change the code I will let you know!

Have a nice day!
Laura

[lslabon]

I got it work now with multiple sensors. Maybe its not the most elegant solution but it was kind of easy for me to implement it.

I did copy the whole Sonar Part (initSonar, updateSonar, getSonarDate) and changed all variables for sonar 1 to ...1, for Sonar 2 to ...2 and so on.
All new functions (e.g. initsonar2(void)) must be defined in the sensor.h ! All new floats (e.g. distance2) in the cpp.
Then in the turtlebot3 core (Arduino) you must add the lines for initializing the sensors and updating the sensor data (e.g. sensors.initSonar2(); / sensors.updateSonar2();)
The last line (where the sensor state is defined) you can leave as it is, because we will only have one output function with the minimum distance.

With my solution you won't be able to see all the distances objects have from the sonar. I decided to set a min function to define the getSonarData: distance = (std::min)(sonar_data_1, sonar_data_2);
I figured out that I had to put std::min in brackets because otherwise there is a multiple definition of min in our whole program. But we have to define using algorithms in our cpp too, because otherwise it won't be able to use the function min.

As I already said I am not familiar to programming at all so there might be a more elegant way to solve this problem :) Although I decided to share mine with you guys as it is working and I could not find a solution to this in the internet.

This is how my sensor.cpp looks with 2 sensors (I am going to add some more):

---

// code for sensor 1

void Turtlebot3Sensor::initSonar1(void)
{
sonar_pin_.trig1 = BDPIN_GPIO_15;
sonar_pin_.echo1 = BDPIN_GPIO_16;

pinMode(sonar_pin_.trig1, OUTPUT);
pinMode(sonar_pin_.echo1, INPUT);
}

void Turtlebot3Sensor::updateSonar1(void)
{
static bool make_pulse1 = TRUE;
static bool get_duration1 = TRUE;

float distance1, duration1;

if (make_pulse1 == TRUE)
{
digitalWrite(sonar_pin_.trig1, LOW);
delayMicroseconds(2);
digitalWrite(sonar_pin_.trig1, HIGH);
delayMicroseconds(10);
digitalWrite(sonar_pin_.trig1, LOW);
}

if (get_duration1 == TRUE)
{
duration1 = pulseIn(sonar_pin_.echo1, HIGH);
distance1 = ((float)(340 * duration1) / 10000) / 2;

make_pulse1 = TRUE;
get_duration1 = TRUE;

}

sonar_data_1 = distance1;
}

float Turtlebot3Sensor::getSonarData1(void)
{
float distance1;

distance1 = sonar_data_1;

return distance1;
}

//****************************************************************
// code for sensor 2

void Turtlebot3Sensor::initSonar2(void)
{
sonar_pin_.trig2 = BDPIN_GPIO_17;
sonar_pin_.echo2 = BDPIN_GPIO_18;

pinMode(sonar_pin_.trig2, OUTPUT);
pinMode(sonar_pin_.echo2, INPUT);
}

void Turtlebot3Sensor::updateSonar2(void)
{
static bool make_pulse2 = TRUE;
static bool get_duration2 = TRUE;

float distance2, duration2;

if (make_pulse2 == TRUE)
{
digitalWrite(sonar_pin_.trig2, LOW);
delayMicroseconds(2);
digitalWrite(sonar_pin_.trig2, HIGH);
delayMicroseconds(10);
digitalWrite(sonar_pin_.trig2, LOW);
}

if (get_duration2 == TRUE)
{
duration2 = pulseIn(sonar_pin_.echo2, HIGH);
distance2 = ((float)(340 * duration2) / 10000) / 2;

make_pulse2 = TRUE;
get_duration2 = TRUE;

}

sonar_data_2 = distance2;
}

float Turtlebot3Sensor::getSonarData2(void)
{
float distance2;

distance2 = sonar_data_2;

return distance2;
}

//**********************************************************************************
// looking for the minimal value to set sensor_state correctly

float Turtlebot3Sensor::getSonarData(void)
{
float distance;

distance = (std::min)(sonar_data_1, sonar_data_2);

return distance;
}

---

[jhbirdchoi]

Hi

I had the same problem.
I solved it using the Newping library.
It works fine with opencr as well.

Please refer to the link below.
https://cafe.naver.com/studyfuchsia/79

thanks