Laser radar data reading machine visualization
When the application scenario is indoor, single line LiDAR can basically meet the requirements. In this article, we will introduce the usage of single line LiDAR, laying a foundation for future SLAM work. The code involved in this article can be found in my GitHub repository, and the link is:
https://github.com/softdream/robot_projects/tree/master/m1c1_lidar_test
- Introduction to Single Line Lidar
1.1 Lidar parameters
In order to pursue the ultimate cost-effectiveness, this project has chosen a relatively inexpensive LiDAR model, which is the M1C1_mini of Guoke Optics. M1C1_mini is a mechanical single line LiDAR with a triangulation ranging method. Some of its performance parameters are as follows:
project
parameter
Light source
780nm wavelength infrared laser
Ranging range
0.10-10m
measurement accuracy
mm@＜1m; 2%@1m-6m
Angle resolution
Approximately 0.93 °
Measurement frequency
10Hz
Due to the measurement resolution of the LiDAR being approximately 0.93 degrees, it can be inferred that scanning a frame will output a point cloud data with approximately 387 points. In practical use, for convenience, I only took data from the first 380 points.
1.1 Installation of LiDAR
The laser is installed at the waist position of the robot, as shown in Figure 1:
- Compilation of driver SDK for LiDAR
Generally, small single line LiDARs of this type use a serial protocol to transmit information to the upper computer due to their small data volume. That is, the LiDAR sends the collected point cloud data to the upper computer through the serial port, and the upper computer only needs to read the serial port information to obtain this data.
Usually, manufacturers of LiDAR provide the driver SDK for LiDAR, and we only need to make modifications based on the SDK. Firstly, go to the official website to download the SDK package for M1C1-Linux, or you can directly download it from my GitHub repository at the following address: https://github.com/softdream/robot_projects/tree/master/M1C1_Lidar Place the downloaded package in any folder on the disk and execute it sequentially:
cd M1C1_Lidar
mkdir build && cd build
cmake ..
make
At this point, a static library named libcspclidar_driver. a and a test execution file named cspclidar_test will be generated in the build directory. We can execute cspclidar_test to see the printed information in the terminal. 3. Use of LiDAR driver
We can already obtain the measurement information of the LiDAR through the official SDK, but this is not enough. We need to use our own defined format to store the LiDAR point cloud data for easy processing in the future. Therefore, we need to modify it by first rebuilding a new project with the following structure:
|-- 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
In this project, the header files under include/lidar_include/have been copied from the official SDK, and some basic operations for radar drivers have been implemented in these files, so we don’t need to touch them. Under the lib directory is the static library generated by our compilation, copy it and place it here. Data_container. h, data_transport. h, data_type. h, and lidar_drive. h are the four files we added ourselves. We have implemented some of the operations we need in these four files. Below, we will provide a detailed explanation of the writing process for these four files. Firstly, data_type. h. In this file, we define the basic data structure of LiDAR scanning frames for storing point cloud data. The core code is as follows:
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;
Note that the coordinates of each point in the laser point cloud are given in polar form, and we use a static size array to store distance information for the convenience of data transmission and copying. In the data_container. h file, the core code is as follows:
class DataContainer
{
private:
std::vector<DataType> data_vec_;
};
using ScanContainer = DataContainer<Eigen::Vector2f>;
It can be seen that this is only an adaptation of the vector (i.e. an adapter), and the vector stores the position of each point in the laser point cloud in the Cartesian coordinate system. Therefore, a conversion is required from LaserScan to ScanContainer, and the conversion function is:
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;
}
}
The principle of converting polar coordinates to Cartesian coordinates is very simple, so I won’t go into detail. Note that there is a static coordinate change process in the middle here, which aims to rotate each point in the point cloud by 180 ° (mainly related to the installation direction of the LiDAR, which is not necessary for the forward installation of the LiDAR). The data_transport. h mainly serves as the interface for network data transmission, so there is no need to worry too much. Lidar_drive. h is the core code of the radar driver, which implements the entire process of radar initialization, startup, and data reception. The process is shown in Figure 2:
The core code is as follows:
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;
}
}
}
The spin() method here is a loop that calls the user callback function every time a frame of LiDAR scanning data is received, and the user can perform corresponding processing in the callback function. The callback function is defined in the main.cpp file, and the code is as follows:
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
}
The processing in the callback function is relatively simple, which is to convert LaserScan into ScanContainer, and then send the data to the desktop software for display through the interface. If you want to display locally, you only need to use the displayScan() function. This function can display data through opencv. 4. Visualization testing of LiDAR data
After correctly connecting the radar, compile and execute the above code to display real-time LiDAR data as follows: