Skip to content
Snippets Groups Projects
Commit 3dabc6df authored by Simon Wittl's avatar Simon Wittl
Browse files

Merge branch 'dev' into 'main'

added tf for artist

See merge request roboct/robo_quality/rq_hardware!2
parents 0b585ba3 7c1b48ce
No related branches found
No related tags found
No related merge requests found
![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
......
......@@ -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,
......
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)
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment