超声波传感器ROS驱动编写 |
您所在的位置：网站首页 › 个人用w10哪个版本好 › 超声波传感器ROS驱动编写 |
超声波传感器ROS驱动编写
|
概述
由于实验室项目需要用到一款超声波传感器，但是厂家并没有提供驱动程序，只能自己编写。完整代码见仓库：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()）： 首先一个字节一个字节地读取，读到SOF(Start of Frame)认为是第一帧第一帧继续读取15个字节一直循环读取16个字节这样就可以保证每次读取到的数据都是完整的数据帧。 由于这里12个通道全部都使用到了，而12个通道的测量数据需要3个数据帧也就是48个字节，因此将每一次读到的结果都先存在data中，每读取到3个完整的数据帧就可以生成一个12通道的测量消息。 主要代码如下： #include #include #include #include #include #include #include #include #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 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 ros::init(argc, argv, "ultrasonic_pub_node"); ros::NodeHandle nh(""); signal(SIGINT, mySigintHandler); ultra_pub = nh.advertise("/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; } |
【本文地址】
今日新闻 |
推荐新闻 |