first commit

src/costmap_2d.cpp

@@ -0,0 +1,66 @@
+#include <costmap_2d/costmap_2d.h>
+#include <cstdio>
+
+using namespace std;
+
+namespace costmap_2d
+{
+    Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
+        double origin_x, double origin_y, unsigned char default_value) :
+    size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
+    origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
+    {
+        access_ = new mutex_t();
+
+        // create the costmap
+        initMaps(size_x_, size_y_);
+        resetMaps();
+    }
+
+    void Costmap2D::deleteMaps()
+    {
+        // clean up data
+        boost::unique_lock<mutex_t> lock(*access_);
+        delete[] costmap_;
+        costmap_ = NULL;
+    }
+
+    void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
+    {
+        boost::unique_lock<mutex_t> lock(*access_);
+        delete[] costmap_;
+        costmap_ = new unsigned char[size_x * size_y];
+    }
+
+    unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
+    {
+        return costmap_[getIndex(mx, my)];
+    }
+
+    void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
+    {
+        costmap_[getIndex(mx, my)] = cost;
+    }
+
+    void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
+    {
+        wx = origin_x_ + (mx + 0.5) * resolution_;
+        wy = origin_y_ + (my + 0.5) * resolution_;
+    }
+
+    bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
+    {
+        if (wx < origin_x_ || wy < origin_y_)
+            return false;
+
+        mx = (int)((wx - origin_x_) / resolution_);
+        my = (int)((wy - origin_y_) / resolution_);
+
+        if (mx < size_x_ && my < size_y_)
+            return true;
+
+        return false;
+    }
+
+
+}