Merge branch 'dev' into 'main'

added tf for artist

See merge request roboct/robo_quality/rq_hardware!2

README.md

-![test](https://mygit.th-deg.de/roboct/robo_quality/gitlab-profile/-/raw/main/resources/rq_hardware.svg?inline=false)
+![test](https://mygit.th-deg.de/roboct/robo_quality/gitlab-profile/-/raw/main/resources/rq_hardware_450.png?inline=false)
 
 # RoboQualityHardware
 

rq_hardware/artist/artist_hardware_service.py

@@ -96,9 +96,12 @@ class ArtistHardwareInterface(BaseHardwareInterfaceService):
         response.projection = request.scan_pose
         response.projection.image = ros2_numpy.msgify(Image, projection_image.astype(np.uint16), 'mono16')
 
-        self.projection_publisher.publish(response.projection)
+        self.projection_publisher.publish(ros2_numpy.msgify(Image, projection_image.astype(np.uint16), 'mono16'))
         self.projection_geometry_publisher.publish(response.projection.projection_geometry)
 
+        self.set_source_pose(projection_geometry.focal_spot_mm / 1000., projection_geometry.focal_spot_orientation_quad)
+        self.set_detector_pose(projection_geometry.detector_postion_mm / 1000., projection_geometry.detector_orientation_quad)
+
         return response
     
     def check_reachability_callback(self,

rq_hardware/base_hardware_service.py

 from rq_interfaces.msg import Projection, ProjectionGeometry, CheckedScanPose
 from rq_interfaces.srv import AquireProjection, ReachabilityCheck, ReachabilityMapCheck
 from sensor_msgs.msg import Image
-from std_msgs.msg import Float64
-from rq_controller.common import PyProjection
+from rq_controller.tf2.static_broadcaster import StaticFramePublisher
 
-import rclpy
 from rclpy.node import Node
 from pathlib import Path
+from tf2_ros import TransformBroadcaster
+
+import numpy as np
+from geometry_msgs.msg import TransformStamped
+import ros2_numpy
 
 
 class BaseHardwareInterfaceService(Node):
@@ -20,7 +23,7 @@ class BaseHardwareInterfaceService(Node):
         projection_tempfolder (Path): The temporary folder for storing projections.
     """
     interface_type: str = 'Base'
-    last_projection: PyProjection = None
+    last_projection_image: np.ndarray = None
     measurement_name: str = 'projection'
     projection_tempfolder: Path = Path(r'C:\dev\rq_workflow\src\rq_hardware\temp')
     
@@ -31,15 +34,17 @@ class BaseHardwareInterfaceService(Node):
         :param node_name: The name of the node. Defaults to 'rq_base_hardware_interface_service'.
         """
         super().__init__(node_name, namespace="rq")
+
         self.aquire_projection_srv = self.create_service(AquireProjection, 'aquire_projection', self.aquire_projection_callback)
         self.reachability_srv = self.create_service(ReachabilityCheck, 'check_reachability', self.check_reachability_callback)
         self.reachability_map_srv = self.create_service(ReachabilityMapCheck, 'check_reachability_map', self.check_reachability_map_callback)
 
-        self.projection_publisher = self.create_publisher(Projection, 'current_projection_image', 5)
+        self.projection_publisher = self.create_publisher(Image, 'current_projection_image', 5)
         self.projection_geometry_publisher = self.create_publisher(ProjectionGeometry, 'current_projection_geometry', 5)
         self.reachabilty_publisher = self.create_publisher(CheckedScanPose, 'checked_scan_pose', 5)
 
-        self.projection_subscriper = self.create_subscription(Projection, 'current_projection_image', self.projection_callback, 5)
+        self.projection_subscriper = self.create_subscription(Image, 'current_projection_image', self.projection_callback, 5)
+        self.transform_broadcaster = TransformBroadcaster(self)
 
     def aquire_projection_callback(self,
                                    request: AquireProjection.Request,
@@ -84,4 +89,56 @@ class BaseHardwareInterfaceService(Node):
         :param msg: The received Projection message.
         """
         self.get_logger().info('got projection ...')
-        self.last_projection = PyProjection.from_message(msg)
+        self.last_projection_image = ros2_numpy.numpify(msg)
+
+    def create_tf2_source_broadcaster(self):
+        return StaticFramePublisher(
+            'rq_focal_spot',
+            np.array([1000., 0., 0.]),
+            np.array([0., 0., 0., 1.]),
+            'rq_source_broadcaster',
+            'object')
+
+    def create_tf2_detector_broadcaster(self):
+        return StaticFramePublisher(
+            'rq_detector',
+            np.array([-1000., 0., 0.]),
+            np.array([0., 0., 0., 1.]),
+            'rq_detector_broadcaster',
+            'object')
+    
+    def set_source_pose(self, position, quarternion):
+        t = TransformStamped()
+        t.header.stamp = self.get_clock().now().to_msg()
+        t.header.frame_id = 'object'
+        t.child_frame_id = 'rq_focal_spot'
+
+        t.transform.translation.x = position[0]
+        t.transform.translation.y = position[1]
+        t.transform.translation.z = position[2]
+
+        t.transform.rotation.x = quarternion[0]
+        t.transform.rotation.y = quarternion[1]
+        t.transform.rotation.z = quarternion[2]
+        t.transform.rotation.w = quarternion[3]
+
+        self.transform_broadcaster.sendTransform(t)
+
+    def set_detector_pose(self, position, quarternion):
+        t = TransformStamped()
+        t.header.stamp = self.get_clock().now().to_msg()
+        t.header.frame_id = 'object'
+        t.child_frame_id = 'rq_detector'
+
+        t.transform.translation.x = position[0]
+        t.transform.translation.y = position[1]
+        t.transform.translation.z = position[2]
+
+        t.transform.rotation.x = quarternion[0]
+        t.transform.rotation.y = quarternion[1]
+        t.transform.rotation.z = quarternion[2]
+        t.transform.rotation.w = quarternion[3]
+
+        self.transform_broadcaster.sendTransform(t)
+
+        

rq_hardware/hardware_client.py

@@ -10,6 +10,7 @@ from rclpy.node import Node
 import numpy as np
 
 
+
 class HardwareClient(Node):
     """
     HardwareClient is a ROS 2 Node that interacts with several services to acquire projections