cartographer导航时丢失odom数据，如何保证定位数据稳定？

Posted 2022-03-22 COCO_PEAK_NOODLE

我们知道odom是电机齿轮编码器的数据，如果按了急停，这时候没有odom数据发布出来，我们要如何处理odom丢失导致定位失效的问题，这里给出一种解决方案

在ordered_multi_queue.cc 中当检测到odom丢失时，KMaxQueueSize是检测到其他传感器累计的数量超过多少时，我们可以在queues中移除odom的使用

void OrderedMultiQueue::CannotMakeProgress(const QueueKey& queue_key) 
  blocker_ = queue_key;
  for (auto& entry : queues_) 
    if (entry.second.queue.Size() > kMaxQueueSize) 
      LOG_EVERY_N(WARNING, 60) << "Queue waiting for data: " << queue_key;

      //here
      queues_.erase(queue_key);

      return;
    
  

本以为这样就可以了，但是由于pose_extrapolator.cc并没有考虑odom丢失的问题，这里也要修改一下

void PoseExtrapolator::TrimOdometryData() 
  //LOG(INFO) << "TrimOdometryData odometry_data_ size " << odometry_data_.size();
  //cartographer
  /*
  while (odometry_data_.size() > 2 && !timed_pose_queue_.empty() &&
         odometry_data_[1].time <= timed_pose_queue_.back().time) 
    odometry_data_.pop_front();
  
  */

  //here
  if(odometry_data_.size()!=0 && timed_pose_queue_.size() != 0)
  
  double second = common::ToSeconds(odometry_data_.back().time
                                  - timed_pose_queue_.back().time) * (-1);

   //odom 丢失时间超过1s
   if(second >1.0 && odometry_data_.size() != 0)
   
     odometry_data_.clear();
     LOG(WARNING) << "okagv odom maybe missing ";
   
  
  

原作者的意思是odometry_data_.size()数量只要不超过2个，则使用激光估计的位姿来进行线速度的估计。所以这里odometry_data_.clear();
之所以这样做（用时间来检测）而不是暴力的移除，反正读者可以测试一下就知道了