added tigre example

README.md

@@ -5,7 +5,7 @@
 Main controller of the Robo Quality project. This should controll all the services via the clients of the sumodules.
 Also all common conversions from ROS2 interfaces to Python and Disk are included.
 
-# Example Workflow
+# Example: Workflow
 
 In this example all service nodes are started as `echo´s` (dummies). The workflow class connects to the standard services and calls them. The main idea is to replaces the service calls with "real" implemenataions / functionalities but keep the client calls for the workflow the same to keep testing and integrating easy independet of the systems. 
 
@@ -28,7 +28,6 @@ The following script can be found [here](https://mygit.th-deg.de/roboct/robo_qua
 ```powershell
 C:\dev\ros2_humble\setup.ps1
 C:\dev\rq_workflow\install\setup.ps1
-ros2 launch C:\dev\rq_workflow\launch\echo_launch.py
 ```
 Run script / file from the second console:
 
@@ -62,3 +61,48 @@ projection_list.append(workflow.aquire_projection(next_scan_pose))
 volume = workflow.get_volume(projection_list, roi[0])
 ```
 
+# Example: Reconstruction
+
+All individual nodes can be used  alone and must not be used as the workflow is intended. In the following example the service node are started in the following configuration:
+
+| Service Node | Mode |
+| --- | --- |
+| `rq_reonstruction` | tigre_service |
+| `rq_hardware` | artist_service |
+| `rq_trajectory` | echo_service |
+| `rq_ddetection` | echo_service |
+
+Therefore the following launch script is used:
+```powershell
+C:\dev\ros2_humble\setup.ps1
+C:\dev\rq_workflow\install\setup.ps1
+ros2 launch C:\dev\rq_workflow\launch\tigre_artist_launch.py
+```
+
+With the following minimal script projcetion get simulated and reconstructed:
+
+```python
+from rq_controller.rq_workflow import WorkflowNode
+
+workflow = WorkflowNode()
+projection_stack = list()
+
+projection = PyProjection.dummy()
+projection.voltage_kv = 200.
+projection.current_ua = 10.
+projection.detector_heigth_mm = 200.
+projection.detector_width_mm = 200.
+
+source = np.array([FOD_MM, 0, 0])
+detector = np.array([FOD_MM - FDD_MM, 0, 0])
+angles = np.linspace(-np.pi, np.pi, NUMBER_OF_PROJECTION, endpoint=False)
+
+for i in range(NUMBER_OF_PROJECTION):
+    rotation = Rotation.from_euler('Z', angles[i], False)
+    scan_pose = projection.look_at(rotation.apply(source), rotation.apply(detector), np.array([0, 0, -1]))
+    projection_stack.append(workflow.aquire_projection(scan_pose))
+
+
+roi = PyRegionOfIntrest(np.zeros((1, 3)), np.ones((1, 3)) * 120., resolution_mm=np.array([0.2, 0.2, 0.2]))
+volume = workflow.get_volume(projection_stack, roi)
+```
\ No newline at end of file

example/tigre_example.py

+from rq_controller.rq_workflow import WorkflowNode
+from rq_controller.common import PyProjection, PyRegionOfIntrest
+
+import rclpy
+
+from scipy.spatial.transform import Rotation
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+# !!!
+# This script assumes that the service nodes a started with the rq_wokflow/launch/tigre_artist_launch.py file.
+# !!!
+
+NUMBER_OF_PROJECTION = 100
+FOD_MM = 1000.
+FDD_MM = 2000.
+
+def main():
+    # Initialize workflow node
+    workflow = WorkflowNode()
+
+    # Setup projection information
+    projection_stack: list[PyProjection] = list()
+    projection = PyProjection.dummy()
+    projection.image = np.zeros((1000, 1000), dtype=np.uint16)
+    projection.voltage_kv = 200.
+    projection.current_ua = 10.
+    projection.detector_heigth_mm = 200.
+    projection.detector_width_mm = 200.
+
+    # create the source and detector positions
+    source = np.array([FOD_MM, 0, 0])
+    detector = np.array([FOD_MM - FDD_MM, 0, 0])
+    angles = np.linspace(-np.pi, np.pi, NUMBER_OF_PROJECTION, endpoint=False)
+
+    # Move source / dtector and aquire projections
+    for i in range(NUMBER_OF_PROJECTION):
+        rotation = Rotation.from_euler('Z', angles[i], False)
+        scan_pose = projection.look_at(rotation.apply(source), rotation.apply(detector), np.array([0, 0, -1]))
+        projection_stack.append(workflow.aquire_projection(scan_pose))
+
+    # Define reconstruction area and call reconstruction client
+    roi = PyRegionOfIntrest(np.zeros((1, 3)), np.ones((1, 3)) * 120., resolution_mm=np.array([0.5, 0.5, 0.5]))
+    volume = workflow.get_volume(projection_stack, roi)
+
+    # visualize middle slice
+    plt.imshow(volume.array[:, volume.shape[1] // 2, :])
+    plt.show()
+
+    
+if __name__ == '__main__':
+    rclpy.init()
+    main()
\ No newline at end of file

rq_controller/common/projection.py

@@ -8,6 +8,7 @@ from .projection_geometry import PyProjectionGeometry
 from rq_interfaces.msg import Projection, ProjectionGeometry
 from sensor_msgs.msg import Image
 import ros2_numpy
+from scipy.spatial.transform import Rotation
 
 
 class PyProjection(PyProjectionGeometry):
@@ -67,6 +68,61 @@ class PyProjection(PyProjectionGeometry):
                    np.zeros((10, 10)),
                    10., 10.)
     
+    @classmethod
+    def from_look_at(cls, focal_spot_mm: ndarray, detector_postion_mm: ndarray, image_shape: tuple[int],
+                          detector_heigth_mm: float, detector_width_mm: float, frame_id: str = 'object', 
+                          voltage_kv: float = 100., current_ua: float = 100., exposure_time_ms: float = 1000.,
+                          up_vector: np.ndarray = np.array([0., 0., 1.])) -> 'PyProjection':
+        image = np.zeros(image_shape, np.uint16)
+        
+        vector = focal_spot_mm - detector_postion_mm
+        vector = vector / np.linalg.norm(vector)
+        up_vector = up_vector / np.linalg.norm(up_vector)
+
+        detector_orientation = np.eye(3)
+        detector_orientation[:, 2] = vector
+        detector_orientation[:, 1] = np.cross(up_vector, vector)
+        detector_orientation[:, 0] = np.cross(detector_orientation[:, 1], detector_orientation[:, 2])
+
+        source_orientation = np.eye(3)
+        source_orientation[:, 2] = -vector
+        source_orientation[:, 1] = np.cross(up_vector, -vector)
+        source_orientation[:, 0] = np.cross(source_orientation[:, 1], source_orientation[:, 2])
+
+        return cls(
+            focal_spot_mm, detector_postion_mm,
+            Rotation.from_matrix(detector_orientation).as_quat(),
+            image, detector_heigth_mm, detector_width_mm,
+            frame_id, Rotation.from_matrix(source_orientation).as_quat(),
+            voltage_kv, current_ua, exposure_time_ms)
+    
+    def look_at(self, focal_spot_mm: ndarray, detector_postion_mm: ndarray, up_vector: np.ndarray = np.array([0., 0., 1.])) -> PyProjection:
+        vector = focal_spot_mm - detector_postion_mm
+        vector = vector / np.linalg.norm(vector)
+        up_vector = up_vector / np.linalg.norm(up_vector)
+
+        z_rot = Rotation.from_euler('Z', -90, True)
+
+        detector_orientation = np.eye(3)
+        detector_orientation[:, 2] = vector
+        detector_orientation[:, 1] = np.cross(up_vector, vector)
+        detector_orientation[:, 0] = np.cross(detector_orientation[:, 1], detector_orientation[:, 2])
+
+        detector_orientation_quad = (Rotation.from_matrix(detector_orientation) * z_rot).as_quat()
+
+        source_orientation = np.eye(3)
+        source_orientation[:, 2] = -vector
+        source_orientation[:, 1] = np.cross(up_vector, -vector)
+        source_orientation[:, 0] = np.cross(source_orientation[:, 1], source_orientation[:, 2])
+        source_orientation_quad = (Rotation.from_matrix(source_orientation) * z_rot).as_quat()
+        
+        return PyProjection(
+            focal_spot_mm, detector_postion_mm, detector_orientation_quad, self.image,
+            self.detector_heigth_mm, self.detector_width_mm,
+            self.frame_id, source_orientation_quad, self.voltage_kv, 
+            self.current_ua, self.exposure_time_ms)
+
+    
     def __str__(self) -> str:
         """
         Returns a string representation of the PyProjection instance.
@@ -138,23 +194,28 @@ class PyProjection(PyProjectionGeometry):
         message = Projection()
         projection_geometry = ProjectionGeometry()
 
-        projection_geometry.focal_spot_postion_mm.x = self.focal_spot_mm[0]
-        projection_geometry.focal_spot_postion_mm.y = self.focal_spot_mm[1]
-        projection_geometry.focal_spot_postion_mm.z = self.focal_spot_mm[2]
+        self.focal_spot_mm = self.focal_spot_mm.reshape((3,))
+        self.detector_postion_mm = self.detector_postion_mm.reshape((3,))
+        self.detector_orientation_quad = self.detector_orientation_quad.reshape((4,))
+        self.focal_spot_orientation_quad = self.focal_spot_orientation_quad.reshape((4,))
+
+        projection_geometry.focal_spot_postion_mm.x = float(self.focal_spot_mm[0])
+        projection_geometry.focal_spot_postion_mm.y = float(self.focal_spot_mm[1])
+        projection_geometry.focal_spot_postion_mm.z = float(self.focal_spot_mm[2])
 
-        projection_geometry.detector_postion_mm.x = self.detector_postion_mm[0]
-        projection_geometry.detector_postion_mm.y = self.detector_postion_mm[1]
-        projection_geometry.detector_postion_mm.z = self.detector_postion_mm[2]
+        projection_geometry.detector_postion_mm.x = float(self.detector_postion_mm[0])
+        projection_geometry.detector_postion_mm.y = float(self.detector_postion_mm[1])
+        projection_geometry.detector_postion_mm.z = float(self.detector_postion_mm[2])
 
-        projection_geometry.detector_orientation_quad.x = self.detector_orientation_quad[0]
-        projection_geometry.detector_orientation_quad.y = self.detector_orientation_quad[1]
-        projection_geometry.detector_orientation_quad.z = self.detector_orientation_quad[2]
-        projection_geometry.detector_orientation_quad.w = self.detector_orientation_quad[3]
+        projection_geometry.detector_orientation_quad.x = float(self.detector_orientation_quad[0])
+        projection_geometry.detector_orientation_quad.y = float(self.detector_orientation_quad[1])
+        projection_geometry.detector_orientation_quad.z = float(self.detector_orientation_quad[2])
+        projection_geometry.detector_orientation_quad.w = float(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]
+        projection_geometry.focal_spot_orientation_quad.x = float(self.focal_spot_orientation_quad[0])
+        projection_geometry.focal_spot_orientation_quad.y = float(self.focal_spot_orientation_quad[1])
+        projection_geometry.focal_spot_orientation_quad.z = float(self.focal_spot_orientation_quad[2])
+        projection_geometry.focal_spot_orientation_quad.w = float(self.focal_spot_orientation_quad[3])
 
         message.projection_geometry = projection_geometry
         message.image = ros2_numpy.msgify(Image, self.image, 'mono16')
@@ -192,3 +253,5 @@ class PyProjection(PyProjectionGeometry):
     @property
     def pixel_pitch_y_mm(self) -> float:
         return self.detector_heigth_mm / self.detector_heigth_px
+    
+