From 2b1e413f738ef01377e7a7e5240c86a7b2f595e5 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Sat, 24 Dec 2022 09:06:23 -0800 Subject: [PATCH] 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 --- depth_image_proc/CMakeLists.txt | 7 + depth_image_proc/package.xml | 3 + depth_image_proc/src/nodelets/register.cpp | 3 - depth_image_proc/test/test_register.py | 119 ++++++++++ depth_image_proc/test/test_register.rviz | 258 +++++++++++++++++++++ depth_image_proc/test/test_register.test | 49 ++++ 6 files changed, 436 insertions(+), 3 deletions(-) create mode 100755 depth_image_proc/test/test_register.py create mode 100644 depth_image_proc/test/test_register.rviz create mode 100644 depth_image_proc/test/test_register.test diff --git a/depth_image_proc/CMakeLists.txt b/depth_image_proc/CMakeLists.txt index 223c1699a..5ef443766 100644 --- a/depth_image_proc/CMakeLists.txt +++ b/depth_image_proc/CMakeLists.txt @@ -54,3 +54,10 @@ install(TARGETS ${PROJECT_NAME} install(FILES nodelet_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + add_rostest(test/test_register.test + DEPENDENCIES ${${PROJECT_NAME}_EXPORTED_TARGETS}) +endif() diff --git a/depth_image_proc/package.xml b/depth_image_proc/package.xml index 38607ba62..cde213da2 100644 --- a/depth_image_proc/package.xml +++ b/depth_image_proc/package.xml @@ -45,4 +45,7 @@ tf2 tf2_ros sensor_msgs + + rostest + tf diff --git a/depth_image_proc/src/nodelets/register.cpp b/depth_image_proc/src/nodelets/register.cpp index 02e6e26be..fe1d234bd 100644 --- a/depth_image_proc/src/nodelets/register.cpp +++ b/depth_image_proc/src/nodelets/register.cpp @@ -247,9 +247,6 @@ bool transform_depth( return true; } -// TODO(lucasw) need a unit test for this, simple low res image (e.g. 4x4 pixels) with -// depth ranges of around 1.0 -// shift it to the right 1.0 units, view from 90 degrees template void RegisterNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::ImagePtr& registered_msg, diff --git a/depth_image_proc/test/test_register.py b/depth_image_proc/test/test_register.py new file mode 100755 index 000000000..e934a1042 --- /dev/null +++ b/depth_image_proc/test/test_register.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python + +import copy +import unittest + +import numpy as np +import rospy +import rostest +from cv_bridge import CvBridge +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import Image + + +class TestRegister(unittest.TestCase): + def test_register(self): + # publish a 2x2 float depth image with depths like + # [1.0, 2.0], [2.0, 1.0] + # publish a camera info with a aov of around 90 degrees + # publish two static tfs in the .test file, one shifted + # by 1.0 in x from the depth camera_info frame + # another by 2.0 in x, and 1.5 in z, with a 90 degree rotation + # looking back at those depth points + # publish another camera info, in series, have it shift from one frame to the other + # subscribe to the depth_image_proc node and verify output + # (make numbers come out even with 90 degree aov) + + self.cv_bridge = CvBridge() + self.depth_image_pub = rospy.Publisher("depth/image_rect", Image, queue_size=2) + self.depth_ci_pub = rospy.Publisher("depth/camera_info", CameraInfo, queue_size=2) + self.rgb_ci_pub = rospy.Publisher("rgb/camera_info", CameraInfo, queue_size=2) + + # TODO(lucasw) use time sync subscriber + self.depth_sub = rospy.Subscriber("depth_registered/image_rect", Image, self.depth_callback, queue_size=2) + self.ci_sub = rospy.Subscriber("depth_registered/camera_info", CameraInfo, self.ci_callback, queue_size=2) + + ci_msg = CameraInfo() + wd = 3 + ht = 3 + ci_msg.header.frame_id = "station1" + ci_msg.height = ht + ci_msg.width = wd + ci_msg.distortion_model = "plumb_bob" + + cx = 1.5 + cy = 1.5 + fx = 1.5 + fy = 1.5 + ci_msg.K = [fx, 0.0, cx, + 0.0, fy, cy, + 0.0, 0.0, 1.0] + ci_msg.R = [1, 0, 0, + 0, 1, 0, + 0, 0, 1] + ci_msg.P = [fx, 0.0, cx, 0.0, + 0.0, fy, cy, 0.0, + 0.0, 0.0, 1.0, 0.0] + + depth = np.empty((ht, wd, 1), np.float32) + depth[0, 0] = 1.0 + depth[0, 1] = 1.0 + depth[0, 2] = 1.0 + depth[1, 0] = 1.0 + depth[1, 1] = 1.0 + depth[1, 2] = 1.0 + depth[2, 0] = 1.0 + depth[2, 1] = 1.0 + depth[2, 2] = 1.0 + rospy.loginfo(depth) + + depth_msg = self.cv_bridge.cv2_to_imgmsg(depth, "32FC1") + ci_msg.header.stamp = rospy.Time.now() + rgb_ci_msg = copy.deepcopy(ci_msg) + rgb_ci_msg.header.frame_id = "station2" + depth_msg.header = ci_msg.header + + for i in range(6): + rospy.loginfo(f"{i} waiting for register connections...") + num_im_sub = self.depth_image_pub.get_num_connections() + num_ci_sub = self.depth_ci_pub.get_num_connections() + num_rgb_ci_sub = self.rgb_ci_pub.get_num_connections() + rospy.sleep(1) + if num_im_sub > 0 and num_ci_sub > 0 and num_rgb_ci_sub > 0: + break + + self.assertGreater(self.depth_image_pub.get_num_connections(), 0) + self.assertGreater(self.depth_ci_pub.get_num_connections(), 0) + self.assertGreater(self.rgb_ci_pub.get_num_connections(), 0) + rospy.sleep(1.0) + + self.count = 0 + rospy.loginfo("publishing depth and ci, wait for callbacks") + for i in range(4): + self.depth_image_pub.publish(depth_msg) + self.depth_ci_pub.publish(ci_msg) + self.rgb_ci_pub.publish(rgb_ci_msg) + rospy.sleep(0.1) + + while i in range(6): + if self.count > 1: + break + rospy.sleep(1) + rospy.loginfo("done waiting") + + def depth_callback(self, msg): + rospy.loginfo("received depth") + registered_depth = self.cv_bridge.imgmsg_to_cv2(msg, "32FC1") + self.assertAlmostEqual(registered_depth[1, 1], 7.0, 1) + self.count += 1 + rospy.loginfo(registered_depth) + + def ci_callback(self, msg): + rospy.logdebug("received camera info") + + +if __name__ == "__main__": + node_name = 'test_register' + rospy.init_node(node_name) + # rostest.rosrun("depth_image_proc", node_name, sys.argv) + rostest.rosrun("depth_image_proc", node_name, TestRegister) diff --git a/depth_image_proc/test/test_register.rviz b/depth_image_proc/test/test_register.rviz new file mode 100644 index 000000000..c96e8f9cf --- /dev/null +++ b/depth_image_proc/test/test_register.rviz @@ -0,0 +1,258 @@ +Panels: + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + - /input DepthCloud1 + - /input DepthCloud1/Auto Size1 + Splitter Ratio: 0.5 + Tree Height: 952 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: input DepthCloud +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XZ + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + map: + Value: true + station1: + Value: true + station2: + Value: true + station3: + Value: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + station1: + {} + station2: + {} + station3: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Auto Size: + Auto Size Factor: 1 + Value: true + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/DepthCloud + Color: 255; 255; 255 + Color Image Topic: "" + Color Transformer: FlatColor + Color Transport Hint: raw + Decay Time: 0 + Depth Map Topic: /depth/image_rect + Depth Map Transport Hint: raw + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: input DepthCloud + Occlusion Compensation: + Occlusion Time-Out: 30 + Value: false + Position Transformer: XYZ + Queue Size: 5 + Selectable: true + Size (Pixels): 10 + Style: Points + Topic Filter: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: jsk_rviz_plugin/CameraInfo + Enabled: true + Image Topic: "" + Name: input CameraInfo + Queue Size: 10 + Topic: /depth/camera_info + Unreliable: false + Value: true + alpha: 0.5 + color: 85; 255; 255 + edge color: 138; 226; 52 + far clip: 1 + not show side polygons: true + show edges: true + show polygons: false + transport hints: raw + use image: false + - Alpha: 1 + Auto Size: + Auto Size Factor: 1 + Value: true + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/DepthCloud + Color: 239; 41; 41 + Color Image Topic: "" + Color Transformer: FlatColor + Color Transport Hint: raw + Decay Time: 0 + Depth Map Topic: /depth_registered/image_rect + Depth Map Transport Hint: raw + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: output DepthCloud + Occlusion Compensation: + Occlusion Time-Out: 30 + Value: false + Position Transformer: XYZ + Queue Size: 5 + Selectable: true + Size (Pixels): 12 + Style: Points + Topic Filter: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: jsk_rviz_plugin/CameraInfo + Enabled: true + Image Topic: "" + Name: output CameraInfo + Queue Size: 10 + Topic: /depth_registered/camera_info + Unreliable: false + Value: true + alpha: 0.5 + color: 85; 255; 255 + edge color: 138; 226; 52 + far clip: 1 + not show side polygons: true + show edges: true + show polygons: false + transport hints: raw + use image: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 7.772470474243164 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0.829400897026062 + Y: -0.3880631923675537 + Z: 0.5697964429855347 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -0.12979792058467865 + Target Frame: + Yaw: 4.640459060668945 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1464 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000298000004b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000004b00000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000004b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000004b00000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008d800000060fc0100000002fb0000000800540069006d00650100000000000008d80000057100fffffffb0000000800540069006d00650100000000000004500000000000000000000004c9000004b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2264 + X: 1478 + Y: 33 diff --git a/depth_image_proc/test/test_register.test b/depth_image_proc/test/test_register.test new file mode 100644 index 000000000..c42c0814f --- /dev/null +++ b/depth_image_proc/test/test_register.test @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + - 'image_transport/compressed' + - 'image_transport/theora' + - 'image_transport/compressedDepth' + + + + + + + + +