#include <costmap_2d/unpreferred_layer.h>
#include <pluginlib/class_list_macros.hpp>

PLUGINLIB_EXPORT_CLASS(costmap_2d::UnPreferredLayer, costmap_2d::Layer)

using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::PREFERRED_SPACE;

namespace costmap_2d
{

UnPreferredLayer::UnPreferredLayer(){}
UnPreferredLayer::~UnPreferredLayer(){}

unsigned char UnPreferredLayer::interpretValue(unsigned char value) 
{
  // check if the static value is above the unknown or lethal thresholds
  if(value == 0) return NO_INFORMATION;
  else if (value >= *this->threshold_)
    return UNPREFERRED_SPACE;

  double scale = (double) value / *this->threshold_;
  return scale * LETHAL_OBSTACLE;
}

}