| 22 | |
| 23 | |
| 24 | class LooperBridgeNode(Node): |
| 25 | def __init__(self, args): |
| 26 | super().__init__("looper_bridge_node") |
| 27 | self.args = args |
| 28 | self.bridge = CvBridge() |
| 29 | |
| 30 | self.cached_camera_info = None |
| 31 | self.last_keyframe_pose = None |
| 32 | self.last_keyframe_time = None |
| 33 | self.last_pose = None |
| 34 | self.last_pose_time = None |
| 35 | self._missing_input_counter = 0 |
| 36 | |
| 37 | self.sensor_qos = QoSProfile(depth=50, reliability=ReliabilityPolicy.RELIABLE) |
| 38 | self.tf_static_qos = QoSProfile( |
| 39 | history=HistoryPolicy.KEEP_LAST, |
| 40 | depth=1, |
| 41 | reliability=ReliabilityPolicy.RELIABLE, |
| 42 | durability=DurabilityPolicy.TRANSIENT_LOCAL, |
| 43 | ) |
| 44 | |
| 45 | self.camera_info_sub = self.create_subscription(CameraInfo, "/camera/camera/infra1/camera_info", self.camera_info_callback, self.sensor_qos) |
| 46 | self.tf_static_sub = self.create_subscription(TFMessage, "/tf_static", self.tf_callback, self.tf_static_qos) |
| 47 | |
| 48 | self.vio_100hz_sub = self.create_subscription( |
| 49 | PoseStamped, "/camera/camera/vio_100hz", self.vio_100hz_callback, 50 |
| 50 | ) |
| 51 | |
| 52 | self.depth_sub = message_filters.Subscriber(self, Image, "/camera/camera/depth/image_rect_raw", qos_profile=self.sensor_qos) |
| 53 | self.pose_sub = message_filters.Subscriber(self, PoseStamped, "/camera/camera/vio_image") |
| 54 | self.image_sub = message_filters.Subscriber(self, Image, "/camera/camera/infra1/image_rect_raw", qos_profile=self.sensor_qos) |
| 55 | self.sync = message_filters.TimeSynchronizer( |
| 56 | [self.depth_sub, self.pose_sub, self.image_sub], queue_size=20 |
| 57 | ) |
| 58 | self.sync.registerCallback(self.sync_callback) |
| 59 | |
| 60 | self.odom_pub = self.create_publisher(Odometry, "/slam/odometry", 10) |
| 61 | self.odom_visual_pub = self.create_publisher( |
| 62 | Odometry, "/slam/odometry_visual", 10 |
| 63 | ) |
| 64 | self.depth_pub = self.create_publisher(Image, "/slam/depth", 10) |
| 65 | self.disparity_pub_vis = self.create_publisher(Image, "/slam/disparity_vis", 10) |
| 66 | self.slam_camera_info_pub = self.create_publisher(CameraInfo, "/slam/camera_info", 10) |
| 67 | self.camera_info_alias_pub = self.create_publisher( |
| 68 | CameraInfo, "/camera/camera/infra2/camera_info", 10 |
| 69 | ) |
| 70 | self.keyframe_pose_visual_pub = self.create_publisher( |
| 71 | Odometry, "/slam/keyframe_odom", 10 |
| 72 | ) |
| 73 | self.keyframe_image_pub = self.create_publisher(Image, "/slam/keyframe_image", 10) |
| 74 | self.keyframe_depth_pub = self.create_publisher(Image, "/slam/keyframe_depth", 10) |
| 75 | |
| 76 | self.get_logger().info( |
| 77 | "Bridging /camera/camera/vio_image + /camera/camera/depth/image_rect_raw + /camera/camera/infra1/image_rect_raw into TinyNav /slam topics." |
| 78 | ) |
| 79 | self.get_logger().info( |
| 80 | "Bridging /camera/camera/vio_100hz into /slam/odometry." |
| 81 | ) |