项目篇：移动平台机械臂视觉定位抓取实施（基于ROS架构）

ROS包内容：代码暂不公开

这篇日志的目的是快速项目实施，所以详细的部分会不断补充。举例如下：

• 项目包1：项目地址

• Pylon Camera

• Aruco_ros

• AUBO Robot

• 大寰机械手AG-95

• 流程集成模块

• 模块包：

• 相机采集包（创建抓取图像服务，单张）

  • Realsense Camera 模块地址（微软深度摄像头，官方提供ros包）
  • Pylon Camera 模块地址（basler相机，官方提供ros包）

• 定位算法包（抓取图像，定位标志物）

  • Aruco_ros 模块地址 （二维码定位，官方提供ros包）
  • Apriltag 模块地址 （二维码定位，官方提供ros包）
  • shape_based_matching 模块地址（linemod，任意形状标志物，官方提供lib包）

• 机械臂控制包（创建控制机械臂服务）

  • UR Robot 模块地址（UR协作机械臂，官方提供ros包）
  • Aubo Robot 模块地址（国产傲博机械臂，官方提供ros包，需自行完善）

• 机械手模块（创建机械手服务）

  • 大寰机械手AG-95 模块地址

• 流程集成模块（集成以上模块完成视觉定位、机械臂抓取任务）

  • 相机与机械臂坐标系转换（也可以称为标定，2D和3D略有不同），后面单独来说
  • 工作流程状态机

• 网络IO模块（modbus协议）

  • 卓兰 模块地址

• 光源控制模块–可选模块 模块地址（创建光源控制服务）

• 手眼标定模块–可选模块 模块地址（用于相机与机械臂外参的计算）

相机与机械臂坐标系转换

• 2D平面转换：满足2个条件，1-此时相机z轴与法兰旋转轴平行，2-同时使用时相机也是垂直与物体平面（固定一个轴的姿态信息已知，由于这种特殊姿态，这种情况下我们可以不用计算相机与机械臂的外参），这种情况下相机像素平面与机械臂XOY平面是平行的，这是通过机械安装的方式达到的，一般精度很高，可以认为两者是平行的。这种情况下，只要进行工业上熟悉的9点标定即可。标定时先旋转相机（目的是找到机械臂末端在相机图像坐标系中的位置，这样机械臂坐标就可以和相机图像坐标一一对应了，为后面9点标定做准备）再平移相机（9点标定）。对应的9点标定方法与使用注意事项也挺复杂。现记录如下：
  我们知道为了通过机械臂上的相机查看桌面上Mark来检测机器人此刻的位置。首先要确定一个机械臂的位姿，即机器人的初始姿态（需要标定时记录，在后期定位机器人位置时也要用，为什么？因为如果不确定一个位姿，那通过相机来确定机器人的相对位置就不可能了，假想相机固定在世界坐标系下不动，机械臂随便移动，那小车的位置就是不确定的了）。ok然后，有了初始位姿，机械臂相对小车的位置就不变了，这时候相机位置就固定了（重点！！！，我们可以想象相机脱离机械臂，固定在了机械臂的上空，就像相机和机械臂脱离模型的9点标定，后面我们做的9点标定和脱离模型一致，只是由于相机在机械臂上，有些数据需要我们自己转换，比如通过旋转末端法兰就可以知道以像素为单位的旋转半径，进而知道法兰末端的像素坐标，最后9点标定平移部分也有一点小坑，由于相机在机械臂上，所以你向左移动机械臂，而图像像素则向右移动，所以在一一对应时要记得反向，ok其他没有什么了）。后记其实相机在机械臂上的情况还是把它作为一种脱离模型在使用。
• 3D平面转换：不满足2D平面转换的2个条件，即此时相机z轴与法兰旋转轴不平行，或者使用相机时，相机不垂直于物体平面（3个轴的姿态信息都不知道），机械臂任何姿态的变换对相机末端都会造成影响，这种糟糕的情况下我们一定要计算出相机与机械臂末端的外参。计算外参又叫做手眼标定。

Pylon Camera

• 驱动包下载并安装，见模块地址。
  驱动包安装：

  • Basler官网 - 产品中心 - 软件 - pylon for Linux x86 - 单击此处下载 - 选择版本
  • 压缩文件目录下对文件进行解压 cpp tar -xvf
  • 打开INSTALL文件 按照步骤进行安装

• 下载pylon camera ros包安装

  • 这个包使用action来请求一张图像，具体使用见项目包。

• 配置相机IP和预览

  • /opt/pylon5/bin/IpConfigurator 来配置相机的静态ip
  • /opt/pylon5/bin/PylonViewerApp 来预览图像。

• 可以使用ros包的action来抓取单张图像，这样就不需要时时通过topic来接受图像，这里注意的地方是，action的名字是 “/pylon_camera_node/grab_images_raw”。因为nh的初始化形式为nh("~")。

Aruco_ros

• 这里有篇文章是我第一次做时用到的，可以参考
  ArUco_ros使用
• 注意第二次我使用了错误的aruco码，导致检测不到，我把这些也穿到云盘上文件夹。由于对aruco还不是很熟悉，默认要使用Original ArUco的模式才能识别。Marker ID和Marker size随意。

Aubo Robot

• 无驱动包，下载安装即可

• 简单说明：配置好机械臂的ip地址（可以用示教器，不过有时会不成功，也可以进入电脑（傲博的控制器就是一个linux系统）中修改），在aubo_driver中param中配置好ip地址，安装运行即可。

大寰机械手AG-95

• 安装使用，详细说明见ros安装包的README.md

• 常见问题1：使用USB模式，linux下没有找到ttyACM0？
  回答1:拔掉USB线，重启一下盒子的电源，然后再开启电源插上USB线，再看看。

• 常见问题2：模式没有切换？
  回答2:是否有插着USB线拨码？USB会给盒子供电，这种时候不是完全断电，所以模式没有切换。

流程集成模块

• Yaml-cpp读写
  • 写yaml，注意头尾不要漏掉BeginSeq、EndSeq。

    std::map<int,tf::Transform>::iterator it_station_s;
    YAML::Emitter out_station_s;
    out_station_s << YAML::BeginSeq;
    i = 0;

    for(it_station_s = tf_targetfront_to_target_.begin(); it_station_s!=tf_targetfront_to_target_.end();++it_station_s)
    {
        //write tf
        tf::Quaternion q = (it_station_s->second).getRotation();
        tf::Vector3 orig = (it_station_s->second).getOrigin();

        std::map<std::string, double> map;
        map["id"]  = it_station_s->first;
        map["qw"]  = q.getW();
        map["qx"]  = q.getX();
        map["qy"]  = q.getY();
        map["qz"]  = q.getZ();
        map["x"]   = orig.getX();
        map["y"]   = orig.getY();
        map["z"]   = orig.getZ();

        std::string nameNode = "tf_targetfront_to_target_" + std::to_string(i);

        out_station_s << YAML::BeginMap;
        out_station_s << YAML::Key << nameNode;
        out_station_s << YAML::Value << YAML::Flow << map;
        out_station_s << YAML::EndMap;

        i++;
    }

    out_station_s << YAML::BeginMap;
    out_station_s << YAML::Key << "count";
    out_station_s << YAML::Value << tf_targetfront_to_target_.size();
    out_station_s << YAML::EndMap;

    package_path = ros::package::getPath("application_driver");
    yaml_path = package_path + "/param/calibration/tf_targetfront_to_target.yaml";

    out_station_s << YAML::EndSeq;
    std::ofstream fout_station_s(yaml_path);
    fout_station_s << out_station_s.c_str();
    fout_station_s.close();

• 读yaml

    //read the mark_hole_tf
    yaml_path = package_path + "/param/calibration/tf_targetfront_to_target.yaml";
    config = YAML::LoadFile(yaml_path);
    count = config.size() - 1; //["count"].as();

    tf_targetfront_to_target_.clear();
    for(int i=0; i<count; i++)
    {
        std::string name = "tf_targetfront_to_target_" + std::to_string(i);

        int id = config[i][name]["id"].as<int>();
        double qw = config[i][name]["qw"].as<double>();
        double qx = config[i][name]["qx"].as<double>();
        double qy = config[i][name]["qy"].as<double>();
        double qz = config[i][name]["qz"].as<double>();

        double x = config[i][name]["x"].as<double>();
        double y = config[i][name]["y"].as<double>();
        double z = config[i][name]["z"].as<double>();

        tf::Quaternion tf_q(qx, qy, qz, qw);
        tf::Vector3 tf_orig(x, y, z);

        tf::Transform tf_temp;
        tf_temp.setOrigin(tf_orig);
        tf_temp.setRotation(tf_q);

        tf_targetfront_to_target_.insert(std::pair<int, tf::Transform>(id,tf_temp));
    }