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 numpy as np
from rq_interfaces.msg import RegionOfIntrest
class PyRegionOfIntrest():
def __init__(self, start_point_mm: np.ndarray, end_point_mm: np.ndarray, frame_id: str = 'object'):
self.start_point_mm = start_point_mm
self.end_point_mm = end_point_mm
self.frame_id = frame_id
@classmethod
def dummy(cls):
return cls(np.ones(3,) * -1, np.ones(3,))
@classmethod
def from_message(cls, msg: RegionOfIntrest):
start_point = np.array([msg.start_point_mm.x,
msg.start_point_mm.y,
msg.start_point_mm.z])
end_point = np.array([msg.end_point_mm.x,
msg.end_point_mm.y,
msg.end_point_mm.z])
frame = msg.header.frame_id
return cls(start_point, end_point, frame)
def as_message(self) -> RegionOfIntrest:
message = RegionOfIntrest()
message.start_point_mm.x = self.start_point_mm[0]
message.start_point_mm.y = self.start_point_mm[1]
message.start_point_mm.z = self.start_point_mm[2]
message.start_point_mm.x = self.start_point_mm[0]
message.start_point_mm.y = self.start_point_mm[1]
message.start_point_mm.z = self.start_point_mm[2]
message.header.frame_id = self.frame_id
return message