diff --git a/include/node2d.h b/include/node2d.h
index 29f37e78..0f6c6c8f 100644
--- a/include/node2d.h
+++ b/include/node2d.h
@@ -2,6 +2,7 @@
 #define NODE2D_H
 
 #include <cmath>
+#include <geometry_msgs/Pose.h>
 
 #include "constants.h"
 namespace HybridAStar {
@@ -48,6 +49,10 @@ class Node2D {
   bool  isDiscovered() const { return d; }
   /// get a pointer to the predecessor
   Node2D* getPred() const { return pred; }
+  /// Get x as on map
+  int getGridX(){ return (int)((x - mp.position.x) / res); }
+  /// Get y as on map
+  int getGridY(){ return (int)((y - mp.position.y) / res); }
 
   // SETTER METHODS
   /// set the x position
@@ -59,7 +64,7 @@ class Node2D {
   /// set the cost-to-come (heuristic value)
   void setH(const float& h) { this->h = h; }
   /// set and get the index of the node in the 2D array
-  int setIdx(int width) { this->idx = y * width + x; return idx;}
+  int setIdx(int width) { this->idx = this->getGridY() * width + this->getGridX(); return idx;}
   /// open the node
   void open() { o = true; c = false; }
   /// close the node
@@ -70,6 +75,8 @@ class Node2D {
   void discover() { d = true; }
   /// set a pointer to the predecessor of the node
   void setPred(Node2D* pred) { this->pred = pred; }
+  /// set map meta data
+  static void setMeta(geometry_msgs::Pose p, float r){ mp = p; res = r; }
 
   // UPDATE METHODS
   /// Updates the cost-so-far for the node x' coming from its predecessor. It also discovers the node.
@@ -118,6 +125,10 @@ class Node2D {
   bool d;
   /// the predecessor pointer
   Node2D* pred;
+  /// map pose
+  static geometry_msgs::Pose mp;
+  /// resolution
+  static float res;
 };
 }
 #endif // NODE2D_H
diff --git a/include/node3d.h b/include/node3d.h
index 6ae6d6f9..1a3b4b02 100644
--- a/include/node3d.h
+++ b/include/node3d.h
@@ -2,6 +2,7 @@
 #define NODE3D_H
 
 #include <cmath>
+#include <geometry_msgs/Pose.h>
 
 #include "constants.h"
 #include "helper.h"
@@ -53,6 +54,10 @@ class Node3D {
   bool isClosed() const { return c; }
   /// determine whether the node is open
   const Node3D* getPred() const { return pred; }
+  /// Get x as on map
+  int getGridX(){ return (int)((x - mp.position.x) / res); }
+  /// Get y as on map
+  int getGridY(){ return (int)((y - mp.position.y) / res); }
 
   // SETTER METHODS
   /// set the x position
@@ -66,13 +71,15 @@ class Node3D {
   /// set the cost-to-come (heuristic value)
   void setH(const float& h) { this->h = h; }
   /// set and get the index of the node in the 3D grid
-  int setIdx(int width, int height) { this->idx = (int)(t / Constants::deltaHeadingRad) * width * height + (int)(y) * width + (int)(x); return idx;}
+  int setIdx(int width, int height) { this->idx = (int)(t / Constants::deltaHeadingRad) * width * height + this->getGridY() * width + this->getGridX(); return idx;}
   /// open the node
   void open() { o = true; c = false;}
   /// close the node
   void close() { c = true; o = false; }
   /// set a pointer to the predecessor of the node
   void setPred(const Node3D* pred) { this->pred = pred; }
+  /// set map meta data
+  static void setMeta(geometry_msgs::Pose p, float r){ mp = p; res = r; }
 
   // UPDATE METHODS
   /// Updates the cost-so-far for the node x' coming from its predecessor. It also discovers the node.
@@ -125,6 +132,10 @@ class Node3D {
   int prim;
   /// the predecessor pointer
   const Node3D* pred;
+  /// map pose
+  static geometry_msgs::Pose mp;
+  /// resolution
+  static float res;
 };
 }
 #endif // NODE3D_H
diff --git a/src/algorithm.cpp b/src/algorithm.cpp
index ea88fb43..19b6be2e 100644
--- a/src/algorithm.cpp
+++ b/src/algorithm.cpp
@@ -432,14 +432,14 @@ void updateH(Node3D& start, const Node3D& goal, Node2D* nodes2D, float* dubinsLo
 
   // if twoD heuristic is activated determine shortest path
   // unconstrained with obstacles
-  if (Constants::twoD && !nodes2D[(int)start.getY() * width + (int)start.getX()].isDiscovered()) {
+  if (Constants::twoD && !nodes2D[start.getGridY() * width + start.getGridX()].isDiscovered()) {
     //    ros::Time t0 = ros::Time::now();
     // create a 2d start node
     Node2D start2d(start.getX(), start.getY(), 0, 0, nullptr);
     // create a 2d goal node
     Node2D goal2d(goal.getX(), goal.getY(), 0, 0, nullptr);
     // run 2d astar and return the cost of the cheapest path for that node
-    nodes2D[(int)start.getY() * width + (int)start.getX()].setG(aStar(goal2d, start2d, nodes2D, width, height, configurationSpace, visualization));
+    nodes2D[start.getGridY() * width + start.getGridX()].setG(aStar(goal2d, start2d, nodes2D, width, height, configurationSpace, visualization));
     //    ros::Time t1 = ros::Time::now();
     //    ros::Duration d(t1 - t0);
     //    std::cout << "calculated 2D Heuristic in ms: " << d * 1000 << std::endl;
@@ -449,7 +449,7 @@ void updateH(Node3D& start, const Node3D& goal, Node2D* nodes2D, float* dubinsLo
     // offset for same node in cell
     twoDoffset = sqrt(((start.getX() - (long)start.getX()) - (goal.getX() - (long)goal.getX())) * ((start.getX() - (long)start.getX()) - (goal.getX() - (long)goal.getX())) +
                       ((start.getY() - (long)start.getY()) - (goal.getY() - (long)goal.getY())) * ((start.getY() - (long)start.getY()) - (goal.getY() - (long)goal.getY())));
-    twoDCost = nodes2D[(int)start.getY() * width + (int)start.getX()].getG() - twoDoffset;
+    twoDCost = nodes2D[start.getGridY() * width + start.getGridX()].getG() - twoDoffset;
 
   }
 
diff --git a/src/node2d.cpp b/src/node2d.cpp
index e12bd829..4f147702 100644
--- a/src/node2d.cpp
+++ b/src/node2d.cpp
@@ -2,6 +2,10 @@
 
 using namespace HybridAStar;
 
+// initialize static data
+geometry_msgs::Pose Node2D::mp;
+float Node2D::res = 1.0;
+
 // possible directions
 const int Node2D::dir = 8;
 // possible movements
@@ -12,7 +16,7 @@ const int Node2D::dy[] = { 0, 1, 1, 1, 0, -1, -1, -1 };
 //                                         IS ON GRID
 //###################################################
 bool Node2D::isOnGrid(const int width, const int height) const {
-  return  x >= 0 && x < width && y >= 0 && y < height;
+  return  (y > mp.position.y && y <= (res * height + mp.position.y)) && (x > mp.position.x && x <= (res * width + mp.position.x));
 }
 
 //###################################################
diff --git a/src/node3d.cpp b/src/node3d.cpp
index ed64f345..48550cf8 100644
--- a/src/node3d.cpp
+++ b/src/node3d.cpp
@@ -2,6 +2,10 @@
 
 using namespace HybridAStar;
 
+//Initialize static map data
+geometry_msgs::Pose Node3D::mp;
+float Node3D::res = 1.0;
+
 // CONSTANT VALUES
 // possible directions
 const int Node3D::dir = 3;
@@ -28,7 +32,7 @@ const float Node3D::dt[] = { 0,         0.1178097,   -0.1178097};
 //                                         IS ON GRID
 //###################################################
 bool Node3D::isOnGrid(const int width, const int height) const {
-  return x >= 0 && x < width && y >= 0 && y < height && (int)(t / Constants::deltaHeadingRad) >= 0 && (int)(t / Constants::deltaHeadingRad) < Constants::headings;
+  return (y > mp.position.y && y <= (res * height + mp.position.y)) && (x > mp.position.x && x <= (res * width + mp.position.x)) && (int)(t / Constants::deltaHeadingRad) >= 0 && (int)(t / Constants::deltaHeadingRad) < Constants::headings;
 }
 
 
@@ -300,3 +304,4 @@ bool Node3D::operator == (const Node3D& rhs) const {
          (std::abs(t - rhs.t) <= Constants::deltaHeadingRad ||
           std::abs(t - rhs.t) >= Constants::deltaHeadingNegRad);
 }
+
diff --git a/src/planner.cpp b/src/planner.cpp
index 906a5c95..2e9fa2de 100644
--- a/src/planner.cpp
+++ b/src/planner.cpp
@@ -52,6 +52,15 @@ void Planner::setMap(const nav_msgs::OccupancyGrid::Ptr map) {
   configurationSpace.updateGrid(map);
   //create array for Voronoi diagram
 //  ros::Time t0 = ros::Time::now();
+
+  geometry_msgs::Pose map_pose;
+  //convert map origin to world
+  map_pose.position.x = map->info.origin.position.x + (map->info.origin.position.x + 0.5) * map->info.resolution;
+  map_pose.position.y = map->info.origin.position.y + (map->info.origin.position.y + 0.5) * map->info.resolution;
+  //set Meta data for Node 2D and 3D
+  Node3D::setMeta(map_pose, map->info.resolution);
+  Node2D::setMeta(map_pose, map->info.resolution);
+
   int height = map->info.height;
   int width = map->info.width;
   bool** binMap;
@@ -110,7 +119,8 @@ void Planner::setStart(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr&
 
   std::cout << "I am seeing a new start x:" << x << " y:" << y << " t:" << Helper::toDeg(t) << std::endl;
 
-  if (grid->info.height >= y && y >= 0 && grid->info.width >= x && x >= 0) {
+  if ((y > grid->info.origin.position.y && y <= ( grid->info.resolution * grid->info.height + grid->info.origin.position.y) ) &&
+          (x > grid->info.origin.position.x && x <= ( grid->info.resolution * grid->info.width + grid->info.origin.position.x) )){
     validStart = true;
     start = *initial;
 
@@ -134,7 +144,8 @@ void Planner::setGoal(const geometry_msgs::PoseStamped::ConstPtr& end) {
 
   std::cout << "I am seeing a new goal x:" << x << " y:" << y << " t:" << Helper::toDeg(t) << std::endl;
 
-  if (grid->info.height >= y && y >= 0 && grid->info.width >= x && x >= 0) {
+  if ((y > grid->info.origin.position.y && y <= ( grid->info.resolution * grid->info.height + grid->info.origin.position.y) ) &&
+      (x > grid->info.origin.position.x && x <= ( grid->info.resolution * grid->info.width + grid->info.origin.position.x) )){
     validGoal = true;
     goal = *end;