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

155
0
2021年1月18日 09时08分

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

 

大致流程如下:

 

微信图片_20210117200507

 

针对于这个项目想实现的是通过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:"<<endl;
            cout<<" x="<<pose.t->data[0]<<"  "<<"y="<<pose.t->data[1]<<" "<<"z="<<pose.t->data[2]<<endl;//输出t

            cout<<"旋转矩阵R="<<endl;  //输出R,
            cout<<pose.R->data[0]<<"  "<<pose.R->data[1]<<"  "<<pose.R->data[2]<<endl;
            cout<<pose.R->data[3]<<"  "<<pose.R->data[4]<<"  "<<pose.R->data[5]<<endl;
            cout<<pose.R->data[6]<<"  "<<pose.R->data[7]<<"  "<<pose.R->data[8]<<endl;

 

这一块是在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相对于相机的相对位姿。

 

微信图片_20210117200559

 

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

 

微信图片_20210117200614

 

微信图片_20210117200627

发表评论

后才能评论