From be60babec4fa30ad5fac4267d7e46ced58030c9c Mon Sep 17 00:00:00 2001
From: "C. Andy Martin" <charles_martin@jabil.com>
Date: Thu, 21 Dec 2023 13:52:10 -0500
Subject: [PATCH] search: add optional found_depth return by pointer

Add an optional found_depth return by pointer. If the found_depth
pointer is set, the depth the search stopped at will be set to what
found_depth points to. This way the caller can determine if the search
ended at the requested depth if that is important to the caller.
---
 octomap/include/octomap/OcTreeBaseImpl.h   |  9 ++++++---
 octomap/include/octomap/OcTreeBaseImpl.hxx | 21 +++++++++++++++------
 2 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/octomap/include/octomap/OcTreeBaseImpl.h b/octomap/include/octomap/OcTreeBaseImpl.h
index fdb51e6c..00692638 100644
--- a/octomap/include/octomap/OcTreeBaseImpl.h
+++ b/octomap/include/octomap/OcTreeBaseImpl.h
@@ -183,23 +183,26 @@ namespace octomap {
     /** 
      *  Search node at specified depth given a 3d point (depth=0: search full tree depth).
      *  You need to check if the returned node is NULL, since it can be in unknown space.
+     *  If the caller needs to know the found node depth, it can be returned by setting found_depth.
      *  @return pointer to node if found, NULL otherwise
      */
-    NODE* search(double x, double y, double z, unsigned int depth = 0) const;
+    NODE* search(double x, double y, double z, unsigned int depth = 0, unsigned int* found_depth = nullptr) const;
 
     /**
      *  Search node at specified depth given a 3d point (depth=0: search full tree depth)
      *  You need to check if the returned node is NULL, since it can be in unknown space.
+     *  If the caller needs to know the found node depth, it can be returned by setting found_depth.
      *  @return pointer to node if found, NULL otherwise
      */
-    NODE* search(const point3d& value, unsigned int depth = 0) const;
+    NODE* search(const point3d& value, unsigned int depth = 0, unsigned int* found_depth = nullptr) const;
 
     /**
      *  Search a node at specified depth given an addressing key (depth=0: search full tree depth)
      *  You need to check if the returned node is NULL, since it can be in unknown space.
+     *  If the caller needs to know the found node depth, it can be returned by setting found_depth.
      *  @return pointer to node if found, NULL otherwise
      */
-    NODE* search(const OcTreeKey& key, unsigned int depth = 0) const;
+    NODE* search(const OcTreeKey& key, unsigned int depth = 0, unsigned int* found_depth = nullptr) const;
 
     /**
      *  Delete a node (if exists) given a 3d point. Will always
diff --git a/octomap/include/octomap/OcTreeBaseImpl.hxx b/octomap/include/octomap/OcTreeBaseImpl.hxx
index 436e8090..2aa5b54a 100644
--- a/octomap/include/octomap/OcTreeBaseImpl.hxx
+++ b/octomap/include/octomap/OcTreeBaseImpl.hxx
@@ -472,33 +472,33 @@ namespace octomap {
   }
 
   template <class NODE,class I>
-  NODE* OcTreeBaseImpl<NODE,I>::search(const point3d& value, unsigned int depth) const {
+  NODE* OcTreeBaseImpl<NODE,I>::search(const point3d& value, unsigned int depth, unsigned int* found_depth) const {
     OcTreeKey key;
     if (!coordToKeyChecked(value, key)){
       OCTOMAP_ERROR_STR("Error in search: ["<< value <<"] is out of OcTree bounds!");
       return NULL;
     }
     else {
-      return this->search(key, depth);
+      return this->search(key, depth, found_depth);
     }
 
   }
 
   template <class NODE,class I>
-  NODE* OcTreeBaseImpl<NODE,I>::search(double x, double y, double z, unsigned int depth) const {
+  NODE* OcTreeBaseImpl<NODE,I>::search(double x, double y, double z, unsigned int depth, unsigned int* found_depth) const {
     OcTreeKey key;
     if (!coordToKeyChecked(x, y, z, key)){
       OCTOMAP_ERROR_STR("Error in search: ["<< x <<" "<< y << " " << z << "] is out of OcTree bounds!");
       return NULL;
     }
     else {
-      return this->search(key, depth);
+      return this->search(key, depth, found_depth);
     }
   }
 
 
   template <class NODE,class I>
-  NODE* OcTreeBaseImpl<NODE,I>::search (const OcTreeKey& key, unsigned int depth) const {
+  NODE* OcTreeBaseImpl<NODE,I>::search (const OcTreeKey& key, unsigned int depth, unsigned int* found_depth) const {
     assert(depth <= tree_depth);
     if (root == NULL)
       return NULL;
@@ -517,9 +517,10 @@ namespace octomap {
     NODE* curNode (root);
 
     int diff = tree_depth - depth;
+    unsigned int current_depth = 0;
 
     // follow nodes down to requested level (for diff = 0 it's the last level)
-    for (int i=(tree_depth-1); i>=diff; --i) {
+    for (int i=(tree_depth-1); i>=diff; --i, ++current_depth) {
       unsigned int pos = computeChildIdx(key_at_depth, i);
       if (nodeChildExists(curNode, pos)) {
         // cast needed: (nodes need to ensure it's the right pointer)
@@ -528,6 +529,10 @@ namespace octomap {
         // we expected a child but did not get it
         // is the current node a leaf already?
         if (!nodeHasChildren(curNode)) { // TODO similar check to nodeChildExists?
+          if (found_depth) {
+            assert(current_depth < depth);
+            *found_depth = current_depth;
+          }
           return curNode;
         } else {
           // it is not, search failed
@@ -535,6 +540,10 @@ namespace octomap {
         }
       }
     } // end for
+    if (found_depth) {
+      assert(current_depth == depth);
+      *found_depth = current_depth;
+    }
     return curNode;
   }