Robotics StackExchange | Archived questions

Node for multiple ultrasonic sensors on raspberry

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 thus 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 starttime=0, endtime=0, time_diff=0, now;

void setupIO(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) {

getPulseLength();

timediff = endtime - start_time;

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

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

ROSINFO("timediff %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.

vectorros::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.radiationtype = sensormsgs::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;

}

Asked by Syrine on 2019-08-26 17:33:30 UTC

Comments

May I suggest using an Arduino that controls all the US sensors and send the results through rosserial? The RPi is not that great at timings

This library has worked fine with 6 sensors and can be a useful resource for your code: https://playground.arduino.cc/Code/NewPing/

Asked by Ktysai on 2019-08-26 19:55:01 UTC

Unfortunately we can't change now. Thank you for the link, I'll look into it

Asked by Syrine on 2019-08-27 02:17:52 UTC

I know this isn't a ROS problem

it would seem that way -- at least for now.

As we have over 48000 questions already, we must try to stay on-topic as much as possible. I would suggest you post your question on a more suitable forum.

Asked by gvdhoorn on 2019-08-27 02:48:42 UTC

Answers