Skip to content
Snippets Groups Projects
Select Git revision
  • main default
1 result
Blame Permalink
hardware_client.py 6.07 KiB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147 






from __future__ import annotations

from rq_interfaces.msg import Projection
from rq_interfaces.srv import AquireProjection, ReachabilityCheck, ReachabilityMapCheck

from rq_controller.common import PyProjection, PyRegionOfIntrest, PyVolume

import rclpy
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 
    and check reachability of given projections and regions of interest.
    """

    def __init__(self, **kwargs):
        """
        Initialize the HardwareClient node, create service clients, and wait for services to be available.

        :param kwargs: Additional keyword arguments to pass to the Node initialization.
        """
        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...')
        self.cli_reachability = self.create_client(ReachabilityCheck, 'check_reachability')
        while not self.cli_projection.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('reachability service not available, waiting again...')
        self.cli_reachability_map = self.create_client(ReachabilityMapCheck, 'check_reachability_map')
        while not self.cli_projection.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('reachability MAP service not available, waiting again...')
        
        self.req_projection = AquireProjection.Request()
        self.req_reachability = ReachabilityCheck.Request()
        self.req_reachability_map = ReachabilityMapCheck.Request()

    def aquire_projection(self, projection: PyProjection) -> rclpy.Future:
        """
        Send a request to acquire a projection.

        :param projection: The PyProjection object containing the scan pose.
        :return: A Future object representing the result of the asynchronous service call.
        """
        self.req_projection.scan_pose = projection.as_message()
        return self.cli_projection.call_async(self.req_projection)
    
    def check_reachability(self, projection: PyProjection) -> rclpy.Future:
        """
        Send a request to check the reachability of a given projection.

        :param projection: The PyProjection object containing the scan pose.
        :return: A Future object representing the result of the asynchronous service call.
        """
        self.req_reachability.scan_pose = projection.as_message()
        return self.cli_reachability.call_async(self.req_reachability)
    
    def check_reachability_map(self, source_position_roi_mm: PyRegionOfIntrest, center_mm: np.ndarray, fdd_mm: float) -> rclpy.Future:
        """
        Send a request to check the reachability map for a given region of interest and focal detector distance.

        :param source_position_roi_mm: The PyRegionOfIntrest object containing the region of interest.
        :param center_mm: A numpy array representing the center position in millimeters.
        :param fdd_mm: The focal detector distance in millimeters.
        :return: A Future object representing the result of the asynchronous service call.
        """
        center_mm = center_mm.reshape((3,))
        self.req_reachability_map.center.x = center_mm[0]
        self.req_reachability_map.center.y = center_mm[1]
        self.req_reachability_map.center.z = center_mm[2]
        self.req_reachability_map.region_of_intrest = source_position_roi_mm.as_message()
        self.req_reachability_map.fdd_mm = fdd_mm
        return self.cli_reachability_map.call_async(self.req_reachability_map)
         
    @staticmethod
    def projection_response_2_py(response: AquireProjection.Response) -> PyProjection:
        """
        Convert an AquireProjection response to a PyProjection object.

        :param response: The AquireProjection response message.
        :return: A PyProjection object.
        """
        return PyProjection.from_message(response.projection)
    
    @staticmethod
    def reachability_response_2_py(response: ReachabilityCheck.Response) -> tuple[bool, float, PyProjection]:
        """
        Convert a ReachabilityCheck response to a tuple containing reachability information.

        :param response: The ReachabilityCheck response message.
        :return: A tuple containing (reachable: bool, cost: float, scan_pose: PyProjection).
        """
        return (response.checked_scan_pose.reachable, 
                response.checked_scan_pose.cost, 
                PyProjection.from_message(response.checked_scan_pose.scan_pose))
    
    @staticmethod
    def reachability_map_response_2_py(response: ReachabilityMapCheck.Response) -> PyVolume:
        """
        Convert a ReachabilityMapCheck response to a PyVolume object.

        :param response: The ReachabilityMapCheck response message.
        :return: A PyVolume object.
        """
        return PyVolume.from_message(response.ocupation_map)
    

def main():
    """
    Main function to initialize the ROS 2 node, perform reachability checks, and acquire projections.
    """
    from matplotlib import pyplot as plt

    rclpy.init()

    minimal_client = HardwareClient()
    scan_pose = PyProjection.dummy()

    future = minimal_client.check_reachability(scan_pose)
    rclpy.spin_until_future_complete(minimal_client, future)

    response = future.result()
    reachability_check = minimal_client.reachability_response_2_py(response)
    minimal_client.get_logger().info(
        f'Pose is: {reachability_check[0]} \nMove cost: {reachability_check[1]}')

    future = minimal_client.aquire_projection(scan_pose)
    rclpy.spin_until_future_complete(minimal_client, future)

    response = future.result()
    projection = minimal_client.projection_response_2_py(response)
    minimal_client.get_logger().info(
        f'Received Projection ...')

    minimal_client.destroy_node()
    rclpy.shutdown()

    plt.imshow(projection.image)
    plt.show()


if __name__ == '__main__':
    main()