Properly configure default publishers and fix photoneo pub

ibpc_tester/ibpc_tester/ibpc_tester.py

@@ -50,6 +50,39 @@ def pose_mat_to_ros(rot: np.ndarray, trans: np.ndarray):
     return msg
 
 
+class DebugPublishers:
+    def __init__(self, camera_name: str, node: Node):
+        # Debug parameters.
+        qos_profile = QoSProfile(
+            depth=10,  # Queue size (adjust as needed)
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+            reliability=ReliabilityPolicy.RELIABLE,  # or RELIABLE, depending on your needs
+        )
+        self.rgb_pub = node.create_publisher(
+            Image, f"/debug/tester/{camera_name}/rgb", qos_profile
+        )
+        self.depth_pub = node.create_publisher(
+            Image, f"/debug/tester/{camera_name}/depth", qos_profile
+        )
+        self.aolp_pub = None
+        self.dolp_pub = None
+        if camera_name != "photoneo":
+            self.aolp_pub = node.create_publisher(
+                Image, f"/debug/tester/{camera_name}/aolp", qos_profile
+            )
+            self.dolp_pub = node.create_publisher(
+                Image, f"/debug/tester/{camera_name}/dolp", qos_profile
+            )
+
+    def publish(self, rgb: Image, depth: Image, aolp: Image = None, dolp: Image = None):
+        self.rgb_pub.publish(rgb)
+        self.depth_pub.publish(depth)
+        if aolp is not None and self.aolp_pub is not None:
+            self.aolp_pub.publish(aolp)
+        if dolp is not None and self.dolp_pub is not None:
+            self.dolp_pub.publish(dolp)
+
+
 class BOPCamera:
     def __init__(self, path, camera_name, img_id):
         self._load_images(path, camera_name, img_id)
@@ -70,27 +103,31 @@ def _load_images(self, path, camera_name, img_id):
             self.dolp = None
         self.br = CvBridge()
 
-    def to_camera_msg(self, node, debug_pub) -> CameraMsg:
+    def to_camera_msg(self, node, debug_pubs: DebugPublishers) -> CameraMsg:
         msg = CameraMsg()
         msg.info.header.frame_id = self.camera_name
         # msg.info.header.stamp = node.get_clock().now()
         msg.info.k = self.K.reshape(-1)
         msg.pose = pose_mat_to_ros(self.R, self.t)
         msg.rgb = self.br.cv2_to_imgmsg(self.rgb, "8UC1")
-        debug_pub.publish(msg.rgb)
         msg.depth = self.br.cv2_to_imgmsg(self.depth, "32FC1")
         msg.aolp = self.br.cv2_to_imgmsg(self.aolp, "8UC1")
         msg.dolp = self.br.cv2_to_imgmsg(self.dolp, "8UC1")
+        if debug_pubs is not None:
+            debug_pubs.publish(msg.rgb, msg.depth, msg.aolp, msg.dolp)
         return msg
 
-    def to_photoneo_msg(self, node, debug_pub) -> PhotoneoMsg:
+    def to_photoneo_msg(self, node, debug_pubs: DebugPublishers) -> PhotoneoMsg:
         msg = PhotoneoMsg()
         msg.info.header.frame_id = self.camera_name
         # msg.info.header.stamp = node.get_clock().now()
         msg.info.k = self.K.reshape(-1)
         msg.pose = pose_mat_to_ros(self.R, self.t)
         msg.rgb = self.br.cv2_to_imgmsg(self.rgb, "8UC1")
         msg.depth = self.br.cv2_to_imgmsg(self.depth, "32FC1")
+        if debug_pubs is not None:
+            debug_pubs.publish(msg.rgb, msg.depth)
+        return msg
 
     def _load_camera_params(self, path, camera_name, img_id):
         self.camera_params = load_scene_camera(
@@ -114,13 +151,18 @@ def main(argv=sys.argv):
     dataset_name = node.get_parameter("dataset_name").get_parameter_value().string_value
     node.get_logger().info("Loading from dataset {dataset_name}.")
 
-    # Debug parameters.
-    qos_profile = QoSProfile(
-        depth=10,  # Queue size (adjust as needed)
-        durability=DurabilityPolicy.TRANSIENT_LOCAL,
-        reliability=ReliabilityPolicy.RELIABLE,  # or RELIABLE, depending on your needs
+    debug_cam_1 = None
+    debug_cam_2 = None
+    debug_cam_3 = None
+    debug_photoneo = None
+    debug: bool = (
+        node.declare_parameter("debug", False).get_parameter_value().bool_value
     )
-    debug_pub = node.create_publisher(Image, "/ibpc_tester_debug_images", qos_profile)
+    if debug:
+        debug_cam_1 = DebugPublishers("cam1", node)
+        debug_cam_2 = DebugPublishers("cam2", node)
+        debug_cam_3 = DebugPublishers("cam3", node)
+        debug_photoneo = DebugPublishers("photoneo", node)
 
     # Load the test split.
     test_split = get_split_params(datasets_path, dataset_name, "test")
@@ -144,15 +186,17 @@ def main(argv=sys.argv):
         for img_id, obj_gts in scene_gt.items():
             request = GetPoseEstimates.Request()
             request.cameras.append(
-                BOPCamera(scene_dir, "cam1", img_id).to_camera_msg(node, debug_pub)
+                BOPCamera(scene_dir, "cam1", img_id).to_camera_msg(node, debug_cam_1)
             )
             request.cameras.append(
-                BOPCamera(scene_dir, "cam2", img_id).to_camera_msg(node, debug_pub)
+                BOPCamera(scene_dir, "cam2", img_id).to_camera_msg(node, debug_cam_2)
             )
             request.cameras.append(
-                BOPCamera(scene_dir, "cam3", img_id).to_camera_msg(node, debug_pub)
+                BOPCamera(scene_dir, "cam3", img_id).to_camera_msg(node, debug_cam_3)
+            )
+            request.photoneo = BOPCamera(scene_dir, "photoneo", img_id).to_photoneo_msg(
+                node, debug_photoneo
             )
-            # request.photoneo = BOPCamera(scene_dir, 'photoneo', img_id).to_photoneo_msg(node)
             # todo(Yadunund): Load corresponding rgb, depth and polarized image for this img_id.
             for obj_gt in obj_gts:
                 request.object_ids.append(int(obj_gt["obj_id"]))

tester.rviz

@@ -0,0 +1,194 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Image1
+        - /Image1/Topic1
+        - /Image2
+        - /Image2/Topic1
+        - /Image3
+        - /Image3/Topic1
+        - /Image4
+        - /Image4/Topic1
+      Splitter Ratio: 0.5
+    Tree Height: 955
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: false
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: false
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 10
+        Durability Policy: Transient Local
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /debug/tester/cam1/rgb
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 100
+      Median window: 10
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 10
+        Durability Policy: Transient Local
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /debug/tester/cam1/depth
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /debug/tester/cam1/aolp
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /debug/tester/cam1/dolp
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: root
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Angle: 0.009999999776482582
+      Class: rviz_default_plugins/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 4.644040584564209
+      Target Frame: <Fixed Frame>
+      Value: TopDownOrtho (rviz_default_plugins)
+      X: 0
+      Y: 0
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1259
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000007fb00000449fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000003f000000f10000001700fffffffb0000000a0049006d0061006700650100000136000000f40000001700fffffffb0000000a0049006d0061006700650100000230000001120000001700fffffffb0000000a0049006d0061006700650100000348000001400000001700fffffffb0000000a0049006d006100670065010000003f000001140000000000000000fb0000000a0049006d0061006700650100000159000000d20000000000000000fb0000000a0049006d0061006700650100000231000001080000000000000000fb0000000a0049006d006100670065010000033f00000149000000000000000000000001000001d600000449fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb000000100044006900730070006c006100790073010000003f00000449000000cc00fffffffb0000000a0056006900650077007300000001cc000002bc000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000822000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000027b00fffffffb0000000800540069006d00650100000000000004500000000000000000000000230000044900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2560
+  X: 0
+  Y: 32