#!/usr/bin/env python
# -*- coding: utf-8 -*-
from __future__ import print_function
import os
os.environ['QT_X11_NO_MITSHM'] = '1'
os.environ['DISPLAY'] = ':0'
import rospy
import cv2
import numpy as np
import time
import yaml
import logging
import threading
import signal
import sys
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
from robot_package.msg import TR_Arm_Msg
# 配置日志
logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(name)s - %(levelname)s - %(message)s')
logger = logging.getLogger('robust_hand_eye_calibration')
class RobustCalibrator:
def __init__(self):
rospy.init_node('robust_hand_eye_calibration', anonymous=True)
# 参数配置
self.pattern_size = rospy.get_param('~pattern_size', (6, 8))
self.square_size = rospy.get_param('~square_size', 0.02)
self.min_poses = rospy.get_param('~min_poses', 15)
self.max_poses = rospy.get_param('~max_poses', 20)
# 数据存储
self.gripper_poses = []
self.target_poses = []
self.images = []
self.camera_info = None
self.T_cam_end = None
# ROS工具
self.bridge = CvBridge()
self.current_image = None
self.current_arm_pose = None
self.last_corners = None
# 生成世界坐标系点
self.objp = np.zeros((self.pattern_size[0]*self.pattern_size[1], 3), np.float32)
self.objp[:, :2] = np.mgrid[0:self.pattern_size[0], 0:self.pattern_size[1]].T.reshape(-1, 2) * self.square_size
logger.info("鲁棒手眼标定系统已启动")
# 订阅者
rospy.Subscriber("/ascamera/rgb0/image", Image, self.image_callback)
rospy.Subscriber("/ascamera/rgb0/camera_info", CameraInfo, self.camera_info_callback)
rospy.Subscriber("/TR_Arm_topic", TR_Arm_Msg, self.arm_pose_callback)
# 创建调试图像发布者
self.debug_pub = rospy.Publisher("/calibration/debug_image", Image, queue_size=1)
# 设置信号处理
signal.signal(signal.SIGINT, self.signal_handler)
signal.signal(signal.SIGTERM, self.signal_handler)
# 状态监控线程
self.monitor_thread = threading.Thread(target=self.monitor_system)
self.monitor_thread.daemon = True
self.monitor_thread.start()
def signal_handler(self, signum, frame):
"""处理中断信号"""
logger.warning("收到中断信号 %d，正在安全关闭...", signum)
self.cleanup_resources()
rospy.signal_shutdown("外部中断")
sys.exit(0)
def monitor_system(self):
"""系统监控线程"""
while not rospy.is_shutdown():
# 检查系统资源
try:
# 监控GPU内存（适用于NVIDIA Jetson）
if os.path.exists('/sys/devices/gpu.0'):
with open('/sys/devices/gpu.0/load', 'r') as f:
gpu_load = int(f.read().strip())
if gpu_load > 90:
logger.warning("GPU负载过高: %d%%，考虑降低图像分辨率", gpu_load)
# 监控CPU温度
if os.path.exists('/sys/class/thermal/thermal_zone0/temp'):
with open('/sys/class/thermal/thermal_zone0/temp', 'r') as f:
temp = int(f.read().strip()) / 1000.0
if temp > 75:
logger.warning("CPU温度过高: %.1f°C，暂停处理10秒", temp)
rospy.sleep(10.0)
rospy.sleep(5.0) # 每5秒检查一次
except Exception as e:
logger.error("监控错误: %s", e)
rospy.sleep(10.0)
def cleanup_resources(self):
"""清理资源"""
logger.info("清理系统资源...")
try:
# 关闭所有OpenCV窗口
cv2.destroyAllWindows()
# 保存当前进度
if len(self.gripper_poses) > 0:
self.save_progress()
logger.info("资源清理完成")
except Exception as e:
logger.error("清理资源时出错: %s", e)
def save_progress(self, filename="calibration_progress.yaml"):
"""保存当前进度"""
try:
data = {
'gripper_poses': [pose.tolist() for pose in self.gripper_poses],
'target_poses': [pose.tolist() for pose in self.target_poses],
'num_poses': len(self.gripper_poses),
'last_update': time.strftime("%Y-%m-%d %H:%M:%S")
}
with open(filename, 'w') as f:
yaml.dump(data, f, default_flow_style=False)
logger.info("标定进度已保存至: %s", filename)
return True
except Exception as e:
logger.error("保存进度失败: %s", e)
return False
def load_progress(self, filename="calibration_progress.yaml"):
"""加载之前保存的进度"""
try:
if not os.path.exists(filename):
logger.warning("进度文件不存在: %s", filename)
return False
with open(filename, 'r') as f:
data = yaml.safe_load(f)
self.gripper_poses = [np.array(pose) for pose in data['gripper_poses']]
self.target_poses = [np.array(pose) for pose in data['target_poses']]
logger.info("已从 %s 加载进度: %d 个位姿", filename, len(self.gripper_poses))
return True
except Exception as e:
logger.error("加载进度失败: %s", e)
return False
def image_callback(self, msg):
try:
# 使用独立线程处理图像，避免阻塞主线程
threading.Thread(target=self.process_image, args=(msg,)).start()
except Exception as e:
logger.error("图像处理线程错误: %s", e)
def process_image(self, msg):
"""在独立线程中处理图像"""
try:
self.current_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
self.detect_corners()
except CvBridgeError as e:
logger.error("图像转换错误: %s", e)
def detect_corners(self):
"""检测角点并发布调试图像"""
if self.current_image is None or self.camera_info is None:
return
try:
gray = cv2.cvtColor(self.current_image, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, self.pattern_size, None)
if ret:
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
corners_refined = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
self.last_corners = corners_refined
# 创建调试图像
debug_img = self.current_image.copy()
cv2.drawChessboardCorners(debug_img, self.pattern_size, corners_refined, ret)
# 添加状态信息
status_text = f"Corners Detected [{len(self.gripper_poses)}/{self.max_poses}]"
cv2.putText(debug_img, status_text, (20, 40),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
# 发布调试图像
try:
debug_msg = self.bridge.cv2_to_imgmsg(debug_img, "bgr8")
self.debug_pub.publish(debug_msg)
except CvBridgeError as e:
logger.error("发布调试图像错误: %s", e)
except Exception as e:
logger.error("角点检测错误: %s", e)
def camera_info_callback(self, msg):
if self.camera_info is None:
self.camera_info = {
'K': np.array(msg.K).reshape(3,3),
'D': np.array(msg.D),
'width': msg.width,
'height': msg.height
}
logger.info("相机内参已获取")
def arm_pose_callback(self, msg):
if len(msg.homogeneousMatrix) == 16:
self.current_arm_pose = np.array(msg.homogeneousMatrix).reshape(4,4).astype(np.float64)
def capture_data(self):
if not all([self.current_image, self.current_arm_pose, self.camera_info]):
logger.error("数据不完整，无法采集")
return False
try:
gray = cv2.cvtColor(self.current_image, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, self.pattern_size, None)
if not ret:
logger.warning("未检测到棋盘格")
return False
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
corners_refined = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
ret, rvec, tvec = cv2.solvePnP(self.objp, corners_refined,
self.camera_info['K'], self.camera_info['D'])
if not ret:
logger.error("solvePnP失败")
return False
R_board_cam, _ = cv2.Rodrigues(rvec)
T_board_cam = np.eye(4)
T_board_cam[:3, :3] = R_board_cam
T_board_cam[:3, 3] = tvec.flatten()
self.gripper_poses.append(self.current_arm_pose.copy())
self.target_poses.append(T_board_cam.copy())
self.images.append(self.current_image.copy())
logger.info("成功采集位姿数据: %d/%d", len(self.gripper_poses), self.max_poses)
# 定期保存进度
if len(self.gripper_poses) % 5 == 0:
self.save_progress()
return True
except Exception as e:
logger.exception("数据采集失败: %s", e)
return False
def calibrate(self):
if len(self.gripper_poses) < self.min_poses:
logger.error("需要至少%d个位姿数据, 当前: %d", self.min_poses, len(self.gripper_poses))
return None
try:
R_gripper2base, t_gripper2base = [], []
R_target2cam, t_target2cam = [], []
for i in range(len(self.gripper_poses)-1):
inv_pose = np.linalg.inv(self.gripper_poses[i])
A = np.dot(inv_pose, self.gripper_poses[i+1])
R_gripper2base.append(A[:3, :3])
t_gripper2base.append(A[:3, 3])
inv_target = np.linalg.inv(self.target_poses[i])
B = np.dot(inv_target, self.target_poses[i+1])
R_target2cam.append(B[:3, :3])
t_target2cam.append(B[:3, 3])
R_cam2gripper = np.eye(3)
t_cam2gripper = np.zeros(3)
# 尝试不同的标定方法
methods = [
cv2.CALIB_HAND_EYE_TSAI,
cv2.CALIB_HAND_EYE_PARK,
cv2.CALIB_HAND_EYE_HORAUD
]
best_error = float('inf')
best_result = None
for method in methods:
try:
R, t = cv2.calibrateHandEye(
R_gripper2base, t_gripper2base,
R_target2cam, t_target2cam,
R_cam2gripper, t_cam2gripper,
method=method
)
# 计算误差
error = self.calculate_error(R, t)
logger.info("方法 %d 标定误差: %.6f", method, error)
if error < best_error:
best_error = error
best_result = (R, t)
except Exception as e:
logger.warning("标定方法 %d 失败: %s", method, str(e))
if best_result is None:
logger.error("所有标定方法均失败")
return None
R_cam2gripper, t_cam2gripper = best_result
self.T_cam_end = np.eye(4)
self.T_cam_end[:3, :3] = R_cam2gripper
self.T_cam_end[:3, 3] = t_cam2gripper
logger.info("最佳标定误差: %.6f", best_error)
logger.info("相机到机械臂末端的变换矩阵 T_cam_end:\n%s", self.T_cam_end)
return self.T_cam_end
except Exception as e:
logger.exception("标定失败: %s", e)
return None
def calculate_error(self, R, t):
"""计算标定误差"""
errors = []
for i in range(len(self.gripper_poses)):
# 计算预测的标定板位姿
T_cam_end = np.eye(4)
T_cam_end[:3, :3] = R
T_cam_end[:3, 3] = t.flatten()
predicted_target = T_cam_end.dot(self.gripper_poses[i]).dot(np.linalg.inv(T_cam_end))
# 计算与实测位姿的差异
error = np.linalg.norm(predicted_target[:3, 3] - self.target_poses[i][:3, 3])
errors.append(error)
return np.mean(errors)
def save_calibration(self, filename="hand_eye_calibration.yaml"):
if self.T_cam_end is None:
logger.error("尚未标定，无法保存结果")
return False
try:
# 计算最终误差
final_error = self.calculate_error(self.T_cam_end[:3, :3], self.T_cam_end[:3, 3])
data = {
'T_cam_end': self.T_cam_end.tolist(),
'camera_matrix': self.camera_info['K'].tolist(),
'distortion_coefficients': self.camera_info['D'].tolist(),
'calibration_date': time.strftime("%Y-%m-%d %H:%M:%S"),
'num_poses': len(self.gripper_poses),
'pattern_size': list(self.pattern_size),
'square_size': self.square_size,
'calibration_error': float(final_error)
}
with open(filename, 'w') as f:
yaml.dump(data, f, default_flow_style=False)
logger.info("标定结果已保存至: %s (误差: %.6f)", filename, final_error)
# 保存采集的图像
self.save_calibration_images()
# 清理进度文件
if os.path.exists("calibration_progress.yaml"):
os.remove("calibration_progress.yaml")
return True
except Exception as e:
logger.exception("保存失败: %s", e)
return False
def save_calibration_images(self):
"""保存采集的图像用于验证"""
try:
save_dir = "calibration_images"
os.makedirs(save_dir, exist_ok=True)
for i, img in enumerate(self.images):
filename = os.path.join(save_dir, f"pose_{i:02d}.png")
cv2.imwrite(filename, img)
logger.info("保存了%d张标定图像到目录: %s", len(self.images), save_dir)
return True
except Exception as e:
logger.error("保存图像失败: %s", e)
return False
def main():
# 修复X11环境
os.system('xhost +local:') # 允许本地连接
os.system('export DISPLAY=:0') # 确保显示设置
# 初始化校准器
calibrator = RobustCalibrator()
# 尝试加载之前的进度
calibrator.load_progress()
rospy.sleep(2.0) # 等待初始数据
logger.info("\n===== 鲁棒标定系统操作指南 =====")
logger.info("1. 移动机械臂使棋盘格在相机视野中央")
logger.info("2. 按回车键采集当前位姿 (需要至少%d个不同位姿)", calibrator.min_poses)
logger.info("3. 采集完成后输入 'c' 开始标定")
logger.info("4. 标定完成后输入 's' 保存结果")
logger.info("5. 输入 'q' 退出程序")
logger.info("6. 系统会自动保存进度，意外中断后可恢复")
try:
while not rospy.is_shutdown() and len(calibrator.gripper_poses) < calibrator.max_poses:
cmd = raw_input("等待命令 (回车采集/'c'标定/'s'保存/'q'退出): ").strip().lower()
if cmd == '':
if calibrator.capture_data():
logger.info("数据采集成功")
elif cmd == 'c':
if len(calibrator.gripper_poses) < calibrator.min_poses:
logger.warning("需要至少%d个位姿，当前只有%d个",
calibrator.min_poses, len(calibrator.gripper_poses))
else:
result = calibrator.calibrate()
if result is not None:
logger.info("标定成功")
elif cmd == 's':
if calibrator.T_cam_end is None:
logger.warning("请先执行标定 ('c')")
else:
calibrator.save_calibration()
elif cmd == 'q':
logger.info("程序退出")
calibrator.cleanup_resources()
break
except rospy.ROSInterruptException:
calibrator.cleanup_resources()
except Exception as e:
logger.exception("主循环错误: %s", e)
calibrator.cleanup_resources()
if __name__ == "__main__":
main()
修改
最新发布
本文介绍了解决在安装程序过程中遇到的PHP时区警告的方法。通过修改php.ini文件中的date.timezone设置并重启Apache服务器，可以有效解决这一问题。
扫一扫
694
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
