import pyrealsense2 as rs
import numpy as np
import cv2
import threading
import time
from collections import deque
import argparse
from queue import Queue, Empty
import time
class DepthCamera3DVisualizer:
"""深度相机三维坐标系统，支持实时数据传递"""
def __init__(self, camera_type='realsense', object_class='red_pink'):
"""初始化系统"""
self.camera_type = camera_type
self.object_class = object_class
self.pipeline = None
self.align = None
self.intrinsics = None
self.depth_scale = None
self.camera_healthy = False
# 坐标数据缓存和队列
self.coordinates = deque(maxlen=100)
self.color_labels = deque(maxlen=100)
# 实时数据传递队列
self.data_output_queue = Queue(maxsize=10) # 数据输出队列
self.data_provider_running = False
self.data_provider_thread = None
# 运行状态
self.running = False
self.camera_thread = None
a
def init_camera(self):
"""初始化深度相机"""
if self.camera_type.lower() == 'realsense':
try:
# 创建管道和配置
self.pipeline = rs.pipeline()
config = rs.config()
# 配置流
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# 启动管道
profile = self.pipeline.start(config)
# 获取深度传感器的深度比例尺
depth_sensor = profile.get_device().first_depth_sensor()
self.depth_scale = depth_sensor.get_depth_scale()
# 创建对齐对象，将深度帧对齐到彩色帧
align_to = rs.stream.color
self.align = rs.align(align_to)
# 获取相机内参
profiles = self.pipeline.get_active_profile()
depth_profile = rs.video_stream_profile(profiles.get_stream(rs.stream.depth))
self.intrinsics = depth_profile.get_intrinsics()
self.camera_healthy = True
print(f"相机初始化成功: {self.camera_type}")
print(f"深度比例尺: {self.depth_scale}")
print(f"相机内参: {self.intrinsics}")
return True
except Exception as e:
print(f"相机初始化失败: {e}")
self.camera_healthy = False
return False
else:
print(f"不支持的相机类型: {self.camera_type}")
return False
def detect_object(self, color_image, depth_frame):
"""检测红色或粉色物体并返回其中心的3D坐标"""
# 转换为HSV颜色空间
hsv = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV)
# 获取颜色阈值（红色+粉色）
lower_red1, upper_red1, lower_red2, upper_red2, lower_pink, upper_pink = self._get_color_thresholds()
# 创建各颜色掩码
mask_red1 = cv2.inRange(hsv, lower_red1, upper_red1)
mask_red2 = cv2.inRange(hsv, lower_red2, upper_red2)
mask_pink = cv2.inRange(hsv, lower_pink, upper_pink)
mask = cv2.bitwise_or(cv2.bitwise_or(mask_red1, mask_red2), mask_pink)
# 形态学操作去噪
kernel = np.ones((5, 5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
# 查找轮廓
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) > 0:
for contour in contours:
# 过滤小轮廓
area = cv2.contourArea(contour)
if area < 300:
continue
# 计算轮廓的中心
M = cv2.moments(contour)
if M["m00"] != 0:
cx = int(M["m10"] / M["m00"])
cy = int(M["m01"] / M["m00"])
# 确定颜色类别（红色或粉色）
color_pixel = hsv[cy, cx]
hue = color_pixel[0]
color_label = "Red" if ((0 <= hue <= 10) or (160 <= hue <= 180)) else "Pink"
# 获取中心点的深度值
depth = depth_frame.get_distance(cx, cy)
# 计算3D坐标
if depth > 0:
x, y, z = self.calculate_3d_coordinates(cx, cy, depth)
# 构建数据输出格式
data_package = {
'timestamp': time.time(),
'coordinates': (x, y, z),
'color_label': color_label,
'depth_value': depth,
'pixel_coordinates': (cx, cy)
}
return (x, y, z), color_label, data_package
return None, None, None
def _get_color_thresholds(self):
"""获取红色和粉色的HSV阈值"""
# 红色低端范围
lower_red1 = np.array([0, 50, 50], dtype=np.uint8)
upper_red1 = np.array([10, 255, 255], dtype=np.uint8)
# 红色高端范围
lower_red2 = np.array([160, 50, 50], dtype=np.uint8)
upper_red2 = np.array([180, 255, 255], dtype=np.uint8)
# 粉色范围
lower_pink = np.array([140, 50, 50], dtype=np.uint8)
upper_pink = np.array([160, 255, 255], dtype=np.uint8)
return lower_red1, upper_red1, lower_red2, upper_red2, lower_pink, upper_pink
def calculate_3d_coordinates(self, pixel_x, pixel_y, depth):
"""将像素坐标和深度值转换为3D坐标（相机坐标系）"""
if depth == 0 or not self.intrinsics:
return 0, 0, 0
# 使用相机内参进行转换
fx, fy = self.intrinsics.fx, self.intrinsics.fy
cx, cy = self.intrinsics.ppx, self.intrinsics.ppy
x = (pixel_x - cx) * depth / fx
y = (pixel_y - cy) * depth / fy
z = depth
return x, y, z
def camera_thread_function(self):
"""相机数据采集线程函数"""
consecutive_timeouts = 0
while self.running:
try:
# 等待获取帧数据，设置较长超时时间
frames = self.pipeline.wait_for_frames(timeout_ms=8000)
# 重置连续超时计数
consecutive_timeouts = 0
# 对齐深度帧和彩色帧
aligned_frames = self.align.process(frames)
aligned_depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
if not aligned_depth_frame or not color_frame:
print("警告：获取的帧数据不完整")
continue
# 转换为numpy数组
color_image = np.asanyarray(color_frame.get_data())
# 检测物体并获取3D坐标
coordinates, color_label, data_package = self.detect_object(color_image, aligned_depth_frame)
# 更新3D坐标数据
if coordinates:
self.coordinates.append(coordinates)
self.color_labels.append(color_label)
# 向数据输出队列添加数据
if data_package and not self.data_output_queue.full():
self.data_output_queue.put(data_package)
except RuntimeError as e:
if "didn't arrive within" in str(e):
consecutive_timeouts += 1
print(f"警告：相机帧获取超时 ({consecutive_timeouts}/3)")
# 连续3次超时后尝试重新连接相机
if consecutive_timeouts >= 3:
print("多次超时，尝试重新初始化相机...")
self.camera_healthy = False
self._reinitialize_camera()
consecutive_timeouts = 0
else:
print(f"相机运行时错误: {e}")
self.camera_healthy = False
self.stop()
break
except Exception as e:
print(f"相机线程错误: {e}")
self.camera_healthy = False
self.stop()
break
def _reinitialize_camera(self):
"""重新初始化相机"""
try:
if self.pipeline:
self.pipeline.stop()
time.sleep(1) # 等待1秒
print("尝试重新连接相机...")
if self.init_camera():
print("相机重新连接成功")
self.camera_healthy = True
else:
print("相机重新连接失败，5秒后再次尝试")
time.sleep(5)
except Exception as e:
print(f"重新初始化相机失败: {e}")
def start_data_provider(self):
"""启动数据提供线程"""
if self.data_provider_running:
return
self.data_provider_running = True
self.data_provider_thread = threading.Thread(target=self.data_provider_function)
self.data_provider_thread.daemon = True
self.data_provider_thread.start()
print("数据提供服务已启动")
def data_provider_function(self):
"""数据提供线程函数，用于向外传递数据"""
while self.running and self.data_provider_running:
try:
if not self.data_output_queue.empty():
data = self.data_output_queue.get(timeout=0.1)
# 示例：打印数据（实际应用中可替换为硬件控制代码）
print(
f"[DATA OUT] {data['color_label']} at ({data['coordinates'][0]:.2f}, {data['coordinates'][1]:.2f}, {data['coordinates'][2]:.2f})m")
except Empty:
continue
except Exception as e:
print(f"数据传递错误: {e}")
def get_latest_data(self, timeout=0.1):
"""获取最新的传感器数据（供外部调用）"""
try:
return self.data_output_queue.get(timeout=timeout)
except Empty:
return None
except Exception as e:
print(f"获取数据错误: {e}")
return None
def start(self):
"""启动系统"""
if not self.init_camera():
return False
self.running = True
self.camera_thread = threading.Thread(target=self.camera_thread_function)
self.camera_thread.daemon = True
self.camera_thread.start()
# 启动数据提供服务
self.start_data_provider()
print("开始跟踪红色和粉色物体...")
print("数据接口已激活，可实时获取传感器数据")
# 主循环（无可视化）
try:
while self.running:
time.sleep(0.1) # 避免CPU占用过高
except KeyboardInterrupt:
self.stop()
return True
def stop(self):
"""停止系统"""
self.running = False
self.data_provider_running = False
if self.camera_thread and self.camera_thread.is_alive():
self.camera_thread.join(timeout=1.0)
if self.data_provider_thread and self.data_provider_thread.is_alive():
self.data_provider_thread.join(timeout=1.0)
if self.pipeline:
self.pipeline.stop()
print("系统已停止")
# 主函数定义
def main():
"""主函数：解析命令行参数并启动系统，储存运行五秒后的坐标"""
# 解析命令行参数
parser = argparse.ArgumentParser(description="深度相机三维坐标数据采集系统")
parser.add_argument("--camera", type=str, default="realsense",
help="相机类型 (realsense)")
parser.add_argument("--object", type=str, default="red_pink",
help="要跟踪的物体类别 (red_pink, person, cup, apple, etc.)")
args = parser.parse_args()
# 创建系统实例
visualizer = DepthCamera3DVisualizer(camera_type=args.camera, object_class=args.object)
# 初始化并启动相机和线程，但不进入主循环
if not visualizer.init_camera():
print("相机初始化失败，程序退出")
return
visualizer.running = True
visualizer.camera_thread = threading.Thread(target=visualizer.camera_thread_function)
visualizer.camera_thread.daemon = True
visualizer.camera_thread.start()
visualizer.start_data_provider()
print("开始跟踪红色和粉色物体...")
# 记录开始时间
start_time = time.time()
# 运行五秒
while time.time() - start_time < 5:
time.sleep(0.1) # 短暂休眠以降低CPU占用
# 获取五秒后的坐标并储存
latest_data = visualizer.get_latest_data()
coordinates_after_five_seconds = None # 初始化变量
if latest_data and 'coordinates' in latest_data:
coordinates_after_five_seconds = latest_data['coordinates']
print(f"运行五秒后的坐标: {coordinates_after_five_seconds}")
else:
print("五秒后未获取到有效坐标数据")
# 停止系统
visualizer.stop()
# 返回或使用储存的坐标（可选）
if coordinates_after_five_seconds:
print(f"储存的坐标可用于后续操作: {coordinates_after_five_seconds}")
X = coordinates_after_five_seconds[0]
Y = coordinates_after_five_seconds[1]
Z = coordinates_after_five_seconds[2]
X = round(X * 100, 2)
Y = round(Y * 100, 2)
Z = round(Z * 100, 2)
#arm_move(X,Y,Z,1)
if __name__ == "__main__":
main()
链接的是那个设备
本文详细介绍了Intel RealSense SDK中的日志系统，包括不同级别的日志严重性定义及其使用方式，如调试、信息、警告、错误和致命错误等。此外，还探讨了如何将日志输出到控制台或文件的方法。
扫一扫
2155
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
