从零开始搭二维激光SLAM --- 了解雷达数据

终于到了写代码的阶段了，哈哈。

上一篇文章我们通过实验知道了雷达数据的各种性质，但是雷达数据在代码里是如何体现的呢？

本篇文章将通过新建一个ros的包来学习一下如何遍历雷达数据，以及如何对雷达数据进行处理。

首先说明一下我使用的代码环境：

Ubuntu版本: 16.04.01
ROS: kinetic 版本
编程语言: C++
IDE推荐: 目前我使用的是 VS code，其如何配置会在之后的文章中讲解

1 ros中激光雷达数据的消息格式

通过在终端中输入如下命令可以打印处ros中激光雷达数据的消息格式

rosmsg show sensor_msgs/LaserScan

结果如下所示

std_msgs/Header header	//　数据的消息头
  uint32 seq			//　数据的序号
  time stamp			//　数据的时间戳
  string frame_id		//　数据的坐标系
float32 angle_min		//　雷达数据的起始角度（最小角度）
float32 angle_max		//　雷达数据的终止角度（最大角度）
float32 angle_increment	//　雷达数据的角度分辨率（角度增量）
float32 time_increment	//　雷达数据每个数据点的时间间隔
float32 scan_time		//　当前帧数据与下一帧数据的时间间隔
float32 range_min		//　雷达数据的最小值
float32 range_max		//　雷达数据的最大值
float32[] ranges		//　雷达数据每个点对应的在极坐标系下的距离值
float32[] intensities	//　雷达数据每个点对应的强度值

2 新建ros包

知道了雷达数据的数据结构，接下来我们将通过代码来更深入的认识下雷达数据。

首先，通过如下命令新建个工作空间，以及一个新的包

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src/
catkin_create_pkg lesson1 roscpp sensor_msgs

由于我们使用c++进行编写，所以第一个依赖为roscpp；由于雷达数据的数据类型为sensor_msgs/LaserScan，所以第二个依赖为sensor_msgs。

2.1 lesson1/src/laser_scan_node.cc

首先，在lesson1/src文件夹下新建个文件，取名为laser_scan_node.cc 。 将如下代码复制进去，代码的意思已经在注释中解释的很清楚了。

#include 
#include 

// 声明一个类
class LaserScan
{
private:
    ros::NodeHandle node_handle_;           // ros中的句柄
    ros::NodeHandle private_node_;          // ros中的私有句柄
    ros::Subscriber laser_scan_subscriber_; // 声明一个Subscriber

public:
    LaserScan();
    ~LaserScan();
    void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
};

// 构造函数
LaserScan::LaserScan() : private_node_("~")
{
    ROS_INFO_STREAM("LaserScan initial.");
    // 将雷达的回调函数与订阅的topic进行绑定
    laser_scan_subscriber_ = node_handle_.subscribe("laser_scan", 1, &LaserScan::ScanCallback, this);
}

LaserScan::~LaserScan()
{
}

// 回调函数
void LaserScan::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    ROS_INFO_STREAM(
        "seqence: " << scan_msg->header.seq << 
        ", time stamp: " << scan_msg->header.stamp << 
        ", frame_id: " << scan_msg->header.frame_id << 
        ", angle_min: " << scan_msg->angle_min << 
        ", angle_max: " << scan_msg->angle_max << 
        ", angle_increment: " << scan_msg->angle_increment << 
        ", time_increment: " << scan_msg->time_increment << 
        ", scan_time: " << scan_msg->scan_time << 
        ", range_min: " << scan_msg->range_min << 
        ", range_max: " << scan_msg->range_max << 
        ", range size: " << scan_msg->ranges.size() << 
        ", intensities size: " << scan_msg->intensities.size());

    // 第5个点的欧式坐标为
    double range = scan_msg->ranges[4];
    double angle = scan_msg->angle_min + scan_msg->angle_increment * 4;
    double x = range * cos(angle);
    double y = range * sin(angle);

    ROS_INFO_STREAM(
        // 第5个数据点对应的极坐标为: 
        "range = " << range << ", angle = " << angle << 
        // 第5个数据点对应的欧式坐标为: 
        ", x = " << x << ", y = " << y
    );

    // 通过ranges中数据的个数进行雷达数据的遍历
    // for (int i = 0; i < scan_msg->ranges.size(); i++)
    // {

    // }

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lesson1_laser_scan_node"); // 节点的名字
    LaserScan laser_scan;

    ros::spin();    // 程序执行到此处时开始进行等待，每次订阅的消息到来都会执行一次ScanCallback()
    return 0;
}

2.2 CMakeLists.txt

由于我们新增了一个.cc文件，所以我们要在CMakeLists.txt中将这个文件添加编译选项，在文件的底部添加如下内容：

# 为指定的文件生成可执行文件
add_executable(${PROJECT_NAME}_laser_scan_node src/laser_scan_node.cc)

# 为生成的可执行文件添加依赖
add_dependencies(${PROJECT_NAME}_laser_scan_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

# 为生成的可执行文件添加库的链接
target_link_libraries(${PROJECT_NAME}_laser_scan_node
  ${catkin_LIBRARIES}
)

2.3 package.xml

由于我们是使用的 catkin_create_pkg 命令生成的包，在生成包的时候它已经将package.xml配置好了，所以我们这里不需要再去改这个文件，只将生成的文件内容列出来。

<package format="2">
  <name>lesson1name>
  <version>0.0.0version>
  <description>The lesson1 packagedescription>

  <maintainer email="[email protected]">lxmaintainer>

  <license>TODOlicense>

  <buildtool_depend>catkinbuildtool_depend>
  <build_depend>roscppbuild_depend>
  <build_depend>sensor_msgsbuild_depend>
  <build_export_depend>roscppbuild_export_depend>
  <build_export_depend>sensor_msgsbuild_export_depend>
  <exec_depend>roscppexec_depend>
  <exec_depend>sensor_msgsexec_depend>
  
  <export>
  export>
package>

2.4 编译

修改好CMakeLists.txt文件后，就可以进行代码的编译了，具体指令如下：

cd ~/catkin_ws
catkin_make

2.5 运行

不出意外的话应该没有产生编译错误.

编译完成后会在 ~/catkin_ws/devel/lib/lesson1 文件夹下 产生了一个叫做 lesson1_laser_scan_node 的可执行文件。

我们可以通过如下命令进行执行

首先，开一个终端，在终端中输入 roscore
再新建一个终端，在终端中输入 
cd ~/catkin_ws/devel/lib/lesson1 
./lesson1_laser_scan_node

将打印出如下的log，

[ INFO] [1606545572.752075473]: LaserScan initial.

此时是没有其他消息打印出来的，这是因为我们的代码没有接收到激光雷达的数据消息，所以我们通过如下命令进行bag的播放，bag数据下载下来后，右键单击进行解压，并放在 ~/bagfiles 文件夹下。

（本文对应的 bag数据 可以去我的 公众号: 从零开始搭SLAM 中回复 lesson1 获取下载链接，）

再次新开一个终端，输入如下命令
cd ~/bagfiles
rosbag play lesson1.bag

这时，在执行 ./lesson1_laser_scan_node 的终端窗口中应该会不停地打印处如下消息

[ INFO] [1606545575.110606737]: seqence: 4131, time stamp: 1606455444.278184417, frame_id: front_laser_link, angle_min: -3.14159, angle_max: 3.14159, angle_increment: 0.00436332, time_increment: 7.15627e-05, scan_time: 0.102979, range_min: 0.01, range_max: 25, range size: 1440, intensities size: 1440
[ INFO] [1606545575.110772238]: range = 2.6, angle = -3.12414, x = -2.5996, y = -0.0453758

2.6 结果分析

从这些信息中我们可以看出：

• 雷达的坐标系为front_laser_link

• 雷达数据的最小角度与最大角度分别为 -3.14159 与 3.14159，可见，这是一个水平视角为360度的雷达

• 雷达数据的最近最远距离分别为 0.01m 与 25m，可见，这个雷达的盲区为1cm

• 雷达扫描一周将返回1440个数据点。

2.7 说明

2.7.1

从雷达数据中我们可以得到雷达数据的最基本消息，但是这些消息不一定是真实可靠的，因为这些数据是雷达的驱动包中写的，有些雷达厂商的代码不规范，在发出雷达数据时填的信息有可能是错误的。如，雷达的盲区很少能有1cm这么小的，一般都10cm以上。

2.7.2 坐标转换

还有，雷达数据中的 ranges 字段中储存的只有极坐标系下的距离值，如果我们想知道每个数据点对应欧几里得坐标，还需要将极坐标进行转换。

转换的方法就是 通过索引来获取 ranges 中的值，再通过索引算出这个值对应的角度

第i个数据点的距离值为 ranges[i]
第i个数据点的角度为 angle = angle_min + angle_increment * i
所以这个点对应的x坐标为 ranges[i] * cos(angle)
所以这个点对应的y坐标为 ranges[i] * sin(angle)

2.7.3 雷达数据的遍历

二维激光雷达数据的遍历只有通过 ranges 字段的个数 进行for 循环，并通过索引进行距离值的获取。

2.8 launch文件

2.8.1 launch文件

目前，我们想要启动这个节点需要开3个终端才能够启动，那有没有更方便的方式进行启动呢。

当然是有的，ROS中使用launch文件来实现这个功能。

首先，先将打开的终端全部关掉，在ubuntu中终止正在执行的东西的命令为 Ctrl+C。

# 新建一个launch文件夹
mkdir -p ~/catkin_ws/src/lesson1/launch 

在新建的launch文件夹中新建一个名为 demo.launch 的文件，并将如下内容填进去。

<launch>

    
    <arg name="bag_filename" default="/home/lx/bagfiles/lesson1.bag"/>

    
    <param name="use_sim_time" value="true" />

    
    <node name="lesson1_laser_scan_node" pkg="lesson1" type="lesson1_laser_scan_node" output="screen" />

    
    <node name="playbag" pkg="rosbag" type="play"
        args="--clock $(arg bag_filename)" />

launch>

接下来只要启动这个launch文件，它就会帮我们启动roscore，想要的节点，以及bag包的播放。

2.8.2 设置环境

由于我们是新建的工作空间，我们首先要执行如下命令将我们新建的包的索引在终端中注册一下，以便终端能够找到我们的包。

rospack profile
source ~/catkin_ws/devel/setup.bash

其中source的这条命令，是每次新开一个终端都要重新执行一次的，也可以通过如下命令将其写入到 .bashrc 中，这样每次新开终端就不再需要执行source命令了。

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

环境弄好了，接下来我们通过roslaunch来启动launch文件

roslaunch lesson1 demo.launch

如果一切顺利的话我们可以得到上边同样的结果。

3 总结

本篇文章中，我们通过获取激光雷达数据的例子，从零新建了一个ROS中的package。

我们讲解了如何通过命令新建包，以及在新建包的时候的依赖如何添加。

之后，我们新建了一个.cc文件，文件中的内容就是按照标准ROS的格式书写的，以后我们的程序都会按照这个模式进行代码的编写。在程序中，我们知道了雷达数据的消息内容，如何进行极坐标与欧式坐标的转换，以及如何对雷达数据进行遍历。

之后，我们修改了CMakeLists.txt文件，并成功编译和运行。

最后，我们讲解了launch文件以及如何设置代码的环境。

4 Next

接下来的文章我们不会再这么细致的进行环境的讲解，我们将更多的关注功能的实现以及代码的实现。

下一篇文章我们将要介绍如何对雷达数据进行处理与过滤，如何在雷达数据中识别出人的小腿，并将人的小腿过滤掉。

---

文章将在 公众号: 从零开始搭SLAM 进行同步更新，欢迎大家关注，以在文章更新的第一时间通知您。

同时，也希望您将这个公众号推荐给您身边做激光SLAM的人们，大家共同进步。

在 公众号 中回复 lesson1 可以获取本文中的bag数据的下载链接
在 公众号 中回复 开源地址 可以获取本文中代码的github地址