Node for multiple ultrasonic sensors on raspberry [closed]

edit

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

}