switched roi interface

rq_controller/common/region_of_intrest.py

 import numpy as np
 
 from rq_interfaces.msg import RegionOfIntrest
+from visualization_msgs.msg import Marker
 
 
 class PyRegionOfIntrest():
-    def __init__(self, start_point_mm: np.ndarray, end_point_mm: np.ndarray, frame_id: str = 'object'):
-        self.start_point_mm = start_point_mm
-        self.end_point_mm = end_point_mm
+    def __init__(self, center_points_mm: np.ndarray, dimensions_mm: np.ndarray, frame_id: str = 'object', 
+                 resolution_mm: np.ndarray = np.array([0.1, 0.1, 0.1])):
+        self.center_points_mm = center_points_mm
+        self.dimensions_mm = dimensions_mm
         self.frame_id = frame_id
+        self.resolution_mm = resolution_mm
 
     @classmethod
     def dummy(cls):
-        return cls(np.ones(3,) * -1, np.ones(3,))
+        return cls((np.random.random((3, 3)) - 0.5) * 20., 
+                   (np.random.random((3, 3)) - 0.5) * 10.)
 
     @classmethod
     def from_message(cls, msg: RegionOfIntrest):
-        start_point = np.array([msg.start_point_mm.x,
-                                msg.start_point_mm.y,
-                                msg.start_point_mm.z])
+        center_points_mm = list()
+        dimensions_mm = list()
 
-        end_point = np.array([msg.end_point_mm.x,
-                              msg.end_point_mm.y,
-                              msg.end_point_mm.z])
-        frame = msg.header.frame_id
+        for roi in msg.region_of_intrest_stack.markers:
+            roi: Marker
+            center_points_mm.append(
+                np.array([roi.pose.position.x,
+                          roi.pose.position.y,
+                          roi.pose.position.z]))
             
-        return cls(start_point, end_point, frame)
+            dimensions_mm.append(
+                np.array([roi.scale.x,
+                          roi.scale.y,
+                          roi.scale.z]))
+            
+            frame = roi.header.frame_id
+        resolution_mm = np.array([msg.resolution.x,
+                                  msg.resolution.y,
+                                  msg.resolution.z])
+        
+        return cls(np.array(center_points_mm), np.array(dimensions_mm), frame, resolution_mm)
+    
+    @property
+    def number_of_rois(self) -> int:
+        return self.center_points_mm.shape[0]
     
     def as_message(self) -> RegionOfIntrest:
         message = RegionOfIntrest()
 
-        message.start_point_mm.x = self.start_point_mm[0]
-        message.start_point_mm.y = self.start_point_mm[1]
-        message.start_point_mm.z = self.start_point_mm[2]
+        for i in range(self.number_of_rois):
+            roi = Marker()
+
+            roi.pose.position.x = self.center_points_mm[i][0]
+            roi.pose.position.y = self.center_points_mm[i][1]
+            roi.pose.position.z = self.center_points_mm[i][2]
+
+            roi.scale.x = self.dimensions_mm[i][0]
+            roi.scale.y = self.dimensions_mm[i][1]
+            roi.scale.z = self.dimensions_mm[i][2]
+
+            roi.header.frame_id = self.frame_id
         
-        message.start_point_mm.x = self.start_point_mm[0]
-        message.start_point_mm.y = self.start_point_mm[1]
-        message.start_point_mm.z = self.start_point_mm[2]
+        message.region_of_intrest_stack.markers = list(message)
 
-        message.header.frame_id = self.frame_id
+        message.resolution.x = self.resolution_mm[0]
+        message.resolution.x = self.resolution_mm[1]
+        message.resolution.x = self.resolution_mm[2]
 
         return message