【Gazebo仿真·二】Gazebo + ur5e + RealSense + Aruco 二维码识别与跟随仿真

【前言】

前一篇文章【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模型和插件

[gazebo仿真]添加RealSense双目相机传感器

 步骤：

1）realsense模型和插件包下载

cd catkin_ws/src/realsense
git clone https://github.com/nilseuropa/realsense_ros_gazebo.git

2）加载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.launch

tips：修改xacro文件中参数，如rate等，可以设置相机话题的发布频率

三、在gazebo环境中添加二维码

3.1 创建墙体，在墙体上添加二维码（二维码与墙体是一个模型）

从gazebo9中在墙上添加二维码

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.launch

3.2 创建二维码模型，墙体与二维码是两个模型（本人使用的这种）

gazebo添加二维码模型

1）添加二维码图片，创建二维码材料：同上

2）创建墙体模型：同上

3）在/home/z/.gazebo/models下创建marker文件夹，以及model.sdf, model.config

cd /home/z/.gazebo/models
mkdir -p marker

4）打开gazebo，Insert墙体和二维码

5）另存为world文件，修改my_world.world文件，调整墙体和二维码的位置

6）调用launch文件，测试效果：同上

3.3 创建一个模型，在模型上添加二维码

贴有图片或二维码的Gazebo模型创建

四、二维码识别，Aruco包

4.1 二维码识别 Aruco包

git地址：GitHub - pal-robotics/aruco_ros at melodic-devel

ARcode生成网址：Online ArUco markers generator

Aruco源码解析：OpenCV Aruco 参数源码完整解析理解！

 步骤：

使用Realsense测试aruco_ros包

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.launch

6）观察识别效果

 rosrun image_view image_view image:=/aruco_single/result

7）查看位姿

rostopic echo /aruco_single/pose

五、机械臂控制，二维码跟随

5.1 控制机械臂（点动）（C++）

ROS下C++控制UR机械臂 - 古月居

tips:

·给定目标位置，控制机械臂移动到目标位置

·代码见git，路径为 .../arm_control/src/arm_control.cpp

5.2 控制机械臂，实现二维码跟随（C++）

ROS中控制机械臂抓取目标例程

ROS 控制Innfos机械臂简单例子

tips:

·识别二维码，获取marker的pose之后，转换为机械臂末端位置，控制机械臂移动到marker上方，marker移动机械臂能够随之移动

·代码见git，路径为 .../arm_control/src/move_with_marker.cpp

Q：

·这里在仿真的时候，代码逻辑能够跑通，但是计算出机械臂位置后，使用moveit进行机械臂路径规划时，总是规划失败，这一点未解决，但不是本篇文章的重点，留到后面去处理。

git地址：GitHub - Aya-nuomi/marker_track

NEXT：视觉伺服（IBVS）+机械臂仿真