Arduino Double Sonar With ROS

I have been playing with ROS the last few days. After my first attempts with NXT (will post about it later) I ended up using my good ol’ Arduino for this task as well.

You probably remember my post about Sonar Sensor SEN136b5. When I ordered it I ordered 3 more (2 for Árpád :)) so I decided to try reading the values of two sensors and publishing them into a ROS system.

Test hardware

I came through more Arduino ROS components when browsing the ROS wiki so if you are not satisfied with my choice you can find (or write) one yourself.

I used the rosserial stack, more concretely the rosserial_arduino package (Tutorials).

After setting up the Arduino IDE and compiling the rosserial stack I moved to the examples to further analyze them. There is a tutorial for a sonar sensor the original team used. I used that code to write my own version of it. Since the sensor I used is PING))) compatible, the same read function can be applied so I pulled in one of my old functions I used created in a university project.

I use one publisher for each of the sensors (so I have two) but only one message object for both of them. It’s not a big hack, just a little resource saving.

rxplot sample

Here’s a picture created with rxgraph about the running system:

rxgraph sample

Here’s a video of the working system where I visualize the sonar values using rxplot:

Thanks for the developers of rosserial_arduino

The code I wrote for the Arduino:

/* 
 * rosserial Ultrasound Example
 * using seeestudio ultrasound sensor
 * (compatible with PING))))
 */

#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Range.h>

ros::NodeHandle  nh;

sensor_msgs::Range range_msg;
ros::Publisher pub_range1("/ultrasound1", &range_msg);
ros::Publisher pub_range2("/ultrasound2", &range_msg);
// "/arduino_ultrasound"

char frameid[] = "/ultrasound";

long duration;
float getRange_Ultrasound(int pin_num)
{
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pin_num, OUTPUT);
  digitalWrite(pin_num, LOW);
  delayMicroseconds(2);
  digitalWrite(pin_num, HIGH);
  delayMicroseconds(10);
  digitalWrite(pin_num, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pin_num, INPUT);
  duration = pulseIn(pin_num, HIGH);

  // convert the time into a distance
  return duration/58; //    duration/29/2, return centimeters
}

void setup()
{
  nh.initNode();
  nh.advertise(pub_range1);
  nh.advertise(pub_range2);

  range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
  range_msg.header.frame_id =  frameid;
  range_msg.field_of_view = 0.1;  // fake
  range_msg.min_range = 0.0;
  range_msg.max_range = 2.0;
}

long range_time;

void loop()
{
  //publish the adc value every 50 milliseconds
  //since it takes that long for the sensor to stabilize
  if ( millis() >= range_time ){
    range_msg.range = getRange_Ultrasound(5);
    range_msg.header.stamp = nh.now();
    pub_range1.publish(&range_msg);

    range_msg.range = getRange_Ultrasound(6);
    range_msg.header.stamp = nh.now();
    pub_range2.publish(&range_msg);

    range_time =  millis() + 50;
  }

  nh.spinOnce();
}
Advertisements

7 thoughts on “Arduino Double Sonar With ROS

    1. Hey,
      You didn’t look close enough 😉
      `getRange_Ultrasound(5);` — this refers to PIN 5, the other one is PIN 6
      Any digital pin will do by the way.

  1. LOL your right, I missed it. But I did figure it out.

    So how did you get the plots and graphs to run, I am new to ROS so still learning.

    Thanks for the reply.

    1. Once you can get to the part where `rostopic echo /ultrasound1` works, you can do `rqt_plot /ultrasound1 /ultrasound2` to get a cute plots of both topics. This post uses rxplot which was the old plotting tool (note that this is a project from 2011!!)

Leave a Reply

Please log in using one of these methods to post your comment:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s