概述

由于实验室项目需要用到一款超声波传感器，但是厂家并没有提供驱动程序，只能自己编写。完整代码见仓库：https://github.com/ZhouZijie77/ultrasonic_ros_driver

产品型号

选用的超声波传感器型号为F40-16TR 7 B，产品接口：CAN2.0，拥有12个 channel。
由于调试时电脑没有CAN接口，因此选择了乐电新南的一款CANUSB（链接）进行数据转换。将传感器输出的CAN信号通过CANUSB转成虚拟串口信号，这样就可以直接通过串口来通信。

代码

首先向传感器发送停止数据发送的命令sp.writeBuffer(STOPCMD, 16)，之后发送索要数据的命令sp.writeBuffer(STARTCMD, 16)（STOPCMD和STARTCMD定义在ultrasonic.h当中）。之后就开始不断读取串口数据serialRead()。
根据传感器的通信协议，一个数据帧是16个字节，第一次采用直接读16个字节的方法。但是测试时发现程序开启的时候可能是从某一帧中间开始读的，这就导致读取数据错位。因此采用另一种方法（见serialRead()）：

1. 首先一个字节一个字节地读取，读到SOF(Start of Frame)认为是第一帧
2. 第一帧继续读取15个字节
3. 一直循环读取16个字节

这样就可以保证每次读取到的数据都是完整的数据帧。
由于这里12个通道全部都使用到了，而12个通道的测量数据需要3个数据帧也就是48个字节，因此将每一次读到的结果都先存在data中，每读取到3个完整的数据帧就可以生成一个12通道的测量消息。
主要代码如下：

#include <ros/ros.h>
#include <iostream>
#include <thread>
#include <serialPort.h>
#include <ultrasonic_ros_driver/ultrarange.h>
#include <std_msgs/Header.h>
#include <ros/time.h>
#include <signal.h>
#include "ultrasonic.h"

using namespace SerialPort;
serialPort sp;
ros::Publisher ultra_pub;
int count = 0;

bool checkData(const uint8_t *data)  //检查是否符合通信协议
{
    if (data[0] != 0xaa || data[3]!=0x08||data[6] != 0x06)
        return false;
    return true;
}

int bcd2demical(uint8_t bcd)
{
    // 16进制BCD到十进制
    int sum = 0, c = 1;
    for (int i = 1; bcd > 0; i++)
    {
        sum += (bcd % 16) * c;
        c *= 10;
        bcd /= 16;
    }
    return sum;
}

bool genMsg(ultrasonic_ros_driver::ultrarange &range, const uint8_t *data) //根据接收到的数据生成ros消息
{
    if (!checkData(data))
        return false;
    // printData(data);
    std_msgs::Header header;
    header.frame_id = "/ultrasonic";
    header.stamp = ros::Time::now();
    header.seq = 1;
    range.header = header;
    if (data[7] == 0x11)
    {
        range.channel_1 = bcd2demical(data[8]) * 100 + bcd2demical(data[9]);
        range.channel_2 = bcd2demical(data[10]) * 100 + bcd2demical(data[11]);
        range.channel_3 = bcd2demical(data[12]) * 100 + bcd2demical(data[13]);
        range.channel_4 = bcd2demical(data[14]) * 100 + bcd2demical(data[15]);
    }
    else if (data[7] == 0x12)
    {
        range.channel_5 = bcd2demical(data[8]) * 100 + bcd2demical(data[9]);
        range.channel_6 = bcd2demical(data[10]) * 100 + bcd2demical(data[11]);
        range.channel_7 = bcd2demical(data[12]) * 100 + bcd2demical(data[13]);
        range.channel_8 = bcd2demical(data[14]) * 100 + bcd2demical(data[15]);
    }
    else
    {
        range.channel_9 = bcd2demical(data[8]) * 100 + bcd2demical(data[9]);
        range.channel_10 = bcd2demical(data[10]) * 100 + bcd2demical(data[11]);
        range.channel_11 = bcd2demical(data[12]) * 100 + bcd2demical(data[13]);
        range.channel_12 = bcd2demical(data[14]) * 100 + bcd2demical(data[15]);
    }
    return true;
}



void serialRead()
{
    int nread;
    uint8_t data[96];
    ultrasonic_ros_driver::ultrarange range;
    bool find_sof = true;
    bool first_frame = true;
    int count = 0;

    while (ros::ok())
    {
        if (find_sof)
        {
            if ((nread = sp.readBuffer(data, 1) == 1))
            {
                if (data[0] == 0xaa)
                {
                    find_sof = false;
                }
            }
        }
        else
        {
            if (first_frame)
            { // 第一帧只读15个字节    
                uint8_t temp_data[15];
                if ((nread = sp.readBuffer(temp_data, 15)) == 15)
                {

                    data[0] = 0xaa;
                    for (int i = 0; i < 15; i++)
                    {
                        data[i + 1] = temp_data[i];
                    }
                    if (genMsg(range, data))
                    {           
                        count++;
                        first_frame = false;
                    }
                }
            }
            else
            { // 之后读16字节
                if ((nread = sp.readBuffer(data, 16)) == 16)
                {
                    if (genMsg(range, data))
                    {
                        count++;
                        if (count == 3)
                        {
                            ultra_pub.publish(range);
                            std::cout << "publish" << std::endl;
                            count = 0;
                        }
                    }
                }
            }
        }
    }
}

void mySigintHandler(int sig)
{
    sp.writeBuffer(STOPCMD, 16);
    std::cout << std::endl;
    std::cout << "ultrasonic stopped" << std::endl;
    sp.ClosePort();
    std::cout << "stop ultrasonic node" << std::endl;
    ros::shutdown();
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ultrasonic_pub_node");
    ros::NodeHandle nh("");
    signal(SIGINT, mySigintHandler);
    ultra_pub = nh.advertise<ultrasonic_ros_driver::ultrarange>("/ultra_range", 10);
    sp.OpenPort("/dev/ttyUSB0");
    sp.setup(9600, 0, 8, 1, 'N');
    sp.writeBuffer(STOPCMD, 16);
    sp.writeBuffer(STARTCMD, 16);
    serialRead();
    return 0;
}