(
self,
map_save_path: str,
verbose_timer: bool = True,
global_frames_ratio: float = 1.1,
)
| 553 | timestamp_ns = int(timestamp_ns) |
| 554 | self._pace_to_timestamp(timestamp_ns) |
| 555 | self._publish_percent_from_timestamp(timestamp_ns) |
| 556 | |
| 557 | # Find publisher + msg type for this topic |
| 558 | pub_and_type = self._topic_publishers.get(topic) |
| 559 | if pub_and_type is None: |
| 560 | # No publisher (should not really happen, but don't crash playback) |
| 561 | self.get_logger().warn(f"No publisher for topic '{topic}'") |
| 562 | return True |
| 563 | |
| 564 | pub, msg_type = pub_and_type |
| 565 | |
| 566 | # Deserialize and publish actual message |
| 567 | msg = deserialize_message(serialized_msg, msg_type) |
| 568 | pub.publish(msg) |
| 569 | |
| 570 | # Publish /clock with the same timestamp (for use_sim_time) |
| 571 | if self._clock_pub is not None: |
| 572 | clock_msg = Clock() |
| 573 | clock_msg.clock.sec = int(timestamp_ns // 1_000_000_000) |
| 574 | clock_msg.clock.nanosec = int(timestamp_ns % 1_000_000_000) |
| 575 | self._clock_pub.publish(clock_msg) |
| 576 | |
| 577 | return True |
| 578 | |
| 579 | class BuildMapNode(Node): |
| 580 | def __init__( |
| 581 | self, |
| 582 | map_save_path: str, |
| 583 | verbose_timer: bool = True, |
| 584 | global_frames_ratio: float = 1.1, |
| 585 | ): |
| 586 | super().__init__('build_map_node') |
| 587 | if global_frames_ratio < 1.0: |
| 588 | raise ValueError(f"global_frames_ratio must be >= 1.0, got {global_frames_ratio}") |
| 589 | self.verbose_timer = verbose_timer |
| 590 | self.global_frames_ratio = global_frames_ratio |
| 591 | # Keyframe count at the last global refinement (COLMAP: ba_prev_num_reg_frames). |
| 592 | self._global_prev_num_frames = 0 |
| 593 | self.logger = logging.getLogger(__name__) |
| 594 | self.stage_timer = StageTimer( |
| 595 | verbose_logger=self.logger.info if verbose_timer else None, |
| 596 | ) |
| 597 | self.super_point_extractor = SuperPointTRT() |
| 598 | self.light_glue_matcher = LightGlueTRT() |
| 599 | self.dinov2_model = Dinov2TRT() |
| 600 | self.semantic_embedder = SigLIPTRT() |
| 601 | |
| 602 | self.bridge = CvBridge() |
| 603 | |
| 604 | self.tf_broadcaster = TransformBroadcaster(self) |
| 605 | |
| 606 | self.camera_info_sub = self.create_subscription(CameraInfo, '/camera/camera/infra2/camera_info', self.info_callback, 10) |
| 607 | self.depth_sub = Subscriber(self, Image, '/slam/keyframe_depth') |
| 608 | self.keyframe_image_sub = Subscriber(self, Image, '/slam/keyframe_image') |
| 609 | self.keyframe_odom_sub = Subscriber(self, Odometry, '/slam/keyframe_odom') |
| 610 | self.rgb_image_sub = Subscriber(self, Image, '/camera/camera/color/image_raw') |
| 611 | self.continuous_odom_sub = self.create_subscription(Odometry, '/slam/odometry', self.continuous_odom_callback, 100) |
| 612 |
nothing calls this directly
no test coverage detected