激光雷达数据读取机器可视化
当应用场景为室内的时候，单线激光雷达基本上能够满足需求。在本篇文章中将介绍单线激光雷达的使用方法，为后续做SLAM打下一个基础。本篇文章所涉及到的代码可见于本人github仓库，链接为：https://github.com/softdream/robot_projects/tree/master/m1c1_lidar_test
1. 单线激光雷达介绍
1.1 激光雷达参数
本项目为了追求极致的性价比，选用了一款比较便宜的激光雷达，其型号为国科光芯的M1C1_mini。M1C1_mini是一款机械式的单线激光雷达，测距方法为三角测距。其部分性能参数如下：
项目
参数
光源
780nm波长红外激光
测距范围
0.10~10m
测量精度
mm 级@＜1m；2%@1m-6m
角度分辨率
约0.93°
测量频率
10Hz
由于该激光雷达的测量叫分辨率大约是0.93度，可以推算出扫描一帧大概会输出一个有387个点的点云数据，在实际使用的时候，为了方便我只取前380个点的数据。
1.1 激光雷达的安装
激光安装在机器人的腰部位置上，如图1所示：-
2 激光雷达的驱动sdk编译
一般这种小型的单线激光雷达由于数据量很小，都是采用串口协议与上位机进行信息传输的，即激光雷达把采集到的点云数据通过串口发送到上位机，上位机只要读取串口信息即可获取这些数据。-
通常激光雷达的生产厂商会提供激光雷达的驱动sdk，我们只需要在sdk的基础上进行修改即可。-
首先去官网下载M1C1_Linux的sdk包，也可以在我的github仓库里直接下载，地址为：-
https://github.com/softdream/robot_projects/tree/master/M1C1_Lidar-
下载下来的包放在磁盘上任意的文件夹下，然后依次执行：
cd M1C1_Lidar
mkdir build && cd build
cmake ..
make
这时候在build目录下会生成一个名为libcspclidar_driver.a的静态库以及一个名为cspclidar_test的测试执行文件。我们执行cspclidar_test即可在terminal中看到打印的信息。
3 激光雷达驱动的使用
我们通过官方的sdk已经可以获取到激光雷达的测量信息了，但是这还不够，我们得使用自己定义的格式来存储激光点云数据，方便后续进行处理。因此我们需要对其进行修改，首先重新建一个工程，工程结构如下：
|-- CMakeLists.txt
|-- include
| |-- data_container.h
| |-- data_transport.h
| |-- data_type.h
| |-- lidar_drive.h
| `-- lidar_include
| |-- C_CSPC_Lidar.h
| |-- angles.h
| |-- cspclidar_driver.h
| |-- cspclidar_protocol.h
| |-- help_info.h
| |-- lock.h
| |-- locker.h
| |-- serial.h
| |-- thread.h
| |-- timer.h
| |-- utils.h
| -- v8stdint.h
|-- lib
| `-- libcspclidar_driver.a
-- src
`-- main.cpp
在这个工程里，其中include/lidar_include/下面的头文件都是从官方的sdk里拷贝过来了，在这些文件里实现了雷达驱动的一些基本操作，我们不用动。-
在lib目录下面是我们上面编译生成的静态库，拷贝过来放在这里。 data_container.h、data_transport.h、data_type.h以及lidar_drive.h是我们自己添加的四个文件，在这四个文件中实现了我们自己所需要的一些操作，下面回来详细讲解这四个文件的编写过程。-
首先是data_type.h，我们在这个文件中定义了激光雷达扫描帧的基础数据结构，用来对点云数据进行存储，其核心代码如下：
template<int Size>
struct LidarScan
{
float angle_min = 0; // 最小测量角度
float angle_max = 2 * M_PI; // 最大测量角度
float angle_increment = 0.0162356209f; // 角度增量，弧度
float scan_time = 0; // 扫描时间
float time_increment = 0; // 时间增量
float range_min = 0.1f; // 测距最小值
float range_max = 10.0; // 测距最大值
float ranges[Size] = { 0 }; // 点云距离数组
float intensities[Size] = { 0 }; // 点云强度数组
};
typedef struct LidarScan<380> LaserScan;
注意激光点云中每个点的坐标是以极坐标形式给出来的，同时我们这里采用静态大小的数组来存储距离信息，是为了方便数据传输和拷贝。-
在data_container.h文件中，核心代码如下：
class DataContainer
{
private:
std::vector<DataType> data_vec_;
};
using ScanContainer = DataContainer<Eigen::Vector2f>;
可以看出这里仅仅是对vector进行一个适配而已(即一个适配器, Adaptor)，而vector中存储的是激光点云中每个点在笛卡尔坐标系下的位置。因此从LaserScan到ScanContainer需要做一个转换，转化函数为：
void laserData2Container( const sensor::LaserScan& scan, sensor::ScanContainer& container )
{
container.clear();
float angle = 0.0;
for ( size_t i = 0; i < scan.size(); i ++ ) {
auto dist = scan.ranges[i];
if ( dist >= 0.1 && dist < 10.0 ) {
auto pt = Eigen::Vector2f( ::cos( angle ) * dist, ::sin( angle ) * dist );
Eigen::Matrix2f R;
R << ::cos( M_PI ), -::sin( M_PI ),
::sin( M_PI ), ::cos( M_PI );
container.addData( R * pt );
}
angle += scan.angle_increment;
}
}
极坐标转笛卡尔坐标原理很简单了，就不细讲，注意这里中间有一个静态坐标变化的过程，其目的是把点云中每个点旋转180°(主要跟激光雷达的安装方向有关，激光雷达正向安装的就不需要这个步骤)。-
data_transport.h中主要是网络数据传输的接口，可以不用过多关心。-
lidar_drive.h就是雷达驱动的核心代码了，在这里实现了雷达的初始化、启动到数据接收的全部过程。其流程如图2所示：-
核心代码如下：
template<typename T>
void spin( CallBackRef<T>&& cb )
{
while ( ret && cspclidar::ok() ) {
bool hardError;
LaserScan scan;
if ( laser_->doProcessSimple(scan, hardError) ) {
T scan_data;
scan_data.angle_increment = ( 2.0 * M_PI / scan.points.size() );
for ( size_t i = 0; i < 380; i ++ ) {
scan_data.ranges[i] = scan.points[i].range;
scan_data.intensities[i] = scan.points[i].intensity;
}
cb( scan_data );// 调用用户回调函数，用户可在回调函数中做处理
}
else {
std::cerr<<"Failed to get lidar data !"<<std::endl;
}
}
}
这里的spin()方法里是一个循环，每当接收到一帧激光雷达扫描帧数据时，就调用一次用户回调函数，用户可在回掉函数中做相应的处理。回掉函数在main.cpp文件中定义，代码如下：
void lidarCallback( const sensor::LaserScan& scan )
{
std::cout<<"lidar data callback ..."<<std::endl;
laserData2Container( scan, container );
//displayScan( container );
scan_sender.send( scan ); // send lidar scan data
}
回掉函数中的处理比较简单，就是把LaserScan转化成ScanContainer，然后通过接口将数据发送出到桌面端软件显示。如果想在本地显示只需要使用displayScan()这个函数即可。这个函数可以通过opencv来显示数据。
4. 激光雷达数据可视化测试
正确连接好雷达之后，编译上述代码并执行，实时显示激光雷达数据的效果如下：-
激光雷达数据可视化