使用里程计（Odometry）数据来去除雷达（Lidar）数据的畸变

使用里程计（Odometry）数据来去除雷达（Lidar）数据的畸变

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <thread>
#include <mutex>
#include <queue>
#include <deque>
#include <vector>
/*
使用里程计（Odometry）数据来去除雷达（Lidar）数据的畸变，
 可以通过将雷达数据与里程计数据进行时间同步，并使用里程计提供的位姿信息对雷达数据的点进行运动补偿。
 这个过程被称为“去畸变”或“运动补偿”。下面是一个完整的ROS节点代码示例。

**/
// 用于存储历史里程计数据
std::vector<nav_msgs::Odometry> odom_buffer;
//std::queue<nav_msgs::Odometry> odom_queue;
std::deque<nav_msgs::Odometry> odom_queue;
ros::Publisher corrected_scan_pub;
std::mutex queue_mutex;
std::mutex scancallback_mutex_;
// 里程计数据回调函数
void odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg) {
    std::lock_guard<std::mutex> lock(queue_mutex);
   // o