【前言】
前一篇文章【Gazebo仿真·一】Gazebo + Moveit + ur5e机械臂仿真，实现了在Gazebo中通过moveit对ur5e机械臂进行控制仿真；这一篇文章旨在机械臂末端添加一个RealSense d435相机，并在gazebo环境中实现二维码识别与跟随的仿真，二维码识别使用Aruco包。
一、仿真前的准备
1.1 realsense 软件包安装
根据系统环境，安装realsense SDK 和 realsense_ros：自用参考链接，有时间更新
1.2 本文工作空间catkin_ws文件结构
|–catkin_ws
|—-src
|——universal_robot
|——robot_gazebo
|———-robot_description
|———-ur5e_moveit_config
|——realsense
|———-realsense_ros
|———-realsense_ros_gazebo
|——my_scene
|———-aruco_ros
|———-arm_control
git地址：GitHub – Aya-nuomi/marker_track
二、在gazebo中进行realsense仿真
2.1 设置realsense模型和插件
步骤：
1）realsense模型和插件包下载
cd catkin_ws/src/realsense
git clone https://github.com/nilseuropa/realsense_ros_gazebo.git2）加载realsense模型和插件（沿用上一篇中的工作空间）
修改…/robot_description/ur5e_robot.urdf.xacro，添加realsense模型和插件
tips：可以参考…/realsense_ros_gazebo/urdf/test.xacro文件
<?xml version="1.0"?>
<robot name="ur5e" xmlns:xacro="http://ros.org/wiki/xacro" >
<!-- common stuff -->
<xacro:include filename="$(find robot_description)/urdf/common.gazebo.xacro" />
<!-- ur5e -->
<xacro:include filename="$(find robot_description)/urdf/ur5e.urdf.xacro" />
<!-- Realsense d435 -->
<xacro:include filename="$(find realsense_ros_gazebo)/xacro/depthcam.xacro"/>
<!-- my_arm -->
<xacro:ur5e_robot prefix="" joint_limited="false"/>
<!-- my_d435 -->
<xacro:realsense_d435 sensor_name="d435" parent_link="tool0" rate="10">
<origin rpy="0 ${-pi/2} 0 " xyz="0 0 0.02"/>
</xacro:realsense_d435>
<link name="world" />
<joint name="world_joint" type="fixed">
<parent link="world" />
<child link = "base_link" />
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</joint>
</robot>这里把realsense相机的父坐标系设置为tool0，并通过<origin>设置了相机的初始位姿。
2.2 编译并运行launch文件，进行realsense仿真
catkin build
source devel/setup.bash
roslaunch ur5e_moveit_config demo_gazebo.launchtips：修改xacro文件中参数，如rate等，可以设置相机话题的发布频率
三、在gazebo环境中添加二维码
3.1 创建墙体，在墙体上添加二维码（二维码与墙体是一个模型）
1）添加二维码图片，创建二维码材料
· ARcode生成网址：Online ArUco markers generator
·通过在线转换网站把.svg格式转变为.png格式：在线将SVG 转换成 PNG
·把二维码图片（png格式）复制到 /usr/share/gazebo-9/media/materials/textures
·创建二维码材料：在/usr/share/gazebo-9/media/materials/scripts中新建 aruco.material
import * from "grid.material"
material Gazebo/aruco-36
{
receive_shadows on
technique
{
pass
{
ambient 0.5 0.5 0.5 1.000000
texture_unit
{
texture aruco_36.png
}
}
}
}tips: 应该也可以把图片和material文件放在model文件夹内：material文件： …/models/my_model/materials/scrips；图片： …/models/my_model/materials/textures
2）创建墙体，修改墙体model文件，添加二维码的<link> <joint>
·打开gazebo
·Edit -> Building Editor, 创建墙体，并保存在~/.gazebo/models文件夹下
·修改墙体模型的sdf文件，把二维码的<link>和<joint>添加进去，<link>的材料选择第1步创建的
tips:
这里注意，如果墙体保存在/home路径下，墙体model就在/home/z/…中
如果把墙体model保存在/home/z/.gazebo/models下，就从这里选择，可以把其他model放在这个路径下。
3）打开gazebo，插入带二维码的墙体
·重新打开gazebo
·在Insert处，插入墙体
4）另存为world文件，并调用
·另存为my_world.world文件，放在…/robot_description/world下
·修改gazebo.launch中world文件的地址
<!-- startup simulated world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<!-- <arg name="world_name" default="worlds/empty.world"/> -->
<arg name="world_name" value="$(find robot_description)/world/my_world.world"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="gui" value="$(arg gazebo_gui)"/>
</include>·运行launch文件，测试gazebo环境是否创建成功
roslaunch ur5e_moveit_config demo_gazebo.launch3.2 创建二维码模型，墙体与二维码是两个模型（本人使用的这种）
1）添加二维码图片，创建二维码材料：同上
2）创建墙体模型：同上
3）在/home/z/.gazebo/models下创建marker文件夹，以及model.sdf, model.config
cd /home/z/.gazebo/models
mkdir -p marker4）打开gazebo，Insert墙体和二维码
5）另存为world文件，修改my_world.world文件，调整墙体和二维码的位置
6）调用launch文件，测试效果：同上
3.3 创建一个模型，在模型上添加二维码
四、二维码识别，Aruco包
4.1 二维码识别 Aruco包
git地址：GitHub – pal-robotics/aruco_ros at melodic-devel
ARcode生成网址：Online ArUco markers generator
Aruco源码解析：OpenCV Aruco 参数源码完整解析理解！
步骤：
1）安装aruco包
2）确保已经正确安装realsense SDK 和 realsense_ros
3）修改aruco包的single.launch文件，修改对应的topic和frame
·修改markerId和markerSize，修改为所使用的二维码的信息；
·修改/camera_info和/image话题，改为对应仿真环境下相机的话题；
·修改camera_frame，目前不知道使用d435_color_optical_frame和camera_link的区别？？？
·修改ref_frame，可以直接获得marker_frame到机械臂base_link的tf转换关系；
<launch>
<!-- demo.launch, copy of single.launch -->
<arg name="markerId" default="36"/>
<arg name="markerSize" default="0.1"/> <!-- in m -->
<arg name="eye" default="left"/>
<arg name="marker_frame" default="aruco_marker_frame"/>
<arg name="ref_frame" default="base_link"/> <!-- leave empty and the pose will be published wrt param parent_name -->
<arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->
<node pkg="aruco_ros" type="single" name="aruco_single" output="screen">
<remap from="/camera_info" to="/d435/color/camera_info" />
<remap from="/image" to="/d435/color/image_raw" />
<param name="image_is_rectified" value="True"/>
<param name="marker_size" value="$(arg markerSize)"/>
<param name="marker_id" value="$(arg markerId)"/>
<param name="reference_frame" value="$(arg ref_frame)"/> <!-- frame in which the marker pose will be refered -->
<param name="camera_frame" value="d435_color_optical_frame"/>
<param name="marker_frame" value="$(arg marker_frame)" />
<param name="corner_refinement" value="$(arg corner_refinement)" />
</node>
</launch>
tips : 发布的位姿是以”ref_frame”为基准，”ref_frame” -> “marker_frame”之间的tf关系。
4）运行realsense
·运行仿真环境下的相机（详见2.2）
roslaunch ur5e_moveit_config demo_gazebo.launch·也可以运行真实的realsense相机进行测试
roslaunch realsense2_camera rs_camera.launch //发布图片5）运行aruco的launch文件
roslaunch aruco_ros demo.launch //copy of single.launch6）观察识别效果
rosrun image_view image_view image:=/aruco_single/result7）查看位姿
rostopic echo /aruco_single/pose五、机械臂控制，二维码跟随
5.1 控制机械臂（点动）（C++）
tips:
·给定目标位置，控制机械臂移动到目标位置
·代码见git，路径为 …/arm_control/src/arm_control.cpp
5.2 控制机械臂，实现二维码跟随（C++）
tips:
·识别二维码，获取marker的pose之后，转换为机械臂末端位置，控制机械臂移动到marker上方，marker移动机械臂能够随之移动
·代码见git，路径为 …/arm_control/src/move_with_marker.cpp
Q：
·这里在仿真的时候，代码逻辑能够跑通，但是计算出机械臂位置后，使用moveit进行机械臂路径规划时，总是规划失败，这一点未解决，但不是本篇文章的重点，留到后面去处理。
git地址：GitHub – Aya-nuomi/marker_track