关于【比较坐标】总结

该功能为了在机器人抓取的时候，防止这个工件有问题，抓不起来。

1、可以在头文件里面定义结构体：

#define Pos Pos1
#define pPos pPos1

//位姿结构体Pos
typedef struct Pos {
    double X;      //位置X坐标
    double Y;      //位置Y坐标
    double Z;      //位置Z坐标
    double Rz;     //位置Rz旋转角,radius弧度表达
    double Ry;     //位置Ry旋转角,radius弧度表达
    double Rx;     //位置Rx旋转角,radius弧度表达

    Pos()
    {
        X = 0;
        Y = 0;
        Z = 0;
        Rz = 0;
        Ry = 0;
        Rx = 0;
    };
}*pPos;

2、定义一个比较坐标的函数，传过来两个坐标，看两个坐标的距离是否大于10，如果大于10，则返回true：

bool compareXYZ(Pos* posNew, Pos* posOld) {

	double result = sqrt(pow((posNew->X - posOld->X), 2) +
		pow((posNew->Y - posOld->Y), 2) +
		pow((posNew->Z - posOld->Z), 2));
	std::cout << "result" << std::endl;
	std::cout << result << std::endl;

	if (result > 10) {
		std::cout << "true" << std::endl;
		return true;

	}
	else
		std::cout << "false" << std::endl;
	return false;
}

3、利用for循环去遍历每一个坐标，如果坐标满足条件则通过，不满足也要抓第二次，第三次就舍弃了。

			int j = 0;
			for (int i = 0; i<tranferCount; i++) {
				if (tranferCount > 1) {
					repeatTime = 0;
					objectPos->X = pFound[i].centerXmm;
					objectPos->Y = pFound[i].centerYmm;
					objectPos->Z = pFound[i].centerZmm;
					objectPos->Rx = pFound[i].rotX;
					objectPos->Ry = pFound[i].rotY;
					objectPos->Rz = pFound[i].rotZ;

					if (compareXYZ(objectPos, comparePos)||repeatTime<3) {

						pFound[j] = pFound[i];
						j++;
					}
				}
			}
			std::cout << j << "j" << std::endl;
			if (j == 0 && repeatTime >= 3) {

				mlen = snprintf(_sendingBuf.data() + offset, _sendingBuf.size() - offset, "0,0,0,0,0,0,");
				offset += mlen;

				goto repeatObj;
			}