最近需要二维码在webots中实现定位并最终实现伺服导航,因此需要将aruco码导入webots。
通过调研发现大部分都是在真实相机上使用aruco_ros进行手眼标定,很少在仿真环境中使用aruco_ros进行视觉伺服,因此记录一下。
Part1 aruco码导入webots
1.首先在线生成aruco码,然后保存。
注意是original aruco!
2.打开webots新增一个solid。
3.在children下新增shape
4.在shape下的geometry新增一个平面让它显示出来,appearance下新增Appearance
5.Appearance下的texture新增一个imageTexture,url下输入你的存放图片地址即可
Part2 aruco_ros识别
1.aruco_ros包的安装及配置
首先安装aruco_ros包,可参考博客和官网。
修改single.launch, 主要修改自己的makerId,makerSize,和上述生成的aruco码匹配,remap 后面的to 更改为自己的相机image话题以及相机camera_info话题。
<arg name="markerId" default="400"/>
<arg name="markerSize" default="0.15"/> <!-- in m -->
<remap from="/camera_info" to="/camera/HeadRGB_info" />
<remap from="/image" to="/camera/headRGB" />
2. 编写camera_info_pub发布camera_info
由于使用的为仿真环境webots中的相机,并不是外接相机的usb_cam,没有usb_cam发布camera_info话题,因此需要自己写一个话题发布者,发布camera_info,话题的消息类型是sensor_msgs/CameraInfo,注意有没有camera_info_manager这个包,其中定义了sensor_msgs/CameraInfo这个类型。。
创建一个功能包camera_info_pub,然后在cmakelist内添加编译规则,填充消息内容,使用rosmsg show sensor_msgs/CameraInfo
查看消息格式:
对消息进行赋值,有关于相机参数的解释可参考博客或者官网。
sensor_msgs::CameraInfo msg;
msg.height = 480;
msg.width = 640;
msg.distortion_model = "plumb_bob";
msg.D.resize(5);//如果不resize,编译不报错,运行时会出现段错误
for (int i = 0; i<5; i++)
{
//畸变参数
msg.D[i] = 0;
}
float R_[] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
float K_[] = {521.1711084,0.0,320,0.0,547.7089685,240,0,0,1};
float P_[] = {521.1711084,0,320,0,0,547.7089685,240,0,0,0,1,0};
for (int i = 0; i<9; i++)
{
//9 旋转矩阵
msg.R[i] = R_[i] ;
//9 内参矩阵 fx 0 cx 0 fy cy 0 0 1
msg.K[i] = K_[i];
}
for (int i = 0; i<12; i++)
{
//12 投影矩阵 fx 0 cx tx 0 fy cy ty 0 0 1 0单目相机tx=ty=0
msg.P[i] = P_[i];
}
msg.binning_x = 0;
msg.binning_y = 0;
msg.roi.x_offset = 0;
msg.roi.y_offset = 0;
msg.roi.height = 0;
msg.roi.width = 0;
msg.roi.do_rectify = false;
//发布者定义
ros::Publisher pub = nh.advertise<sensor_msgs::CameraInfo>("/camera/HeadRGB_info",10);
//模板,定义消息类型 //话题名//队列
3.webots仿真环境结合aruco_ros对aruco码进行识别
catkin_make编译通过
1)运行roscore
2)运行camera_info_pub(也可以将其写入相机控制器,则省去第三步)
3)启动webots
4)运行aruco包下的修改后的single.launch,便实现了aruco码的识别,其实aruco_ros就是将相机参数和图像读入,再转化成位置和姿态(/aruco_single/pose)。
5)通过rqt_image_view,可以看到结果如图所示。
6)查看位姿:
通过rostopic echo /camera/HeadRGB_info
可以看到发布的符合预期。