diff --git a/README.md b/README.md
index 24857293d96a2aa2d112dff63ef97059a188ba4b..c5105cc5cee88fbf4e6ecdfede9f63b04be96548 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,4 @@
-
+
# RoboQualityHardware
diff --git a/rq_hardware/artist/artist_hardware_service.py b/rq_hardware/artist/artist_hardware_service.py
index faa6eaff1d09522dd2b054c111aad6c2caf19cac..5e81b6cf4e2922c6b8061f2c2d779f6431f35c6c 100644
--- a/rq_hardware/artist/artist_hardware_service.py
+++ b/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,
diff --git a/rq_hardware/base_hardware_service.py b/rq_hardware/base_hardware_service.py
index a1a12ad085934d8bbc0ec960ed0b019b860b31f4..9ae848308f68e41b34b14814e0c2dd2eb5bbd2f8 100644
--- a/rq_hardware/base_hardware_service.py
+++ b/rq_hardware/base_hardware_service.py
@@ -1,12 +1,15 @@
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)
+
+
diff --git a/rq_hardware/hardware_client.py b/rq_hardware/hardware_client.py
index 4a9ca57c741c97cc6ee93eb60cfff193263bab62..d9bcb63b565530373e2aafa7db5c19a53a6d500f 100644
--- a/rq_hardware/hardware_client.py
+++ b/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