Revision history [back]

I was having the same issue with my Robot. Currently, the CostMap2D uses Bresenham2D algorithm for clearing the obstacles. As per your video, the blob gets left out in almost the same location and that I think is due to the round off error of Bresenham2D algorithm. It marks a particular cell as obstacle but the next time when the sensor reading changes instantaneously, it no longer traces it through the same path and hence, the blob seem remains in the costmap .

So, for solving this problem , I amended the function below in costmap_2d.h in the navigation package of ros. I have added 2 lines of code to the Bresenham2D algorithm. This basically clears the cell to the left and right of the grid through which the Line segment constructed by the algorithm passes. This results in loosing some resolution of the map but the solution works pretty well in real life application and I have had no problems after this hacky fix which is not too bad.

template<class ActionType>
    inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a,
                            int offset_b, unsigned int offset, unsigned int max_length)
    {
      unsigned int end = std::min(max_length, abs_da);
      for (unsigned int i = 0; i < end; ++i)
      {
        at(offset);
        at(offset+1); // **ADDED THIS LINE**
        at(offset-1); // **ADDED THIS LINE**
        offset += offset_a;
        error_b += abs_db;
        if ((unsigned int)error_b >= abs_da)
        {
          offset += offset_b;
          error_b -= abs_da;
        }
      }
      at(offset);
    }