一、启动机械臂和相机
source /opt/ros/noetic/setup.bash source ~/aubo_ws/devel/setup.bash roslaunch aubo_driver aubo_real_camera.launch二、验证TF和点云话题
1.检查TF坐标树
rosrun tf tf_echo base_link camera_color_optical_frame2 检查点云话题是否正常发布
rostopic list | grep -E 'points|depth'3.检查点云发布频率
rostopic hz /camera/depth/color/points4.检查点云消息类型
rostopic type /camera/depth/color/pointssensor_msgs/PointCloud2
5. 确认点云坐标系
rostopic echo -n 1 /camera/depth/color/points/headerframe_id: "camera_color_optical_frame" 点云中的 XYZ 坐标是以相机光学坐标系为参考的
6.检查点云结构和颜色
rostopic echo -n 1 /camera/depth/color/points/fields rostopic echo -n 1 /camera/depth/color/points/point_step rostopic echo -n 1 /camera/depth/color/points/row_step rostopic echo -n 1 /camera/depth/color/points/is_dense7.点云处理依赖
1. 检查依赖
python3 -c "import rospy, tf2_ros, tf2_sensor_msgs, numpy, open3d; print('依赖检查通过')"2. 安装缺失的依赖（没有则跳过）
sudo apt update sudo apt install -y ros-noetic-tf2-sensor-msgs然后重新加载 ROS 环境：
source /opt/ros/noetic/setup.bash source ~/aubo_ws/devel/setup.bash三、编写实时点云显示和保存程序
编写程序
nano ~/aubo_ws/grape_project/scripts/view_l515_cloud.py#!/usr/bin/env python3 # -*- coding: utf-8 -*- import os import time import threading from datetime import datetime import numpy as np import rospy import open3d as o3d import tf2_ros import tf.transformations as tft import sensor_msgs.point_cloud2 as pc2 from sensor_msgs.msg import PointCloud2, PointField # ============================================================ # 1. 参数配置 # ============================================================ TOPIC = "/camera/depth/color/points" # 原始点云坐标系 SOURCE_FRAME = "camera_color_optical_frame" # 最终显示和保存的坐标系 BASE_FRAME = "base_link" CHECK_FRAME = True # 相机坐标系下的深度过滤范围 MIN_DEPTH = 0.20 MAX_DEPTH = 2.00 # 体素降采样 VOXEL_SIZE = 0.005 # 点云处理间隔 PROCESS_INTERVAL = 0.20 QUEUE_SIZE = 1 BUFF_SIZE = 32 * 1024 * 1024 SAVE_DIR = os.path.expanduser( "~/aubo_ws/grape_project/pointcloud_output" ) WINDOW_NAME = "L515 Point Cloud - base_link" WINDOW_WIDTH = 1280 WINDOW_HEIGHT = 800 POINT_SIZE = 2.0 BACKGROUND_COLOR = [0.05, 0.05, 0.05] # base_link 通常为 x 前、y 左、z 上。 # 以下是 Open3D 初始观察方向，可根据实际显示调整。 VIEW_UP = [0.0, 0.0, 1.0] VIEW_FRONT = [-1.0, 0.0, 0.0] DEFAULT_COLOR = [0.20, 0.75, 0.95] # ============================================================ # 2. 点云显示类 # ============================================================ class L515CloudViewer: def __init__(self): rospy.init_node( "l515_base_cloud_viewer", anonymous=False ) os.makedirs(SAVE_DIR, exist_ok=True) self.msg_lock = threading.Lock() self.cloud_lock = threading.Lock() self.latest_msg = None self.msg_version = 0 self.processed_msg_version = -1 self.latest_cloud = None self.latest_tf_matrix = None self.latest_stamp = None self.latest_source_frame = None self.cloud_version = 0 self.displayed_version = -1 self.worker_stop = threading.Event() self.pcd = o3d.geometry.PointCloud() self.geometry_added = False self.quit_requested = False # ---------------------------------------------------- # TF2 # ---------------------------------------------------- self.tf_buffer = tf2_ros.Buffer( cache_time=rospy.Duration(10.0) ) self.tf_listener = tf2_ros.TransformListener( self.tf_buffer ) # ---------------------------------------------------- # Open3D # ---------------------------------------------------- self.vis = ( o3d.visualization.VisualizerWithKeyCallback() ) created = self.vis.create_window( window_name=WINDOW_NAME, width=WINDOW_WIDTH, height=WINDOW_HEIGHT, visible=True ) if not created: raise RuntimeError("Open3D 窗口创建失败") render_option = self.vis.get_render_option() if render_option is not None: render_option.point_size = POINT_SIZE render_option.background_color = np.asarray( BACKGROUND_COLOR, dtype=np.float64 ) self.vis.register_key_callback( ord("S"), self.save_callback ) self.vis.register_key_callback( ord("s"), self.save_callback ) self.vis.register_key_callback( ord("Q"), self.quit_callback ) self.vis.register_key_callback( ord("q"), self.quit_callback ) # ---------------------------------------------------- # ROS Subscriber # ---------------------------------------------------- self.sub = rospy.Subscriber( TOPIC, PointCloud2, self.cloud_callback, queue_size=QUEUE_SIZE, buff_size=BUFF_SIZE ) rospy.loginfo("======================================") rospy.loginfo("L515 基座坐标系点云程序启动") rospy.loginfo("点云话题: %s", TOPIC) rospy.loginfo("源坐标系: %s", SOURCE_FRAME) rospy.loginfo("目标坐标系: %s", BASE_FRAME) rospy.loginfo("深度范围: %.2f - %.2f m", MIN_DEPTH, MAX_DEPTH) rospy.loginfo("体素尺寸: %.3f m", VOXEL_SIZE) rospy.loginfo("保存目录: %s", SAVE_DIR) rospy.loginfo("按 S 保存 PLY 和变换信息") rospy.loginfo("按 Q 退出") rospy.loginfo("======================================") # ======================================================== # 3. ROS 回调 # ======================================================== def cloud_callback(self, msg): with self.msg_lock: self.latest_msg = msg self.msg_version += 1 # ======================================================== # 4. RGB 解码 # ======================================================== @staticmethod def decode_rgb(packed_color, datatype): if datatype == PointField.FLOAT32: packed = np.asarray( packed_color, dtype=np.float32 ).view(np.uint32).item() elif datatype in ( PointField.UINT32, PointField.INT32 ): packed = int(packed_color) & 0xFFFFFFFF else: raise ValueError( "不支持的 RGB 字段类型: {}".format(datatype) ) r = (packed >> 16) & 0xFF g = (packed >> 8) & 0xFF b = packed & 0xFF return ( r / 255.0, g / 255.0, b / 255.0 ) # ======================================================== # 5. TF2 坐标变换 # ======================================================== def transform_points_to_base(self, xyz, msg): source_frame = msg.header.frame_id if CHECK_FRAME and source_frame != SOURCE_FRAME: rospy.logerr_throttle( 5.0, "点云源坐标系不匹配: %s，预期: %s", source_frame, SOURCE_FRAME ) return None, None # 不允许使用零时间戳代替真实采集时间 if msg.header.stamp == rospy.Time(0): rospy.logwarn_throttle( 5.0, "点云时间戳为零，拒绝进行动态 TF 转换" ) return None, None try: # 查询点云采集时刻的： # base_link <- camera_color_optical_frame transform_stamped = ( self.tf_buffer.lookup_transform( BASE_FRAME, source_frame, msg.header.stamp, rospy.Duration(0.5) ) ) trans = transform_stamped.transform.translation rot = transform_stamped.transform.rotation q = [ rot.x, rot.y, rot.z, rot.w ] R = tft.quaternion_matrix(q)[:3, :3] t = np.array([ trans.x, trans.y, trans.z ], dtype=np.float64) # P_base = R * P_camera + t xyz_base = xyz @ R.T + t # 保存完整 4×4 变换矩阵 T = np.eye(4, dtype=np.float64) T[:3, :3] = R T[:3, 3] = t return xyz_base, T except ( tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException ) as exc: rospy.logwarn_throttle( 2.0, "TF 转换失败: %s", str(exc) ) return None, None # ======================================================== # 6. 点云读取与处理 # ======================================================== def process_cloud(self, msg): frame_id = msg.header.frame_id if CHECK_FRAME and frame_id != SOURCE_FRAME: rospy.logwarn_throttle( 5.0, "点云坐标系错误: %s", frame_id ) return None, None field_dict = { field.name: field for field in msg.fields } for name in ("x", "y", "z"): if name not in field_dict: rospy.logwarn_throttle( 5.0, "缺少点云字段: %s", name ) return None, None color_field = None if "rgb" in field_dict: color_field = "rgb" elif "rgba" in field_dict: color_field = "rgba" has_rgb = color_field is not None if has_rgb: color_datatype = field_dict[ color_field ].datatype points_list = [] colors_list = [] if has_rgb: generator = pc2.read_points( msg, field_names=( "x", "y", "z", color_field ), skip_nans=False ) for point in generator: x, y, z, packed_color = point points_list.append((x, y, z)) colors_list.append( self.decode_rgb( packed_color, color_datatype ) ) else: generator = pc2.read_points( msg, field_names=("x", "y", "z"), skip_nans=False ) for point in generator: x, y, z = point points_list.append((x, y, z)) if not points_list: return None, None xyz = np.asarray( points_list, dtype=np.float64 ) valid_mask = np.isfinite(xyz).all(axis=1) xyz = xyz[valid_mask] if has_rgb: rgb = np.asarray( colors_list, dtype=np.float64 ) rgb = rgb[valid_mask] else: rgb = np.tile( np.asarray([DEFAULT_COLOR]), (len(xyz), 1) ) if len(xyz) == 0: return None, None # ---------------------------------------------------- # 必须在相机坐标系下进行 Z 深度过滤 # ---------------------------------------------------- depth_mask = ( (xyz[:, 2] >= MIN_DEPTH) & (xyz[:, 2] <= MAX_DEPTH) ) xyz = xyz[depth_mask] rgb = rgb[depth_mask] if len(xyz) == 0: return None, None # ---------------------------------------------------- # TF2 转换到 base_link # ---------------------------------------------------- xyz_base, T = self.transform_points_to_base( xyz, msg ) if xyz_base is None: return None, None # 再检查变换结果 valid_mask = np.isfinite( xyz_base ).all(axis=1) xyz_base = xyz_base[valid_mask] rgb = rgb[valid_mask] if len(xyz_base) == 0: return None, None # ---------------------------------------------------- # 创建 Open3D 点云 # ---------------------------------------------------- cloud = o3d.geometry.PointCloud() cloud.points = ( o3d.utility.Vector3dVector(xyz_base) ) cloud.colors = ( o3d.utility.Vector3dVector( np.clip(rgb, 0.0, 1.0) ) ) original_count = len(cloud.points) # ---------------------------------------------------- # 体素降采样 # ---------------------------------------------------- if VOXEL_SIZE > 0: cloud = cloud.voxel_down_sample( VOXEL_SIZE ) final_count = len(cloud.points) if final_count == 0: return None, None rospy.loginfo_throttle( 5.0, "基座坐标系点云: 处理前=%d，降采样后=%d", original_count, final_count ) return cloud, T # ======================================================== # 7. 独立处理线程 # ======================================================== def processing_worker(self): rospy.loginfo("点云处理线程启动") while ( not rospy.is_shutdown() and not self.worker_stop.is_set() ): start_time = time.monotonic() msg = None version = -1 with self.msg_lock: if ( self.latest_msg is not None and self.msg_version != self.processed_msg_version ): msg = self.latest_msg version = self.msg_version if msg is not None: try: cloud, T = self.process_cloud(msg) if cloud is not None: with self.cloud_lock: self.latest_cloud = cloud self.latest_tf_matrix = T.copy() self.latest_stamp = msg.header.stamp self.latest_source_frame = ( msg.header.frame_id ) self.cloud_version += 1 except Exception as exc: rospy.logerr_throttle( 5.0, "点云处理异常: %s", str(exc) ) finally: with self.msg_lock: self.processed_msg_version = max( self.processed_msg_version, version ) elapsed = time.monotonic() - start_time sleep_time = max( 0.01, PROCESS_INTERVAL - elapsed ) self.worker_stop.wait(sleep_time) rospy.loginfo("点云处理线程退出") # ======================================================== # 8. 保存点云和元数据 # ======================================================== def save_callback(self, vis): with self.cloud_lock: if self.latest_cloud is None: rospy.logwarn("当前没有可保存的点云") return False points = np.asarray( self.latest_cloud.points ).copy() colors = np.asarray( self.latest_cloud.colors ).copy() T = self.latest_tf_matrix.copy() stamp = self.latest_stamp source_frame = self.latest_source_frame if len(points) == 0: return False cloud = o3d.geometry.PointCloud() cloud.points = ( o3d.utility.Vector3dVector(points) ) cloud.colors = ( o3d.utility.Vector3dVector(colors) ) timestamp = datetime.now().strftime( "%Y%m%d_%H%M%S_%f" ) filename = "l515_base_" + timestamp + ".ply" save_path = os.path.join( SAVE_DIR, filename ) metadata_path = save_path.replace( ".ply", "_metadata.txt" ) try: success = o3d.io.write_point_cloud( save_path, cloud, write_ascii=False ) if not success: rospy.logerr("PLY 保存失败") return False with open( metadata_path, "w", encoding="utf-8" ) as f: f.write("Point cloud coordinate metadata\n") f.write("--------------------------------\n") f.write( "target_frame: {}\n".format(BASE_FRAME) ) f.write( "source_frame: {}\n".format(source_frame) ) f.write( "ros_stamp_secs: {}\n".format(stamp.secs) ) f.write( "ros_stamp_nsecs: {}\n".format(stamp.nsecs) ) f.write( "point_count: {}\n".format(len(points)) ) f.write( "\nT_base_camera:\n" ) np.savetxt( f, T, fmt="%.10f" ) rospy.loginfo("================================") rospy.loginfo("基座坐标系点云保存成功") rospy.loginfo("PLY: %s", save_path) rospy.loginfo("Metadata: %s", metadata_path) rospy.loginfo("坐标系: %s", BASE_FRAME) rospy.loginfo("点数: %d", len(points)) rospy.loginfo("================================") except Exception as exc: rospy.logerr("保存异常: %s", str(exc)) return False # ======================================================== # 9. 退出 # ======================================================== def quit_callback(self, vis): rospy.loginfo("收到退出指令") self.quit_requested = True self.worker_stop.set() return False # ======================================================== # 10. 实时显示 # ======================================================== def run(self): self.worker_thread = threading.Thread( target=self.processing_worker, name="l515_cloud_worker", daemon=True ) self.worker_thread.start() rospy.loginfo( "Open3D 显示启动，当前目标坐标系: %s", BASE_FRAME ) try: while ( not rospy.is_shutdown() and not self.quit_requested ): new_points = None new_colors = None new_version = -1 with self.cloud_lock: if ( self.latest_cloud is not None and self.cloud_version != self.displayed_version ): new_points = np.asarray( self.latest_cloud.points ).copy() new_colors = np.asarray( self.latest_cloud.colors ).copy() new_version = self.cloud_version if ( new_points is not None and len(new_points) > 0 ): self.pcd.points = ( o3d.utility.Vector3dVector( new_points ) ) self.pcd.colors = ( o3d.utility.Vector3dVector( new_colors ) ) if not self.geometry_added: self.vis.add_geometry(self.pcd) self.geometry_added = True view_control = ( self.vis.get_view_control() ) view_control.set_lookat( self.pcd.get_center().tolist() ) view_control.set_up(VIEW_UP) view_control.set_front(VIEW_FRONT) view_control.set_zoom(0.7) rospy.loginfo( "首次显示成功，点数=%d，坐标系=%s", len(new_points), BASE_FRAME ) else: self.vis.update_geometry(self.pcd) self.displayed_version = new_version alive = self.vis.poll_events() self.vis.update_renderer() if not alive: self.quit_requested = True break time.sleep(0.01) finally: self.worker_stop.set() if hasattr(self, "worker_thread"): self.worker_thread.join(timeout=3.0) self.sub.unregister() self.vis.destroy_window() rospy.loginfo("L515 点云程序已退出") # ============================================================ # 11. 主函数 # ============================================================ def main(): viewer = None try: viewer = L515CloudViewer() viewer.run() except rospy.ROSInterruptException: pass except KeyboardInterrupt: rospy.loginfo("用户按下 Ctrl+C") except Exception as exc: rospy.logerr("程序运行失败: %s", str(exc)) raise if __name__ == "__main__": main()运行
python3 ~/aubo_ws/grape_project/scripts/view_l515_cloud.py四、保存并验证点云
点云保存目录：~/aubo_ws/pointcloud_output/
检查保存的点云：
python3 - <<'PY' import open3d as o3d import glob import os import numpy as np files = glob.glob( os.path.expanduser("~/aubo_ws/pointcloud_output/*.ply") ) if not files: print("没有找到 PLY 文件") raise SystemExit path = max(files, key=os.path.getmtime) pcd = o3d.io.read_point_cloud(path) points = np.asarray(pcd.points) print("文件:", path) print("点数:", len(points)) print("是否有颜色:", pcd.has_colors()) if len(points) > 0: print("坐标最小值:", points.min(axis=0)) print("坐标最大值:", points.max(axis=0)) o3d.visualization.draw_geometries( [pcd], window_name="Saved L515 Point Cloud" ) PY
