Newer
Older
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
import rclpy
from rclpy.node import Node
from shape_msgs.msg import Mesh, MeshTriangle
from geometry_msgs.msg import Point
import numpy as np
import open3d as o3d
from rq_controller.common import PyProjection
from rq_hardware.hardware_client import HardwareClient
rclpy.init()
client = HardwareClient()
projection = PyProjection.dummy()
projection.focal_spot_mm = 1000. * (np.random.random(3) - 0.5)
projection.detector_postion_mm = 1000. * (np.random.random(3) - 0.5)
future = client.check_reachability(projection)
rclpy.spin_until_future_complete(client, future)
reachable, cost, projetion_2 = client.reachability_response_2_py(future.result())
print(reachable, cost)
future = client.aquire_projection(projetion_2)
rclpy.spin_until_future_complete(client, future)
projection_simu = client.projection_response_2_py(future.result())
projection.focal_spot_mm = 100. * (np.random.random(3) - 0.5)
future = client.check_reachability(projection)
rclpy.spin_until_future_complete(client, future)
reachable, cost, projetion_2 = client.reachability_response_2_py(future.result())
print(reachable, cost)