vuer-cli 0.0.3__py3-none-any.whl → 0.0.5__py3-none-any.whl

vuer_cli/mcap_extractor.py

@@ -0,0 +1,866 @@
+"""
+MCAP Data Extractor
+Extracts data from MCAP file into readable formats:
+- RGB images as JPEG/PNG (compressed or raw sensor_msgs/msg/Image)
+- Depth images as PNG (compressed or raw sensor_msgs/msg/Image)
+- LiDAR point clouds as PLY files
+- IMU data as CSV
+- Camera info as JSON
+- Transforms as CSV (from /tf or /tf_static topics)
+- Odometry as CSV (from /odom topic)
+- Joint states as CSV (from joint_states topics)
+"""
+
+import json
+import os
+import struct
+from collections import defaultdict
+from pathlib import Path
+
+import cv2
+import numpy as np
+import pandas as pd
+from mcap.reader import make_reader
+from mcap_ros2.decoder import DecoderFactory
+
+
+class MCAPExtractor:
+    """Extract data from MCAP files"""
+
+    def __init__(
+        self,
+        mcap_path: str,
+        output_dir: str = "demcap_data",
+        create_videos: bool = True,
+        video_fps: int = 30,
+        start_time_ns: int = None,
+        end_time_ns: int = None,
+    ):
+        self.mcap_path = Path(mcap_path)
+        self.output_dir = Path(output_dir)
+        self.output_dir.mkdir(exist_ok=True)
+        self.create_videos = create_videos
+        self.video_fps = video_fps
+        self.start_time_ns = start_time_ns
+        self.end_time_ns = end_time_ns
+
+        # Create subdirectories
+        self.dirs = {
+            "images": self.output_dir / "images",
+            "depth": self.output_dir / "depth",
+            "lidar": self.output_dir / "lidar",
+            "imu": self.output_dir / "imu",
+            "transforms": self.output_dir / "transforms",
+            "other": self.output_dir / "other",
+            "videos": self.output_dir / "videos",
+        }
+        for d in self.dirs.values():
+            d.mkdir(exist_ok=True)
+
+        self.counters = defaultdict(int)
+        self.imu_data = defaultdict(list)
+        self.tf_data = []
+        self.tf_empty_messages = 0
+        self.camera_intrinsics = {}
+        self.decoder_factory = DecoderFactory()
+        self.topic_info = {}
+        self.other_data = defaultdict(list)
+        self.odom_data = []
+        self.joint_states_data = []
+
+    def extract_all(self):
+        """Extract all data from MCAP file"""
+        print(f"{'='*80}")
+        print(f"Extracting data from: {self.mcap_path.name}")
+        print(f"Output directory: {self.output_dir}")
+        if self.start_time_ns is not None and self.end_time_ns is not None:
+            print(f"Time filter: {self.start_time_ns} to {self.end_time_ns}")
+        print(f"{'='*80}")
+
+        # First pass: discover all topics
+        self._discover_topics()
+
+        topic_counts = defaultdict(int)
+
+        with open(self.mcap_path, "rb") as f:
+            reader = make_reader(f)
+
+            for schema, channel, message in reader.iter_messages():
+                topic = channel.topic
+
+                # Check timestamp filter
+                if self.start_time_ns is not None and message.log_time < self.start_time_ns:
+                    continue
+                if self.end_time_ns is not None and message.log_time > self.end_time_ns:
+                    continue
+
+                topic_counts[topic] += 1
+
+                try:
+                    handled = False
+
+                    # Route to appropriate extractor
+                    if "compressedDepth" in topic:
+                        self._extract_depth_image(topic, message, schema, channel)
+                        handled = True
+                    elif "depth/image_rect_raw" in topic or "depth/image_raw" in topic:
+                        self._extract_depth_image_raw(topic, message, schema, channel)
+                        handled = True
+                    elif "color/image_compressed" in topic:
+                        self._extract_rgb_image(topic, message, schema, channel)
+                        handled = True
+                    elif "color/image_raw" in topic:
+                        self._extract_rgb_image_raw(topic, message, schema, channel)
+                        handled = True
+                    elif "pandar_points" in topic or "hesai" in topic:
+                        self._extract_lidar(topic, message)
+                        handled = True
+                    elif "imu" in topic.lower() or "vectornav" in topic.lower():
+                        self._extract_imu(topic, message, schema, channel)
+                        handled = True
+                    elif "camera_info" in topic:
+                        self._extract_camera_info(topic, message, schema, channel)
+                        handled = True
+                    elif "/tf" in topic:
+                        self._extract_transform(topic, message, schema, channel)
+                        handled = True
+                    elif "/odom" in topic:
+                        self._extract_odometry(topic, message, schema, channel)
+                        handled = True
+                    elif "joint_states" in topic.lower():
+                        self._extract_joint_states(topic, message, schema, channel)
+                        handled = True
+
+                    if not handled:
+                        self._extract_other(topic, message, schema, channel)
+
+                    # Progress update
+                    total = sum(topic_counts.values())
+                    if total % 100000 == 0:
+                        print(f"Processed {total} messages...")
+
+                except Exception as e:
+                    print(f"Error processing {topic}: {e}")
+                    continue
+
+        # Save accumulated data
+        self._save_imu_data()
+        self._save_transform_data()
+        self._save_odometry_data()
+        self._save_joint_states_data()
+        self._save_camera_intrinsics()
+        self._save_other_data()
+
+        # Create videos from RGB images
+        if self.create_videos:
+            self._create_videos(fps=self.video_fps)
+
+        # Print summary
+        self._print_summary()
+
+    def _discover_topics(self):
+        """Discover all topics and their schemas in the MCAP file"""
+        print("Discovering topics...")
+
+        with open(self.mcap_path, "rb") as f:
+            reader = make_reader(f)
+
+            for schema, channel, message in reader.iter_messages():
+                topic = channel.topic
+
+                if topic not in self.topic_info:
+                    schema_name = schema.name if schema else "Unknown"
+                    self.topic_info[topic] = {
+                        "topic": topic,
+                        "schema": schema_name,
+                        "encoding": channel.message_encoding,
+                        "count": 0,
+                    }
+
+                self.topic_info[topic]["count"] += 1
+
+        print(f"\nFound {len(self.topic_info)} topics:")
+        for topic, info in sorted(self.topic_info.items()):
+            print(f"  {topic}   [Schema: {info['schema']}, Messages: {info['count']}]")
+
+    def _extract_other(self, topic, message, schema, channel):
+        """Extract data from unhandled topics"""
+        timestamp_ns = message.log_time
+        timestamp_s = timestamp_ns / 1e9
+
+        topic_name = self._sanitize_topic_name(topic)
+
+        try:
+            if schema and schema.name:
+                decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+                msg = decoder(message.data)
+
+                msg_dict = self._ros_msg_to_dict(msg)
+                msg_dict["timestamp_ns"] = timestamp_ns
+                msg_dict["timestamp_s"] = timestamp_s
+                msg_dict["topic"] = topic
+
+                self.other_data[topic_name].append(msg_dict)
+                self.counters[f"other_{topic_name}"] += 1
+        except Exception:
+            raw_dir = self.dirs["other"] / topic_name / "raw"
+            raw_dir.mkdir(parents=True, exist_ok=True)
+
+            filename = f"{timestamp_ns:019d}.bin"
+            output_path = raw_dir / filename
+
+            with open(output_path, "wb") as f:
+                f.write(message.data)
+
+            os.chmod(output_path, 0o644)
+            self.counters[f"other_{topic_name}_raw"] += 1
+
+    def _ros_msg_to_dict(self, msg):
+        """Convert ROS message to dictionary recursively"""
+        result = {}
+
+        for attr in dir(msg):
+            if attr.startswith("_"):
+                continue
+
+            try:
+                value = getattr(msg, attr)
+
+                if callable(value):
+                    continue
+
+                if hasattr(value, "__dict__") and not isinstance(value, (str, bytes)):
+                    result[attr] = self._ros_msg_to_dict(value)
+                elif isinstance(value, (list, tuple)):
+                    if len(value) > 0 and hasattr(value[0], "__dict__"):
+                        result[attr] = [self._ros_msg_to_dict(item) for item in value]
+                    else:
+                        result[attr] = list(value)
+                else:
+                    result[attr] = value
+
+            except Exception:
+                continue
+
+        return result
+
+    def _extract_rgb_image(self, topic, message, schema=None, channel=None):
+        """Extract RGB image"""
+        camera_name = self._sanitize_topic_name(topic)
+        data = message.data[4:]
+
+        jpeg_start = data.find(b"\xff\xd8")
+        if jpeg_start == -1:
+            return
+
+        jpeg_data = data[jpeg_start:]
+
+        camera_dir = self.dirs["images"] / camera_name
+        camera_dir.mkdir(exist_ok=True)
+
+        # Get ROS timestamp from message header (preferred) or fall back to log_time
+        timestamp_ns = message.log_time  # fallback
+        if schema and channel:
+            try:
+                decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+                msg = decoder(message.data)
+                timestamp_ns = msg.header.stamp.sec * 1_000_000_000 + msg.header.stamp.nanosec
+            except Exception:
+                pass  # Use fallback log_time
+
+        filename = f"{timestamp_ns:019d}.jpg"
+        output_path = camera_dir / filename
+
+        with open(output_path, "wb") as f:
+            f.write(jpeg_data)
+
+        os.chmod(output_path, 0o644)
+        self.counters[f"rgb_{camera_name}"] += 1
+
+        if self.counters[f"rgb_{camera_name}"] % 100000 == 0:
+            print(f"  Extracted {self.counters[f'rgb_{camera_name}']} RGB images from {camera_name}")
+
+    def _extract_rgb_image_raw(self, topic, message, schema, channel):
+        """Extract raw RGB image (sensor_msgs/msg/Image)"""
+        camera_name = self._sanitize_topic_name(topic)
+
+        try:
+            decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+            msg = decoder(message.data)
+
+            height = msg.height
+            width = msg.width
+            encoding = msg.encoding
+
+            if encoding == "rgb8":
+                img_data = np.frombuffer(msg.data, dtype=np.uint8).reshape(height, width, 3)
+                img_data = cv2.cvtColor(img_data, cv2.COLOR_RGB2BGR)
+            elif encoding == "bgr8":
+                img_data = np.frombuffer(msg.data, dtype=np.uint8).reshape(height, width, 3)
+            elif encoding == "mono8":
+                img_data = np.frombuffer(msg.data, dtype=np.uint8).reshape(height, width)
+            else:
+                print(f"  Warning: Unsupported encoding {encoding} for {camera_name}")
+                return
+
+            camera_dir = self.dirs["images"] / camera_name
+            camera_dir.mkdir(exist_ok=True)
+
+            # Use ROS timestamp from message header
+            timestamp_ns = msg.header.stamp.sec * 1_000_000_000 + msg.header.stamp.nanosec
+            filename = f"{timestamp_ns:019d}.jpg"
+            output_path = camera_dir / filename
+
+            cv2.imwrite(str(output_path), img_data)
+            os.chmod(output_path, 0o644)
+
+            self.counters[f"rgb_{camera_name}"] += 1
+
+            if self.counters[f"rgb_{camera_name}"] % 100000 == 0:
+                print(f"  Extracted {self.counters[f'rgb_{camera_name}']} RGB images from {camera_name}")
+
+        except Exception as e:
+            print(f"  Error extracting raw RGB image from {camera_name}: {e}")
+
+    def _extract_depth_image(self, topic, message, schema=None, channel=None):
+        """Extract depth image"""
+        data = message.data[4:]
+
+        png_start = data.find(b"\x89PNG")
+        if png_start != -1:
+            png_data = data[png_start:]
+            camera_name = self._sanitize_topic_name(topic)
+            camera_dir = self.dirs["depth"] / camera_name
+            camera_dir.mkdir(exist_ok=True)
+
+            # Get ROS timestamp from message header (preferred) or fall back to log_time
+            timestamp_ns = message.log_time  # fallback
+            if schema and channel:
+                try:
+                    decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+                    msg = decoder(message.data)
+                    timestamp_ns = msg.header.stamp.sec * 1_000_000_000 + msg.header.stamp.nanosec
+                except Exception:
+                    pass  # Use fallback log_time
+
+            filename = f"{timestamp_ns:019d}.png"
+            output_path = camera_dir / filename
+
+            with open(output_path, "wb") as f:
+                f.write(png_data)
+
+            os.chmod(output_path, 0o644)
+            self.counters[f"depth_{camera_name}"] += 1
+
+            if self.counters[f"depth_{camera_name}"] % 100000 == 0:
+                print(f"  Extracted {self.counters[f'depth_{camera_name}']} depth images from {camera_name}")
+
+    def _extract_depth_image_raw(self, topic, message, schema, channel):
+        """Extract raw depth image (sensor_msgs/msg/Image)"""
+        camera_name = self._sanitize_topic_name(topic)
+
+        try:
+            decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+            msg = decoder(message.data)
+
+            height = msg.height
+            width = msg.width
+            encoding = msg.encoding
+
+            if encoding == "16UC1":
+                depth_data = np.frombuffer(msg.data, dtype=np.uint16).reshape(height, width)
+            elif encoding == "32FC1":
+                depth_data = np.frombuffer(msg.data, dtype=np.float32).reshape(height, width)
+                depth_data = (depth_data * 1000).astype(np.uint16)
+            else:
+                print(f"  Warning: Unsupported depth encoding {encoding} for {camera_name}")
+                return
+
+            camera_dir = self.dirs["depth"] / camera_name
+            camera_dir.mkdir(exist_ok=True)
+
+            # Use ROS timestamp from message header
+            timestamp_ns = msg.header.stamp.sec * 1_000_000_000 + msg.header.stamp.nanosec
+            filename = f"{timestamp_ns:019d}.png"
+            output_path = camera_dir / filename
+
+            cv2.imwrite(str(output_path), depth_data)
+            os.chmod(output_path, 0o644)
+
+            self.counters[f"depth_{camera_name}"] += 1
+
+            if self.counters[f"depth_{camera_name}"] % 100000 == 0:
+                print(f"  Extracted {self.counters[f'depth_{camera_name}']} depth images from {camera_name}")
+
+        except Exception as e:
+            print(f"  Error extracting raw depth image from {camera_name}: {e}")
+
+    def _extract_lidar(self, topic, message):
+        """Extract LiDAR point cloud as binary file"""
+        try:
+            data = message.data[4:]
+
+            timestamp_ns = message.log_time
+            filename = f"{timestamp_ns:019d}.bin"
+            output_path = self.dirs["lidar"] / filename
+
+            with open(output_path, "wb") as f:
+                f.write(data)
+
+            os.chmod(output_path, 0o644)
+            self.counters["lidar"] += 1
+
+            if self.counters["lidar"] % 100000 == 0:
+                print(f"  Extracted {self.counters['lidar']} LiDAR scans")
+
+        except Exception as e:
+            print(f"Error extracting LiDAR: {e}")
+
+    def _extract_imu(self, topic, message, schema, channel):
+        """Extract IMU data"""
+        timestamp_ns = message.log_time
+        timestamp_s = timestamp_ns / 1e9
+
+        topic_name = self._sanitize_topic_name(topic)
+
+        if schema and schema.name:
+            if "Imu" in schema.name:
+                try:
+                    decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+                    msg = decoder(message.data)
+
+                    self.imu_data[topic_name].append(
+                        {
+                            "timestamp_ns": timestamp_ns,
+                            "timestamp_s": timestamp_s,
+                            "type": "imu",
+                            "orientation_x": msg.orientation.x,
+                            "orientation_y": msg.orientation.y,
+                            "orientation_z": msg.orientation.z,
+                            "orientation_w": msg.orientation.w,
+                            "angular_velocity_x": msg.angular_velocity.x,
+                            "angular_velocity_y": msg.angular_velocity.y,
+                            "angular_velocity_z": msg.angular_velocity.z,
+                            "linear_acceleration_x": msg.linear_acceleration.x,
+                            "linear_acceleration_y": msg.linear_acceleration.y,
+                            "linear_acceleration_z": msg.linear_acceleration.z,
+                        }
+                    )
+                except Exception as e:
+                    print(f"    Warning: Could not deserialize IMU message for {topic_name}: {e}")
+
+            elif "Temperature" in schema.name:
+                try:
+                    data = message.data[4:]
+                    if len(data) >= 16:
+                        temp = struct.unpack("<d", data[-16:-8])[0]
+                        self.imu_data[topic_name].append(
+                            {
+                                "timestamp_ns": timestamp_ns,
+                                "timestamp_s": timestamp_s,
+                                "temperature": temp,
+                                "type": "temperature",
+                            }
+                        )
+                except Exception:
+                    pass
+
+            elif "Float64MultiArray" in schema.name:
+                try:
+                    decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+                    msg = decoder(message.data)
+
+                    bias_values = list(msg.data) if hasattr(msg, "data") else []
+
+                    data_dict = {
+                        "timestamp_ns": timestamp_ns,
+                        "timestamp_s": timestamp_s,
+                        "type": "bias",
+                    }
+
+                    for i, val in enumerate(bias_values):
+                        data_dict[f"bias_{i}"] = val
+
+                    self.imu_data[topic_name].append(data_dict)
+
+                except Exception as e:
+                    print(f"    Warning: Could not deserialize bias data for {topic_name}: {e}")
+
+            elif "MagneticField" in schema.name:
+                try:
+                    decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+                    msg = decoder(message.data)
+
+                    self.imu_data[topic_name].append(
+                        {
+                            "timestamp_ns": timestamp_ns,
+                            "timestamp_s": timestamp_s,
+                            "type": "magnetic",
+                            "magnetic_field_x": msg.magnetic_field.x,
+                            "magnetic_field_y": msg.magnetic_field.y,
+                            "magnetic_field_z": msg.magnetic_field.z,
+                        }
+                    )
+                except Exception as e:
+                    print(f"    Warning: Could not deserialize magnetic field for {topic_name}: {e}")
+
+            elif "FluidPressure" in schema.name:
+                try:
+                    decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+                    msg = decoder(message.data)
+
+                    self.imu_data[topic_name].append(
+                        {
+                            "timestamp_ns": timestamp_ns,
+                            "timestamp_s": timestamp_s,
+                            "type": "pressure",
+                            "fluid_pressure": msg.fluid_pressure,
+                            "variance": msg.variance if hasattr(msg, "variance") else None,
+                        }
+                    )
+                except Exception as e:
+                    print(f"    Warning: Could not deserialize pressure data for {topic_name}: {e}")
+
+    def _extract_camera_info(self, topic, message, schema, channel):
+        """Extract camera calibration info"""
+        camera_name = self._sanitize_topic_name(topic)
+
+        if camera_name in self.camera_intrinsics:
+            self.counters[f"camera_info_{camera_name}"] += 1
+            return
+
+        try:
+            decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+            msg = decoder(message.data)
+
+            intrinsics = {
+                "topic": topic,
+                "timestamp_ns": message.log_time,
+                "header": {
+                    "stamp": {"sec": msg.header.stamp.sec, "nanosec": msg.header.stamp.nanosec},
+                    "frame_id": msg.header.frame_id,
+                },
+                "height": msg.height,
+                "width": msg.width,
+                "distortion_model": msg.distortion_model,
+                "D": list(msg.d) if hasattr(msg, "d") else [],
+                "K": list(msg.k) if hasattr(msg, "k") else [],
+                "R": list(msg.r) if hasattr(msg, "r") else [],
+                "P": list(msg.p) if hasattr(msg, "p") else [],
+                "binning_x": msg.binning_x,
+                "binning_y": msg.binning_y,
+                "roi": {
+                    "x_offset": msg.roi.x_offset,
+                    "y_offset": msg.roi.y_offset,
+                    "height": msg.roi.height,
+                    "width": msg.roi.width,
+                    "do_rectify": msg.roi.do_rectify,
+                },
+            }
+
+            self.camera_intrinsics[camera_name] = intrinsics
+
+        except Exception as e:
+            print(f"    Warning: Could not deserialize camera_info for {camera_name}: {e}")
+
+        self.counters[f"camera_info_{camera_name}"] += 1
+
+    def _extract_transform(self, topic, message, schema, channel):
+        """Extract transform data"""
+        timestamp_ns = message.log_time
+        timestamp_s = timestamp_ns / 1e9
+
+        try:
+            decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+            msg = decoder(message.data)
+
+            if hasattr(msg, "transforms") and len(msg.transforms) > 0:
+                for transform in msg.transforms:
+                    self.tf_data.append(
+                        {
+                            "timestamp_ns": timestamp_ns,
+                            "timestamp_s": timestamp_s,
+                            "topic": topic,
+                            "parent_frame": transform.header.frame_id,
+                            "child_frame": transform.child_frame_id,
+                            "transform_sec": transform.header.stamp.sec,
+                            "transform_nanosec": transform.header.stamp.nanosec,
+                            "translation_x": transform.transform.translation.x,
+                            "translation_y": transform.transform.translation.y,
+                            "translation_z": transform.transform.translation.z,
+                            "rotation_x": transform.transform.rotation.x,
+                            "rotation_y": transform.transform.rotation.y,
+                            "rotation_z": transform.transform.rotation.z,
+                            "rotation_w": transform.transform.rotation.w,
+                        }
+                    )
+                self.counters[f'transform_{topic.replace("/", "")}'] += len(msg.transforms)
+            else:
+                self.tf_empty_messages += 1
+
+        except Exception as e:
+            print(f"    Warning: Could not deserialize transform for {topic}: {e}")
+            self.counters["transform_errors"] += 1
+
+    def _extract_odometry(self, topic, message, schema, channel):
+        """Extract odometry data"""
+        timestamp_ns = message.log_time
+        timestamp_s = timestamp_ns / 1e9
+
+        try:
+            decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+            msg = decoder(message.data)
+
+            self.odom_data.append(
+                {
+                    "timestamp_ns": timestamp_ns,
+                    "timestamp_s": timestamp_s,
+                    "topic": topic,
+                    "frame_id": msg.header.frame_id,
+                    "child_frame_id": msg.child_frame_id,
+                    "position_x": msg.pose.pose.position.x,
+                    "position_y": msg.pose.pose.position.y,
+                    "position_z": msg.pose.pose.position.z,
+                    "orientation_x": msg.pose.pose.orientation.x,
+                    "orientation_y": msg.pose.pose.orientation.y,
+                    "orientation_z": msg.pose.pose.orientation.z,
+                    "orientation_w": msg.pose.pose.orientation.w,
+                    "linear_velocity_x": msg.twist.twist.linear.x,
+                    "linear_velocity_y": msg.twist.twist.linear.y,
+                    "linear_velocity_z": msg.twist.twist.linear.z,
+                    "angular_velocity_x": msg.twist.twist.angular.x,
+                    "angular_velocity_y": msg.twist.twist.angular.y,
+                    "angular_velocity_z": msg.twist.twist.angular.z,
+                }
+            )
+
+            self.counters["odometry"] += 1
+
+        except Exception as e:
+            print(f"    Warning: Could not deserialize odometry for {topic}: {e}")
+
+    def _extract_joint_states(self, topic, message, schema, channel):
+        """Extract joint states data"""
+        timestamp_ns = message.log_time
+        timestamp_s = timestamp_ns / 1e9
+
+        try:
+            decoder = self.decoder_factory.decoder_for(channel.message_encoding, schema)
+            msg = decoder(message.data)
+
+            joint_state = {
+                "timestamp_ns": timestamp_ns,
+                "timestamp_s": timestamp_s,
+                "topic": topic,
+                "frame_id": msg.header.frame_id,
+            }
+
+            for i, name in enumerate(msg.name):
+                joint_state[f"{name}_position"] = msg.position[i] if i < len(msg.position) else None
+                joint_state[f"{name}_velocity"] = msg.velocity[i] if i < len(msg.velocity) else None
+                joint_state[f"{name}_effort"] = msg.effort[i] if i < len(msg.effort) else None
+
+            self.joint_states_data.append(joint_state)
+            self.counters["joint_states"] += 1
+
+        except Exception as e:
+            print(f"    Warning: Could not deserialize joint states for {topic}: {e}")
+
+    def _save_imu_data(self):
+        """Save accumulated IMU data to CSV files"""
+        print("\nSaving IMU data to CSV...")
+        for topic_name, data_list in self.imu_data.items():
+            if data_list:
+                df = pd.DataFrame(data_list)
+                output_path = self.dirs["imu"] / f"{topic_name}.csv"
+                df.to_csv(output_path, index=False)
+                print(f"  Saved {len(data_list)} IMU records to {output_path.name}")
+
+    def _save_transform_data(self):
+        """Save transform data to CSV"""
+        if self.tf_data:
+            print("\nSaving transform data to CSV...")
+            df = pd.DataFrame(self.tf_data)
+            output_path = self.dirs["transforms"] / "transforms.csv"
+            df.to_csv(output_path, index=False)
+            print(f"  Saved {len(self.tf_data)} transform records to {output_path.name}")
+
+    def _save_odometry_data(self):
+        """Save odometry data to CSV"""
+        if self.odom_data:
+            print("\nSaving odometry data to CSV...")
+            df = pd.DataFrame(self.odom_data)
+            output_path = self.output_dir / "odometry.csv"
+            df.to_csv(output_path, index=False)
+            print(f"  Saved {len(self.odom_data)} odometry records to {output_path.name}")
+
+    def _save_joint_states_data(self):
+        """Save joint states data to CSV"""
+        if self.joint_states_data:
+            print("\nSaving joint states data to CSV...")
+            df = pd.DataFrame(self.joint_states_data)
+            output_path = self.output_dir / "joint_states.csv"
+            df.to_csv(output_path, index=False)
+            print(f"  Saved {len(self.joint_states_data)} joint state records to {output_path.name}")
+
+    def _save_camera_intrinsics(self):
+        """Save all camera intrinsics to a single JSON file"""
+        if self.camera_intrinsics:
+            print("\nSaving camera intrinsics...")
+            output_path = self.output_dir / "camera_intrinsics.json"
+            with open(output_path, "w") as f:
+                json.dump(self.camera_intrinsics, f, indent=2)
+            print(f"  Saved intrinsics for {len(self.camera_intrinsics)} cameras to {output_path.name}")
+
+    def _save_other_data(self):
+        """Save other topic data to JSON files"""
+        if self.other_data:
+            print("\nSaving other topic data...")
+            for topic_name, data_list in self.other_data.items():
+                if data_list:
+                    topic_dir = self.dirs["other"] / topic_name
+                    topic_dir.mkdir(parents=True, exist_ok=True)
+
+                    output_path = topic_dir / f"{topic_name}.json"
+                    with open(output_path, "w") as f:
+                        json.dump(data_list, f, indent=2, default=str)
+                    print(f"  Saved {len(data_list)} messages from {topic_name}")
+
+    def _create_videos(self, fps=30):
+        """Create videos from extracted RGB images"""
+        images_dir = self.dirs["images"]
+        videos_dir = self.dirs["videos"]
+
+        camera_dirs = [d for d in images_dir.iterdir() if d.is_dir()]
+
+        if not camera_dirs:
+            print("\nNo RGB images found to create videos.")
+            return
+
+        print(f"\n{'='*80}")
+        print("CREATING VIDEOS FROM RGB IMAGES")
+        print(f"{'='*80}")
+
+        for camera_dir in sorted(camera_dirs):
+            camera_name = camera_dir.name
+
+            image_files = sorted(camera_dir.glob("*.jpg"))
+
+            if not image_files:
+                continue
+
+            print(f"Processing {camera_name}...")
+            print(f"  Found {len(image_files)} images")
+
+            try:
+                first_img = cv2.imread(str(image_files[0]))
+                if first_img is None:
+                    print("  Error: Could not read first image")
+                    continue
+
+                height, width = first_img.shape[:2]
+                print(f"  Image size: {width}x{height}")
+
+                video_path = videos_dir / f"{camera_name}.mp4"
+                fourcc = cv2.VideoWriter_fourcc(*"mp4v")
+                video_writer = cv2.VideoWriter(str(video_path), fourcc, fps, (width, height))
+
+                if not video_writer.isOpened():
+                    print("  Error: Could not create video writer")
+                    continue
+
+                frame_count = 0
+                for img_file in image_files:
+                    img = cv2.imread(str(img_file))
+                    if img is not None:
+                        video_writer.write(img)
+                        frame_count += 1
+                        if frame_count % 1000 == 0:
+                            print(f"  Processed {frame_count}/{len(image_files)} frames...")
+
+                video_writer.release()
+
+                duration = frame_count / fps
+                print(f"  Created video: {video_path.name}")
+                print(f"    Frames: {frame_count}, Duration: {duration:.2f}s, FPS: {fps}")
+
+            except Exception as e:
+                print(f"  Error creating video for {camera_name}: {e}")
+                continue
+
+        print(f"\n{'='*80}\n")
+
+    def _sanitize_topic_name(self, topic):
+        """Convert topic name to filesystem-safe name"""
+        return topic.replace("/", "_").strip("_")
+
+    def _print_summary(self):
+        """Print extraction summary"""
+        print(f"\n{'='*80}")
+        print("EXTRACTION SUMMARY")
+        print(f"{'='*80}")
+
+        print(f"\nOutput directory: {self.output_dir}")
+
+        print(f"\nTotal topics found: {len(self.topic_info)}")
+
+        if any("rgb_" in k for k in self.counters.keys()):
+            print("\nRGB Images:")
+            for k, v in sorted(self.counters.items()):
+                if "rgb_" in k:
+                    print(f"  {k.replace('rgb_', '')}: {v} images")
+
+        if any("depth_" in k and not k.startswith("camera_info_") for k in self.counters.keys()):
+            print("\nDepth Images:")
+            for k, v in sorted(self.counters.items()):
+                if "depth_" in k and not k.startswith("camera_info_"):
+                    print(f"  {k.replace('depth_', '')}: {v} images")
+
+        if "lidar" in self.counters:
+            print(f"\nLiDAR Point Clouds: {self.counters['lidar']} scans")
+
+        if self.imu_data:
+            print("\nIMU Data:")
+            for topic, data in self.imu_data.items():
+                print(f"  {topic}: {len(data)} records")
+
+        if self.tf_data or self.tf_empty_messages > 0:
+            print("\nTransforms:")
+            print(f"  Total transform records: {len(self.tf_data)}")
+            if self.tf_empty_messages > 0:
+                total_tf_messages = self.topic_info.get("/tf", {}).get("count", 0) + self.topic_info.get(
+                    "/tf_static", {}
+                ).get("count", 0)
+                print(f"  Total TFMessage messages: {total_tf_messages}")
+                print(f"  Empty TFMessage messages: {self.tf_empty_messages} (common in ROS, not an error)")
+                print(f"  Messages with transforms: {total_tf_messages - self.tf_empty_messages}")
+
+        if self.camera_intrinsics:
+            print(f"\nCamera Intrinsics: {len(self.camera_intrinsics)} cameras")
+            for name, info in sorted(self.camera_intrinsics.items()):
+                if info["K"] and len(info["K"]) == 9:
+                    fx, fy = info["K"][0], info["K"][4]
+                    cx, cy = info["K"][2], info["K"][5]
+                    print(f"  {name}: {info['width']}x{info['height']}, fx={fx:.1f}, fy={fy:.1f}")
+
+        if self.odom_data:
+            print(f"\nOdometry: {len(self.odom_data)} records")
+
+        if self.joint_states_data:
+            print(f"\nJoint States: {len(self.joint_states_data)} records")
+
+        other_topics = [k for k in self.counters.keys() if k.startswith("other_")]
+        if other_topics:
+            print("\nOther Topics:")
+            for k in sorted(other_topics):
+                if "_raw" in k:
+                    topic_name = k.replace("other_", "").replace("_raw", "")
+                    print(f"  {topic_name} (raw): {self.counters[k]} messages")
+                else:
+                    topic_name = k.replace("other_", "")
+                    print(f"  {topic_name}: {self.counters[k]} messages")
+
+        topic_info_path = self.output_dir / "topic_info.json"
+        with open(topic_info_path, "w") as f:
+            json.dump(self.topic_info, f, indent=2)
+        print(f"\nTopic information saved to: {topic_info_path.name}")