ROS2服务与机械臂仿真代码

dofbot_moveit功能包包含了ROS2正逆运动学解算服务器、服务器和仿真环境下可视化机械臂的启动文件（launch）、以及用于描述三维机械臂模型的URDF描述文件。

<robot name="dofbot">
    <!-- 实体描述 -->
    <link name="base_link">
        <!-- 视觉描述 -->
        <visual>
            <origin
                    xyz="0 0 0"
                    rpy="0 0 0"/>
            <geometry>
                <mesh
                        filename="package://dofbot_moveit/meshes/base_link.STL"/>
            </geometry>
            <material
                    name="">
                <color
                        rgba="0.7 0.7 0.7 1"/>
            </material>
        </visual>
        <!-- 碰撞描述 -->
        <collision>
            <origin
                    xyz="0 0 0"
                    rpy="0 0 0"/>
            <geometry>
                <mesh
                        filename="package://dofbot_moveit/meshes/base_link.STL"/>
            </geometry>
        </collision>
    </link>

robot标签定义了机器人的名称为dofbot，在此标签下有多个子标签，如link、joint等，每个子标签定义了当前机器人的组成部分。每个组成部分也拥有属于自己的子标签。这些子标签为该组成部分的属性，例如这个具体关节所在空间的位置和姿态，以及关节的颜色及材质等。通过urdf文件对机器人的描述，可以在仿真环境中可视化机械臂，如图1所示。

图1 机械臂可视化模型

“dofbot_moveit/src/dofbot_server.cpp”文件定义了正逆运动学解的服务器，其核心代码如下。

srvicecallback()函数为正逆运动学解服务器的回调函数，两个参数分别为客户端发送的请求（request）和服务端回复（response）的响应。二者皆由前文中提到的srv消息类型文件定义。当客户端发送的请求模式为求解正运动学（fk），则调用响应函数，进行响应运算，获得目标位置的坐标和姿态角，存入响应变量（response）中，反之同理。

bool srvicecallback(const std::shared_ptr<dofbot_info::srv::Kinemarics::Request> request,
                    std::shared_ptr<dofbot_info::srv::Kinemarics::Response> response) {

    if (request->kin_name == "fk") {
        double joints[]{request->cur_joint1, request->cur_joint2, request->cur_joint3, request->cur_joint4,
                        request->cur_joint5};
        // 定义目标关节角容器
        vector<double> initjoints;
        // 定义位姿容器
        vector<double> initpos;
        // 目标关节角度单位转换,由角度转换成弧度
        for (int i = 0; i < 5; ++i) {
            initjoints.push_back((joints[i] - 90) * DE2RA);
        }
        // 调取正解函数,获取当前位姿
        dofbot.dofbot_getFK(urdf_file, initjoints, initpos);
        cout << "--------- Fk ---------" << a << "--------- Fk ---------" << endl;
        cout << "XYZ坐标 ： " << initpos.at(0) << " ," << initpos.at(1) << " ," << initpos.at(2) << endl;
        cout << "Roll,Pitch,Yaw： " << initpos.at(3) << " ," << initpos.at(4) << " ," << initpos.at(5) << endl;
        response->x = initpos.at(0);
        response->y = initpos.at(1);
        response->z = initpos.at(2);
        response->roll = initpos.at(3);
        response->pitch = initpos.at(4);
        response->yaw = initpos.at(5);
    }