Rplidar学习—— ROS下进行rplidar雷达数据采集源码分析
Posted Blue Mountain
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了Rplidar学习—— ROS下进行rplidar雷达数据采集源码分析相关的知识，希望对你有一定的参考价值。
一、子函数分析
1、发布数据子函数
（1）雷达数据数据类型
Header header # timestamp in the header is the acquisition time of # the first ray in the scan. # # in frame frame_id, angles are measured around # the positive Z axis (counterclockwise, if Z is up) # with zero angle being forward along the x axis float32 angle_min # start angle of the scan [rad] float32 angle_max # end angle of the scan [rad] float32 angle_increment # angular distance between measurements [rad] float32 time_increment # time between measurements [seconds] - if your scanner # is moving, this will be used in interpolating position # of 3d points float32 scan_time # time between scans [seconds] float32 range_min # minimum range value [m] float32 range_max # maximum range value [m] float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded) float32[] intensities # intensity data [device-specific units]. If your # device does not provide intensities, please leave # the array empty.
（2）雷达发布子函数分析
/***********************打印函数*************************/ void publish_scan(ros::Publisher *pub, //对象 rplidar_response_measurement_node_t *nodes, //雷达数据 size_t node_count, ros::Time start, //长度，起始时间 double scan_time, bool inverted, //扫描时间，是否转换 float angle_min, float angle_max, //角度范围 std::string frame_id) //id { static int scan_count = 0; sensor_msgs::LaserScan scan_msg; //ros已有的雷达数据模型 scan_msg.header.stamp = start; //扫描起始时间 scan_msg.header.frame_id = frame_id; //序列id scan_count++; //计数 //角度修正，从小到大 bool reversed = (angle_max > angle_min); if ( reversed ) { scan_msg.angle_min = M_PI - angle_max; scan_msg.angle_max = M_PI - angle_min; } else { scan_msg.angle_min = M_PI - angle_min; scan_msg.angle_max = M_PI - angle_max; } //扫描精度 scan_msg.angle_increment = (scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count-1); scan_msg.scan_time = scan_time; //扫描时间开始 scan_msg.time_increment = scan_time / (double)(node_count-1); //时间间隔 scan_msg.range_min = 0.15; //最小的范围 scan_msg.range_max = 8.0; //最大的范围 scan_msg.intensities.resize(node_count); //信号质量 scan_msg.ranges.resize(node_count); //范围 bool reverse_data = (!inverted && reversed) || (inverted && !reversed); if (!reverse_data) { for (size_t i = 0; i < node_count; i++) { float read_value = (float) nodes[i].distance_q2/4.0f/1000; //距离信息 if (read_value == 0.0) scan_msg.ranges[i] = std::numeric_limits<float>::infinity(); else scan_msg.ranges[i] = read_value; scan_msg.intensities[i] = (float) (nodes[i].sync_quality >> 2);//信号质量信息 } } else { for (size_t i = 0; i < node_count; i++) { float read_value = (float)nodes[i].distance_q2/4.0f/1000; if (read_value == 0.0) scan_msg.ranges[node_count-1-i] = std::numeric_limits<float>::infinity(); else scan_msg.ranges[node_count-1-i] = read_value; scan_msg.intensities[node_count-1-i] = (float) (nodes[i].sync_quality >> 2); } } pub->publish(scan_msg); }
2、得到设备信息子函数
/*****************得到设备信息**************************/ bool getRPLIDARDeviceInfo(RPlidarDriver * drv) { u_result op_result; rplidar_response_device_info_t devinfo; //得到设备信息 op_result = drv->getDeviceInfo(devinfo); //失败信息 if (IS_FAIL(op_result)) { if (op_result == RESULT_OPERATION_TIMEOUT) { fprintf(stderr, "Error, operation time out.\n"); } else { fprintf(stderr, "Error, unexpected error, code: %x\n", op_result); } return false; } // print out the device serial number, firmware and hardware version number..序列号 printf("RPLIDAR S/N: "); for (int pos = 0; pos < 16 ;++pos) { printf("%02X", devinfo.serialnum[pos]); } //版本 printf("\n" "Firmware Ver: %d.%02d\n" "Hardware Rev: %d\n" , devinfo.firmware_version>>8 , devinfo.firmware_version & 0xFF , (int)devinfo.hardware_version); return true; }
3、检查rplidar设备健康信息
bool checkRPLIDARHealth(RPlidarDriver * drv) { u_result op_result; rplidar_response_device_health_t healthinfo; op_result = drv->getHealth(healthinfo); if (IS_OK(op_result)) { printf("RPLidar health status : %d\n", healthinfo.status); if (healthinfo.status == RPLIDAR_STATUS_ERROR) { fprintf(stderr, "Error, rplidar internal error detected." "Please reboot the device to retry.\n"); return false; } else { return true; } } else { fprintf(stderr, "Error, cannot retrieve rplidar health code: %x\n", op_result); return false; } }
4、雷达开始/结束
bool stop_motor(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { if(!drv) return false; ROS_DEBUG("Stop motor"); drv->stop(); drv->stopMotor(); return true; } bool start_motor(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { if(!drv) return false; ROS_DEBUG("Start motor"); drv->startMotor(); drv->startScan();; return true; }
5、main函数分析
int main(int argc, char * argv[]) { ros::init(argc, argv, "rplidar_node"); std::string serial_port; //串口号 int serial_baudrate = 115200; //串口波特率 std::string frame_id; //id bool inverted = false; //是否转换标志位 bool angle_compensate = true; //角度补偿标志位 ros::NodeHandle nh; ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000); ros::NodeHandle nh_private("~"); //launch可以进行一些初始化 nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0"); nh_private.param<int>("serial_baudrate", serial_baudrate, 115200); nh_private.param<std::string>("frame_id", frame_id, "laser_frame"); nh_private.param<bool>("inverted", inverted, false); nh_private.param<bool>("angle_compensate", angle_compensate, true); printf("RPLIDAR running on ROS package rplidar_ros\n" "SDK Version: "RPLIDAR_SDK_VERSION"\n"); u_result op_result; // create the driver instance,创建静态接口 drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT); if (!drv) //创建失败退出-2 { fprintf(stderr, "Create Driver fail, exit\n"); return -2; } // make connection...建立链接，失败返回-1 if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) { fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" , serial_port.c_str()); RPlidarDriver::DisposeDriver(drv); return -1; } // get rplidar device info 获得设备信息 if (!getRPLIDARDeviceInfo(drv)) { return -1; } // check health...检查设备是否健康 if (!checkRPLIDARHealth(drv)) { RPlidarDriver::DisposeDriver(drv); return -1; } //添加回调函数 ros::ServiceServer stop_motor_service = nh.advertiseService("stop_motor", stop_motor); ros::ServiceServer start_motor_service = nh.advertiseService("start_motor", start_motor); //开电机进行扫描 drv->startMotor(); drv->startScan(); //记录扫描起始时间，持续时间 ros::Time start_scan_time; ros::Time end_scan_time; double scan_duration; while (ros::ok()) { //储存信号质量，角度信息，距离信息的变量 rplidar_response_measurement_node_t nodes[360*2]; size_t count = _countof(nodes); //得到数组长度 //得到扫描时间，雷达数据 start_scan_time = ros::Time::now(); op_result = drv->grabScanData(nodes, count); end_scan_time = ros::Time::now(); scan_duration = (end_scan_time - start_scan_time).toSec() * 1e-3; //成功扫描 if (op_result == RESULT_OK) { //得到排序后的雷达数据 op_result = drv->ascendScanData(nodes, count); float angle_min = DEG2RAD(0.0f); float angle_max = DEG2RAD(359.0f); if (op_result == RESULT_OK) { //进行角度补偿 if (angle_compensate) { const int angle_compensate_nodes_count = 360; const int angle_compensate_multiple = 1; int angle_compensate_offset = 0; //初始化，清0 rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count]; memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t)); int i = 0, j = 0; for( ; i < count; i++ ) { if (nodes[i].distance_q2 != 0) //距离不是0 { //计算当前角度，如果角度比上次小则，记录 float angle = (float)((nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f); int angle_value = (int)(angle * angle_compensate_multiple); if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value; for (j = 0; j < angle_compensate_multiple; j++) //只修正一个 { angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i]; } } } publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count, start_scan_time, scan_duration, inverted, angle_min, angle_max, frame_id); } else { int start_node = 0, end_node = 0; int i = 0; // find the first valid node and last valid node while (nodes[i++].distance_q2 == 0); start_node = i-1; i = count -1; while (nodes[i--].distance_q2 == 0); end_node = i+1; angle_min = DEG2RAD((float)(nodes[start_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f); angle_max = DEG2RAD((float)(nodes[end_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f); publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +1, start_scan_time, scan_duration, inverted, angle_min, angle_max, frame_id); } } else if (op_result == RESULT_OPERATION_FAIL) { // All the data is invalid, just publish them float angle_min = DEG2RAD(0.0f); float angle_max = DEG2RAD(359.0f); publish_scan(&scan_pub, nodes, count, start_scan_time, scan_duration, inverted, angle_min, angle_max, frame_id); } } ros::spinOnce(); } // done! drv->stop(); drv->stopMotor(); RPlidarDriver::DisposeDriver(drv); return 0; }
二、rplidar.launch文件分析
<launch> <node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen"> <param name="serial_port" type="string" value="/dev/ttyUSB0"/> <param name="serial_baudrate" type="int" value="115200"/> <param name="frame_id" type="string" value="laser"/> <param name="inverted" type="bool" value="false"/> <param name="angle_compensate" type="bool" value="true"/> </node> </launch>
1、运行launch文件
（1）格式：
roslaunch package_name launch_file_name
Tip1: rosrun只能运行一个nodes， roslaunch可以同时运行多个nodes
（2）详细显示(request verbosity)
（3） 结束launch文件
ctrl+c
2、创建launch文件
(1) launch文件一般以.launch后缀作为文件名，放在package的launch文件夹下。最简单的launch文件可以仅包含几个nodes。
(2) Launch文件是XML文件，每个XML文件必须有一个root element。而launch文件的root element由一对launch 标签定义。
<launch>
...
</launch>
(3) launch文件的核心是一系列node elements，每个node element启动一个node。Node element看起来如下
<node pkg=”package_name” type=”executable_name” name=”node_name” /> #Tip1: 最后的“/”是必不可少的。 #Tip2: 也可以写成<node pkg=”..” type=”...” name=”...”></node> #如果该node中有其他tags，则必须使用这种形式。
(4) 一个node element包含三个必须的属性：pkg, type, name.
pkg和type属性指出ROS应该运行哪个pkg中的哪个node，注意：此处的type是可执行文件的名称，而name则是可以任意给出的，它覆盖了原有文件中ros::init指定的node name。
(5) node日志文件(log file)
运行roslaunch和rosrun运行单个节点的区别之一是，默认情况下，roslaunch运行的nodes的标准输出会重定向到log file，而不是控制台
~/.ros/log/run_id/node_name-number-stdout.log
(6) 输出到控制台
用output属性， output=”screen”；这种方法仅显示一个node。
若显示所有nodes的输出，用--screen命令行。
roslaunch --screen package_name launch_file_name
如果正在运行的文件没有显示想要对输出，可以查看该node属性集中是否有 output=”screen”.
(7) 在独立的窗口运行各nodes
我们在各自的termin运行rosrun node_name；但是运行roslaunch时，所有的nodes共用一个相同的terminal，这对于那些需要从控制台输入的nodes很不方便。可以使用launch-prefix属性。
launch-prefix=”command-prefix”
Eg：launch-prefix=”xterm -e” 等价于 xterm -e rosrun turtlesim turtle_teleop_key
xterm 命令表示新建一个terminal； -e参数告诉xterm执行剩下的命令行。
当然，launch-prefix属性不仅仅限于xterm。它可用于调试（通过gdb或valgrind），或用于降低进程的执行顺序（通过nice）.
以上是关于Rplidar学习—— ROS下进行rplidar雷达数据采集源码分析的主要内容，如果未能解决你的问题，请参考以下文章
Rplidar学习—— rplidar使用cartographer_ros进行地图云生成
ROS与激光雷达入门-ROS中使用激光雷达(RPLIDAR)