private void SetDimensions(uint w, uint h, float res, Messages.geometry_msgs.Point position, Messages.geometry_msgs.Quaternion orientation)
 {
     pwidth  = w;
     pheight = h;
     width   = w * res;
     height  = h * res;
     mapPos  = new Vector3((float)position.x + (width / 2f), (float)position.y + (height / 2f), (float)position.z);
     //mapRot = new Quaternion((float) orientation.x, (float) orientation.y, (float) orientation.z, (float) orientation.w);
     mapScale = new Vector3(height, width, 1f);
 }
 public void SetGoal(Point goal)
 {
     xController.SetTarget(goal.x);
     yController.SetTarget(goal.y);
     zController.SetTarget(goal.z);
 }