/*
使用里程计（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);
// odom_queue.push(*odom_msg);
odom_queue.push_back(*odom_msg);
// 只保留一定数量的里程计数据，确保不会占用太多内存
if (odom_queue.size() > 100) {
// odom_queue.pop();
odom_queue.pop_front();
}
}
// 线性插值计算
nav_msgs::Odometry interpolateOdom(const nav_msgs::Odometry& start_odom, const nav_msgs::Odometry& end_odom, const ros::Time& time) {
nav_msgs::Odometry interpolated_odom;
double t = (time - start_odom.).toSec() / (end_odom. - start_odom.).toSec();
// 插值位置
interpolated_odom. = start_odom. + t * (end_odom. - start_odom.);
interpolated_odom. = start_odom. + t * (end_odom. - start_odom.);
interpolated_odom. = start_odom. + t * (end_odom. - start_odom.);
// 插值四元数
tf::Quaternion start_q, end_q, interpolated_q;
tf::quaternionMsgToTF(start_odom., start_q);
tf::quaternionMsgToTF(end_odom., end_q);
interpolated_q = start_q.slerp(end_q, t);
tf::quaternionTFToMsg(interpolated_q, interpolated_odom.);
interpolated_odom. = time;
return interpolated_odom;
}
// 查找指定时间的里程计数据，并进行插值
bool getOdomAtTime(const ros::Time& time, nav_msgs::Odometry& odom_out) {
std::lock_guard<std::mutex> lock(queue_mutex);
if (odom_queue.empty()) return false;
// 寻找最近的前后里程计数据
for (size_t i = 0; i < odom_queue.size() - 1; ++i) {
if (odom_queue[i]. <= time && odom_queue[i+1]. >= time) {
odom_out = interpolateOdom(odom_queue[i], odom_queue[i+1], time);
return true;
}
}
// 如果没有找到合适的里程计数据，直接返回最近的一个数据
if (time < odom_queue.front().) {
odom_out = odom_queue.front();
} else if (time > odom_queue.back().) {
odom_out = odom_queue.back();
} else {
return false;
}
return true;
}
// 异步处理雷达数据的线程函数
void processScanData(const sensor_msgs::LaserScan::ConstPtr& scan_msg) {
// 尝试非阻塞上锁
std::unique_lock<std::mutex> lock(scancallback_mutex_, std::try_to_lock);
if (!lock.owns_lock()) {
// 如果锁不可用，直接跳过本次循环
std::cout<<"scan_CB 如果锁不可用，直接跳过本次循环"<<std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(50));
return ;
}
nav_msgs::Odometry start_odom;
nav_msgs::Odometry end_odom;
// 获取雷达数据开始和结束时刻的里程计数据
if (!getOdomAtTime(scan_msg->, start_odom) ||
!getOdomAtTime(scan_msg-> + ros::Duration(scan_msg->ranges.size() * scan_msg->time_increment), end_odom)) {
ROS_WARN("Could not find matching odometry data. Skipping scan correction.");
return;
}
// 计算里程计增量
double delta_x = end_odom. - start_odom.;
double delta_y = end_odom. - start_odom.;
double delta_theta = tf::getYaw(end_odom.) - tf::getYaw(start_odom.);
sensor_msgs::LaserScan corrected_scan = *scan_msg;
// 修正每个激光点的位置
for (size_t i = 0; i < scan_msg->ranges.size(); ++i) {
double angle = scan_msg->angle_min + i * scan_msg->angle_increment;
double range = scan_msg->ranges[i];
// 当前激光点的坐标
double original_x = range * cos(angle);
double original_y = range * sin(angle);
// 根据时间偏移量和里程计增量来修正坐标
double time_offset = i * scan_msg->time_increment;
double ratio = time_offset / (scan_msg->ranges.size() * scan_msg->time_increment);
double corrected_x = original_x + ratio * delta_x - ratio * original_y * sin(delta_theta);
double corrected_y = original_y + ratio * delta_y + ratio * original_x * sin(delta_theta);
// 转换回极坐标系
corrected_scan.ranges[i] = hypot(corrected_x, corrected_y);
}
// 发布修正后的雷达数据
corrected_scan_pub.publish(corrected_scan);
}
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_msg) {
std::thread(processScanData, scan_msg).detach();
}
int main(int argc, char **argv) {
ros::init(argc, argv, "trilateration_test46_node");
ros::NodeHandle nh;
// 订阅里程计和雷达数据
ros::Subscriber odom_sub = nh.subscribe("/odom", 1000, odomCallback);
ros::Subscriber scan_sub = nh.subscribe("/scan", 1000, scanCallback);
// 初始化发布器，发布修正后的雷达数据
corrected_scan_pub = nh.advertise<sensor_msgs::LaserScan>("/scan_corrected", 100);
ros::spin();
return 0;
}