Syrine
Posts: 1
Joined: Tue Aug 27, 2019 7:51 am

Code for multiple ultrasonic sensors ROS related

Tue Aug 27, 2019 7:54 am

Hi,

I have 7 ultrasound sensors (hc-sr04) on my robot wired on the raspberry pi. I wanted to write a node that would first trigger only 4 of them and publish each on its own topic. Only the code down here gives me negative travel time and so on negative distance. Can you tell me what mistakes I've made? I know this isn't a ROS problem but I appreciate any help! Thank you

#include "vector"

#include "stdio.h"

#include "stdlib.h"

#include "ros/ros.h"

#include "sensor_msgs/Range.h"

#include "wiringPi.h"

using namespace std;

const int size=7;

int trigger_[size]={18,12,13,19,23,20,27};

int echo_[size]={17,06,16,26,05,21,22};

long start_time=0, end_time=0, time_diff=0, now;

void setup_IO(int trigger_[size], int echo_[size]){

for (int i = 0; i < size; ++i) {

pinMode(trigger_, OUTPUT);

pinMode(echo_, INPUT);

digitalWrite(trigger_, LOW);

delay(500);
}

}

void getPulseLength() {

for (int i = 0; i < size; ++i) {

if (i % 2 == 0) {
start_time = micros();

while (digitalRead(echo_) == HIGH)

end_time = micros();

}

}

}

double getRangeFirstSet(int timeout) {

double distance[size];

for (int i = 0; i < size; ++i) {

delay(10);

if (i % 2 == 0) {
digitalWrite(trigger_, HIGH);

delayMicroseconds(10);

digitalWrite(trigger_, LOW);

now=micros();

while (digitalRead(echo_) == LOW && micros()-now<timeout) {<="" p="">

getPulseLength();

time_diff = end_time - start_time;

distance = (time_diff/1000000)*343/2; }

if (distance < 0.02 || distance > 2) {

ROS_INFO("time_diff %ld; Out of Range %f m", time_diff, distance[i]);}

} else {

digitalWrite(trigger_[i], LOW);

digitalWrite(echo_[i], LOW);

}

return distance[i];

}

}

int main(int argc, char **argv) {

// Start ROS node.

ROS_INFO("Starting node");

ros::init(argc, argv, "hc_sr04s");

ros::NodeHandle n;

ros::Rate rate(10);

wiringPiSetupSys(); // uses gpio pin numbering

if (wiringPiSetup()<0){

cout<<"failed\n";

}

// Build a publisher for each sonar.

vector<ros::publisher> sonar_pubs;

for (int i = 0; i < size; ++i) {

stringstream ss;

ss << "range_" << i+1;

sonar_pubs.push_back(n.advertise<sensor_msgs::Range>(ss.str(), 10));
}

// Build base range message that will be used for

// each time a msg is published.

sensor_msgs::Range range;

range.radiation_type = sensor_msgs::Range::ULTRASOUND;

range.min_range = 0.02;

range.max_range = 2;

while(ros::ok()) {

for (int i = 0; i < size; ++i) {

stringstream ss;

ss << "sonar_" << i+1;

range.header.stamp = ros::Time::now();

range.range = getRangeFirstSet(30000);

range.header.frame_id = ss.str();

sonar_pubs[i].publish(range);

}

ros::spinOnce();

rate.sleep();
}

return 0;

}

User avatar
mikronauts
Posts: 2725
Joined: Sat Jan 05, 2013 7:28 pm
Contact: Website

Re: Code for multiple ultrasonic sensors ROS related

Tue Aug 27, 2019 2:44 pm

You can only trigger one at a time if you expect usable result as all the triggered sensors can pick up the echo from the closes object.

Syrine wrote:
Tue Aug 27, 2019 7:54 am
Hi,

I have 7 ultrasound sensors (hc-sr04) on my robot wired on the raspberry pi. I wanted to write a node that would first trigger only 4 of them and publish each on its own topic. Only the code down here gives me negative travel time and so on negative distance. Can you tell me what mistakes I've made? I know this isn't a ROS problem but I appreciate any help! Thank you

#include "vector"

#include "stdio.h"

#include "stdlib.h"

#include "ros/ros.h"

#include "sensor_msgs/Range.h"

#include "wiringPi.h"

using namespace std;

const int size=7;

int trigger_[size]={18,12,13,19,23,20,27};

int echo_[size]={17,06,16,26,05,21,22};

long start_time=0, end_time=0, time_diff=0, now;

void setup_IO(int trigger_[size], int echo_[size]){

for (int i = 0; i < size; ++i) {

pinMode(trigger_, OUTPUT);

pinMode(echo_, INPUT);

digitalWrite(trigger_, LOW);

delay(500);
}

}

void getPulseLength() {

for (int i = 0; i < size; ++i) {

if (i % 2 == 0) {
start_time = micros();

while (digitalRead(echo_) == HIGH)

end_time = micros();

}

}

}

double getRangeFirstSet(int timeout) {

double distance[size];

for (int i = 0; i < size; ++i) {

delay(10);

if (i % 2 == 0) {
digitalWrite(trigger_, HIGH);

delayMicroseconds(10);

digitalWrite(trigger_, LOW);

now=micros();

while (digitalRead(echo_) == LOW && micros()-now<timeout) {<="" p="">

getPulseLength();

time_diff = end_time - start_time;

distance = (time_diff/1000000)*343/2; }

if (distance < 0.02 || distance > 2) {

ROS_INFO("time_diff %ld; Out of Range %f m", time_diff, distance[i]);}

} else {

digitalWrite(trigger_[i], LOW);

digitalWrite(echo_[i], LOW);

}

return distance[i];

}

}

int main(int argc, char **argv) {

// Start ROS node.

ROS_INFO("Starting node");

ros::init(argc, argv, "hc_sr04s");

ros::NodeHandle n;

ros::Rate rate(10);

wiringPiSetupSys(); // uses gpio pin numbering

if (wiringPiSetup()<0){

cout<<"failed\n";

}

// Build a publisher for each sonar.

vector<ros::publisher> sonar_pubs;

for (int i = 0; i < size; ++i) {

stringstream ss;

ss << "range_" << i+1;

sonar_pubs.push_back(n.advertise<sensor_msgs::Range>(ss.str(), 10));
}

// Build base range message that will be used for

// each time a msg is published.

sensor_msgs::Range range;

range.radiation_type = sensor_msgs::Range::ULTRASOUND;

range.min_range = 0.02;

range.max_range = 2;

while(ros::ok()) {

for (int i = 0; i < size; ++i) {

stringstream ss;

ss << "sonar_" << i+1;

range.header.stamp = ros::Time::now();

range.range = getRangeFirstSet(30000);

range.header.frame_id = ss.str();

sonar_pubs[i].publish(range);

}

ros::spinOnce();

rate.sleep();
}

return 0;

}
http://Mikronauts.com - home of EZasPi, RoboPi, Pi Rtc Dio and Pi Jumper @Mikronauts on Twitter
Advanced Robotics, I/O expansion and prototyping boards for the Raspberry Pi

PhatFil
Posts: 1373
Joined: Thu Apr 13, 2017 3:55 pm
Location: Oxford UK

Re: Code for multiple ultrasonic sensors ROS related

Tue Aug 27, 2019 2:55 pm

Syrine wrote:
Tue Aug 27, 2019 7:54 am
Hi,

I have 7 ultrasound sensors (hc-sr04) on my robot wired on the raspberry pi. I wanted to write a node that would first trigger only 4 of them and publish each on its own topic. Only the code down here gives me negative travel time and so on negative distance. Can you tell me what mistakes I've made? I know this isn't a ROS problem but I appreciate any help! Thank you
hi and welcome..
Dont know what or who Ros is but if your having problems with sensors/code on a pi this is a good place to start, but try putting code in a readable format for those who could help to read it.. Use code tags..

Code: Select all

#include "vector"

#include "stdio.h"

#include "stdlib.h"

#include "ros/ros.h"

#include "sensor_msgs/Range.h"

#include "wiringPi.h"

using namespace std;

const int size=7;

int trigger_[size]={18,12,13,19,23,20,27};

int echo_[size]={17,06,16,26,05,21,22};

long start_time=0, end_time=0, time_diff=0, now;

void setup_IO(int trigger_[size], int echo_[size]){

for (int i = 0; i < size; ++i) {

pinMode(trigger_[i], OUTPUT);

pinMode(echo_[i], INPUT);

digitalWrite(trigger_[i], LOW);

delay(500);
}

}

void getPulseLength() {

for (int i = 0; i < size; ++i) {

if (i % 2 == 0) {
start_time = micros();

while (digitalRead(echo_[i]) == HIGH)

end_time = micros();

}

}

}

double getRangeFirstSet(int timeout) {

double distance[size];

for (int i = 0; i < size; ++i) {

delay(10);

if (i % 2 == 0) {
digitalWrite(trigger_[i], HIGH);

delayMicroseconds(10);

digitalWrite(trigger_[i], LOW);

now=micros();

while (digitalRead(echo_[i]) == LOW && micros()-now<timeout) {<="" p="">

getPulseLength();

time_diff = end_time - start_time;

distance[i] = (time_diff/1000000)*343/2; }

if (distance[i] < 0.02 || distance[i] > 2) {

ROS_INFO("time_diff %ld; Out of Range %f m", time_diff, distance[i]);}

} else {

digitalWrite(trigger_[i], LOW);

digitalWrite(echo_[i], LOW);

}

return distance[i];

}

}

int main(int argc, char **argv) {

// Start ROS node.

ROS_INFO("Starting node");

ros::init(argc, argv, "hc_sr04s");

ros::NodeHandle n;

ros::Rate rate(10);

wiringPiSetupSys(); // uses gpio pin numbering

if (wiringPiSetup()<0){

cout<<"failed\n";

}

// Build a publisher for each sonar.

vector<ros::publisher> sonar_pubs;

for (int i = 0; i < size; ++i) {

stringstream ss;

ss << "range_" << i+1;

sonar_pubs.push_back(n.advertise<sensor_msgs::Range>(ss.str(), 10));
}

// Build base range message that will be used for

// each time a msg is published.

sensor_msgs::Range range;

range.radiation_type = sensor_msgs::Range::ULTRASOUND;

range.min_range = 0.02;

range.max_range = 2;

while(ros::ok()) {

for (int i = 0; i < size; ++i) {

 stringstream ss;

ss << "sonar_" << i+1;

  range.header.stamp = ros::Time::now();

  range.range = getRangeFirstSet(30000);

  range.header.frame_id = ss.str();

  sonar_pubs[i].publish(range);

}

ros::spinOnce();

rate.sleep();
}

return 0;

}

Return to “Beginners”