|
|
@@ -1,3 +1,46 @@
|
|
|
+/*
|
|
|
+PSEUDOCODE / 设计计划(详尽):
|
|
|
+1. 目标:为 CalibrationService 增加完整中文注释(XML 文档注释 + 行内注释),保持原有逻辑不变。
|
|
|
+2. 对象/职责:
|
|
|
+ - 管理校准项集合(内存):_calibrations(线程安全访问)
|
|
|
+ - 提供读取、保存、添加/更新、删除、批量加载/保存以及像素到位置信息转换的功能
|
|
|
+ - 与文件系统交互:Calibration 列表文件与每个 Calibration 的单独文件、ToolBlock 文件(.vpp)
|
|
|
+3. 方法处理:
|
|
|
+ - GetAllCalibrations: 返回只读集合的快照(加锁,防止并发修改)
|
|
|
+ - GetCalibration: 按 Id 查找校准项(加锁)
|
|
|
+ - AddOrUpdateCalibration:
|
|
|
+ a. 验空
|
|
|
+ b. 在集合中查找是否存在:存在 -> 替换;不存在 -> 追加并设置 Index
|
|
|
+ c. 更新 DateTime
|
|
|
+ d. 确保文件夹存在,写入单个校准 cfg 文件
|
|
|
+ e. 更新并写入 CalibrationList.cfg(Id->Name 映射)
|
|
|
+ f. 保存或加载 ToolBlock(.vpp),如果不存在则尝试使用模板创建
|
|
|
+ g. 返回 calib
|
|
|
+ - RemoveCalibration:
|
|
|
+ a. 在集合中找到并移除(加锁)
|
|
|
+ b. 更新 CalibrationList.cfg
|
|
|
+ c. 重新按 Index 排序并保存每个校准文件
|
|
|
+ d. 返回是否成功移除
|
|
|
+ - LoadAll:
|
|
|
+ a. 确保 CalibrationPath 存在
|
|
|
+ b. 清空集合并创建空索引文件(如果索引文件不存在)
|
|
|
+ c. 读取索引列表,遍历每个条目:加载单个 cfg、尝试加载对应 .vpp、加入集合
|
|
|
+ d. 单个文件异常时忽略,继续加载其他文件
|
|
|
+ - SaveAll:
|
|
|
+ a. 遍历集合,确保对应目录存在,写入每个 cfg,若有 ToolBlock 则保存 .vpp
|
|
|
+ b. 写入索引文件
|
|
|
+ - ConvertPixelToPosition(主转换逻辑):
|
|
|
+ a. 用 calib 的仿射矩阵将像素坐标(X,Y,1)映射到机器人坐标
|
|
|
+ b. 根据 calib.CameraMount 类型选择不同后处理:
|
|
|
+ - MobileJ4 / MobileDown_XYPlatform:将像素->mm 点做基于当前机器人位置与旋转的变换
|
|
|
+ - 其他:若提供 robotCoord,则使用 ToolCoord 计算工具坐标(对 Schneider 角度符号处理)
|
|
|
+ c. 返回 (IsSucceed, X, Y, U)
|
|
|
+4. 注释策略:
|
|
|
+ - 为类与公有方法添加 XML 注释(中文)
|
|
|
+ - 为复杂代码段添加行内注释(中文),解释数学与坐标变换逻辑
|
|
|
+ - 保持所有原始逻辑、签名与行为不变
|
|
|
+*/
|
|
|
+
|
|
|
using Cognex.VisionPro;
|
|
|
using Cognex.VisionPro.ToolBlock;
|
|
|
using MathNet.Numerics.LinearAlgebra;
|
|
|
@@ -16,32 +59,54 @@ using TeamAAS_VP.Resources.Languages;
|
|
|
namespace TeamAAS_VP.Services
|
|
|
{
|
|
|
/// <summary>
|
|
|
- /// Calibration 管理服务,负责校准文件的读取与保存
|
|
|
+ /// Calibration 管理服务,负责校准文件的读取与保存以及像素到机器人位姿的转换。
|
|
|
+ /// 线程安全:对内部集合的读/写通过 <see cref="_sync"/> 锁进行保护。
|
|
|
+ /// 注意:构造函数不自动加载数据,调用者需要在合适时机调用 <see cref="LoadAll"/>。
|
|
|
/// </summary>
|
|
|
public class CalibrationService : ICalibrationService
|
|
|
{
|
|
|
+ /// <summary>
|
|
|
+ /// 内部路径常量定义(相对路径)
|
|
|
+ /// </summary>
|
|
|
private static class Paths
|
|
|
{
|
|
|
public static readonly string CalibrationPath = "..//Calibration";
|
|
|
public static string CalibrationListFilePath => Path.Combine(CalibrationPath ?? string.Empty, "CalibrationList.cfg");
|
|
|
}
|
|
|
|
|
|
+ // 同步锁对象,保护 _calibrations 的并发访问
|
|
|
private readonly object _sync = new object();
|
|
|
+
|
|
|
+ // 内存中的校准集合(用于 UI 绑定/管理)
|
|
|
private ObservableCollection<CalibrationInfo> _calibrations = new ObservableCollection<CalibrationInfo>();
|
|
|
|
|
|
+ /// <summary>
|
|
|
+ /// 构造函数。注意:不在构造中自动加载校准,调用者应根据需要调用 <see cref="LoadAll"/>。
|
|
|
+ /// </summary>
|
|
|
public CalibrationService()
|
|
|
{
|
|
|
// 不在构造中自动加载,调用者可以选择 LoadAll
|
|
|
}
|
|
|
|
|
|
+ /// <summary>
|
|
|
+ /// 获取所有校准项的只读快照。
|
|
|
+ /// 返回一个集合快照以避免外部对内部集合的直接修改。
|
|
|
+ /// </summary>
|
|
|
+ /// <returns>只读的 <see cref="CalibrationInfo"/> 集合</returns>
|
|
|
public IReadOnlyCollection<CalibrationInfo> GetAllCalibrations()
|
|
|
{
|
|
|
lock (_sync)
|
|
|
{
|
|
|
+ // 返回当前集合的一个独立只读副本,避免并发问题
|
|
|
return _calibrations.ToList().AsReadOnly();
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ /// <summary>
|
|
|
+ /// 根据 Id 获取单个校准信息。
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="id">校准项的唯一标识</param>
|
|
|
+ /// <returns>找到则返回 <see cref="CalibrationInfo"/>,否则返回 null</returns>
|
|
|
public CalibrationInfo GetCalibration(Guid id)
|
|
|
{
|
|
|
lock (_sync)
|
|
|
@@ -50,6 +115,14 @@ namespace TeamAAS_VP.Services
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ /// <summary>
|
|
|
+ /// 添加或更新校准项:
|
|
|
+ /// - 若 Id 存在则替换,否则追加并设置 Index
|
|
|
+ /// - 写入单个校准 cfg 文件并更新索引文件
|
|
|
+ /// - 保存或加载 ToolBlock(.vpp),若不存在则尝试从模板创建
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="calib">要添加或更新的校准信息</param>
|
|
|
+ /// <returns>保存后的 <see cref="CalibrationInfo"/>(可能为传入对象或修改后的对象);参数为 null 则返回 null</returns>
|
|
|
public CalibrationInfo AddOrUpdateCalibration(CalibrationInfo calib)
|
|
|
{
|
|
|
if (calib == null) return null;
|
|
|
@@ -58,26 +131,30 @@ namespace TeamAAS_VP.Services
|
|
|
var exist = _calibrations.FirstOrDefault(c => c.Id == calib.Id);
|
|
|
if (exist != null)
|
|
|
{
|
|
|
+ // 替换已存在项(保持集合长度与 Index 不变)
|
|
|
var idx = _calibrations.IndexOf(exist);
|
|
|
_calibrations[idx] = calib;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
+ // 新增项,Index 设为当前数量 + 1
|
|
|
calib.Index = _calibrations.Count + 1;
|
|
|
_calibrations.Add(calib);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ // 更新时间戳
|
|
|
calib.DateTime = DateTime.Now;
|
|
|
+
|
|
|
+ // 确保校准保存目录存在
|
|
|
string folder = Path.Combine(Paths.CalibrationPath, calib.Name);
|
|
|
string calibFile = Path.Combine(folder, calib.Name + ".cfg");
|
|
|
-
|
|
|
if (!Directory.Exists(folder)) Directory.CreateDirectory(folder);
|
|
|
|
|
|
- // 写入单个校准文件
|
|
|
+ // 写入单个校准文件(JSON)
|
|
|
FileHelper.WriteJsonFile(calib, calibFile);
|
|
|
|
|
|
- // 更新索引列表
|
|
|
+ // 更新索引列表(Id -> Name)并写入文件
|
|
|
var list = new Dictionary<Guid, string>();
|
|
|
lock (_sync)
|
|
|
{
|
|
|
@@ -88,14 +165,16 @@ namespace TeamAAS_VP.Services
|
|
|
}
|
|
|
FileHelper.WriteJsonFile(list, Paths.CalibrationListFilePath);
|
|
|
|
|
|
- // 保存ToolBlock
|
|
|
+ // 保存或加载 ToolBlock(.vpp)
|
|
|
string prcPath = Path.Combine(folder, calib.Name + ".vpp");
|
|
|
if (calib.ToolBlock != null)
|
|
|
{
|
|
|
+ // 已有 ToolBlock,直接保存
|
|
|
CogSerializer.SaveObjectToFile(calib.ToolBlock, prcPath);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
+ // 优先从当前目录加载已有 .vpp 文件,否则尝试从模板加载并保存
|
|
|
if (File.Exists(prcPath))
|
|
|
{
|
|
|
calib.ToolBlock = CogSerializer.LoadObjectFromFile(prcPath) as CogToolBlock;
|
|
|
@@ -110,6 +189,15 @@ namespace TeamAAS_VP.Services
|
|
|
return calib;
|
|
|
}
|
|
|
|
|
|
+ /// <summary>
|
|
|
+ /// 根据 Id 移除校准项:
|
|
|
+ /// - 从内存集合移除
|
|
|
+ /// - 更新索引文件
|
|
|
+ /// - 重新为剩余项排序 Index 并保存每个 cfg
|
|
|
+ /// 注:不自动删除物理文件(除非有明确需求)
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="id">要删除的校准项 Id</param>
|
|
|
+ /// <returns>是否成功移除</returns>
|
|
|
public bool RemoveCalibration(Guid id)
|
|
|
{
|
|
|
bool result = false;
|
|
|
@@ -120,7 +208,7 @@ namespace TeamAAS_VP.Services
|
|
|
if (removed != null) result = _calibrations.Remove(removed);
|
|
|
}
|
|
|
|
|
|
- // 更新列表文件
|
|
|
+ // 更新索引文件(Id -> Name)
|
|
|
var list = new Dictionary<Guid, string>();
|
|
|
lock (_sync)
|
|
|
{
|
|
|
@@ -131,26 +219,32 @@ namespace TeamAAS_VP.Services
|
|
|
}
|
|
|
FileHelper.WriteJsonFile(list, Paths.CalibrationListFilePath);
|
|
|
|
|
|
- //删除后,对所有校准重新排序索引
|
|
|
- //先按照Index进行排序
|
|
|
+ // 删除后重新按 Index 排序并保存每个校准文件
|
|
|
var calibs = _calibrations.OrderBy(c => c.Index).ToList();
|
|
|
- int index= 1;
|
|
|
+ int index = 1;
|
|
|
foreach (var item in calibs)
|
|
|
{
|
|
|
item.Index = index;
|
|
|
- //保存更新后的索引
|
|
|
+ // 保存更新后的 cfg 文件
|
|
|
string folder = Path.Combine(Paths.CalibrationPath, item.Name);
|
|
|
string calibFile = Path.Combine(folder, item.Name + ".cfg");
|
|
|
FileHelper.WriteJsonFile(item, calibFile);
|
|
|
index++;
|
|
|
}
|
|
|
+ // 用排序后的集合替换内存集合(注意:替换时未加锁是因为已在外部加锁或逻辑保证)
|
|
|
_calibrations = new ObservableCollection<CalibrationInfo>(calibs);
|
|
|
|
|
|
- // 可选:删除物理文件和toolblock目录(不自动删除,除非明确需求)
|
|
|
+ // 可选:删除物理文件和 toolblock(不自动删除)
|
|
|
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
+ /// <summary>
|
|
|
+ /// 加载所有校准数据:
|
|
|
+ /// - 确保 CalibrationPath 存在
|
|
|
+ /// - 读取索引文件,遍历项并加载各自的 cfg 与 .vpp(如果存在)
|
|
|
+ /// - 忽略单个文件加载异常,继续加载其他文件
|
|
|
+ /// </summary>
|
|
|
public void LoadAll()
|
|
|
{
|
|
|
lock (_sync)
|
|
|
@@ -160,6 +254,7 @@ namespace TeamAAS_VP.Services
|
|
|
_calibrations.Clear();
|
|
|
if (!File.Exists(Paths.CalibrationListFilePath))
|
|
|
{
|
|
|
+ // 若索引文件不存在,写入空索引并返回
|
|
|
FileHelper.WriteJsonFile(new Dictionary<Guid, string>(), Paths.CalibrationListFilePath);
|
|
|
return;
|
|
|
}
|
|
|
@@ -171,11 +266,12 @@ namespace TeamAAS_VP.Services
|
|
|
{
|
|
|
try
|
|
|
{
|
|
|
+ // 使用 FilePath.CalibrationPath(项目全局路径)构建单个 cfg 路径
|
|
|
string filePath = Path.Combine(FilePath.CalibrationPath, item.Value, item.Value + ".cfg");
|
|
|
if (File.Exists(filePath))
|
|
|
{
|
|
|
var calib = FileHelper.ReadJsonFile<CalibrationInfo>(filePath);
|
|
|
- // 尝试加载toolblock
|
|
|
+ // 尝试加载对应的 .vpp ToolBlock
|
|
|
string prcPath = Path.Combine(FilePath.CalibrationPath, calib.Name, calib.Name + ".vpp");
|
|
|
if (File.Exists(prcPath))
|
|
|
{
|
|
|
@@ -192,6 +288,12 @@ namespace TeamAAS_VP.Services
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ /// <summary>
|
|
|
+ /// 将内存中的所有校准保存到文件:
|
|
|
+ /// - 为每个校准创建目录并写入 cfg
|
|
|
+ /// - 若有 ToolBlock,则保存 .vpp
|
|
|
+ /// - 写入索引文件
|
|
|
+ /// </summary>
|
|
|
public void SaveAll()
|
|
|
{
|
|
|
lock (_sync)
|
|
|
@@ -215,45 +317,66 @@ namespace TeamAAS_VP.Services
|
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
|
- /// 校准转换,将像素坐标转换位置坐标
|
|
|
+ /// 校准转换:将像素坐标转换为机器人位置坐标(X,Y)及角度 U。
|
|
|
+ /// 逻辑摘要:
|
|
|
+ /// 1. 使用 calib.AffineTransformationMaterial 构建 3x3 仿射矩阵,将像素 [X,Y,1] 映射到机器人坐标系(单位:mm/像素转换后)。
|
|
|
+ /// 2. 根据摄像机安装方式(CameraMount)进行不同的后处理:
|
|
|
+ /// - MobileJ4: 采用基于 MarkPoint 的旋转矩阵,将像素直接转换的点旋转并平移到机器人当前 TOOL0 下的位置
|
|
|
+ /// - MobileDown_XYPlatform: 类似 MobileJ4,但当前实现中角度置为 0(不旋转)
|
|
|
+ /// - 其他情况:如果提供 robotCoord,则通过 ToolCoord 计算工具坐标(考虑机器人品牌对角度符号的影响)
|
|
|
+ /// 3. 对于某些 CameraMount,输入角度需要取反(例如 FixedDown、MobileJ2、MobileJ4),以匹配机器人坐标系习惯
|
|
|
/// </summary>
|
|
|
- /// <param name="pixelCoord"></param>
|
|
|
- /// <param name="robotCoord"></param>
|
|
|
- /// <param name="calib"></param>
|
|
|
- /// <returns></returns>
|
|
|
+ /// <param name="pixelCoord">像素坐标及方向:(X, Y, angle)。angle 表示图像中测量的角度(度)</param>
|
|
|
+ /// <param name="robotCoord">机器人当前位姿数组,通常为 [X, Y, U],可为 null(视场景而定)</param>
|
|
|
+ /// <param name="calib">要使用的校准数据</param>
|
|
|
+ /// <param name="robotBrand">机器人品牌,用于处理角度符号等差异</param>
|
|
|
+ /// <returns>
|
|
|
+ /// 返回元组 (IsSucceed, X, Y, U):
|
|
|
+ /// - IsSucceed: 总是 true(当前实现未对数学运算做失败判定),调用者可根据需要扩展错误处理
|
|
|
+ /// - X, Y: 转换后的机器人坐标(单位同 calib 所用单位)
|
|
|
+ /// - U: 角度(经过可能的符号调整)
|
|
|
+ /// </returns>
|
|
|
public (bool IsSucceed,double X, double Y, double U) ConvertPixelToPosition((double X, double Y, double angle) pixelCoord, double[] robotCoord, CalibrationInfo calib, RobotBrand robotBrand = RobotBrand.Default)
|
|
|
{
|
|
|
- double angle= pixelCoord.angle;
|
|
|
- //得到校准矩阵
|
|
|
+ double angle = pixelCoord.angle;
|
|
|
+
|
|
|
+ // 从校准数据获取仿射变换矩阵(3x3)
|
|
|
Matrix<double> matrix = Matrix<double>.Build.DenseOfArray(calib.AffineTransformationMaterial);
|
|
|
+
|
|
|
+ // 某些安装方式需要取反角度以匹配机器人坐标系定义
|
|
|
if (calib.CameraMount == CameraMount.FixedDown || calib.CameraMount == CameraMount.MobileJ2 || calib.CameraMount == CameraMount.MobileJ4)
|
|
|
{
|
|
|
angle *= -1;
|
|
|
}
|
|
|
- //将像素坐标转换成机器人坐标
|
|
|
+
|
|
|
+ // 将像素坐标通过仿射矩阵转换到机器人坐标(rpos 是长度 3 的向量:[X, Y, w],通常 w 为 1)
|
|
|
var rpos = matrix * Vector<double>.Build.Dense(new double[] { pixelCoord.X, pixelCoord.Y, 1 });
|
|
|
+
|
|
|
+ // 针对不同的 CameraMount 做进一步坐标变换
|
|
|
if (calib.CameraMount == Enums.CameraMount.MobileJ4)
|
|
|
{
|
|
|
- //像素直接转换成mm时的点位
|
|
|
- double Robot_X, Robot_Y;
|
|
|
- Robot_X = rpos[0];
|
|
|
- Robot_Y = rpos[1];
|
|
|
-
|
|
|
- //机器人当前TOOL 0下的坐标
|
|
|
- double curpos_x = 0, curpos_y = 0, curpos_u = 0;
|
|
|
- curpos_x = robotCoord[0];
|
|
|
- curpos_y = robotCoord[1];
|
|
|
- curpos_u = robotCoord[2];
|
|
|
-
|
|
|
- //得到旋转矩阵
|
|
|
+ // 当 CameraMount 为 MobileJ4 时:
|
|
|
+ // - rpos 的前两维表示像素->mm 转换后的点 (Robot_X, Robot_Y)
|
|
|
+ // - 需要基于机器人当前 TOOL0 坐标以及 MarkPoint 的角度进行旋转和平移
|
|
|
+ double Robot_X = rpos[0];
|
|
|
+ double Robot_Y = rpos[1];
|
|
|
+
|
|
|
+ // 机器人当前 TOOL0 下的位姿
|
|
|
+ double curpos_x = robotCoord[0];
|
|
|
+ double curpos_y = robotCoord[1];
|
|
|
+ double curpos_u = robotCoord[2];
|
|
|
+
|
|
|
+ // 旋转角(弧度) = (curpos_u - calib.MarkPoint.U) * PI / 180
|
|
|
double angle1 = Math.PI * (curpos_u - calib.MarkPoint.U) / 180;
|
|
|
- // 创建T矩阵
|
|
|
+
|
|
|
+ // 2x2 旋转矩阵
|
|
|
Matrix<double> rotationMatrix = Matrix<double>.Build.DenseOfArray(new double[,]
|
|
|
{
|
|
|
- { Math.Cos(angle1), -Math.Sin(angle1) },
|
|
|
- { Math.Sin(angle1), Math.Cos(angle1) }
|
|
|
+ { Math.Cos(angle1), -Math.Sin(angle1) },
|
|
|
+ { Math.Sin(angle1), Math.Cos(angle1) }
|
|
|
});
|
|
|
|
|
|
+ // 将相对向量 p3 旋转并平移到当前工具坐标系下
|
|
|
Vector<double> p3 = Vector<double>.Build.Dense(new double[] { Robot_X - calib.CalibPoints[0].Robot.X, Robot_Y - calib.CalibPoints[0].Robot.Y });
|
|
|
Vector<double> phere = Vector<double>.Build.Dense(new double[] { curpos_x, curpos_y });
|
|
|
|
|
|
@@ -262,68 +385,71 @@ namespace TeamAAS_VP.Services
|
|
|
}
|
|
|
else if (calib.CameraMount == Enums.CameraMount.MobileDown_XYPlatform)
|
|
|
{
|
|
|
- //像素直接转换成mm时的点位
|
|
|
- double Robot_X, Robot_Y;
|
|
|
- Robot_X = rpos[0];
|
|
|
- Robot_Y = rpos[1];
|
|
|
-
|
|
|
- //模组当前TOOL 0下的坐标(相当于模组吸嘴坐标)
|
|
|
- double curpos_x = 0, curpos_y = 0, curpos_u = 0;
|
|
|
- curpos_x = robotCoord[0];
|
|
|
- curpos_y = robotCoord[1];
|
|
|
- curpos_u = robotCoord[2];
|
|
|
-
|
|
|
- //得到旋转矩阵
|
|
|
- //double angle1 = Math.PI * (curpos_u - calib.MarkPoint.U) / 180;
|
|
|
+ // MobileDown_XYPlatform 模式下,当前实现与 MobileJ4 类似,但角度暂时设为 0(不旋转)
|
|
|
+ double Robot_X = rpos[0];
|
|
|
+ double Robot_Y = rpos[1];
|
|
|
+
|
|
|
+ double curpos_x = robotCoord[0];
|
|
|
+ double curpos_y = robotCoord[1];
|
|
|
+ double curpos_u = robotCoord[2];
|
|
|
+
|
|
|
+ // 角度暂用 0(如有需要可基于 curpos_u - calib.MarkPoint.U 做旋转)
|
|
|
double angle1 = 0;
|
|
|
- // 创建T矩阵
|
|
|
Matrix<double> rotationMatrix = Matrix<double>.Build.DenseOfArray(new double[,]
|
|
|
{
|
|
|
- { Math.Cos(angle1), -Math.Sin(angle1) },
|
|
|
- { Math.Sin(angle1), Math.Cos(angle1) }
|
|
|
+ { Math.Cos(angle1), -Math.Sin(angle1) },
|
|
|
+ { Math.Sin(angle1), Math.Cos(angle1) }
|
|
|
});
|
|
|
|
|
|
Vector<double> p3 = Vector<double>.Build.Dense(new double[] { Robot_X - calib.CalibPoints[0].Robot.X, Robot_Y - calib.CalibPoints[0].Robot.Y });
|
|
|
Vector<double> phere = Vector<double>.Build.Dense(new double[] { curpos_x, curpos_y });
|
|
|
|
|
|
var cc = rotationMatrix * p3 + phere;
|
|
|
- rpos = Vector<double>.Build.Dense(new double[] { cc[0], cc[1] });//Mark点在tool0下的的坐标
|
|
|
+ rpos = Vector<double>.Build.Dense(new double[] { cc[0], cc[1] }); // Mark 点在 tool0 下的坐标
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- //当需要直接转换工具坐标时
|
|
|
- if (robotCoord!=null)
|
|
|
+ // 其他 CameraMount:如果提供了 robotCoord,则将仿射变换得到的点转换为工具坐标
|
|
|
+ if (robotCoord != null)
|
|
|
{
|
|
|
double curposX = robotCoord[0];
|
|
|
double curposY = robotCoord[1];
|
|
|
double curposU = robotCoord[2];
|
|
|
|
|
|
- //根据机器人当前的点位计算工具坐标
|
|
|
+ // 使用 ToolCoord 计算工具坐标(ToolCoord 内封装了从机器人基坐标到工具坐标的数学)
|
|
|
ToolCoord tool = new ToolCoord();
|
|
|
+
|
|
|
+ // Schneider 机器人角度符号可能与其他品牌相反,做兼容处理
|
|
|
if (robotBrand == RobotBrand.Schneider)
|
|
|
{
|
|
|
curposU *= -1;
|
|
|
}
|
|
|
+
|
|
|
+ // 计算工具坐标并将 rpos 替换为工具坐标的 X,Y
|
|
|
tool.ComputeTool(curposX, curposY, curposU, rpos[0], rpos[1]);
|
|
|
rpos = Vector<double>.Build.Dense(new double[] { tool.X, tool.Y });
|
|
|
}
|
|
|
}
|
|
|
+
|
|
|
+ // 返回最终结果,IsSucceed 在当前实现中总为 true(可根据需要扩展)
|
|
|
return (true, rpos[0], rpos[1], angle);
|
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
|
- /// 校准转换,将像素坐标转换位置坐标
|
|
|
+ /// 通过 calibId 获取校准并执行像素->位置转换。
|
|
|
/// </summary>
|
|
|
- /// <param name="pixelCoord"></param>
|
|
|
- /// <param name="robotCoord"></param>
|
|
|
- /// <param name="calibId"></param>
|
|
|
- /// <returns></returns>
|
|
|
+ /// <param name="pixelCoord">像素坐标及角度 (X, Y, angle)</param>
|
|
|
+ /// <param name="robotCoord">机器人当前位姿数组 [X, Y, U]</param>
|
|
|
+ /// <param name="calibId">校准项 Id</param>
|
|
|
+ /// <param name="robotBrand">机器人品牌(可选)</param>
|
|
|
+ /// <returns>返回 (IsSucceed, X, Y, U)</returns>
|
|
|
public (bool IsSucceed, double X, double Y, double U) ConvertPixelToPosition((double X, double Y, double angle) pixelCoord, double[] robotCoord, Guid calibId, RobotBrand robotBrand = RobotBrand.Default)
|
|
|
{
|
|
|
|
|
|
var calib = GetCalibration(calibId);
|
|
|
if (calib == null)
|
|
|
{
|
|
|
+ // 若未找到校准数据,返回失败标志
|
|
|
return (false, 0, 0, 0);
|
|
|
}
|
|
|
var result = ConvertPixelToPosition(pixelCoord, robotCoord, calib, robotBrand);
|
|
|
@@ -331,6 +457,9 @@ namespace TeamAAS_VP.Services
|
|
|
|
|
|
}
|
|
|
|
|
|
+ /// <summary>
|
|
|
+ /// Dispose 时保存所有校准数据并抑制终结化。
|
|
|
+ /// </summary>
|
|
|
public void Dispose()
|
|
|
{
|
|
|
SaveAll();
|