ROS Arduino Serial

Please refer to this tutorial of the ultrasound sensor HC-SR04.

Ultrasound sensors measures distance by sending sound waves against objects in front of it and receiving the echos (so it doesn’t work well against objects that absorb sound, e.g. furs).

This code runs in the Arduino, and communicates the message serially through the USB cable to the roscore on the laptop.

#define trigPin 12
#define echoPin 13

// 12 and 13 refers to the pin numbers on Arduino
// You can choose to use any pins on the Arduino for digital 
// signal read and write (which sensor uses) 

#include <ros.h>
#include <std_msgs/Int16.h>

ros::NodeHandle nh;

std_msgs::Int16 int_msg;
ros::Publisher arduino("distance", &int_msg);

// Standard ROS node intialization

void setup() { // Setup code runs only once
  //Serial.begin (9600);  
  // You can choose to see output on a serial monitor
  // on the computer 
  // But from what I found, Arduino can't seem to maintain
  // both serial communication to ROS and serial monitor
  pinMode(trigPin, OUTPUT); // marking pins to be read and write 
  pinMode(echoPin, INPUT); 
  nh.initNode();
  nh.advertise(arduino); 
}

void loop() { // This part of the code runs repeatedly
  long duration;
  int distance;

  digitalWrite(trigPin, LOW); // This is just setup code 
  delayMicroseconds(2);       // So that ultrasound sensor would
  digitalWrite(trigPin, HIGH);// start reading
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW); 

  duration = pulseIn(echoPin, HIGH); 
  // The data coming in from ultrasound sensor is just 
  // digital data (0, 1), but it signifies distance data 
  // (0 ~ 200) by sending "pulses" of certain length 

  distance = (duration/2) / 29.1;
  int_msg.data = distance;

  if (distance >= 2000 ) {
    //Serial.println("Out of range");
  }
  else {
    //Serial.print(distance);
    //Serial.println(" cm");
    arduino.publish(&int_msg);
    nh.spinOnce();
    // Standard publish routine for ROS
  }
  delay(500); // Wait 500 ms before running again
}
~