roi nn bug fix

example/volume_example.py

 from rq_controller.common import PyVolume
 from rq_controller.common.io.rq_json import RqJsonWriter, RqJsonLoader
 from pathlib import Path
+import numpy as np
 
 
 FOLDER = Path('./example/data')
@@ -22,6 +23,14 @@ def main():
     volume_3 = loader.load_volume(writer.get_volume_save_path_i(FOLDER, 1))
     print(f'Shape (x / y / z): {volume_3.shape}')
 
+    grid = volume_3.roi.get_grid()
+    print(f'grid shape: {grid.shape}')
+
+    pos = volume_3.roi.next_neighbor(grid, np.array([0., 0., 0.]))
+    print(pos)
+    value = volume_3.array[pos[0], pos[1], pos[2]]
+    print(value)
+
 
 if __name__ == '__main__':
     main()

rq_controller/common/region_of_intrest.py

+from __future__ import annotations
+
 import numpy as np
 
 from rq_interfaces.msg import RegionOfIntrest
@@ -49,8 +51,7 @@ class PyRegionOfIntrest():
     def shape(self) -> tuple:
         shape = self.dimensions_mm[0] // self.resolution_mm[0]
         return (int(shape[0]), int(shape[1]), int(shape[2]))
-
-    
+      
     def as_message(self) -> RegionOfIntrest:
         message = RegionOfIntrest()
         roi_list = list()
@@ -77,3 +78,32 @@ class PyRegionOfIntrest():
         message.resolution.z = float(self.resolution_mm[0][2])
 
         return message
+    
+    def get_grid(self, indice: int = 0) -> np.ndarray:
+        start = self.center_points_mm[indice] - (self.dimensions_mm[indice] / 2.)
+        end = self.center_points_mm[indice] + (self.dimensions_mm[indice] / 2.)
+        
+        x_ = np.linspace(start[0], end[0], self.shape[0])
+        y_ = np.linspace(start[1], end[1], self.shape[1])
+        z_ = np.linspace(start[2], end[2], self.shape[2])
+
+        x, y, z = np.meshgrid(x_, y_, z_, indexing='ij')
+
+        return np.concatenate((
+            np.expand_dims(x, -1),
+            np.expand_dims(y, -1),
+            np.expand_dims(z, -1)),
+            -1)
+    
+    @staticmethod
+    def next_neighbor(grid_mm: np.ndarray, point_mm: np.ndarray) -> np.ndarray:
+        
+        x = grid_mm[:, 0, 0, 0]
+        y = grid_mm[0, :, 0, 1]
+        z = grid_mm[0, 0, :, 2]
+
+        xx = int(np.argmin((x - point_mm[0])**2))
+        yy = int(np.argmin((y - point_mm[1])**2))
+        zz = int(np.argmin((z - point_mm[2])**2))
+
+        return np.array([xx, yy, zz], dtype=np.int32)
\ No newline at end of file