Ask Your Question
0

Sonar Publisher with Arduino

asked 2017-04-25 05:15:36 -0500

mattMGN gravatar image

updated 2017-04-25 05:19:28 -0500

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

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2017-04-25 23:52:38 -0500

billy gravatar image

updated 2017-04-25 23:59:20 -0500

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/kobuk...

edit flag offensive delete link more
0

answered 2017-04-26 04:36:04 -0500

mattMGN gravatar image

updated 2017-04-26 05:06:04 -0500

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

edit flag offensive delete link more

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.

billy gravatar image billy  ( 2017-04-26 11:23:46 -0500 )edit

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.

billy gravatar image billy  ( 2017-04-26 11:29:11 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-04-25 05:15:36 -0500

Seen: 294 times

Last updated: Apr 26 '17