ros_arduino_bridge中使用sr04超声波传感器

ros_arduino_bridge提供了单只引脚的超声波测距传感器，但是我买的sr04传感器是两个引脚的（除电源和地外），在网上有把sr04改成单引脚的。

修改教程如下：www.cascologix.com/2015/09/19/hc-sr04-ping-sensor-hardware-mod/。

我本人比较懒不想改硬件，于是想方设法改软件咯。

我把ros_arduino_python如何传感器使用的流程大概缕了一下，比较简陋：

arduino_node.py——>调arduino_sensor.py——>调arduino_driver.py,arduino_driver.py是通过串口给arduino板子发送指令的。

一、首先修改arduino板子的源代码，分析sensor.h源码

/* Functions for various sensor types */

int echopin=53;
float microsecondsToCm(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per cm.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

long Ping(int pin) {
  long duration, range;

  // 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, OUTPUT);
  pinMode(echopin, INPUT);
  digitalWrite(pin, LOW);
  delayMicroseconds(2);
  digitalWrite(pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pin, 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.
 
  duration = pulseIn(echopin, HIGH);

  // convert the time into meters
  range = microsecondsToCm(duration);
 
  return(range);
}

很明显这个段代码只支持收发都是一个引脚的超声波传感器。下面一段代码就是改成两个引脚一个发送指令另一个接受指令的传感器sr04的代码：

/* Functions for various sensor types */
//int echopin=53;
float microsecondsToCm(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per cm.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

long Ping(int pin) {
  long duration, range;

  // 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, OUTPUT);
  pinMode(echopin, INPUT);
  digitalWrite(pin, LOW);
  delayMicroseconds(2);
  digitalWrite(pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pin, 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.
 
  duration = pulseIn(echopin, HIGH);

  // convert the time into meters
  range = microsecondsToCm(duration);
 
  return(range);
}
long Ping2(int pin,int echopin2) {
  long duration, range;

  // 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, OUTPUT);
  pinMode(echopin2, INPUT);
  digitalWrite(pin, LOW);
  delayMicroseconds(2);
  digitalWrite(pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pin, LOW);
 
  duration = pulseIn(echopin2, HIGH);
  range = microsecondsToCm(duration);
  return(range);
}

然后在commands.h文件中增加一个指令#define PING2          'q'，这是串口接收到q时会去调Ping2函数。

并在主程序里加  

case PING2:

      Serial.println(Ping2(arg1,arg2));

      break;

这段是把传感器的测量值通过串口传给上位机。这样底盘aduino的程序就改完了，可以使用arduino IDE的串口监控程序发指令：q 51 53（51触发信号口，53是输出口）如果得到测距数据，那就OK了。

二、修改上位机（树莓派）的程序：

1、修改arduino_drive.py增加发到arduino板的q(pin,pin2)指令。

2.在arduino_sensors.py中修改代码如下：

就是从基类开始修改，一直改到实现。应该还有别的改法但是对python确实不熟悉，所以就使用这样的笨法来修改了。

3.修改arduino_node.py

4.修改arduino_params.yaml

接下来就可以launch了，通过rostopic echo /arduino/sensor/sonar_front_right就可以输出range数据了。