Dobot magician + realsense D435i 手眼标定（外参）C++

Dobot magician + realsense D435i 手眼标定（外参）C++

之前写过一篇用python实现的，最近在ubuntu18.04弄了一个c++的，记录一下

一、pipeline:

1.下载aruco_ros、realsense-ros，打印aruco二维码，最好打印这个ros包里面的二维码，写好启动realsense和aruco的文件，直接通过获取ros消息获得二维码在相机坐标系里的位置，默认topic是“/aruco_tracker/position”；

2.dobot跑8个目标点，获得对应8个二维码坐标位置；

3.利用T*aruco_position=dobot_position, T=dobot_positon*inv(aruco_position)求出变换矩阵T。

二、过程中的坑

1.注意dobot运动到目标点后才读取aruco位置数据，需要对dobot运行的cmd消息队列中的index和完成的cmd index进行比较延时。一开始想着用dobot_ros ，调用服务的方式进行，不过太麻烦，改正直接用dobot的API，注意用得出的变化矩阵，通过相机得到的数据变换到dobot base基坐标下不是末端TCP点的位置，有个偏差，因为如图所示，二维码的中心点位置与吸盘有一定偏差，所以在后面用的时候主要把这个修正这个偏差。

2.ros中的消息可以按需求时再去订阅，不用循环订阅，代码如下

	aruco_position_msg = ros::topic::waitForMessage("/aruco_tracker/position",ros::Duration(5));
	aruco_position(0,i)=aruco_position_msg->vector.x;
    aruco_position(1,i)=aruco_position_msg->vector.y;
	aruco_position(2,i)=aruco_position_msg->vector.z;
	aruco_position(3,i)=1;

三、完整代码

#include 
#include
#include
#include 
#include
#include
#include 
#include
#include
using namespace std;

#include "ros/ros.h"
//#include "std_msgs/String.h"
//#include "std_msgs/Float32MultiArray.h"
#include "geometry_msgs/Vector3Stamped.h"

#include "DobotDll.h"

//用eigen计算伪逆矩阵函数
template 
_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = 
    std::numeric_limits::epsilon()) 
{  
    Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);  
    double tolerance = epsilon * std::max(a.cols(), a.rows()) *svd.singularValues().array().abs()(0);  
    return svd.matrixV() *  (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint(); 
}  



int main(int argc, char **argv)
{
   //connecte dobot
    if (argc < 2) {
        cout<<"Application portName"<> x) {
			count++;
			if (count < 5)
			{
				 dobot_target_data[i][j] = x;
				std::cout<< dobot_target_data[i][j] <<"t";
				j++;
			}
			
		}
		std::cout << std::endl;
		i++;
		j = 0;
		count = 0;
		
	}
	in.close();

      //初始化节点
	ros::init(argc, argv, "aruco_position_subscriber");
	ros::NodeHandle n;
	// ros::ServiceClient client;
	ros::Rate loop_rate(2);
    ROS_INFO("arocu_position_subscriber running.../n");

    boost::shared_ptr  aruco_position_msg;
	Eigen::MatrixXd  aruco_position(4,8);
	Eigen::MatrixXd dobot_target_position(4,8);

     SetCmdTimeout(3000);
     SetQueuedCmdClear();
     SetQueuedCmdStartExec();

    uint64_t executedCmdIndex=0 ;
    uint64_t  queuedCmdIndex;
    PTPCmd  ptpcmd[8 ];
    WAITCmd cmd;
    cmd.timeout = 1500;
    for(i=0;i<8;i++)
    {
        ptpcmd[i].ptpMode = 1;
        ptpcmd[i].x = dobot_target_data[i][0] ;
        ptpcmd[i].y = dobot_target_data[i][1] ;
        ptpcmd[i].z = dobot_target_data[i][2] ;
        ptpcmd[i].r = dobot_target_data[i][3] ;

        SetPTPCmd(&ptpcmd[i], true, &queuedCmdIndex);
        while(executedCmdIndex != queuedCmdIndex)
        {
          
            GetQueuedCmdCurrentIndex(&executedCmdIndex);
            sleep(2);
        }
        SetWAITCmd(&cmd, true, &queuedCmdIndex);
        cout<<"if delay print?"<("/aruco_tracker/position",ros::Duration(5));
		aruco_position(0,i)=aruco_position_msg->vector.x;
		aruco_position(1,i)=aruco_position_msg->vector.y;
		aruco_position(2,i)=aruco_position_msg->vector.z;
		aruco_position(3,i)=1;
		cout<<"aruco_position is "<，
    {
        for(int j = 0;j<4;j++)
        {

            output_stream << image_to_arm (i,j);
            if(j<3)
                output_stream << "t";
            else
                output_stream<<"n";
        }
        
    }
    output_stream.close();



}