小型家庭自主巡检机器人--第四篇:激光雷达数据读取及其可视化

激光雷达数据读取机器可视化

当应用场景为室内的时候,单线激光雷达基本上能够满足需求。在本篇文章中将介绍单线激光雷达的使用方法,为后续做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所示:-

图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所示:-

图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. 激光雷达数据可视化测试

正确连接好雷达之后,编译上述代码并执行,实时显示激光雷达数据的效果如下:-
激光雷达数据可视化激光雷达数据可视化

求大佬提供下底层控制板的原理图、pcb文件以及bom表,在此感谢

提问有深度