Question

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

Asked by mattMGN on 2017-04-25 05:15:36 UTC

Answer 1

The scan messages includes range and intensity fields, along with a correlation matrix. With header and setup like

scan->angle_min = 0.0;
scan->angle_max = 2.0*M_PI;
scan->angle_increment = (2.0*M_PI/180.0);
scan->range_min = 0.06;
scan->range_max = 5.0;
scan->ranges.resize(180);
scan->intensities.resize(180);   
scan->header.frame_id = frame_id;
scan->header.stamp = ros::Time::now();

then populate the arrays (I do it one measurement at a time but pointer your should work)

scan->ranges[index + indexOffset] = floatrange;
scan->intensities[index + indexOffset] = intensity;

and publish as

laser_pub.publish(scan);

I don't see where you size the arrays in your message. That may be the issue, or maybe you need to assign intensity values. You also need to set min and max range values, outside of which the values are largely ignored.

I have a set of bumper switches I use to update cost map when it hits something the laser scanner can't see. It uses the pointcloud message which may be a better fit for your application. I borrowed heavily from here to get it working: http://docs.ros.org/kinetic/api/kobuki_bumper2pc/html/kobuki__bumper2pc_8cpp_source.html

Asked by billy on 2017-04-25 23:52:38 UTC

Comments

Answer 2

Thank you for this answer. I have modified my script regarding your advice, I still have troubles on some points :

I can't resize ranges and intensities arrays, both scan.ranges.resize(5) and scan->ranges.resize(5) are no accepted when compiling (not a type from sensor_msgs::LaserScan, Arduino and ROS classes are not exactly the same i guess)
After integrating the loop for populate ranges and intensities arrays, I now lost synchronisation between ROS node and Arduino :

[ERROR] [WallTime: 1493197519.087966] Lost sync with device, restarting...

I don't know how to deal with this problem. From this rosanswer, the problem seems to be that i don't call spinOnce() frequently enough. But i don't understand why the populating loop slow down the script at this point.

Here is my new Arduino script :

  //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;
  scan.range_min = 0.0;
  scan.range_max = 450.0;
  //scan.ranges.resize(5);
  //scan->ranges.resize(5);
  time_current_scan =  millis();
  scan.scan_time = time_current_scan - time_previous_scan;
  time_previous_scan = millis();
  for (i=0;i<=4;i++){
    scan.ranges[i] = val_ranges[i];
    scan.intensities[i] = val_ranges[i];
  }

  // publish
  pu.publish(&scan );

Suggestions are welcomed :)

Regards

Matthieu

Asked by mattMGN on 2017-04-26 04:36:04 UTC

Comments

If the resize call doesn't work in Arduino, there must be a different way to set the message size. Investigate.

I'm not familiar at all with Arduino but if it allows to you to write to the log, then you should attempt to write the values to the log to verify they were copied in.

Asked by billy on 2017-04-26 11:23:46 UTC

Help me understand you spinonce() and then sonarCB(). Which subscriber is being spun? I don't know what defined behaviors is if you call spin with no subscribers. Try without the spinning as it doesn't seem to have function here (unless something strange with Arduino). Or you are missing subscriber.

Asked by billy on 2017-04-26 11:29:11 UTC

Sonar Publisher with Arduino

Comments

Answers

Comments

Comments