ROS costmap_2d局部障碍物无法清除和机器人到点摇摆

博客转自：没趣啊

局部障碍无法清除

这个问题的发现比较有戏剧性。在我们前期测试的时候，由于基本不怎么有活动障碍物，这个问题一直没有暴露。偶尔出现小车无法动的情况我们凑上去看的时候，这下小车扫到有障碍物，之前的就被清除，结果又可以动了。一直到展示的前一天拖到人多的环境测试的时候，我们才发现人走来走去会导致局部地图都是没清除的障碍。ros讨论的issue
LIDAR A2雷达在扫不到的时候会把这条线的距离设为最大距离。理论上来说，如果激光在一条线上扫不到障碍物，那么这条线上之前的障碍物都应该清除。
这个问题明显跟costmap2d里的obstacle layer里的清除障碍的代码有关。我第一个怀疑的是obstacle layer在清除障碍前有一个最大距离的check，但是读了半天发现不存在的。而且我顺着逻辑在脑海里跑了好几遍代码，感觉清除障碍的代码一点问题都没有，只要这条线上有数据，那么小于raytrace_range的障碍物都会被清除。这个还挺合理的，节约点计算量。
后来又读了几遍，发现ObstacleLayer::laserScanCallback里面有这么一行：

projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);

按理说这是平平无奇的一行代码，不过是调用laser_geometry包转换一下统一格式。当时我也找不到问题，所以随便看看里面在干啥。emm... 结果发现居然真的是这一行在搞鬼！

if (range_cutoff < 0)
      range_cutoff = scan_in.range_max;

    unsigned int count = 0;
    for (unsigned int index = 0; index< scan_in.ranges.size(); index++)
    {
      const float range = ranges(0, index);
      if (preservative || ((range < range_cutoff) && (range >= scan_in.range_min))) //if valid or preservative

laser_geometry里面居然有最大距离的check... 也就是说这一行偷偷摸摸把超过最大距离的点拿掉了，这导致后面清除障碍的代码收不到这条线上的数据，自然就不会清除之前的障碍了。

解决方法很简单，要么直接改下代码，要么手动把最大距离字段的限制修大点，相当于老是有一个比较远的障碍物。不过这个需要注意别对其他模块造成误导。首先是obstacle_range，不能小于这个，要不然老是认为有障碍物了；然后是amcl跟gmapping的最大距离限制，一定要比它们大，不然会有虚假的障碍。

小车到目标时摇摆

DWA规划器每次都是先到位置再把角度转到位。实际测试的时候我们发现最后转到位的阶段小车老是摇摆不定，好像它在犹豫往左转好还是往右转好。ros上有个issue讨论这个问题。
这个问题还是比较容易理解的。最后原地旋转的时候，小车有可能产生位移导致偏离目标超过tolerance，导致局部规划又重新启动，结果就出现摇摆不定的情况。ros专门为这种情况设定了一个latch_xy_goal_tolerance 的参数，也就是说一旦到达过一次目标就不再启动局部规划器，旋转到位就好。
然而，实际中我们发现设置好了也没用，小车还是摇摆不定。难道代码写错了？我又去看了一遍代码，发现主要部分没什么错，不过重置的逻辑有点问题。
ros在小车第一次到达目标的时候会锁住局部规划器不让它运行。然而，为了保证下次有新目标的时候正常跑，需要在有新目标的时候把锁打开：

bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
    if (! isInitialized()) {
      ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
      return false;
    }
    //when we get a new plan, we also want to clear any latch we may have on goal tolerances
    latchedStopRotateController_.resetLatching();

    ROS_INFO("Got new plan");
    return dp_->setPlan(orig_global_plan);
  }

全局规划一般是1秒一次。全局规划仅仅在小车到达目标的时候才停止，而在小车最后旋转到位的阶段，角度的目标还没到位呢。这两条导致最后阶段局部规划器根本锁不住，所以latch_xy_goal_tolerance 就没意义了。

这个改起来也简单，准备打开锁的时候看看终点位置变化了没有就行，没变化就不开锁了。也可以把全局规划频率设为0，这样全局规划只会在有新目标跟局部规划失败的时候才运行。不过这样在有动态障碍的时候不太好。