int limit_ = 5000;
  int i = 0;
  int found_ = 0;

  // get the volume of the explored  space
  Eigen::Vector3d volumeMin, volumeMax;

  // OCTOMAP will provide bounding box in the MAP frame
  getMapBounds(&volumeMin, &volumeMax);

  std::cout << "VOL MIN " << volumeMin[0] << " , " << volumeMin[1] << " , " << volumeMin[2] << std::endl;

  std::cout << "VOL Max " << volumeMax[0] << " , " << volumeMax[1] << " , " << volumeMax[2] << std::endl;

  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator (seed);
  std::uniform_real_distribution<float> dis0 (volumeMin[0], volumeMax[0]);
  std::uniform_real_distribution<float> dis1(volumeMin[1], volumeMax[1]);
  std::uniform_real_distribution<float> dis2(volumeMin[2], volumeMax[2]);


  // iterate untilgoal reached or thershold reached
  while ((found_ == 0) && (i < limit_)){

      float x = dis0(generator);
      float y = dis1(generator);
      float z = dis2(generator);

      std::cout << "random point : " << x << " ," << y << " , " << z << std::endl;

      found_ = tree_->iterate(x, y, z);
      i++;
  }

  if (i >= limit_)
      ROS_INFO("NO SOLUTION FOUND");
  else
      tree_->smoothPath();

ROS_INFO("ITERATE CALL 1");

// In this function a new configuration is sampled and added to the tree.
StateVec newState;

newState[0] = x;
newState[1] = y;
newState[2] = z;

// Find nearest neighbour
kdres * nearest = kd_nearest3(kdTree_, newState.x(), newState.y(), newState.z());

if (kd_res_size(nearest) <= 0) {
    kd_res_free(nearest);
    return 0;
}

// create a new node and fill x,y,z value
Node<StateVec> * newParent = (volumetric_mapping::Node<StateVec> *) kd_res_item_data(nearest);

// Check for collision of new connection plus some overshoot distance.
Eigen::Vector3d origin(newParent->state_[0], newParent->state_[1], newParent->state_[2]);

std::cout << "Origin (Parent) : " << origin[0] << " , " << origin[1] << " , " << origin[2] << std::endl;

Eigen::Vector3d direction(newState[0] - origin[0], newState[1] - origin[1], newState[2] - origin[2]);

//stepping
if (direction.norm() > params_.extensionRange_) {
    direction = params_.extensionRange_ * direction.normalized();
}

newState[0] = origin[0] + direction[0];
newState[1] = origin[1] + direction[1];
newState[2] = origin[2] + direction[2];

// add the random point to the marker - reference frame map
geometry_msgs::Point p;
p.x = newState[0];
p.y = newState[1];
p.z = newState[2];
marker.points.push_back(p);
nh_.publish(marker);

// check collision
// bounding box represents the box in which the drone is contained - parameter to set! information available into resource/area_exploration.yaml
// see how to combine them with octomapManager and so on
if (volumetric_mapping::OctomapManager::CellStatus::kFree
        == manager_->getLineStatusBoundingBox(origin,
                direction + origin /*+ direction.normalized() * params_.dOvershoot*/,
                params_.boundingBox_)) {

    std::cout << " NO COLLISION" << std::endl;

    // Sample the new orientation
    newState[3] = 2.0 * M_PI * (((double) rand()) / ((double) RAND_MAX) - 0.5);

    // Create new node and insert into tree
    Node<StateVec> * newNode = new volumetric_mapping::Node<StateVec>;
    newNode->state_ = newState;
    newNode->parent_ = newParent;
    newNode->distance_ = newParent->distance_ + direction.norm();
    newParent->children_.push_back(newNode);

    // insert the new node
    kd_insert3(kdTree_, newState.x(), newState.y(), newState.z(), newNode);

    std::cout << "Insert new node : " << newState.x() << " , " << newState.y() << " , " << newState.z() << std::endl;

    // Display new node
    publishNode(newNode);

    ROS_INFO("NEW NODE!");

    counter_++;

    Eigen::Vector3d origin2_(newState[0], newState[1], newState[2]);
    Eigen::Vector3d goal_(goalNode_->state_[0], goalNode_->state_[1], goalNode_->state_[2]);

    // at this stage, we check if there is a straight line between this node and the goal which is free of obstacle
    if (volumetric_mapping::OctomapManager::CellStatus::kFree
            == manager_->getLineStatus(origin2_, goal_)) {
        // insert goal node in the tree
        newNode->children_.push_back(goalNode_);
        goalNode_->parent_ = newNode;

        // if so setup goalIsReached
        goalReached_ = 1;
        // Display new node (not sure here it is really useful)
        publishNode(goalNode_);

        ROS_INFO("-----------------------GOAL REACHED!--------------------------");
    }

}

if (goalReached_)
    showPath(goalNode_);

return goalReached_;