制作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);      }    }}