制作ros自主导航用的简易超声雷达

学习ros自主导航没有雷达怎么办？

如果你手里已经有了一个超声模块hcsr04、一个舵机 和一个arduino，再加上一点动手能力，那么就万事大吉了。

以下代码实现简易的超声雷达，ros节点可以通过串口与单片机通讯。ros节点把数据转换成标准消息后再发送到所需的主题。

性能参数

分辨率 64点

扫描频率约 0.5-2hz。

距离识别 1-400cm

扫描范围0-180度，看舵机范围。

//简易超声雷达
//串口协议 两个字节为一个点数据
//0x04 0x99 第一字节低四位表示m 第二字节表示cm =499cm
//其中第一字节高四位的第一、二位1表示一串点数据包的开始，第四位1表示正向1101否则为反向1100 不是开始的点第一字高四位节皆为0
//0xdxxx正向开头1101xxxx xxxxxxxx
//0xcxxx反向开头1100xxxx xxxxxxxx
#include <ST_HW_HC_SR04.h>
//#include <Wire.h>

#include <Servo.h>

Servo myservo;
// Change the pins if you wish.
ST_HW_HC_SR04 ultrasonicSensor(13, 12); // ST_HW_HC_SR04(TRIG, ECHO)
unsigned int ddd=0;
char dis=16;//分辨率1024/16=64，每个运动周期往返采样两次，期望每秒采128个点，1000/128*34.4=268.76cm（测量范围<=134cm）

void setup() {
Serial.begin(115200);

while(!Serial); // Wait for the Serial connection;
ultrasonicSensor.setTimeout(23200);//23.2*34.4=798cm(8m)最大测量范围4m

//pmw引脚
myservo.attach(9);  // attaches the servo on pin 9 to the servo object
}
void sendData(byte head,float d){
byte dat_bai=(byte) (d/100.0);  //百位 m
byte dat2=(byte) (int(d)%100);//cm
byte dat_ge=dat2%10;//个位
byte dat_shi=(dat2-dat_ge)/10;//十位
byte b1=head|dat_bai;//第一字节
byte b2=(dat_shi<<4)|dat_ge;//第二字节
Serial.write(b1);
Serial.write(b2);
}
void loop() {

//pmw引脚 scale it to use it with the servo (value between 0 and 180)
myservo.write(map(ddd, 0, 1024, 0, 180));

int hitTime = ultrasonicSensor.getHitTime(); // In microseconds
float d=(float)hitTime/1000.0*34.4;// 距离 多少cm;

//23.2ms声波传播 23.2*34.4=8米，测量4米
int costTime= hitTime*2/1000;//ms
if(costTime<7){
delay((7-costTime)); // 128hz 期望测量周期为7.8125ms （测量范围<=134cm）
}else{
//超出期望周期说明测两距离过长。
//测量周期变长，测量频率减慢 最慢1000/23.2=43hz
}
sendData(0x00,d);//输出数据

ddd+=dis;
if(ddd>=1024||ddd<=0){
dis*=-1;
//输出包头
if(dis>0){
//正向
sendData(0xD0,d);
}else{
//反向
sendData(0xC0,d);
}
}
}