slam定位练习笔记(一)

实现上一篇学习笔记里面的想法。

首先总结lida_localization里面test_frame_work的整体工作。

1.订阅了三个传感器数据。其中cloud_data提供当前帧的点云，gnss提供t向量，imu提供R矩阵，然后由t和R的数据整合生成transform的4×4矩阵。

2.将订阅到的点云转换为PCL格式，然后使用PCL库函数处理，其中主要是使用transform矩阵进行位置变换，然后在将点云转换为ROS格式，然后再发布出去。

3.利用前面得到的transform矩阵，发布odometry。

主要是这个流程，现在有之前跟这李太白大佬学习二维slam建图的数据lesson1.bag。看了一下包的数据格式：

 发布的话题有/laser_scan和/odom所有想着实现和上面一样的效果，每一帧点云都在转换在里程计上面。

具体流程：

1.订阅/laser_scan，并将这个转换为PCL格式。

2.订阅/odom，并获得transform。

3.利用PCL库函数将PCL格式的点云带上transform变换，然后转换回ROS格式并发布，再发布odom。

首先要创建工作空间，确定相关的依赖。

mkdir -p node1/src
cd node1/src
catkin_init_workspace

再确定相关的依赖，直接看lida_localization里面package.xml文件内容：

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>

  <depend>sensor_msgs</depend>
  <depend>pcl_ros</depend>
  <depend>std_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>tf</depend>
  <depend>eigen_conversions</depend>

依赖包有roscpp、rospy、sensor_msgs、pcl_ros、std_msgs、geometry_msgs、tf、eigen_conversions、nav_msgs。直接照抄就可以了。

一、设置PCL点云数据格式

这里直接参考lidar_localization里面cloud的数据格式。

cloud_data.hpp

#ifndef FRAME_EXERCISE_SENSOR_DATA_CLOUD_DATA_HPP_
#define FRAME_EXERCISE_SENSOR_DATA_CLOUD_DATA_HPP_

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

namespace frame_exercise
{
    class CloudData 
    {
    public:
        using POINT = pcl::PointXYZ;
        using CLOUD = pcl::PointCloud<POINT>;
        using CLOUD_PTR = CLOUD::Ptr;

    public:
        CloudData()
        :cloud_ptr(new CLOUD()) {
        }

    public:
        double time = 0.0;
        CLOUD_PTR cloud_ptr;
    };    
}

 注意这里同时也要设置好cmake文件里面关于pcl的设置

find_package(PCL 1.7 REQUIRED)
list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")

include_directories(${PCL_INCLUDE_DIRS})
list(APPEND ALL_TARGET_LIBRARIES ${PCL_LIBRARIES})

二、订阅bag包发送的/laser_scan

和lidar_localization里面的处理方式一样，写单独的类，并用构造函数来订阅topic。

这里有一点点需要变换，在lidar_localization里面

它订阅的是sensor_msgs::PointCloud2::ConstPtr数据类型，

而这里订阅的是sensor_msgs/LaserScan数据类型。所以需要修改。

cloud_subscriber.hpp

#ifndef FRAME_EXERCISE_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_
#define FRAME_EXERCISE_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_

#include <deque>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>

#include "frame_exercise/sensor_data/cloud_data.hpp"

namespace frame_exercise
{
    class CloudSubscriber {
    public:
        CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size);
        CloudSubscriber() = default;
        void ParseData(std::deque<CloudData>& deque_cloud_data);

    private:
        void msg_callback(const sensor_msgs::LaserScan::ConstPtr& cloud_msg_ptr);

    private:
        ros::NodeHandle nh_;
        ros::Subscriber subscriber_;

        std::deque<CloudData> new_cloud_data_;
    };    
} // namespace frame_exercise

将格式改变后，突然发现不能借助pcl库的函数进行转换了，得自己来写。。。

这里参靠李太白大佬的文章，来写将laserscan转化为pointcloud2格式。

比较简单，将参数格式一改，查询一下相关的rosmsg show就好了

 将LaserScan的格式转换为PointCloud2的格式。

cloud_subscriber.cpp:

#include "frame_exercise/subscriber/cloud_subscriber.hpp"


namespace frame_exercise
{
CloudSubscriber::CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size)
    :nh_(nh) {
    subscriber_ = nh_.subscribe(topic_name, buff_size, &CloudSubscriber::msg_callback, this);
}

void CloudSubscriber::msg_callback(const sensor_msgs::LaserScan::ConstPtr& scan_msg) {
    CloudData cloud_data;
    cloud_data->cloud_ptr->points.resize(scan_msg->ranges.size());

    cloud_data.time = scan_msg->header.stamp.toSec();
    // pcl::fromROSMsg(*cloud_msg_ptr, *(cloud_data.cloud_ptr));
    // 这里不能使用这个函数，得自己手写数据转换方式
    for(unsigned int i = 0; i < scan_msg->ranges.size(); ++i )
    {
        pcl::PointXYZ & point_tmp = cloud_data->cloud_ptr->points[i];
        float range = scan_msg->ranges[i]; 

        if (!std::isfinite(range))
        {
            // std::cout << " " << i << " " << scan_msg->ranges[i];
            point_tmp = invalid_point_;
            continue;
        }
        if (range > scan_msg->range_min && range < scan_msg->range_max)
        {
            // 获取第i个点对应的角度
            float angle = scan_msg->angle_min + i * scan_msg->angle_increment;
            // 获取第i个点在笛卡尔坐标系下的坐标
            point_tmp.x = range * cos(angle);
            point_tmp.y = range * sin(angle);
            point_tmp.z = 0.0;
        }
        else
        {
            // 无效点
            point_tmp = invalid_point_;
        }
        cloud_data->cloud_ptr->width = scan_msg->ranges.size();
        cloud_data->cloud_ptr->height = 1;
        cloud_data->cloud_ptr->is_dense = false;    // contains nans

        cloud_data->cloud_ptr->header.seq = scan_msg->header->seq;
        cloud_data->cloud_ptr->header.frame_id = scan_msg->header->frame_id;

    }

    new_cloud_data_.push_back(cloud_data);
}

void CloudSubscriber::ParseData(std::deque<CloudData>& cloud_data_buff) {
    if (new_cloud_data_.size() > 0) {
        cloud_data_buff.insert(cloud_data_buff.end(), new_cloud_data_.begin(), new_cloud_data_.end());
        new_cloud_data_.clear();
    }
}
} // namespace frame_exercise

三、订阅bag包发送的/odom

查看Odometry的数据格式

 思考：在订阅odometry的时候要不要也像cloud_subscriber一样添加一个专门的clouddata的格式，应该不用，我只需要提取每一帧的odom中的transform就行了，不需要对他进行专门的数据处理。

主要用到了四元数与旋转矩阵之间的转换，这篇文章写的很好，参考这篇文章提到的函数。

odom_subscriber.hpp

#ifndef FRAME_EXERCISE_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_
#define FRAME_EXERCISE_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_

// 订阅到odom得到transform矩阵。
#include <deque>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Dense>

namespace frame_exercise
{
class OdomSubscriber{
    public:
        OdomSubscriber(ros::NodeHandle& nh_, std::string topic_name, std::size_t buff_size);
        std::deque<Eigen::Matrix4f> odom_transform;
    private:
        void msg_callback(const nav_msgs::Odometry::ConstPtr& odom_msg);
    private:
        ros::NodeHandle nh_;
        ros::Subscriber subscriber_;

};
} // namespace frame_exercise

特地写了一个用于存放odomtransform的队列。

OdomSubscriber.cpp

#include "frame_exercise/subscriber/odom_subscriber.hpp"

namespace frame_exercise
{
    OdomSubscriber::OdomSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size):
        nh_(nh){
            subscriber_ = nh_.subscriber(topic_name, buff_size, &OdomSubscriber::msg_callback, this);
        }
    void OdomSubscriber::msg_callback(const nav_msgs::Odometry::ConstPtr& odom_msg)
    {
        Eigen::Maxtrix4f transform;
        // t向量
        transform(0, 3) = odom_msg.pose.pose.position.x;
        transform(1, 3) = odom_msg.pose.pose.position.y;
        transform(2, 3) = odom_msg.pose.pose.position.z;
        // R旋转矩阵
        Eigen::Quaternionf q;
        q.x() = odom_msg.pose.pose.orientation.x;
        q.y() = odom_msg.pose.pose.orientation.y;
        q.z() = odom_msg.pose.pose.orientation.z;
        transform.block<3,3>(0,0) = q.matrix();
        odom_transfrom.push_back(transform);
    }
} // namespace frame_exercise

感觉应该没一个函数都要测试一下能不能正常使用，达到我想要的效果，不能这样一股闹的全部写完再测试。

四、发布环节

主要应用到函数：

pcl::transformPointCloud(*cloud_data.cloud_ptr, *cloud_data.cloud_ptr, odometry_matrix);

现在的问题是odometry_matrix这个已经存在了，但不知道怎么提取出来。

还是采用了lidar_localization的框架，在里面单独使用一个队列，然后在callback函数里面处理这个队列。

里面遇到了很多坑，都是我对于pcl没有特别理解的后果。。。

a.回调函数里面关于引用的处理要带上*。

b.关于头文件相互包含的问题。(这个错误卡了我一个下午，最后发现是宏定义出了问题，我当时直接复制了cloud_subscriber.hpp的宏定义！！！)

直接将发布环节写在frame_exercise.cpp里面了：

#include "ros/ros.h"
#include "pcl/common/transforms.h"
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>

#include "frame_exercise/subscriber/cloud_subscriber.hpp"
#include "frame_exercise/subscriber/odom_subscriber.hpp"

using namespace frame_exercise;

int main(int argc, char** argv)
{
    ros::init(argc, argv, "test_frame");
    ros::NodeHandle nh;
    ros::Publisher pub_odom;
    ros::Publisher pub_cloud;

    // pub_odom = nh.advertise<nav_msgs::Odometry>("/odom1", 10);
    pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("/cloud1", 10);

    std::shared_ptr<CloudSubscriber> cloud_sub_ptr = std::make_shared<CloudSubscriber>(nh, "/laser_scan",  1000);
    std::shared_ptr<OdomSubscriber> odom_sub_ptr = std::make_shared<OdomSubscriber>(nh, "/odom", 1000);
    

    std::deque<CloudData> cloud_data_buff;
    std::deque<Eigen::Matrix4f> transform_buff;

    ros::Rate rate(100);

    while(ros::ok())
    {
        ros::spinOnce();
        cloud_sub_ptr->ParseData(cloud_data_buff);
        odom_sub_ptr->ParseData(transform_buff);

        while(cloud_data_buff.size() > 0 && transform_buff.size())
        {
            CloudData cloud_data = cloud_data_buff.front();
            Eigen::Matrix4f odometry_matrix = transform_buff.front();

            cloud_data_buff.pop_front();
            transform_buff.pop_front();

            pcl::transformPointCloud(*cloud_data.cloud_ptr, *cloud_data.cloud_ptr, odometry_matrix);
            // 如果想要发布odom的话，还需要定义一个nav_msgs::Odometry对象，太复杂了，先不写了。
            pub_cloud.publish(cloud_data.cloud_ptr);

        }
        rate.sleep();
    }
    return 0;
}

最开始编译的时候 会报这个错误，想了很久也不知道是哪里的问题，最后是看了这篇文章解决。

 然后就可以成功编译。

但显示明明生成了可执行文件，但还是rosrun的时候报错说没有。

 我在devel的lib中没有发现，但在build里面的frame_exercise文件里面找到了。导致rosrun的时候找不到可执行文件，解决这个问题主要看了这篇文章。

然后发现了很多bug，暂时解决不了。先列举一下发现的bug：

1.四元数是有w的。

2.发布需要变成rosmsg格式的，要改进。

3.pcl::transformPointCloud(*cloud_data.cloud_ptr, *cloud_data.cloud_ptr, odometry_matrix);运行到这里就会报错，不知道是哪里的问题。

先挖个坑，以后在解决这些问题。

实践出真知