basic depth_image_proc unit test of a camera situated behind the dept…

…h camera, next try a camera at an angle to the depth points next

depth_image_proc/CMakeLists.txt

@@ -57,3 +57,11 @@ install(TARGETS ${PROJECT_NAME}
 install(FILES nodelet_plugins.xml
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+
+if(CATKIN_ENABLE_TESTING)
+  find_pcakge(rostest REQUIRED)
+
+  add_rostest(test/test_register.test
+    DEPENDENCIES ${${PROJECT_NAME}_EXPORTED_TARGETS})
+)
+endif()

depth_image_proc/package.xml

@@ -44,4 +44,6 @@
   <run_depend>nodelet</run_depend>
   <run_depend>tf2</run_depend>
   <run_depend>tf2_ros</run_depend>
+
+  <test_depend>rostest</test_depend>
 </package>

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)

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: <Fixed 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: <Fixed 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