def METHOD_NAME(self):
    # Test if footprint cost is calculated correctly
    # Create test grid 10 pixels wide by 10 pixels long, at 1 meters per pixel
    # AKA 10 meters x 10 meters
    occupancyGrid_ = OccupancyGrid()
    occupancyGrid_.info.resolution = 1.0
    occupancyGrid_.info.width = 10
    occupancyGrid_.info.height = 10
    occupancyGrid_.info.origin.position.x = 0.0
    occupancyGrid_.info.origin.position.y = 0.0
    map_data = [0] * 10 * 10
    occupancyGrid_.data = map_data
    costmap_ = PyCostmap2D(occupancyGrid_)
    fcc_ = FootprintCollisionChecker()
    fcc_.setCostmap(costmap_)
    # Create square footprint 1m x 1m
    footprint = Polygon()
    point = Point32()
    point.x = 0.0
    point.y = 0.0
    point.z = 0.0
    footprint.points.append(point)
    point = Point32()
    point.x = 1.0
    point.y = 1.0
    point.z = 0.0
    footprint.points.append(point)
    point = Point32()
    point.x = 1.0
    point.y = 0.0
    point.z = 0.0
    footprint.points.append(point)
    point = Point32()
    point.x = 0.0
    point.y = 1.0
    point.z = 0.0
    footprint.points.append(point)
    self.assertEqual(fcc_.footprintCost(footprint), 0.0)
    # Test none-zero cost
    # Create in the map center a full box of cost value 100
    for i in range(24, 28):
        map_data[i] = 100
        map_data[i + 10] = 100
        map_data[i + 20] = 100
        map_data[i + 30] = 100
        map_data[i + 40] = 100
    occupancyGrid_.data = map_data
    costmap_ = PyCostmap2D(occupancyGrid_)
    fcc_ = FootprintCollisionChecker()
    fcc_.setCostmap(costmap_)
    self.assertEqual(fcc_.footprintCostAtPose(4.0, 4.0, 0.0, footprint), 100)
    # Append a point that is outside the map
    point = Point32()
    point.x = 30.0
    point.y = 5.0
    point.z = 3.0
    footprint.points.append(point)
    self.assertEqual(fcc_.footprintCost(footprint), LETHAL_OBSTACLE)