Skip to content

Commit 4b2c633

Browse files
YadunundYadunund
authored
Properly configure default publishers and fix photoneo pub (opencv#13)
* Properly configure default publishers and fix photoneo pub Signed-off-by: Yadunund <[email protected]> * Style Signed-off-by: Yadunund <[email protected]> --------- Signed-off-by: Yadunund <[email protected]>
1 parent 50ec787 commit 4b2c633

File tree

2 files changed

+251
-13
lines changed

2 files changed

+251
-13
lines changed

‎ibpc_tester/ibpc_tester/ibpc_tester.py‎

Lines changed: 57 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,39 @@ def pose_mat_to_ros(rot: np.ndarray, trans: np.ndarray):
5050
return msg
5151

5252

53+
class DebugPublishers:
54+
def __init__(self, camera_name: str, node: Node):
55+
# Debug parameters.
56+
qos_profile = QoSProfile(
57+
depth=10, # Queue size (adjust as needed)
58+
durability=DurabilityPolicy.TRANSIENT_LOCAL,
59+
reliability=ReliabilityPolicy.RELIABLE, # or RELIABLE, depending on your needs
60+
)
61+
self.rgb_pub = node.create_publisher(
62+
Image, f"/debug/tester/{camera_name}/rgb", qos_profile
63+
)
64+
self.depth_pub = node.create_publisher(
65+
Image, f"/debug/tester/{camera_name}/depth", qos_profile
66+
)
67+
self.aolp_pub = None
68+
self.dolp_pub = None
69+
if camera_name != "photoneo":
70+
self.aolp_pub = node.create_publisher(
71+
Image, f"/debug/tester/{camera_name}/aolp", qos_profile
72+
)
73+
self.dolp_pub = node.create_publisher(
74+
Image, f"/debug/tester/{camera_name}/dolp", qos_profile
75+
)
76+
77+
def publish(self, rgb: Image, depth: Image, aolp: Image = None, dolp: Image = None):
78+
self.rgb_pub.publish(rgb)
79+
self.depth_pub.publish(depth)
80+
if aolp is not None and self.aolp_pub is not None:
81+
self.aolp_pub.publish(aolp)
82+
if dolp is not None and self.dolp_pub is not None:
83+
self.dolp_pub.publish(dolp)
84+
85+
5386
class BOPCamera:
5487
def __init__(self, path, camera_name, img_id):
5588
self._load_images(path, camera_name, img_id)
@@ -70,27 +103,31 @@ def _load_images(self, path, camera_name, img_id):
70103
self.dolp = None
71104
self.br = CvBridge()
72105

73-
def to_camera_msg(self, node, debug_pub) -> CameraMsg:
106+
def to_camera_msg(self, node, debug_pubs: DebugPublishers) -> CameraMsg:
74107
msg = CameraMsg()
75108
msg.info.header.frame_id = self.camera_name
76109
# msg.info.header.stamp = node.get_clock().now()
77110
msg.info.k = self.K.reshape(-1)
78111
msg.pose = pose_mat_to_ros(self.R, self.t)
79112
msg.rgb = self.br.cv2_to_imgmsg(self.rgb, "8UC1")
80-
debug_pub.publish(msg.rgb)
81113
msg.depth = self.br.cv2_to_imgmsg(self.depth, "32FC1")
82114
msg.aolp = self.br.cv2_to_imgmsg(self.aolp, "8UC1")
83115
msg.dolp = self.br.cv2_to_imgmsg(self.dolp, "8UC1")
116+
if debug_pubs is not None:
117+
debug_pubs.publish(msg.rgb, msg.depth, msg.aolp, msg.dolp)
84118
return msg
85119

86-
def to_photoneo_msg(self, node, debug_pub) -> PhotoneoMsg:
120+
def to_photoneo_msg(self, node, debug_pubs: DebugPublishers) -> PhotoneoMsg:
87121
msg = PhotoneoMsg()
88122
msg.info.header.frame_id = self.camera_name
89123
# msg.info.header.stamp = node.get_clock().now()
90124
msg.info.k = self.K.reshape(-1)
91125
msg.pose = pose_mat_to_ros(self.R, self.t)
92126
msg.rgb = self.br.cv2_to_imgmsg(self.rgb, "8UC1")
93127
msg.depth = self.br.cv2_to_imgmsg(self.depth, "32FC1")
128+
if debug_pubs is not None:
129+
debug_pubs.publish(msg.rgb, msg.depth)
130+
return msg
94131

95132
def _load_camera_params(self, path, camera_name, img_id):
96133
self.camera_params = load_scene_camera(
@@ -114,13 +151,18 @@ def main(argv=sys.argv):
114151
dataset_name = node.get_parameter("dataset_name").get_parameter_value().string_value
115152
node.get_logger().info("Loading from dataset {dataset_name}.")
116153

117-
# Debug parameters.
118-
qos_profile = QoSProfile(
119-
depth=10, # Queue size (adjust as needed)
120-
durability=DurabilityPolicy.TRANSIENT_LOCAL,
121-
reliability=ReliabilityPolicy.RELIABLE, # or RELIABLE, depending on your needs
154+
debug_cam_1 = None
155+
debug_cam_2 = None
156+
debug_cam_3 = None
157+
debug_photoneo = None
158+
debug: bool = (
159+
node.declare_parameter("debug", False).get_parameter_value().bool_value
122160
)
123-
debug_pub = node.create_publisher(Image, "/ibpc_tester_debug_images", qos_profile)
161+
if debug:
162+
debug_cam_1 = DebugPublishers("cam1", node)
163+
debug_cam_2 = DebugPublishers("cam2", node)
164+
debug_cam_3 = DebugPublishers("cam3", node)
165+
debug_photoneo = DebugPublishers("photoneo", node)
124166

125167
# Load the test split.
126168
test_split = get_split_params(datasets_path, dataset_name, "test")
@@ -144,15 +186,17 @@ def main(argv=sys.argv):
144186
for img_id, obj_gts in scene_gt.items():
145187
request = GetPoseEstimates.Request()
146188
request.cameras.append(
147-
BOPCamera(scene_dir, "cam1", img_id).to_camera_msg(node, debug_pub)
189+
BOPCamera(scene_dir, "cam1", img_id).to_camera_msg(node, debug_cam_1)
148190
)
149191
request.cameras.append(
150-
BOPCamera(scene_dir, "cam2", img_id).to_camera_msg(node, debug_pub)
192+
BOPCamera(scene_dir, "cam2", img_id).to_camera_msg(node, debug_cam_2)
151193
)
152194
request.cameras.append(
153-
BOPCamera(scene_dir, "cam3", img_id).to_camera_msg(node, debug_pub)
195+
BOPCamera(scene_dir, "cam3", img_id).to_camera_msg(node, debug_cam_3)
196+
)
197+
request.photoneo = BOPCamera(scene_dir, "photoneo", img_id).to_photoneo_msg(
198+
node, debug_photoneo
154199
)
155-
# request.photoneo = BOPCamera(scene_dir, 'photoneo', img_id).to_photoneo_msg(node)
156200
# todo(Yadunund): Load corresponding rgb, depth and polarized image for this img_id.
157201
for obj_gt in obj_gts:
158202
request.object_ids.append(int(obj_gt["obj_id"]))

‎tester.rviz‎

Lines changed: 194 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,194 @@
1+
Panels:
2+
- Class: rviz_common/Displays
3+
Help Height: 78
4+
Name: Displays
5+
Property Tree Widget:
6+
Expanded:
7+
- /Global Options1
8+
- /Status1
9+
- /Image1
10+
- /Image1/Topic1
11+
- /Image2
12+
- /Image2/Topic1
13+
- /Image3
14+
- /Image3/Topic1
15+
- /Image4
16+
- /Image4/Topic1
17+
Splitter Ratio: 0.5
18+
Tree Height: 955
19+
- Class: rviz_common/Selection
20+
Name: Selection
21+
- Class: rviz_common/Tool Properties
22+
Expanded:
23+
- /2D Goal Pose1
24+
- /Publish Point1
25+
Name: Tool Properties
26+
Splitter Ratio: 0.5886790156364441
27+
- Class: rviz_common/Views
28+
Expanded:
29+
- /Current View1
30+
Name: Views
31+
Splitter Ratio: 0.5
32+
- Class: rviz_common/Time
33+
Experimental: false
34+
Name: Time
35+
SyncMode: 0
36+
SyncSource: ""
37+
Visualization Manager:
38+
Class: ""
39+
Displays:
40+
- Alpha: 0.5
41+
Cell Size: 1
42+
Class: rviz_default_plugins/Grid
43+
Color: 160; 160; 164
44+
Enabled: false
45+
Line Style:
46+
Line Width: 0.029999999329447746
47+
Value: Lines
48+
Name: Grid
49+
Normal Cell Count: 0
50+
Offset:
51+
X: 0
52+
Y: 0
53+
Z: 0
54+
Plane: XY
55+
Plane Cell Count: 10
56+
Reference Frame: <Fixed Frame>
57+
Value: false
58+
- Class: rviz_default_plugins/Image
59+
Enabled: true
60+
Max Value: 1
61+
Median window: 5
62+
Min Value: 0
63+
Name: Image
64+
Normalize Range: true
65+
Topic:
66+
Depth: 10
67+
Durability Policy: Transient Local
68+
History Policy: Keep Last
69+
Reliability Policy: Reliable
70+
Value: /debug/tester/cam1/rgb
71+
Value: true
72+
- Class: rviz_default_plugins/Image
73+
Enabled: true
74+
Max Value: 100
75+
Median window: 10
76+
Min Value: 0
77+
Name: Image
78+
Normalize Range: true
79+
Topic:
80+
Depth: 10
81+
Durability Policy: Transient Local
82+
History Policy: Keep Last
83+
Reliability Policy: Reliable
84+
Value: /debug/tester/cam1/depth
85+
Value: true
86+
- Class: rviz_default_plugins/Image
87+
Enabled: true
88+
Max Value: 1
89+
Median window: 5
90+
Min Value: 0
91+
Name: Image
92+
Normalize Range: true
93+
Topic:
94+
Depth: 5
95+
Durability Policy: Transient Local
96+
History Policy: Keep Last
97+
Reliability Policy: Reliable
98+
Value: /debug/tester/cam1/aolp
99+
Value: true
100+
- Class: rviz_default_plugins/Image
101+
Enabled: true
102+
Max Value: 1
103+
Median window: 5
104+
Min Value: 0
105+
Name: Image
106+
Normalize Range: true
107+
Topic:
108+
Depth: 5
109+
Durability Policy: Transient Local
110+
History Policy: Keep Last
111+
Reliability Policy: Reliable
112+
Value: /debug/tester/cam1/dolp
113+
Value: true
114+
Enabled: true
115+
Global Options:
116+
Background Color: 48; 48; 48
117+
Fixed Frame: root
118+
Frame Rate: 30
119+
Name: root
120+
Tools:
121+
- Class: rviz_default_plugins/Interact
122+
Hide Inactive Objects: true
123+
- Class: rviz_default_plugins/MoveCamera
124+
- Class: rviz_default_plugins/Select
125+
- Class: rviz_default_plugins/FocusCamera
126+
- Class: rviz_default_plugins/Measure
127+
Line color: 128; 128; 0
128+
- Class: rviz_default_plugins/SetInitialPose
129+
Covariance x: 0.25
130+
Covariance y: 0.25
131+
Covariance yaw: 0.06853891909122467
132+
Topic:
133+
Depth: 5
134+
Durability Policy: Volatile
135+
History Policy: Keep Last
136+
Reliability Policy: Reliable
137+
Value: /initialpose
138+
- Class: rviz_default_plugins/SetGoal
139+
Topic:
140+
Depth: 5
141+
Durability Policy: Volatile
142+
History Policy: Keep Last
143+
Reliability Policy: Reliable
144+
Value: /goal_pose
145+
- Class: rviz_default_plugins/PublishPoint
146+
Single click: true
147+
Topic:
148+
Depth: 5
149+
Durability Policy: Volatile
150+
History Policy: Keep Last
151+
Reliability Policy: Reliable
152+
Value: /clicked_point
153+
Transformation:
154+
Current:
155+
Class: rviz_default_plugins/TF
156+
Value: true
157+
Views:
158+
Current:
159+
Angle: 0.009999999776482582
160+
Class: rviz_default_plugins/TopDownOrtho
161+
Enable Stereo Rendering:
162+
Stereo Eye Separation: 0.05999999865889549
163+
Stereo Focal Distance: 1
164+
Swap Stereo Eyes: false
165+
Value: false
166+
Invert Z Axis: false
167+
Name: Current View
168+
Near Clip Distance: 0.009999999776482582
169+
Scale: 4.644040584564209
170+
Target Frame: <Fixed Frame>
171+
Value: TopDownOrtho (rviz_default_plugins)
172+
X: 0
173+
Y: 0
174+
Saved: ~
175+
Window Geometry:
176+
Displays:
177+
collapsed: false
178+
Height: 1259
179+
Hide Left Dock: false
180+
Hide Right Dock: false
181+
Image:
182+
collapsed: false
183+
QMainWindow State: 000000ff00000000fd0000000400000000000007fb00000449fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000003f000000f10000001700fffffffb0000000a0049006d0061006700650100000136000000f40000001700fffffffb0000000a0049006d0061006700650100000230000001120000001700fffffffb0000000a0049006d0061006700650100000348000001400000001700fffffffb0000000a0049006d006100670065010000003f000001140000000000000000fb0000000a0049006d0061006700650100000159000000d20000000000000000fb0000000a0049006d0061006700650100000231000001080000000000000000fb0000000a0049006d006100670065010000033f00000149000000000000000000000001000001d600000449fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb000000100044006900730070006c006100790073010000003f00000449000000cc00fffffffb0000000a0056006900650077007300000001cc000002bc000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000822000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000027b00fffffffb0000000800540069006d00650100000000000004500000000000000000000000230000044900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
184+
Selection:
185+
collapsed: false
186+
Time:
187+
collapsed: false
188+
Tool Properties:
189+
collapsed: false
190+
Views:
191+
collapsed: false
192+
Width: 2560
193+
X: 0
194+
Y: 32

0 commit comments

Comments
 (0)