【SLAM学习】获取IMU和雷达消息并发布

news2024/9/21 0:36:17

本文主要记录如何将rosbag的消息进行获取并进行发布以及后续处理。

测试数据集：

链接: https://pan.baidu.com/s/1DthWE45V5Zhq7UUrfTt_CQ 提取码: mxvn

查看数据集bag包里面都有那些话题：

rosbag info indoor_lab_RS.bag

可以看到包含了两个话题topic：

/imu/data
/rslidar_points

本文件包含两个回调函数：

imu回调函数（获取时间戳、角速度值、加速度值）
lidar回调函数（获取时间戳，将消息类型转化为PCL点云，进行体素滤波下采样，并将最终下采样的点云转化为消息的类型发布出来）

#include <iostream>
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/PointCloud2.h"

#include "pcl/point_types.h"
#include "pcl/filters/voxel_grid.h"
#include "pcl_ros/point_cloud.h"

// rsliar的数据结构
namespace rslidar_ros
{
    struct EIGEN_ALIGN16 Point
    {
        PCL_ADD_POINT4D;
        std::uint8_t intensity;
        std::uint16_t ring = 0;
        double timestamp = 0;
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
} // namespace rslidar_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(rslidar_ros::Point,
    (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(std::uint16_t, ring, ring)(double, timestamp, timestamp))


bool first_imu = true;
double first_imu_time;

// IMU消息回调函数
void Imu_callback(const sensor_msgs::Imu &msg)
{
    if (first_imu){
        first_imu_time = msg.header.stamp.toSec(); // 获取第一帧时间戳
        first_imu = false;
    }
    std::cout << "IMU time:" << msg.header.stamp.toSec() - first_imu_time << std::endl;
    std::cout << "angular_velocity:" 
              << msg.angular_velocity.x << " " 
              << msg.angular_velocity.y << " "
              << msg.angular_velocity.z << std::endl;
    std::cout << "linear_acceleration:" 
              << msg.linear_acceleration.x << " "
              << msg.linear_acceleration.y << " "
              << msg.linear_acceleration.z << std::endl;

}

bool first_lidar = true;
double first_lidar_time;

// 下采样,体素滤波
pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
pcl::PointCloud<pcl::PointXYZI> pl_filtered;

ros::Publisher publaserCloud;

// 雷达消息回调函数
void laser_callback(const sensor_msgs::PointCloud2 &msg)
{
    if (first_lidar){
        first_lidar_time = msg.header.stamp.toSec(); // 获取第一帧雷达的时间戳
        first_lidar = false;
    }

    std::cout << "Lidar time:" << msg.header.stamp.toSec() - first_lidar_time << std::endl;

    pcl::PointCloud<rslidar_ros::Point> pl_orig;
    pcl::fromROSMsg(msg, pl_orig); // 从ROS的消息转化到点云PCL
    int plsize = pl_orig.points.size(); // 获取点云的数量
    std::cout << "Lidar point size:" << plsize << std::endl; // 原始点云的大小

    pl_filtered.clear();
    for (int i = 0; i < plsize; i++){
        pcl::PointXYZI point;
        point.x = pl_orig.points[i].x;
        point.y = pl_orig.points[i].y;
        point.z = pl_orig.points[i].z;
        point.intensity = pl_orig.points[i].intensity;

        pl_filtered.points.push_back(point);
    }

    // 下采样
    downSizeFilter.setLeafSize(0.05, 0.05, 0.05); // 设置下采样大小
    downSizeFilter.setInputCloud(pl_filtered.makeShared());
    downSizeFilter.filter(pl_filtered);

    std::cout << "Lidar filtered size:" << pl_filtered.points.size() << std::endl; // 滤波后的点云大小

    // 将点云通过消息的类型发布出来
    sensor_msgs::PointCloud2 laserCloud;
    pcl::toROSMsg(pl_filtered, laserCloud);
    laserCloud.header.stamp = msg.header.stamp;
    laserCloud.header.frame_id = "camera_init";
    publaserCloud.publish(laserCloud);
}



int main(int argc, char** argv)
{
    ros::init(argc, argv, "lio_test");
    ros::NodeHandle nh;

    // 订阅IMU和lidar消息
    ros::Subscriber imu_sub = nh.subscribe("/imu/data",10,Imu_callback);

    ros::Subscriber lidar_sub = nh.subscribe("/rslidar_points",10,laser_callback);

    // 发布雷达点云
    publaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/cloud", 100000);
    while(ros::ok())
    {
        ros::spinOnce();
    }

    return 0;
}

CMakeLists.txt文件：

cmake_minimum_required(VERSION 3.0.2)
project(LIO_TEST)

find_package(catkin REQUIRED COMPONENTS
  pcl_ros
  roscpp
  sensor_msgs
  std_msgs
)

find_package(PCL REQUIRED)

add_executable(lio_test src/lio_test.cpp)

target_link_libraries(lio_test
  ${catkin_LIBRARIES}
  ${PCL_LIBRARIES}
)

IMU数据输出结果：