ROS：驱动编写与对应节点消息发布

前言

  在使用ROS的过程中，不可避免的会遇到要自己手动编写驱动、节点、行为库、服务、消息等内容，尤其是需要一些传感器，而所需传感器没有相关ROS驱动时，下面将讲述的是在使用的传感器中，没有ROS驱动时，我们将如何使用该传感器。

 

  在接下来的博客中将根据本身经验来编写一些ROS相关内容，权当是作为记忆来分享。

ROS驱动——点激光扫描仪

  下面将讲述使用ROS官方未有的传感器来作为样例，我使用的是莱旭光电的CHT-10点激光传感器作为 介绍，首先这款传感器通信协议部分比较简单，基本上打开串口就可以读出数据，在将数据进行转换就可以得到想要的距离，比较方便，免去复杂的部分，对教程也有帮助，毕竟教程主要讲述的是ROS驱动部分。

  相关ROS驱动代码可以在github上找到进行下载：https://github.com/Playfish/cht10_node

  CHT-10是点激光，扫描范围是0.05-10m，真实的应该是0.15-10m，10m未测试，不过0.15盲区还是有的。

  下图是CHT-10的通信协议部分介绍：

  可以看到想要的数据在9~12位（四字节），得到的数据需要除以1000才能得到真实的距离（米），ROS消息中基本单位是米。

消息选定

  关于选择什么消息作为该传感器的ROS消息，这个相对来说不是很重要，不过作为通用项来说，尽量向通用靠近（即使用ROS官方消息定义，而不是自己定义消息）。目前激光消息的话，有两个选择，第一个是sensor_msgs/LaserScan消息类型，不过该消息类型适合有角度的传感器，即180°或360°的2D雷达或激光，而目前使用的激光是单点类型，和激光笔差不多，只有一个点，那么可以选择sensor_msgs/Range消息类型，该消息类型适用于超声波传感器、红外传感器单点类型，CHT-10就用这个类型。

关于该消息：可以查看sensor_msgs/Range

准备工作

  传感器选型选定、通信消息选定，那么接下来还需要做：

• Frame_id：Frame在ROS中作用至关重要，消息将和TF绑定才可以读取数据，在这里作为通用可配置，暂定内容为：laser，用户可自定义设置（通过ROS Parameters设置）。
• 串口：避免和其他传感器串口冲突，因此在这里预留出一个串口设置参数，用户可以自定义设置（通过ROS Parameters设置）。
• 话题：消息内容需要通过话题发布，并且话题需要唯一，不然容易崩溃，在这里选择话题为“range”

  当然以上部分可以不考虑，这个是作为通用型需要考虑的，暂时可以先忘记以上部分，下面开启代码部分

测试代码编写

  在这部分，主要先编写测试代码，如果测试代码无问题，就可以写到node中，测试代码和ROS无关系，只是通过串口读取数据，并且在终端显示出来，整体思路如下：

  传感器选择 -> 测试程序测试（类似串口助手） -> ROS node绑定（将测试程序读取部分加入到ROS中） -> 将node加入到nodelet中封装

  下面是测试程序部分，也可以在cht10_node/test中找到，名为test_cht10.cpp：

[cpp] view plain copy

1. #include <string>  
2. #include <cht10_node/seiral_func.hpp>  
3. #include <cstdlib>  
4.   
5. #include <iostream>  
6. #include <stdint.h>  
7. #define BUFSIZE 17  
8.   
9. int main(int argc, char** argv){  
10.   cht10_seiral_func::Cht10Driver cht10driver_;  
11.   
12.   std::string serialNumber_;  
13.   serialNumber_ = "/dev/ttyUSB0";  
14.   int baudRate_ = 115200;  
15.   
16.   std::stringstream ostream;  
17.   int fd, len, rcv_cnt;  
18.   bool success_flag;  
19.   char buf[40], temp_buf[BUFSIZE],result_buf[BUFSIZE];  
20.   int laser_data=0;  
21.   char data_buf[4];  
22.   rcv_cnt = 0;  
23.   success_flag = false;  
24.   memset(buf, 0xba, sizeof(buf));  
25.   memset(temp_buf, 0xba, sizeof(temp_buf));  
26.   memset(result_buf, 0xba, sizeof(result_buf));  
27.   
28.   fd = open(serialNumber_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY );  
29.   if(fd < 0){  
30.     std::cout<<"Open Serial: "<<serialNumber_.c_str()<<" Error!";  
31.     exit(0);  
32.   }  
33.     
34.   cht10driver_.UART0_Init(fd,baudRate_,0,8,1,'N');  
35.   while(1){  
36.     len = cht10driver_.UART0_Recv(fd, buf,40);  
37.     if(len>0){  
38.       for(int i = 0; i < len; i++){  
39.         if(rcv_cnt<=BUFSIZE){    
40.           result_buf[rcv_cnt++] = buf[i];  
41.           if(rcv_cnt == BUFSIZE){  
42.             success_flag = true;  
43.           }  
44.         }//end if  
45.         else{  
46.           /**** 
47.           *  checkout received data 
48.           */  
49. //          std::cout<<"Received data, with :[";  
50. //          for(int j=0;j<BUFSIZE;j++){  
51. //            printf("%c ",(unsigned char) result_buf[j]);  
52. //          }  
53. //          printf("] \n");  
54.           success_flag = false;  
55.   
56.           for(int count = 0; count < 4; count++){  
57.             data_buf[count] = result_buf[9+count];  
58.           }  
59.           sscanf(data_buf, "%x", &laser_data);  
60.           std::cout<<"sensor data:"<<laser_data<<", Distance: "<<(double)laser_data/1000<<std::endl;  
61.           /**** 
62.            *  data writing end 
63.            */  
64.           if('~' == buf[i]){  
65.             rcv_cnt = 0;  
66.             result_buf[rcv_cnt++] = buf[i];  
67.           }  
68.         }//end else  
69.       }//end for      
70.     }  
71.   }  
72.   
73.     
74. }  

  我将串口部分封装成一个类名为Cht10Driver，该类包含串口读/写部分、初始化。

  通过使用catkin_make可以得到一个名为test_cht10的可执行文件，使用./test_cht10可以运行，将CHT-10插入到电脑上，并且电脑只有一个ttyUSB0，就可以读取数据，数据将显示为sensor data: 传感器毫米数据， Distance: 传感器米数据。

ROS驱动编写

  上部分讲述了测试程序编写，主要作用是通过串口读取传感器数据，那么得到传感器数据之后，就可以将传感器数据填充到ROS消息中，然后通过话题形式发布出去，如下是将传感器数据填充到ROS消息并封装成nodelet类的主要部分：

  完整代码可以查看： cht10_node.cpp

[cpp] view plain copy

1. /** 
2.  * @file /Cht10_serial_func/src/Cht10_serial_func.cpp 
3.  * 
4.  * @brief Implementation for dirver with read data from Cht10 nodelet 
5.  * 
6.  * @author Carl 
7.  * 
8.  **/  
9.   
10. /***************************************************************************** 
11.  ** Includes 
12.  *****************************************************************************/  
13. #include <string>  
14. #include <ros/ros.h>  
15. #include <std_msgs/String.h>  
16. #include <nodelet/nodelet.h>  
17. #include <ecl/threads/thread.hpp>  
18. #include <sensor_msgs/LaserScan.h>    
19. #include <sensor_msgs/Range.h>    
20. #include <pluginlib/class_list_macros.h>  
21. #include <cht10_node/seiral_func.hpp>  
22.   
23.   
24. namespace cht10_seiral_func{  
25.   
26. class Cht10Func : public nodelet::Nodelet{  
27. #define BUFSIZE 17  
28. #define SCALE 1000  
29.   
30. public:  
31.   Cht10Func() : shutdown_requested_(false),serialNumber_("/dev/USB0"),frame_id("laser"){}  
32.   
33.   ~Cht10Func(){  
34.     NODELET_DEBUG_STREAM("Waiting for update thread to finish.");  
35.     shutdown_requested_ = true;  
36.     update_thread_.join();  
37.   }  
38.   
39.   virtual void onInit(){  
40.   
41.     ros::NodeHandle nh = this->getPrivateNodeHandle();  
42.     std::string name = nh.getUnresolvedNamespace();  
43.     nh.getParam("serialNumber", serialNumber_);  
44.     nh.getParam("baudRate", baudRate_);  
45.     nh.getParam("frame_id", frame_id);  
46.     rcv_cnt = 0;  
47.     success_flag = 0;  
48.   
49.     fd = open(serialNumber_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY );  
50.     if(fd < 0){  
51.       ROS_ERROR_STREAM("Open Serial: "<<serialNumber_.c_str()<<" Error!");  
52.       exit(0);  
53.     }  
54.   
55.     int countSeq=0;  
56.     scan_pub = nh.advertise<sensor_msgs::Range>("range",100);  
57.     memset(buf, 0, sizeof(buf));  
58.     memset(temp_buf, 0, sizeof(temp_buf));  
59.     memset(result_buf, 0, sizeof(result_buf));  
60.     Cht10driver_.UART0_Init(fd,baudRate_,0,8,1,'N');  
61.     ROS_INFO_STREAM("Open serial: ["<< serialNumber_.c_str() <<" ] successful, with idex: "<<fd<<".");  
62.     NODELET_INFO_STREAM("Cht10Func initialised. Spinning up update thread ... [" << name << "]");  
63.     update_thread_.start(&Cht10Func::update, *this);  
64.   }  
65.   
66.   double data_to_meters(int &data, int scale){  
67.     return (double)data/scale;  
68.   }  
69.   
70.   void publish_scan(ros::Publisher *pub,  
71.                     double nodes, ros::Time start,  
72.                     std::string frame_id){  
73.   
74.     float final_range;  
75.     sensor_msgs::Range range_msg;  
76.     range_msg.field_of_view = 0.05235988;  
77.     range_msg.max_range = 10.0;  
78.     range_msg.min_range = 0.05;  
79.     range_msg.header.frame_id = frame_id;  
80.     range_msg.radiation_type = sensor_msgs::Range::INFRARED;  
81.     if(nodes > range_msg.max_range){  
82.       final_range = std::numeric_limits<float>::infinity();  
83.     }else if(nodes < range_msg.min_range){  
84.       final_range = -std::numeric_limits<float>::infinity();  
85.     }else{  
86.       final_range = nodes;  
87.     }  
88.     range_msg.header.stamp = start;  
89.     range_msg.header.seq = countSeq;  
90.     range_msg.range = final_range;  
91.     scan_pub.publish(range_msg);  
92.   
93.   }  
94.   
95.   bool get_scan_data(){  
96.     len = Cht10driver_.UART0_Recv(fd, buf,40);  
97.     if(len>0){  
98.       for(int i = 0; i < len; i++){  
99.         if(rcv_cnt<=BUFSIZE){    
100.           result_buf[rcv_cnt++] = buf[i];  
101.           if(rcv_cnt == BUFSIZE){  
102.             success_flag = true;  
103.           }  
104.         }//end if  
105.         else{  
106.           /**** 
107.           *  checkout received data 
108.           */  
109.           success_flag = false;  
110.            for(int count = 0; count < 4; count++){  
111.             data_buf[count] = result_buf[9+count];  
112.           }  
113.           sscanf(data_buf, "%x", &laser_data);  
114.             
115.           //std::cout<<"sensor data:"<<laser_data<<std::endl;  
116.   
117.           /**** 
118.            *  data writing end 
119.            */  
120.           if('~' == buf[i]){  
121.             rcv_cnt = 0;  
122.             result_buf[rcv_cnt++] = buf[i];  
123.           }  
124.         }//end else  
125.       }//end for      
126.     }  
127.   }  
128. private:  
129.   void update(){  
130.     rcv_cnt = 0;  
131.     success_flag = 0;  
132.     laser_data = 0;  
133.     ros::Rate spin_rate(50);  
134.     memset(buf, 0, sizeof(buf));  
135.     memset(temp_buf, 0, sizeof(temp_buf));  
136.     memset(result_buf, 0, sizeof(result_buf));  
137.     ROS_INFO_STREAM("Begin receive data from "<<serialNumber_.c_str()<<", with idex:"<<fd<<".");  
138.     fd = open(serialNumber_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY );  
139.     Cht10driver_.UART0_Init(fd,baudRate_,0,8,1,'N');  
140.     while (! shutdown_requested_ && ros::ok())  
141.     {  
142.       start_scan_time = ros::Time::now();  
143.       success_flag = get_scan_data();  
144.             
145.       //Send data  
146.       publish_scan(&scan_pub, data_to_meters(laser_data,SCALE),  
147.                        start_scan_time, frame_id);  
148.       spin_rate.sleep();  
149.   
150.       countSeq++;  
151.     }  
152.   
153.     ROS_INFO_STREAM("Shotdown and close serial: "<<serialNumber_.c_str()<<".");  
154.     Cht10driver_.UART0_Close(fd);  
155.   }  
156. private:  
157.   int fd, len, rcv_cnt;  
158.   int success_flag;  
159.   char buf[40], temp_buf[BUFSIZE],result_buf[BUFSIZE];  
160.     
161.   Cht10Driver Cht10driver_;  
162.   ecl::Thread update_thread_;  
163.   bool shutdown_requested_;  
164.   ros::Publisher scan_pub;  
165.   int laser_data;  
166.   char data_buf[4];  
167.   // ROS Parameters  
168.   std::string serialNumber_;  
169.   int baudRate_;  
170.   int countSeq;  
171.     
172.   std::string frame_id;  
173.   
174.   ros::Time start_scan_time;  
175. };  
176.   
177. } //namespace Cht10_serial_func  
178. PLUGINLIB_EXPORT_CLASS(cht10_seiral_func::Cht10Func,  
179. nodelet::Nodelet);  

  其中 get_scan_data()函数，就是测试代码内容，得到数据后将数据发送到publish_scan()函数中，就可以发布传感器数据了。

 

测试

  代码完成后，按照格式进行编写CMakeList、package文件，运行catkin_make就可以得到名为cht10_node.so动态库，运行launch文件即可加载，命令如下：

[plain] view plain copy

1. roslaunch cht10_node standalone.launch  

  如果一切成才，运行rostopic list 可以得到/range话题。

  使用rostopic echo /range就可以得到传感器的ROS消息内容，包括传感器距离。

 或者直接运行

[plain] view plain copy

1. roslaunch cht10_node view_range.launch  

 就可以看到图形化形式为传感器数据。