[SLAM Learning] Obtain and publish IMU and radar messages

This article mainly records how to obtain and publish rosbag messages and follow-up processing.

Test dataset:

Link: https://pan.baidu.com/s/1DthWE45V5Zhq7UUrfTt_CQ Extraction code: mxvn 
 

Check out the topics in the dataset bag:

rosbag info indoor_lab_RS.bag 

 You can see that two topics are included:

• /imu/date
• /rslidar_points

This file contains two callback functions:

1. imu callback function (get timestamp, angular velocity value, acceleration value)
2. lidar callback function (acquire timestamp, convert message type to PCL point cloud, perform voxel filtering and downsampling, and convert the final downsampled point cloud into message type and publish it)

#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 file:

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 data output results:

 

---

Radar data voxel filtering downsampling results:

 

 

---

 

View the data type of IMU: 

rosmsg show sensor_msgs/Imu

View the data type of the radar: