Skip to content

Commit 5aedf45

Browse files
lucaswlucasw
committed
basic depth_image_proc unit test of a camera situated behind the depth camera, next try a camera at an angle to the depth points next
1 parent 888c603 commit 5aedf45

File tree

5 files changed

+435
-0
lines changed

5 files changed

+435
-0
lines changed

‎depth_image_proc/CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,3 +57,10 @@ install(TARGETS ${PROJECT_NAME}
5757
install(FILES nodelet_plugins.xml
5858
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
5959
)
60+
61+
if(CATKIN_ENABLE_TESTING)
62+
find_package(rostest REQUIRED)
63+
64+
add_rostest(test/test_register.test
65+
DEPENDENCIES ${${PROJECT_NAME}_EXPORTED_TARGETS})
66+
endif()

‎depth_image_proc/package.xml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,4 +44,6 @@
4444
<run_depend>nodelet</run_depend>
4545
<run_depend>tf2</run_depend>
4646
<run_depend>tf2_ros</run_depend>
47+
48+
<test_depend>rostest</test_depend>
4749
</package>

‎depth_image_proc/test/test_register.py‎

Lines changed: 119 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,119 @@
1+
#!/usr/bin/env python
2+
3+
import copy
4+
import unittest
5+
6+
import numpy as np
7+
import rospy
8+
import rostest
9+
from cv_bridge import CvBridge
10+
from sensor_msgs.msg import CameraInfo
11+
from sensor_msgs.msg import Image
12+
13+
14+
class TestRegister(unittest.TestCase):
15+
def test_register(self):
16+
# publish a 2x2 float depth image with depths like
17+
# [1.0, 2.0], [2.0, 1.0]
18+
# publish a camera info with a aov of around 90 degrees
19+
# publish two static tfs in the .test file, one shifted
20+
# by 1.0 in x from the depth camera_info frame
21+
# another by 2.0 in x, and 1.5 in z, with a 90 degree rotation
22+
# looking back at those depth points
23+
# publish another camera info, in series, have it shift from one frame to the other
24+
# subscribe to the depth_image_proc node and verify output
25+
# (make numbers come out even with 90 degree aov)
26+
27+
self.cv_bridge = CvBridge()
28+
self.depth_image_pub = rospy.Publisher("depth/image_rect", Image, queue_size=2)
29+
self.depth_ci_pub = rospy.Publisher("depth/camera_info", CameraInfo, queue_size=2)
30+
self.rgb_ci_pub = rospy.Publisher("rgb/camera_info", CameraInfo, queue_size=2)
31+
32+
# TODO(lucasw) use time sync subscriber
33+
self.depth_sub = rospy.Subscriber("depth_registered/image_rect", Image, self.depth_callback, queue_size=2)
34+
self.ci_sub = rospy.Subscriber("depth_registered/camera_info", CameraInfo, self.ci_callback, queue_size=2)
35+
36+
ci_msg = CameraInfo()
37+
wd = 3
38+
ht = 3
39+
ci_msg.header.frame_id = "station1"
40+
ci_msg.height = ht
41+
ci_msg.width = wd
42+
ci_msg.distortion_model = "plumb_bob"
43+
44+
cx = 1.5
45+
cy = 1.5
46+
fx = 1.5
47+
fy = 1.5
48+
ci_msg.K = [fx, 0.0, cx,
49+
0.0, fy, cy,
50+
0.0, 0.0, 1.0]
51+
ci_msg.R = [1, 0, 0,
52+
0, 1, 0,
53+
0, 0, 1]
54+
ci_msg.P = [fx, 0.0, cx, 0.0,
55+
0.0, fy, cy, 0.0,
56+
0.0, 0.0, 1.0, 0.0]
57+
58+
depth = np.empty((ht, wd, 1), np.float32)
59+
depth[0, 0] = 1.0
60+
depth[0, 1] = 1.0
61+
depth[0, 2] = 1.0
62+
depth[1, 0] = 1.0
63+
depth[1, 1] = 1.0
64+
depth[1, 2] = 1.0
65+
depth[2, 0] = 1.0
66+
depth[2, 1] = 1.0
67+
depth[2, 2] = 1.0
68+
rospy.loginfo(depth)
69+
70+
depth_msg = self.cv_bridge.cv2_to_imgmsg(depth, "32FC1")
71+
ci_msg.header.stamp = rospy.Time.now()
72+
rgb_ci_msg = copy.deepcopy(ci_msg)
73+
rgb_ci_msg.header.frame_id = "station2"
74+
depth_msg.header = ci_msg.header
75+
76+
for i in range(6):
77+
rospy.loginfo(f"{i} waiting for register connections...")
78+
num_im_sub = self.depth_image_pub.get_num_connections()
79+
num_ci_sub = self.depth_ci_pub.get_num_connections()
80+
num_rgb_ci_sub = self.rgb_ci_pub.get_num_connections()
81+
rospy.sleep(1)
82+
if num_im_sub > 0 and num_ci_sub > 0 and num_rgb_ci_sub > 0:
83+
break
84+
85+
self.assertGreater(self.depth_image_pub.get_num_connections(), 0)
86+
self.assertGreater(self.depth_ci_pub.get_num_connections(), 0)
87+
self.assertGreater(self.rgb_ci_pub.get_num_connections(), 0)
88+
rospy.sleep(1.0)
89+
90+
self.count = 0
91+
rospy.loginfo("publishing depth and ci, wait for callbacks")
92+
for i in range(4):
93+
self.depth_image_pub.publish(depth_msg)
94+
self.depth_ci_pub.publish(ci_msg)
95+
self.rgb_ci_pub.publish(rgb_ci_msg)
96+
rospy.sleep(0.1)
97+
98+
while i in range(6):
99+
if self.count > 1:
100+
break
101+
rospy.sleep(1)
102+
rospy.loginfo("done waiting")
103+
104+
def depth_callback(self, msg):
105+
rospy.loginfo("received depth")
106+
registered_depth = self.cv_bridge.imgmsg_to_cv2(msg, "32FC1")
107+
self.assertAlmostEqual(registered_depth[1, 1], 7.0, 1)
108+
self.count += 1
109+
rospy.loginfo(registered_depth)
110+
111+
def ci_callback(self, msg):
112+
rospy.logdebug("received camera info")
113+
114+
115+
if __name__ == "__main__":
116+
node_name = 'test_register'
117+
rospy.init_node(node_name)
118+
# rostest.rosrun("depth_image_proc", node_name, sys.argv)
119+
rostest.rosrun("depth_image_proc", node_name, TestRegister)

‎depth_image_proc/test/test_register.rviz‎

Lines changed: 258 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,258 @@
1+
Panels:
2+
- Class: rviz/Displays
3+
Help Height: 138
4+
Name: Displays
5+
Property Tree Widget:
6+
Expanded:
7+
- /Global Options1
8+
- /Status1
9+
- /Grid1
10+
- /input DepthCloud1
11+
- /input DepthCloud1/Auto Size1
12+
Splitter Ratio: 0.5
13+
Tree Height: 952
14+
- Class: rviz/Selection
15+
Name: Selection
16+
- Class: rviz/Tool Properties
17+
Expanded:
18+
- /2D Pose Estimate1
19+
- /2D Nav Goal1
20+
- /Publish Point1
21+
Name: Tool Properties
22+
Splitter Ratio: 0.5886790156364441
23+
- Class: rviz/Views
24+
Expanded:
25+
- /Current View1
26+
Name: Views
27+
Splitter Ratio: 0.5
28+
- Class: rviz/Time
29+
Experimental: false
30+
Name: Time
31+
SyncMode: 0
32+
SyncSource: input DepthCloud
33+
Preferences:
34+
PromptSaveOnExit: true
35+
Toolbars:
36+
toolButtonStyle: 2
37+
Visualization Manager:
38+
Class: ""
39+
Displays:
40+
- Alpha: 0.5
41+
Cell Size: 1
42+
Class: rviz/Grid
43+
Color: 160; 160; 164
44+
Enabled: true
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: XZ
55+
Plane Cell Count: 10
56+
Reference Frame: <Fixed Frame>
57+
Value: true
58+
- Class: rviz/TF
59+
Enabled: true
60+
Frame Timeout: 15
61+
Frames:
62+
All Enabled: true
63+
map:
64+
Value: true
65+
station1:
66+
Value: true
67+
station2:
68+
Value: true
69+
station3:
70+
Value: true
71+
Marker Alpha: 1
72+
Marker Scale: 1
73+
Name: TF
74+
Show Arrows: true
75+
Show Axes: true
76+
Show Names: true
77+
Tree:
78+
map:
79+
station1:
80+
{}
81+
station2:
82+
{}
83+
station3:
84+
{}
85+
Update Interval: 0
86+
Value: true
87+
- Alpha: 1
88+
Auto Size:
89+
Auto Size Factor: 1
90+
Value: true
91+
Autocompute Intensity Bounds: true
92+
Autocompute Value Bounds:
93+
Max Value: 10
94+
Min Value: -10
95+
Value: true
96+
Axis: Z
97+
Channel Name: intensity
98+
Class: rviz/DepthCloud
99+
Color: 255; 255; 255
100+
Color Image Topic: ""
101+
Color Transformer: FlatColor
102+
Color Transport Hint: raw
103+
Decay Time: 0
104+
Depth Map Topic: /depth/image_rect
105+
Depth Map Transport Hint: raw
106+
Enabled: true
107+
Invert Rainbow: false
108+
Max Color: 255; 255; 255
109+
Min Color: 0; 0; 0
110+
Name: input DepthCloud
111+
Occlusion Compensation:
112+
Occlusion Time-Out: 30
113+
Value: false
114+
Position Transformer: XYZ
115+
Queue Size: 5
116+
Selectable: true
117+
Size (Pixels): 10
118+
Style: Points
119+
Topic Filter: true
120+
Use Fixed Frame: true
121+
Use rainbow: true
122+
Value: true
123+
- Class: jsk_rviz_plugin/CameraInfo
124+
Enabled: true
125+
Image Topic: ""
126+
Name: input CameraInfo
127+
Queue Size: 10
128+
Topic: /depth/camera_info
129+
Unreliable: false
130+
Value: true
131+
alpha: 0.5
132+
color: 85; 255; 255
133+
edge color: 138; 226; 52
134+
far clip: 1
135+
not show side polygons: true
136+
show edges: true
137+
show polygons: false
138+
transport hints: raw
139+
use image: false
140+
- Alpha: 1
141+
Auto Size:
142+
Auto Size Factor: 1
143+
Value: true
144+
Autocompute Intensity Bounds: true
145+
Autocompute Value Bounds:
146+
Max Value: 10
147+
Min Value: -10
148+
Value: true
149+
Axis: Z
150+
Channel Name: intensity
151+
Class: rviz/DepthCloud
152+
Color: 239; 41; 41
153+
Color Image Topic: ""
154+
Color Transformer: FlatColor
155+
Color Transport Hint: raw
156+
Decay Time: 0
157+
Depth Map Topic: /depth_registered/image_rect
158+
Depth Map Transport Hint: raw
159+
Enabled: true
160+
Invert Rainbow: false
161+
Max Color: 255; 255; 255
162+
Min Color: 0; 0; 0
163+
Name: output DepthCloud
164+
Occlusion Compensation:
165+
Occlusion Time-Out: 30
166+
Value: false
167+
Position Transformer: XYZ
168+
Queue Size: 5
169+
Selectable: true
170+
Size (Pixels): 12
171+
Style: Points
172+
Topic Filter: true
173+
Use Fixed Frame: true
174+
Use rainbow: true
175+
Value: true
176+
- Class: jsk_rviz_plugin/CameraInfo
177+
Enabled: true
178+
Image Topic: ""
179+
Name: output CameraInfo
180+
Queue Size: 10
181+
Topic: /depth_registered/camera_info
182+
Unreliable: false
183+
Value: true
184+
alpha: 0.5
185+
color: 85; 255; 255
186+
edge color: 138; 226; 52
187+
far clip: 1
188+
not show side polygons: true
189+
show edges: true
190+
show polygons: false
191+
transport hints: raw
192+
use image: false
193+
Enabled: true
194+
Global Options:
195+
Background Color: 48; 48; 48
196+
Default Light: true
197+
Fixed Frame: map
198+
Frame Rate: 30
199+
Name: root
200+
Tools:
201+
- Class: rviz/Interact
202+
Hide Inactive Objects: true
203+
- Class: rviz/MoveCamera
204+
- Class: rviz/Select
205+
- Class: rviz/FocusCamera
206+
- Class: rviz/Measure
207+
- Class: rviz/SetInitialPose
208+
Theta std deviation: 0.2617993950843811
209+
Topic: /initialpose
210+
X std deviation: 0.5
211+
Y std deviation: 0.5
212+
- Class: rviz/SetGoal
213+
Topic: /move_base_simple/goal
214+
- Class: rviz/PublishPoint
215+
Single click: true
216+
Topic: /clicked_point
217+
Value: true
218+
Views:
219+
Current:
220+
Class: rviz/Orbit
221+
Distance: 7.772470474243164
222+
Enable Stereo Rendering:
223+
Stereo Eye Separation: 0.05999999865889549
224+
Stereo Focal Distance: 1
225+
Swap Stereo Eyes: false
226+
Value: false
227+
Field of View: 0.7853981852531433
228+
Focal Point:
229+
X: 0.829400897026062
230+
Y: -0.3880631923675537
231+
Z: 0.5697964429855347
232+
Focal Shape Fixed Size: true
233+
Focal Shape Size: 0.05000000074505806
234+
Invert Z Axis: false
235+
Name: Current View
236+
Near Clip Distance: 0.009999999776482582
237+
Pitch: -0.12979792058467865
238+
Target Frame: <Fixed Frame>
239+
Yaw: 4.640459060668945
240+
Saved: ~
241+
Window Geometry:
242+
Displays:
243+
collapsed: false
244+
Height: 1464
245+
Hide Left Dock: false
246+
Hide Right Dock: false
247+
QMainWindow State: 000000ff00000000fd000000040000000000000298000004b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000004b00000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000004b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000004b00000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008d800000060fc0100000002fb0000000800540069006d00650100000000000008d80000057100fffffffb0000000800540069006d00650100000000000004500000000000000000000004c9000004b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
248+
Selection:
249+
collapsed: false
250+
Time:
251+
collapsed: false
252+
Tool Properties:
253+
collapsed: false
254+
Views:
255+
collapsed: false
256+
Width: 2264
257+
X: 1478
258+
Y: 33

0 commit comments

Comments
 (0)