制作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 
//#include 

#include 

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