(self, depth_msg: Image, pose_msg: PoseStamped, image_msg: Image)
| 190 | return disp_color_msg |
| 191 | |
| 192 | def sync_callback(self, depth_msg: Image, pose_msg: PoseStamped, image_msg: Image): |
| 193 | if self.cached_camera_info is None: |
| 194 | self.log_missing_inputs() |
| 195 | return |
| 196 | |
| 197 | T_world_camera = pose_msg2np(pose_msg) |
| 198 | stamp = pose_msg.header.stamp |
| 199 | |
| 200 | odom_msg = self.build_odom(T_world_camera, stamp) |
| 201 | self.odom_visual_pub.publish(odom_msg) |
| 202 | depth_m = self.decode_depth_meters(depth_msg) |
| 203 | depth_out = self.build_depth_msg(depth_m, stamp) |
| 204 | disparity_vis_msg = self.build_disparity_vis(depth_m, stamp) |
| 205 | |
| 206 | self.get_logger().info( |
| 207 | "sync_callback: " |
| 208 | f"t={self.stamp_to_sec(stamp):.3f}, " |
| 209 | f"depth={depth_m.shape}, image={image_msg.height}x{image_msg.width}" |
| 210 | ) |
| 211 | |
| 212 | image_out = copy.deepcopy(image_msg) |
| 213 | image_out.header.stamp = stamp |
| 214 | image_out.header.frame_id = "camera" |
| 215 | |
| 216 | camera_info_out = copy.deepcopy(self.cached_camera_info) |
| 217 | camera_info_out.header.stamp = stamp |
| 218 | camera_info_out.header.frame_id = "camera" |
| 219 | |
| 220 | self.depth_pub.publish(depth_out) |
| 221 | self.disparity_pub_vis.publish(disparity_vis_msg) |
| 222 | self.slam_camera_info_pub.publish(camera_info_out) |
| 223 | self.camera_info_alias_pub.publish(camera_info_out) |
| 224 | |
| 225 | if self.should_add_keyframe(T_world_camera, stamp): |
| 226 | self.keyframe_pose_visual_pub.publish(odom_msg) |
| 227 | self.keyframe_image_pub.publish(image_out) |
| 228 | self.keyframe_depth_pub.publish(depth_out) |
| 229 | self.last_keyframe_pose = T_world_camera.copy() |
| 230 | self.last_keyframe_time = self.stamp_to_sec(stamp) |
| 231 | |
| 232 | |
| 233 | def parse_args(): |
nothing calls this directly
no test coverage detected