diff --git a/.gitignore b/.gitignore index f39164246fe572816637bece6d14f89740277436..ea231de4a50466cd7ce27ef8a1d7741f8fb8769c 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ -/.vscode/ \ No newline at end of file +/.vscode/ +/temp/ \ No newline at end of file diff --git a/examples/01_try_mesh.py b/examples/01_try_mesh.py index 700b048b58f04dab77b087a4fce5e98acfdd940b..94f90d199ac24d764b624185c65935151d629862 100644 --- a/examples/01_try_mesh.py +++ b/examples/01_try_mesh.py @@ -10,7 +10,7 @@ from rq_hardware.artist.artist_hardware_service import ArtistHardwareInterface class MeshPublisher(Node): def __init__(self): - super().__init__('mesh_publisher') + super().__init__('mesh_publisher', namespace='rq') self.publisher = self.create_publisher(Mesh, 'rq_artist_collision_mesh_mm', 10) def publish_mesh(self, msg): diff --git a/examples/02_try_recahbility_map.py b/examples/02_try_recahbility_map.py index 187d61e41cd77a4569305771dee515c1617d5cd6..eaab9987a4b0afc9497b5d95feec937aca6ec900 100644 --- a/examples/02_try_recahbility_map.py +++ b/examples/02_try_recahbility_map.py @@ -12,7 +12,7 @@ from rq_hardware.hardware_client import HardwareClient rclpy.init() -client = HardwareClient(namespace="rq") +client = HardwareClient() roi = PyRegionOfIntrest.dummy() roi.dimensions_mm = roi.dimensions_mm * 100. roi.resolution_mm = roi.resolution_mm * 10. @@ -26,4 +26,5 @@ volume = client.reachability_map_response_2_py(future.result()) print(volume.shape) -print(volume.array.sum()) \ No newline at end of file +print(volume.array.sum()) + diff --git a/examples/04_aquire_projection.py b/examples/04_aquire_projection.py new file mode 100644 index 0000000000000000000000000000000000000000..3f287b6695da786ba671b3eab3f567b0ad642158 --- /dev/null +++ b/examples/04_aquire_projection.py @@ -0,0 +1,33 @@ + +import rclpy +import numpy as np + +from rq_controller.common import PyProjection +from rq_hardware.hardware_client import HardwareClient + +import matplotlib.pyplot as plt + +rclpy.init() + +hardware_client = HardwareClient() + +proj = PyProjection.dummy() +proj.focal_spot_mm = np.array([0., 0., 1000.]) +proj.detector_postion_mm = np.array([0., 0., -1000.]) +proj.detector_orientation_quad = np.array([0., 0., 0., 1.]) + + +proj.voltage_kv = 100. +proj.current_ua = 40. + + +future = hardware_client.aquire_projection(proj) + +rclpy.spin_until_future_complete(hardware_client, future) + +proj_response = hardware_client.projection_response_2_py(future.result()) + +plt.imshow(proj_response.image) +plt.show() + + diff --git a/rq_hardware/base_hardware_service.py b/rq_hardware/base_hardware_service.py index e968462994feae320f03f56f3aff315579ffc0e2..4c12e08433d79432d418c94cf182eaa5d1708a7a 100644 --- a/rq_hardware/base_hardware_service.py +++ b/rq_hardware/base_hardware_service.py @@ -12,10 +12,10 @@ class BaseHardwareInterfaceService(Node): interface_type: str = 'Base' last_projection: PyProjection = None measurement_name: str = 'projection' - projection_tempfolder: Path = Path(r'C:\ros\ros\humble_embedded\rq_workflow\src\rq_hardware\temp') + projection_tempfolder: Path = Path(r'C:\dev\rq_workflow\src\rq_hardware\temp') def __init__(self, node_name: str = 'rq_base_hardware_interface_service'): - super().__init__(node_name) + 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) diff --git a/rq_hardware/hardware_client.py b/rq_hardware/hardware_client.py index e16e8b67ee1d59f9f3d0dac51248b2a2ba3a8172..aafadba0c788a0b489d3efe474f1aed8b13a3da5 100644 --- a/rq_hardware/hardware_client.py +++ b/rq_hardware/hardware_client.py @@ -13,7 +13,7 @@ import numpy as np class HardwareClient(Node): def __init__(self, **kwargs): - super().__init__('rq_hardware_client', **kwargs) + super().__init__('rq_hardware_client', namespace="rq", **kwargs) self.cli_projection = self.create_client(AquireProjection, 'aquire_projection') while not self.cli_projection.wait_for_service(timeout_sec=1.0): self.get_logger().info('projection service not available, waiting again...') diff --git a/rq_hardware/thd_roboct/thd_roboct_service.py b/rq_hardware/thd_roboct/thd_roboct_service.py index 94e20b7b7d79549d1dd80f8dccb14d5c9dfcb2b5..da6c06d91bab7d7444e1636d95fe3c8d55c4e75e 100644 --- a/rq_hardware/thd_roboct/thd_roboct_service.py +++ b/rq_hardware/thd_roboct/thd_roboct_service.py @@ -33,7 +33,8 @@ try: from trajectory_toolbox.util.util_poses import Frame, ScanPoseList, ScanPoseTuple # type: ignore except ModuleNotFoundError: - raise ModuleNotFoundError("Packages are spedific for the RoboCT of the Deggendorf Institute of Technology. Please Contact: simon.wittl@th-deg.de") + raise ModuleNotFoundError("Packages are spedific for the RoboCT of the Deggendorf Institute of Technology. \n" + + "Please Contact: simon.wittl@th-deg.de") class TurntablePlannerParameter: @@ -44,7 +45,7 @@ class TurntablePlannerParameter: class ThdRoboCTHardwareInterface(BaseHardwareInterfaceService): interface_type: str = 'THD' - def __init__(self, node_name: str = 'rq_hardware_interface_service'): + def __init__(self, node_name: str = 'rq_thd_hardware_interface_service'): super().__init__(node_name)