ROS中使用arduino连接ultrasonic sensor

ultrasonic sensor与arduino接线方式：

1 、使用 Arduino 采用数字引脚给 SR04 的 Trig 引脚至少 10 μ s 的高电平信号，触发 SR04 模块测距功能；

2 、触发后，模块会自动发送 8 个 40KHz 的超声波脉冲，并自动检测是否有信号返回。这步会由模块内部自动完成。

3 、如有信号返回， Echo 引脚会输出高电平，高电平持续的时间就是超声波从发射到返回的时间。此时，我们能使用 pulseIn() 函数获取到测距的结果，并计算出距被测物的实际距离。

代码如下：

#include<ros.h>
#include<sensor_msgs/Range.h>
ros::NodeHandle nh;
sensor_msgs::Range range_msg;
ros::Publisher pub_range("range_data",&range_msg);

const int TrigPin = 2;
const int EchoPin = 3;

char frameid[] = "/ult_ranger";

float getRange(int pin_num)
{
  int sample;
  sample = pulseIn(pin_num,HIGH)/5882.00;
  return sample;
}   

void setup() {
  nh.initNode();
  nh.advertise(pub_range);
  
  pinMode(TrigPin,OUTPUT);
  pinMode(EchoPin,INPUT);
  
  range_msg.radiation_type = sensor_msgs::range::ULTRASOUND;
  range_msg.header.frame_id ="utl_ranger";
  range_msg.field_of_view = 0.1;
  range_msg.min_range = 0.002;
  range_msg.max_range =4;

}

void loop() {
digitalWrite(TrigPin,LOW);
delayMicroseconds(2);
digitalWrite(TrigPin,HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin,LOW);

range_msg.range = getRange(EchoPin);
range_msg.header.stamp = nh.now();
pub_range.publish(&range_msg);
nh.spinOnce();
delay(1000);
}

编辑完以上代码后，下载到自己用的arduino中，在terminal中输入以下命令，即可看到超声波测距的结果：

roscore

rosrun rosserial_python serial_node.py /dev/ttyACM0

rostopic echo /range_data