Apriltags+Ros+Ur5项目--Apriltags位姿输出

进入课题组的初期，老师分配了一个实际项目，下面写一下搭的项目的大致框架及其心得感悟，因为现在已经越发浮躁，想踏下心来好好做一些回顾一下自己的新手上路。需要准备材料：apriltags下tag标签，usb摄像头，标定板，ur5（也可以用gazebo模型代替）
大致流程如下：

针对于这个项目想实现的是通过apriltags算法能够识别空间中指定标号的tag，从而能够算出apriltags相对于相机的位姿，并作为话题发布出去，因为这里apriltags算法已经是成熟的源码包，所以这里只是跑通了这个包，但这里本身包发布的话题很多，不单单包含位姿输出，为了减少实时传输量，所以创建了一个订阅节点（atnode_vision），订阅此位姿。然后将此位姿作为服务器服务端消息发布，在另一个节点（apriltags2_client）上，进行机械臂UR5的轨迹规划。

整个项目的github链接：https://github.com/harrycomeon/apriltags2_ros-and-ur5
上面介绍了整个项目的大致流程：
下面先分析一下Apriltags位姿输出部分：
首先自己先是找到了Apriltags官网，官网上有很多论文，如果想深入研究算法，这里绝对有很好的资料。自己先根据此博客在非ROS下利用opencv实现了具体的位姿输出，当然要移植成自己的需要对以下部分进行修改：
github链接https://github.com/harrycomeon/apriltags

 apriltag_detection_info_t info;
            info.tagsize = 0.0535;   //打印的qrcode(二维码)尺寸大小，单位米
            info.fx = 931.4964;	   //单位像素
            info.fy = 935.2657;
            info.cx = 352.2813;
            info.cy = 315.9761;      //初始化对应的相机参数

---

 for (int i = 0; i < zarray_size(detections); i++) {
            apriltag_detection_t *det;
            zarray_get(detections, i, &det);
            info.det = det;             //将det赋给info用于计算
            apriltag_pose_t pose;
            double err = estimate_tag_pose(&info, &pose);
            //double err=estimate_pose_for_tag_homography(&info, &pose);
	    //其中pose.R、pose.t分别表示Tag相对于相机坐标系的旋转矩阵和平移向量
            cout<<"平移向量t:"<data[1]<<"  "<data[2]<data[3]<<"  "<data[4]<<"  "<data[5]<data[6]<<"  "<data[7]<<"  "<data[8]<

这一块是在ubuntu上建立工程项目实现了位姿的输出，那末我们向将位姿传给UR5，就必须在ros上实现此功能，并发布位姿话题。这部分也是以后点云识别位姿要实现了的，算法这部分也是主要出论文的部分。

那麽现在说一下自己在ros上实现Apriltags位姿输出的过程。
在github上我们可以找到很多相关的代码，我用的是https://github.com/shijiwensjw/apriltags2_ros 下的代码实现了Apriltags位姿输出，既然有了算法，那麽我们需要通过摄像头输入图像，既然有了摄像头引入，我们便需要进行相机内参的标定，只有这样才可以保证精度。

具体实现：
首先下载usb_cam的包，然后在在launch文件修改如下：
“/dev/video1” /是插入usb摄像头具体的设备口
相机标定实现参考网址：https://blog.csdn.net/harrycomeon/article/details/91126256

然后在ros下，建立工作空间，将apriltags2，apriltags2_ros,camera_calibration,usb_cam文件夹复制到所建的工作空间的src目录下。catkin_make编译通过后，便可以依次运行

roslaunch usb_cam usb_cam-test.launch
roslaunch apriltags2_ros continuous_detection.launch

然后自己将两个launch文件整合到一起，只需要运行一个就可以

 roslaunch apriltags2_ros continuous_detection.launch

此时已经发布了话题，然后我们便可以查看话题下的内容，其中在/tag_detections和/tf话题下，发布了tag相对于相机的相对位姿。

注意下面文件下有指定识别tag的标号和实际大小