(self, msg: Bool)
| 680 | if T_infra1_optical_to_infra1 is not None and T_rgb_optical_to_rgb is not None and T_infra1_to_link is not None and T_rgb_to_link is not None: |
| 681 | self.T_rgb_to_infra1 = np.linalg.inv(T_infra1_optical_to_infra1) @ np.linalg.inv(T_infra1_to_link) @ T_rgb_to_link @ T_rgb_optical_to_rgb |
| 682 | if tf_messages and self.T_rgb_to_infra1 is not None: |
| 683 | np.save(f"{self.map_save_path}/tf_messages.npy", tf_messages, allow_pickle=True) |
| 684 | if self.tf_sub is not None: |
| 685 | self.destroy_subscription(self.tf_sub.sub) |
| 686 | self.tf_sub = None |
| 687 | if self.tf_static_sub is not None: |
| 688 | self.destroy_subscription(self.tf_static_sub.sub) |
| 689 | self.tf_static_sub = None |
| 690 | self.get_logger().info("Saved tf_messages.npy and unsubscribed from /tf and /tf_static") |
| 691 | |
| 692 | def rgb_camera_info_callback(self, msg:CameraInfo): |
| 693 | if self.rgb_camera_K is None: |
| 694 | self.rgb_camera_K = np.array(msg.k).reshape(3, 3) |
| 695 | |
| 696 | def info_callback(self, msg:CameraInfo): |
| 697 | if self.K is None: |
| 698 | self.get_logger().info("Camera intrinsics received.") |
| 699 | self.K = np.array(msg.k).reshape(3, 3) |
| 700 | fx = self.K[0, 0] |
| 701 | Tx = msg.p[3] |
| 702 | self.baseline = -Tx / fx |
| 703 | self.destroy_subscription(self.camera_info_sub) |
nothing calls this directly
no test coverage detected