Revision history [back]

I've dealt with the same issue; I ended up using the occupancy_grid_utils library to calculate the cost of the cells inscribed in the footprint.

#include "occupancy_grid_utils/coordinate_conversions.h"
#include "occupancy_grid_utils/geometry.h"

namespace ogu = occupancy_grid_utils;
namespace gm = geometry_msgs;
namespace nm = nav_msgs;

bool isValidPosition(const gm::Polygon& oriented_footprint, const nm::OccupancyGrid& occupancy_grid,
                     const double grid_cost_cuttoff_p)
{
  try
  {
    auto cells = ogu::cellsInConvexPolygon(occupancy_grid.info, oriented_footprint);
    for (auto& cell : cells)
    {
      auto grid_idx = ogu::cellIndex(occupancy_grid.info, cell);
      auto occupied_p = occupancy_grid.data[grid_idx];
      if (occupied_p >= grid_cost_cuttoff_p)
        return false;
    }
    return true;
  }
  catch (ogu::CellOutOfBoundsException e)
  {
    ROS_DEBUG("Cell out of bounds %s", e.what());
  }
  catch (ogu::GridUtilsException e)
  {
    ROS_ERROR("Error validating pose %s", e.what());
  }
  return false;
}

Note I used

costmap2d::transformFootprint()

to get the oriented footprint