从0学激光slam lesson1~3

一、lesson1    激光特征提取 及bag文件讲解  

原因：这是由于 ROS tf 的发布时间晚于 topic 的时间，Rviz 在做 msg 的 tf 变换时，默认把过时的 msg 丢掉。为了解决这一问题，可以让系统以 msg 对应的 simulated time 运行，而不是实际的 wall-clock time.

bag文件的位置需要根据自己 电脑的用户名 来进行配置

首先，要将参数 use_sim_time 设置为true，代表当前ros的时间为bag文件中的时间。

同时，需要在播放bag时加上 –clock 选项，以表示使用bag文件中的时间为ros的当前时间。

重点理解发布和订阅

#录制前

$ rosparam set /use_sim_time false

#播放时

$ rosparam set /use_sim_time true

把播放bag数据集放进launch文件中

<launch>

    <!-- bag的地址与名称，需要改成自己电脑的对应的地址 -->
    <arg name="bag_filename" default="/home/lx/bagfiles/lesson1.bag"/>

    <!-- 使用bag的时间戳 -->
    <param name="use_sim_time" value="true" />

    <!-- 启动节点 -->
    <node name="lesson1_laser_scan_node" pkg="lesson1" type="lesson1_laser_scan_node" output="screen" />

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

</launch>

if (!std::isfinite(scan_msg->ranges[i]))   //std::isfinite 在输入无效值时将返回false，所以进行取反
        {
            // std::cout << " " << i << " " << scan_msg->ranges[i];
            continue;
        }

单线雷达提取角点效果如图所示：

二、lesson2   PCL

1、

catkin_create_pkg lesson2 pcl_conversions pcl_ros roscpp sensor_msgs 

roscpp以及sensor_msgs在之前的文章说过了，roscpp是c++的依赖，sensor_msgs是雷达数据的消息类型的依赖。

pcl_conversions 以及 pcl_ros 是ROS官方为了在ROS中方便的使用PCL而写的包。

pcl_conversions 包含了一些方法，实现了 ROS的消息类型 与 PCL的消息类型 的转换。

pcl_ros 定义了一些其他的功能，如在ROS中使用标准的Publisher直接发布PCL的数据格式、将PCL的点云根据tf进行坐标变换、实现了一些常用的功能接口，使得可以直接在ROS中调用PCL的函数，例如进行体素滤波等等。

具体理解

perception_pcl理解 --- pcl_conversions 与 pcl_ros_李太白lx的博客-CSDN博客_pcl_conversions

2.2 修改CMakeLists.txt
需要在CMakeLists.txt额外添加下面语句，代表我们需要依赖PCL。

find_package(PCL REQUIRED QUIET)
include_directories( 
  include
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)

2.3修改Package.xml

需要额外添加如下两句话，代表依赖PCL。

<build_depend>libpcl-all-dev</build_depend>
<exec_depend>libpcl-all</exec_depend>

由于已经配置好了rviz，所以将直接出现下边的图片。

这张图片展示的将sensor_msgs/LaserScan转换成的sensor_msgs/PointCloud2的数据

 2.2基于ICP的帧间匹配

上一篇文章讲解了如何将激光雷达的sensor_msgs/LaserScan格式转换成pcl::PointCloud< pcl::PointXYZ>格式, 本篇文章将要讲解如何使用这个格式调用ICP算法进行相邻2帧雷达数据间坐标变换的计算

*
 * 构造函数
 */
ScanMatchICP::ScanMatchICP()
{
    // \033[1;32m，\033[0m 终端显示成绿色
    ROS_INFO_STREAM("\033[1;32m----> Scan Match with ICP started.\033[0m");

    laser_scan_subscriber_ = node_handle_.subscribe(
        "laser_scan", 1, &ScanMatchICP::ScanCallback, this);

    // 第一帧数据的标志
    is_first_scan_ = true;

    // 指针的初始化
    current_pointcloud_ = boost::shared_ptr<PointCloudT>(new PointCloudT());
    last_pointcloud_ = boost::shared_ptr<PointCloudT>(new PointCloudT());
}

is_first_scan_是第一帧雷达数据到来的标志，因为第一帧数据到来时，只有一帧数据，是没办法进行匹配的，所以要对第一帧数据进行特殊处理．

current_pointcloud_ 与 last_pointcloud_ 分别保存的是当前帧雷达数据转成pcl点云格式后的数据，以及上一帧雷达数据转成pcl点云格式后的数据．

这里是对这两个智能指针进行初始化，他们的类型为 boost::shared_ptr .

Affine3f (有旋转和平移成员）

2.2.3ICP部分

void ScanMatchICP::ScanMatchWithICP(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    // ICP 输入数据,输出数据的设置,还可以进行参数配置,这里使用默认参宿
    icp_.setInputSource(last_pointcloud_);
    icp_.setInputTarget(current_pointcloud_);

    // 开始迭代计算
    pcl::PointCloud<pcl::PointXYZ> unused_result;
    icp_.align(unused_result);

    // std::cout << "has converged:" << icp_.hasConverged() << " score: " << icp_.getFitnessScore() << std::endl;

    // 如果迭代没有收敛,不进行输出
    if (icp_.hasConverged() == false)
    {
        std::cout << "not Converged" << std::endl;
        return;
    }
    else
    {
        // 收敛了之后, 获取坐标变换
        Eigen::Affine3f transfrom;
        transfrom = icp_.getFinalTransformation();

        // 将Eigen::Affine3f转换成x, y, theta, 并打印出来
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(transfrom, x, y, z, roll, pitch, yaw);
        std::cout << "transfrom: (" << x << ", " << y << ", " << yaw * 180 / M_PI << ")" << std::endl;
    }
}

2.2.4ros的理解

  构造函数中创建需要的，和节柄等，  关键在于构造函数中创建的订阅者，  订阅的话题，和回调函数，  回调函数之后， 每接受一次，就会调用该函数，

重点理解这个回调函数的使用

void ScanMatchICP::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    // step1 进行数据类型转换
    std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now();
    
    // 对第一帧数据进行特殊处理
    if (is_first_scan_ == true)
    {
        // 进行第一帧数据的处理,只转换数据类型 并 保存到current_pointcloud_
        ConvertScan2PointCloud(scan_msg);
        is_first_scan_ = false;
        return;
    }
    else    
        // 在将新一帧数据转换到当前帧之前,
        // 先将current_pointcloud_赋值到last_pointcloud_进行保存
        *last_pointcloud_ = *current_pointcloud_;   

    // 进行数据类型转换
    ConvertScan2PointCloud(scan_msg);

    std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time);
    std::cout << "\n转换数据格式用时: " << time_used.count() << " 秒。" << std::endl;

    // step2 使用ICP计算 雷达前后两帧间的坐标变换
    start_time = std::chrono::steady_clock::now();

    // 调用ICP进行计算
    ScanMatchWithICP(scan_msg);

    end_time = std::chrono::steady_clock::now();
    time_used = std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time);
    std::cout << "ICP计算用时: " << time_used.count() << " 秒。" << std::endl;
}

效果如图所示 

1、ICP计算的需要讲雷达的msgs scan转化为 pointcloud 格式， 

• scan_to_cloud_converter: 将 sensor_msgs/LaserScan 数据转成 sensor_msgs/PointCloud2 的数据格式．
• 也可以手动

2、 发布显示的仍然是   msgs中的 laser_scan   格式