思岚rplidar A1激光点云数据读取

本文使用ROS开发包进行思岚rplidar A1激光点云数据读取.对应的架构很简单,Node发布/scan这个topic,然后通过client去订阅.因此client.cpp中有如何读取激光点云数据的对应函数.

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include 
using namespace std;

#define RAD2DEG(x) ((x)*180./M_PI)


void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    int count = scan->scan_time / scan->time_increment;
    ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count);
    cout<<"scan_time"<<scan->scan_time<<" time_increment"<<scan->time_increment<<endl;
    ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));
    cout<<"angle_increment"<<scan->angle_increment<<endl;
//    for(int i = 0; i < count; i++) {
//        float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
//        ROS_INFO(": [%f, %f]", degree, scan->ranges[i]);
//    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "rplidar_node_client");
    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback);

    ros::spin();

    return 0;
}

读取关键就在于scanCallback这个函数.

count为激光雷达旋转一圈所对应获得激光反射点的数量,scan->scan_time为旋转一圈所需要的时间,scan->time_increment为每读取一个点所需要的时间,因此两者相除就是读取一圈点后点的个数,基本上稳定在358或359,因此接近于1度一个点,旋转一周的时间为0.135s和0.143s交替.

scan->angle_min和scan->angle_max分别对应旋转一周中最小的角度和最大的角度,分别为-pi和pi,通过RAD2DEG转化成角度.

下面的for循环为输出旋转一周里所有点云的角度和距离,相当于对应极坐标. scan->angle_increment为两个点之间的角度差值,接近于pi/180,也就是1度. scan->range[i]为每个点离原点的距离.具体极坐标定义图如下: