initial SLAM viewer

.gitignore

@@ -0,0 +1,168 @@
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+*.dll
+*.dylib
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+# Usually these files are written by a python script from a template
+# before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+.python-version
+
+# pipenv
+# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+# Therefore, we exclude only the virtual environment folder.
+Pipfile
+# However, if you prefer to exclude Pipfile as well, uncomment the next line:
+# Pipfile
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# ROS 2 specific
+install/
+build/
+log/
+colcon_*
+*.qws
+*.swp
+*.swo
+
+# PyVista / VTK cache
+vtk_*
+*.vtu
+*.vtp
+*.ply
+*.pcd
+*.las
+*.xyz
+
+# IDEs and editors
+.vscode/
+.idea/
+*.swp
+*.swo
+.DS_Store
+Thumbs.db
+
+# Logs and databases
+*.log
+*.sqlite
+
+# Local configuration
+config.local.py
+secrets.py
+
+# Temporary files
+*.tmp
+*.temp

lidar_viewer.py

@@ -0,0 +1,95 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+import numpy as np
+import open3d as o3d
+from pyvistaqt import BackgroundPlotter
+import pyvista as pv
+import queue
+import threading
+
+class LidarViewer(Node):
+    def __init__(self, data_queue):
+        super().__init__('lidar_pyvista_viewer')
+        self.data_queue = data_queue
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            '/rs_lidar/points',  # 替换为你的 topic
+            self.pointcloud_callback,
+            10
+        )
+        self.get_logger().info('等待点云数据...')
+
+    def pointcloud_callback(self, msg):
+        # 这里只做快速解析，不进行可视化操作！
+        points = self.pointcloud2_to_array(msg)
+        if points is not None:
+            # 将点云放入队列（线程安全）
+            self.data_queue.put(points)
+            self.get_logger().info(f'显示 {len(points)} 个点')
+
+    def pointcloud2_to_array(self, msg):
+        # 简化版：假设是 XYZ 格式（根据你的实际格式调整）
+        try:
+            data = np.frombuffer(msg.data, dtype=np.float32)
+            points = data.reshape(-1, 4)[:, :3]  # 取 x, y, z
+            return points
+        except Exception as e:
+            self.get_logger().error(f'解析失败: {e}')
+            return None
+
+
+def main():
+    rclpy.init()
+    data_queue = queue.Queue()
+    node = LidarViewer(data_queue)
+
+    # 启动 ROS 2 spin 线程（非阻塞）
+    ros_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    ros_thread.start()
+
+    # 初始化 PyVista 可视化（主线程）
+    plotter = BackgroundPlotter()
+    plotter.add_axes()
+    point_cloud_actor = None
+
+    try:
+        while rclpy.ok():
+            # 从队列取最新点云（非阻塞）
+            try:
+                points = data_queue.get_nowait()
+                cloud = pv.PolyData(points)
+
+                # 如果之前有点云 actor，先移除它
+                if point_cloud_actor is not None:
+                    plotter.remove_actor(point_cloud_actor)
+
+                # 添加新的点云并获取新的 actor
+                point_cloud_actor = plotter.add_points(
+                    cloud,
+                    render_points_as_spheres=False,
+                    point_size=2
+                )
+
+                # 自动缩放到包含所有点云的范围
+                plotter.reset_camera()
+
+            except queue.Empty:
+                pass
+
+            # 让 PyVista 处理 GUI 事件（关键！）
+            plotter.app.processEvents()
+            # 短暂休眠避免 CPU 占用过高
+            import time
+            time.sleep(0.01)
+
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        plotter.close()
+
+
+if __name__ == '__main__':
+    main()