Sonar Publisher with Arduino
Hello,
I am working with a Raspberry Pi and an Arduino on ROS indigo. I am using five ultrasonic sonar (Grove US Ranger v2.0) in order to publish their ranges over LaserScan sensor_msgs type.
Here is my code :
#include <Wire.h>
#include <Timer.h>
#include <ros.h>
#include <std_msgs/Float32.h>
#include <sensor_msgs/LaserScan.h>
#include <Ultrasonic.h>
Ultrasonic ultrasonic1(3);
Ultrasonic ultrasonic2(4);
Ultrasonic ultrasonic3(5);
Ultrasonic ultrasonic4(6);
Ultrasonic ultrasonic5(7);
Timer _timer_100(10); //100Hz
//
unsigned long time;
unsigned long time_previous_scan;
unsigned long time_current_scan;
float val_ranges[5];
ros::NodeHandle nh;
//Def
sensor_msgs::LaserScan scan;
//Pub/sub
ros::Publisher pu("sonar", &scan);
void setup() {
Serial.begin(115200);
nh.getHardware()->setBaud(115200);
nh.initNode();
nh.advertise(pu);
_timer_100.start((unsigned long) millis());
time_previous_scan = millis();
}
void sonarCb(){
// get sonar measures
val_ranges[0] = ultrasonic1.MeasureInCentimeters();
val_ranges[1] = ultrasonic2.MeasureInCentimeters();
val_ranges[2] = ultrasonic3.MeasureInCentimeters();
val_ranges[3] = ultrasonic4.MeasureInCentimeters();
val_ranges[4] = ultrasonic5.MeasureInCentimeters();
//populate the sonar message
scan.header.frame_id = "/ultrasound";
scan.angle_increment = 0.26; // rad
scan.angle_min = -0.52; //
scan.angle_max = 0.52;
time_current_scan = millis();
scan.scan_time = time_current_scan - time_previous_scan;
time_previous_scan = millis();
scan.ranges = val_ranges;
// publish
pu.publish(&scan );
}
void loop() {
if (_timer_100.delay(millis())){ //100Hz or 10ms
nh.spinOnce();
sonarCb();
}
}
When I run rostopic echo /sonar
, the range array is empty, although I am sure to get the ultrasonic values (tested with the Arduino serial monitor) :
header:
seq: 19546
stamp:
secs: 0
nsecs: 0
frame_id: /ultrasound
angle_min: -0.519999980927
angle_max: 0.519999980927
angle_increment: 0.259999990463
time_increment: 0.0
scan_time: 92.0
range_min: 0.0
range_max: 0.0
ranges: []
intensities: []
---
Does anybody know how to populate range_msg.ranges array ?
Regards
Matthieu