projection fix

.vscode/c_cpp_properties.json

+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "c:\\dev\\rq_workflow\\install\\include\\**",
+        "C:\\dev\\rq_workflow\\install\\include\\**",
+        "C:\\dev\\ros2_humble\\include\\**"
+      ],
+      "name": "ROS"
+    }
+  ],
+  "version": 4
+}
\ No newline at end of file

.vscode/settings.json

+{
+    "python.autoComplete.extraPaths": [
+        "c:\\dev\\rq_workflow\\install\\Lib\\site-packages",
+        "C:\\dev\\rq_workflow\\install\\Lib\\site-packages",
+        "C:\\ci\\ws\\install\\lib\\python3.8\\dist-packages",
+        "C:\\dev\\ros2_humble\\Lib\\site-packages",
+        ""
+    ],
+    "python.analysis.extraPaths": [
+        "c:\\dev\\rq_workflow\\install\\Lib\\site-packages",
+        "C:\\dev\\rq_workflow\\install\\Lib\\site-packages",
+        "C:\\ci\\ws\\install\\lib\\python3.8\\dist-packages",
+        "C:\\dev\\ros2_humble\\Lib\\site-packages",
+        ""
+    ]
+}
\ No newline at end of file

rq_controller/common/projection.py

+from __future__ import annotations
+
 import numpy as np
 
 from numpy import ndarray
 from .projection_geometry import PyProjectionGeometry
 
 from rq_interfaces.msg import Projection, ProjectionGeometry
+from sensor_msgs.msg import Image
+import ros2_numpy
+
 
 class PyProjection(PyProjectionGeometry):
     def __init__(self, focal_spot_mm: ndarray, detector_postion_mm: ndarray, detector_orientation_quad: ndarray, image: np.ndarray,
-                 detector_heigth_mm: float, detector_width_mm: float, frame_id: str = 'object') -> None:
-        super().__init__(focal_spot_mm, detector_postion_mm, detector_orientation_quad, frame_id)
-        self.image = image
+                 detector_heigth_mm: float, detector_width_mm: float, frame_id: str = 'object', 
+                 focal_spot_orientation_quad: np.ndarray = np.array([0., 0., 0, 1.]),
+                 voltage_kv: float = 100., current_ua: float = 100., exposure_time_ms: float = 1000.) -> None:
+        super().__init__(focal_spot_mm, detector_postion_mm, detector_orientation_quad, frame_id, focal_spot_orientation_quad)
+        self.image = image.astype(np.uint16)
         self.detector_heigth_mm = detector_heigth_mm
         self.detector_width_mm = detector_width_mm
+        self.voltage_kv = voltage_kv
+        self.current_ua = current_ua
+        self.exposure_time_ms = exposure_time_ms
 
     @classmethod
     def dummy(cls):
-        return cls(np.array([1., 0., 0]), 
-                   np.array([-1., 0., 0]),
+        return cls(np.array([0., 100., 0]), 
+                   np.array([0., -100., 0]),
                    np.array([1., 0., 0, 1.]),
                    np.zeros((10, 10)),
                    10., 10.)
@@ -36,12 +46,25 @@ class PyProjection(PyProjectionGeometry):
                                               msg.projection_geometry.detector_orientation_quad.z,
                                               msg.projection_geometry.detector_orientation_quad.w])
         
-        image = msg.image
+        focal_spot_orientation = np.array([msg.projection_geometry.focal_spot_orientation_quad.x,
+                                           msg.projection_geometry.focal_spot_orientation_quad.y,
+                                           msg.projection_geometry.focal_spot_orientation_quad.z,
+                                           msg.projection_geometry.focal_spot_orientation_quad.w])
+        
+        
         detector_heigth_mm = msg.detector_heigth_mm
         detector_width_mm = msg.detector_width_mm
         frame_id = msg.projection_geometry.header.frame_id
 
-        return cls(focal_spot_mm, detector_center_mm, detector_orientation_quad, image, detector_heigth_mm, detector_width_mm, frame_id)
+        image = ros2_numpy.numpify(msg.image)
+
+        voltage_kv = msg.voltage_kv
+        current_ua = msg.current_ua
+        exposure_time_ms = msg.exposure_time_ms
+
+        return cls(focal_spot_mm, detector_center_mm, detector_orientation_quad, image, 
+                   detector_heigth_mm, detector_width_mm, frame_id, focal_spot_orientation,
+                   voltage_kv, current_ua, exposure_time_ms)
     
     def as_message(self) -> ProjectionGeometry:
         message = Projection()
@@ -60,13 +83,23 @@ class PyProjection(PyProjectionGeometry):
         projection_geometry.detector_orientation_quad.z = self.detector_orientation_quad[2]
         projection_geometry.detector_orientation_quad.w = self.detector_orientation_quad[3]
 
+        projection_geometry.focal_spot_orientation_quad.x = self.focal_spot_orientation_quad[0]
+        projection_geometry.focal_spot_orientation_quad.y = self.focal_spot_orientation_quad[1]
+        projection_geometry.focal_spot_orientation_quad.z = self.focal_spot_orientation_quad[2]
+        projection_geometry.focal_spot_orientation_quad.w = self.focal_spot_orientation_quad[3]
+
         message.projection_geometry = projection_geometry
-        message.image = self.image
+        message.image = ros2_numpy.msgify(Image, self.image, 'mono16')
+
         message.detector_heigth_mm = self.detector_heigth_mm
         message.detector_width_mm = self.detector_width_mm
 
         message.projection_geometry.header.frame_id = self.frame_id
 
+        message.voltage_kv = self.voltage_kv
+        message.current_ua = self.current_ua
+        message.exposure_time_ms = self.exposure_time_ms
+
         return message
     
     @property

rq_controller/common/projection_geometry.py

+from __future__ import annotations
+
 import numpy as np
 
-from rq_interfaces.msg import ProjectionGeometry
+from rq_interfaces.msg import ProjectionGeometry, Projection
 
 class PyProjectionGeometry():
-    def __init__(self, focal_spot_mm: np.ndarray, detector_postion_mm: np.ndarray, detector_orientation_quad: np.ndarray,
-                 frame_id: str = 'object') -> None:
+    def __init__(self, focal_spot_mm: np.ndarray, detector_postion_mm: np.ndarray, 
+                 detector_orientation_quad: np.ndarray,
+                 frame_id: str = 'object', focal_spot_orientation_quad: np.ndarray = np.array([0., 0., 0, 1.])
+                 ) -> None:
         self.focal_spot_mm = focal_spot_mm
         self.detector_postion_mm = detector_postion_mm
+        self.focal_spot_orientation_quad = focal_spot_orientation_quad
         self.detector_orientation_quad = detector_orientation_quad
         self.frame_id = frame_id
 
@@ -14,7 +19,7 @@ class PyProjectionGeometry():
     def dummy(cls):
         return cls(np.array([1., 0., 0]), 
                    np.array([-1., 0., 0]),
-                   np.array([1., 0., 0, 1.]))
+                   np.array([0., 0., 0, 1.]))
 
     @classmethod
     def from_message(cls, msg: ProjectionGeometry):
@@ -30,9 +35,15 @@ class PyProjectionGeometry():
                                               msg.detector_orientation_quad.y,
                                               msg.detector_orientation_quad.z,
                                               msg.detector_orientation_quad.w])
+        
+        focal_spot_orientation = np.array([msg.focal_spot_orientation_quad.x,
+                                           msg.focal_spot_orientation_quad.y,
+                                           msg.focal_spot_orientation_quad.z,
+                                           msg.focal_spot_orientation_quad.w])
+        
         frame_id = msg.header.frame_id
 
-        return cls(focal_spot_mm, detector_center_mm, detector_orientation_quad, frame_id)
+        return cls(focal_spot_mm, detector_center_mm, detector_orientation_quad, frame_id, focal_spot_orientation)
     
     def as_message(self) -> ProjectionGeometry:
         message = ProjectionGeometry()
@@ -45,6 +56,11 @@ class PyProjectionGeometry():
         message.detector_postion_mm.y = self.detector_postion_mm[1]
         message.detector_postion_mm.z = self.detector_postion_mm[2]
 
+        message.focal_spot_orientation_quad.x = self.focal_spot_orientation_quad[0]
+        message.focal_spot_orientation_quad.y = self.focal_spot_orientation_quad[1]
+        message.focal_spot_orientation_quad.z = self.focal_spot_orientation_quad[2]
+        message.focal_spot_orientation_quad.w = self.focal_spot_orientation_quad[3]
+
         message.detector_orientation_quad.x = self.detector_orientation_quad[0]
         message.detector_orientation_quad.y = self.detector_orientation_quad[1]
         message.detector_orientation_quad.z = self.detector_orientation_quad[2]