博客轉自：沒趣啊
局部障礙無法清除
這個問題的發現比較有戲劇性。在我們前期測試的時候，由於基本不怎么有活動障礙物，這個問題一直沒有暴露。偶爾出現小車無法動的情況我們湊上去看的時候，這下小車掃到有障礙物，之前的就被清除，結果又可以動了。一直到展示的前一天拖到人多的環境測試的時候，我們才發現人走來走去會導致局部地圖都是沒清除的障礙。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，這樣全局規划只會在有新目標跟局部規划失敗的時候才運行。不過這樣在有動態障礙的時候不太好。