webots导入自定义纹理textures（aruco码为例），并用aruco_ros包识别aruco码

最近需要二维码在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可以看到发布的符合预期。