Add costmap_2d package sources

Convert navigations/costmap_2d from gitlink to normal tracked files.

navigations/costmap_2d/plugins/preferred_layer.cpp

@@ -0,0 +1,28 @@
+#include <costmap_2d/preferred_layer.h>
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(costmap_2d::PreferredLayer, costmap_2d::Layer)
+
+using costmap_2d::NO_INFORMATION;
+using costmap_2d::FREE_SPACE;
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::PREFERRED_SPACE;
+
+namespace costmap_2d
+{
+
+PreferredLayer::PreferredLayer(){}
+PreferredLayer::~PreferredLayer(){}
+
+unsigned char PreferredLayer::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 PREFERRED_SPACE;
+
+  double scale = (double) value / *this->threshold_;
+  return scale * LETHAL_OBSTACLE;
+}
+
+}