激光雷达使用学习

雷达型号：速腾聚创Helios-16p

雷点点云格式

sensor_msgs/PointCloud

std_msgs/Header header
  uint32 seq            //序列ID
  time stamp            //数据采集时间
  string frame_id        //坐标系id
geometry_msgs/Point32[] points        //3维坐标
  float32 x
  float32 y
  float32 z
sensor_msgs/ChannelFloat32[] channels    //与每个point相关联的可选数据，比如强度值等
  string name
  float32[] values

sensor_msgs/PointCloud2

std_msgs/Header header
  uint32 seq        //序列ID
  time stamp        //时间戳
  string frame_id   //坐标系ID
uint32 height        //2D点云的时候，存储高和宽
uint32 width        //无序点云的时候，高度为1，宽度是点云的长度
sensor_msgs/PointField[] fields  //每个点的详细信息
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name                    //比如三维坐标,"x","y","z"
  uint32 offset                  //应该指的是内存中的偏移量吧
  uint8 datatype
  uint32 count
bool is_bigendian    //低地址存放高有效字节
uint32 point_step    //点的长度
uint32 row_step      //行的长度 以字节为单
uint8[] data         //实际点云数据，数量=row_step * height;直接解析没有意义，要反解析
bool is_dense          //没有无效点云时为true

可惜我这边没有具体的例子来说明每一项指标的具体含义。这里参考一下解析PointCloud2格式有举部分例子来说明。

sensor_msgs/LaserScan

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.

这个消息格式就比较明朗，基本看名字就了解含义。

rotopic echo /scan --noarr(前提是下图)

 

运行结果

 雷达所测数据不是所有点都有效，inf表示数值无穷大。

运行实际查看——尝试1：使用终端指令查看PointCloud2数据（保存为txt文件）

启动雷达节点

roslaunch rslidar_sdk start.launch
rqt_graph

另开一个终端

rostopic echo /rslidar_points > lidar.txt

录了2秒钟左右，txt文件就达到了74.5MB，以下是部分截图

 这里可以看到激光雷达数据是16*1800，16线激光雷达，每条线160°对应1800个点。

 可以看到速腾聚创的驱动发布的雷达数据格式为PointCloud2格式，header包含序列ID：seq:62，时间戳，坐标系id：rslidar；每个点的name、offset、datatype、count、intensity等信息，数据部分暂时未读懂（果然没有意义）。

后面尝试自己敲代码读取data[]的数据，使用%f打印，结果全是0。

运行实际查看——尝试2：C++读取PointCloud的三维坐标

所以我参考网上代码自己写了一个C++程序来获取我所感兴趣的数据。

首先创建一个工作空间

mkdir -p catkin_ws/src
cd catkin_ws/src
catkin_init_workspace
cd ..
catkin_make        

然后创建功能包

cd catkin_ws/src
catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs
cd ..
catkin_make

以下是cpp代码，cpp文件名是lidar_node.cpp

#include<ros/ros.h>
#include<sensor_msgs/PointCloud.h>
#include<sensor_msgs/point_cloud_conversion.h>
#include<sensor_msgs/PointCloud2.h>
#include <queue>
#include <deque>
//#include <pcl/point_cloud.h>

ros::Publisher lidar_pub;
ros::Subscriber lidar_sub;
void LidarCallback(const sensor_msgs::PointCloud2ConstPtr& msg) 
{
    //cloudQueue.push_back(*msg);
    //pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
    sensor_msgs::PointCloud out_pointcloud;
	sensor_msgs::convertPointCloud2ToPointCloud(*msg, out_pointcloud);
    lidar_pub.publish(out_pointcloud);
    for (int i=0; i<out_pointcloud.points.size(); i++) 
    {
		//cout << out_pointcloud.points[i].x << ", " << out_pointcloud.points[i].y << ", " << out_pointcloud.points[i].z << endl;
        ROS_INFO("No. %d",i);
        ROS_INFO("X = %f",out_pointcloud.points[i].x);
        ROS_INFO("Y = %f",out_pointcloud.points[i].y);
        ROS_INFO("Z = %f",out_pointcloud.points[i].z);
	}

}
int main(int argc,char *argv[])
{
    setlocale(LC_ALL,"");
    ROS_INFO("test");
    std::deque<sensor_msgs::PointCloud2> cloudQueue;
    sensor_msgs::PointCloud2 currentCloudMsg;
    //pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;
    ros::init(argc,argv,"lidar_node");

    ros::NodeHandle n;
    lidar_sub = n.subscribe("/rslidar_points",10,&LidarCallback);
    lidar_pub = n.advertise<sensor_msgs::PointCloud>("/rslidar_to_point",1);
    
    ros::spin();

    return 0;
}

将以上cpp文件复制到catkin_ws/src/lidar_pkg/src下

接下来添加可执行文件以及链接库

在lidar_pkg下的CMakeLists.txt文件下合适位置添加如下内容

add_executable(lidar_node src/lidar_node.cpp)
target_link_libraries(lidar_node
   ${catkin_LIBRARIES}
 )

在终端回到catkin_ws目录下，source一下

echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc 
catkin_make

如果没问题的就准备差不多了

接下来就是运行了

新开一个终端

roslaunch rslidar_sdk start.launch

再从原来工作空间那个终端输入如下指令

rosrun lidar_pkg lidar_node

就可以看到打印的信息如下

 再开一个终端

rostopic echo /rslidar_to_point > lidar.txt

这次保存的文件就小多了。包含信息如下，可以看到点的形式以三维坐标呈现，其实也就是sensor_msgs/PointCloud格式

但是也存在问题，我不知道每个点对应的ring，以及角度，而且我也不知道每个点的距离值。