source /opt/ros/humble/setup.bash source ~/slam_ws/install/setup.bash source ~/super_ws/install/setup.bash FULL_BAG=$(cat ~/rosbags/.latest_indoor_hover) OUT_PLY=~/rosbags/drobotics_cloud_registered_preview.ply echo "Bag: $FULL_BAG" echo "Output: $OUT_PLY" python3 - <<'PY' from pathlib import Path import sqlite3 import struct import math import random from rclpy.serialization import deserialize_message from rosidl_runtime_py.utilities import get_message bag_path = Path.home().joinpath("rosbags/.latest_indoor_hover").read_text().strip() db_files = sorted(Path(bag_path).glob("*.db3")) if not db_files: raise SystemExit(f"ERROR: no db3 file found in {bag_path}") db_path = db_files[0] out_path = Path.home().joinpath("rosbags/drobotics_cloud_registered_preview.ply") conn = sqlite3.connect(str(db_path)) cur = conn.cursor() topic_row = cur.execute( "SELECT id, type FROM topics WHERE name = '/cloud_registered'" ).fetchone() if topic_row is None: raise SystemExit("ERROR: /cloud_registered not found in bag") topic_id, topic_type = topic_row MsgType = get_message(topic_type) rows = cur.execute( "SELECT data FROM messages WHERE topic_id = ? ORDER BY timestamp", (topic_id,), ) points = [] max_frames = 160 frame_stride = 4 point_stride = 25 max_points = 250000 def read_points_xyz(msg): fields = {f.name: f for f in msg.fields} if not all(k in fields for k in ("x", "y", "z")): return xoff = fields["x"].offset yoff = fields["y"].offset zoff = fields["z"].offset step = msg.point_step data = msg.data endian = ">" if msg.is_bigendian else "<" for i in range(0, msg.width * msg.height, point_stride): base = i * step try: x = struct.unpack_from(endian + "f", data, base + xoff)[0] y = struct.unpack_from(endian + "f", data, base + yoff)[0] z = struct.unpack_from(endian + "f", data, base + zoff)[0] except struct.error: continue if math.isfinite(x) and math.isfinite(y) and math.isfinite(z): points.append((x, y, z)) frame_count = 0 used_frames = 0 for index, (blob,) in enumerate(rows): frame_count += 1 if index % frame_stride != 0: continue msg = deserialize_message(blob, MsgType) read_points_xyz(msg) used_frames += 1 if used_frames >= max_frames or len(points) >= max_points: break if len(points) > max_points: random.seed(7) points = random.sample(points, max_points) with out_path.open("w") as f: f.write("ply\n") f.write("format ascii 1.0\n") f.write("comment Drobotics / 卓博泰智能科技 / www.drobotics.tech\n") f.write(f"element vertex {len(points)}\n") f.write("property float x\n") f.write("property float y\n") f.write("property float z\n") f.write("end_header\n") for x, y, z in points: f.write(f"{x:.4f} {y:.4f} {z:.4f}\n") print(f"DB: {db_path}") print(f"Frames in bag scanned: {frame_count}") print(f"Frames used: {used_frames}") print(f"Points written: {len(points)}") print(f"Output: {out_path}") PY ls -lh "$OUT_PLY"

scp drobotics@172.20.10.3:~/rosbags/drobotics_cloud_registered_preview.ply ~/Desktop/

python3 -m pip install open3d python3 - <<'PY' import open3d as o3d from pathlib import Path ply = Path.home() / "Desktop/drobotics_cloud_registered_preview.ply" pcd = o3d.io.read_point_cloud(str(ply)) print("File:", ply) print("Points:", len(pcd.points)) print("Bounds min:", pcd.get_min_bound()) print("Bounds max:", pcd.get_max_bound()) print("Center:", pcd.get_center()) pcd = pcd.voxel_down_sample(voxel_size=0.05) points = pcd.points colors = [] zs = [p[2] for p in points] zmin, zmax = min(zs), max(zs) span = max(zmax - zmin, 1e-6) for p in points: t = (p[2] - zmin) / span colors.append([t, 0.4, 1.0 - t]) pcd.colors = o3d.utility.Vector3dVector(colors) axis = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5) o3d.visualization.draw_geometries( [pcd, axis], window_name="Drobotics / 卓博泰智能科技 — cloud_registered preview", width=1280, height=800, ) PY