睿尔曼6自由度机械臂ROS驱动包功能拓展之查询指令
1：主要环境预览
1：系统：Ubuntu 20.04
2：ROS：noetic
3：对于系统要求需根据相关手册完成机械臂相关依赖安装，能够运行机械臂本身基本功能， 包括 moveit。
4：准备资料，完成功能拓展我们需要依赖以下资料《睿尔曼 6 自由度机械臂 JSON 通信协议v3.5.3》。
2：添加ROS 机械臂驱动包功能说明
机械臂内部有许多的指令，可将其分为三类。
- 首先是设置，通俗的理解就是通过一些设置指令改变机械臂的内部参数。
- 第二是查询，通过一些查询的方式，使机械臂将本身的参数信息反馈出来。
- 第三是反馈，通过对机械臂反馈的指令的解析获取到机械臂的当前配置数据、当前状态或我们在配置后是否成功的反馈。
3：ROS 机械臂驱动包功能添加
3.1 ROS机械臂驱动包查询指令添加
本节我们主要了解一下查询指令是如何添加的，伴随查询指令不可或缺的就是反馈指令的解析， 所以在这里我们将会为大家直接介绍系统性的查询流程，包括查询指令的发布以及查询后收到反馈指令的解析，以手册中的第一个查询指令为例“查询关节最大速度”，下面为主要步骤和操作方式。
1：在 rm_robot.h 文件中声明订阅函数。
该订阅函数的作用是帮助我们时刻检测是否有查询关节最大速度的话题发出，如果收到该话题， 就会订阅其回调函数，向下发布查询指令。在 rm_robot.h 中添加如下代码声明订阅方。
ros::Subscriber Sub_get_Joint_Max_Speed_Cmd;
实际代码如下：
2：在 rm_driver.cpp 文件中配置订阅方的相关话题和回调函数。
Sub_get_Joint_Max_Speed_Cmd = nh_.subscribe("/rm_driver/Get_Joint_Max_Speed_ Cmd", 10, Get_Joint_Max_Speed_Callback);
代码中主要参数解析：/rm_driver/Get_Joint_Max_Speed_Cmd 为订阅方所接收的话题。10 为缓存队列Get_Joint_Max_Speed_Callback 为接受到话题后需要执行的回调函数。
3：在 rm_driver.cpp 文件中构建回调函数实体
void Get_Joint_Max_Speed_Callback(const std_msgs::Empty msg)
{
ROS_INFO("Get_Joint_Max_Speed_Callback!\n"); int res = 0;
res = Get_Joint_Max_Speed(); if(res != 0)
{
ROS_ERROR("Get_Joint_Max_Speed_Callback cmd failed!\n");
}
}
代码解析：其中的核心代码为 res = Get_Joint_Max_Speed();Get_Joint_Max_Speed()函数主要包含了与机械臂通信时的通信函数。通信成功返回 0，失败返回其他错误码，并提示错误信息。
4：在 rm_robot.h 文件中构建通信函数
在 rm_robot.h 中添加如下代码构建与机械臂的通信函数。
int Get_Joint_Max_Speed()
{
cJSON *root; char *data;
char buffer[200];
int res; int r = 0;
root = cJSON_CreateObject();
cJSON_AddStringToObject(root, "command", "get_joint_max_speed");
data = cJSON_Print(root); sprintf(buffer, "%s\r\n", data);
res = package_send(Arm_Socket, buffer, strlen(buffer), 0); cJSON_Delete(root);
free(data);
if (res < 0)
{
return 1;
}
return 0;
}
实际代码如下：
代码解析： 其中用到了 JSON 的数据格式进行数据处理，想要理解的话需要多查看一些 cJSON 的相关资料，这里主要做使用说明。 在这里就需要结合我们的手册进行通信指令的发布了。
| 功能描述 | 查询关节最大速度 |
| 参数说明 | get_joint_max_speed：查询关节最大速度 |
| 命令格式 | {s:s} |
| 示例 | {"command":"get_joint_max_speed"} |
| 说明：查询关节最大速度 | |
| 返回值 | 见表1.3-（1） |
我们在《睿尔曼 6 自由度机械臂 JSON 通信协议 v3.5.3》手册中的查询关节最大速度的相关章 节中可以看到查询关节最大速度的指令示例{"command":"get_joint_max_speed"}。 所以我们在查询时对外发布的指令为：
cJSON_AddStringToObject(root, "command", "get_joint_max_speed");
我们在进行其他数据或配置的查询时也需要参照此方法，更改"get_joint_max_speed"为对应 值即可。
最终通过 package_send(Arm_Socket, buffer, strlen(buffer), 0);函数将处理好的数据发送出 去，到此查询通信完成。
5. 反馈处理代码的添加
反馈处理实际上我们是在主函数中的 while(1)大循环中完成的。
在大循环函数中，我们循环检测接收到的数据，在接收到机械臂发来的数据后我们会调用 Parser_Msg(char *msg)函数进行处理，但是在处理接收到的每一个不同的数据时，其处理逻辑也 各有不同，所以我们需要在 Parser_Msg(char *msg)函数体中添加新的处理逻辑。
在 rm_driver.h 的 Parser_Msg(char *msg)函数中添加如下代码。
json_state = cJSON_GetObjectItem(root, "joint_speed");
if(json_state != NULL)
{
json_state = cJSON_GetObjectItem(root, "state");
if((json_state != NULL) && !strcmp("joint_max_speed",json_state->valuestr ing))
{
res = Parser_Joint_Max_Speed(msg);
if(res == 0)
{
cJSON_Delete(root);
return ARM_JONIT_MAX_SPEED;
}
else
{
cJSON_Delete(root);
return -2;
}
}
}
实际代码如下：
还需要在 rm_driver.h 的宏定义中，添加获取关节最大速度的宏定义说明，代码如下。
#define ARM_JONIT_MAX_SPEED 0x20
实际代码：
代码解析：
在该段代码中用到了 cJSON 对读取到的代码的处理，包括获取和值的比较，在这里我们也需 要用到《睿尔曼 6 自由度机械臂 JSON 通信协议 v3.5.3》手册进行数据和通信协议的比对。
反馈关节最大速度
| 功能描述 | 反馈所有关节最大速度 |
| 参数说明 | joint_max_speed:反馈关节最大速度 |
| 命令格式 | {s;s,s:[i,i,i,i,i]} |
| 示例 | {"state":"joint_max_speed","joint_speed":[30,30,30,30,30,30]} |
| 说明：依次反馈个关节最大转速均为0.03RPM，单位RPM，分辨率：0.001RPM |
如上图所示，反馈关节最大速度的指令格式为{s:s,s:[i,i,i,i,i,i]}，其含义为数据类型的简写 {string:string,string:[int,int,int,int,int,int]}，可以看到其是由 string 类型和 int 类型的数据组成的， 实际的数据构成为：{"state":"joint_max_speed","joint_speed":[30,30,30,30,30,30]}。
在实际使用时关节最大速度 string 类型的数据不会改变，主要是 int 类型的数据值会因为我们 的设置发生变化。
在上面的代码中我们通过对 string 的数据判断是否为对应的我们要处理的数据。
在确保数据没有问题后我们使用 Parser_Joint_Max_Speed(char *msg)函数进行反馈数据的 解析。
6. 添加反馈数据解析程序
在 rm_robot.h 文件中添加如下代码，处理反馈程序。
int Parser_Joint_Max_Speed(char *msg)
{
cJSON *root = NULL, *result, *json_sub;
/*解码函数，解析接收到的 msg 信息，解析为 JSON 格式，用以后面进行分析。 */
root = cJSON_Parse(msg);
int data[6];
int i = 0;
root = cJSON_Parse(msg);
result = cJSON_GetObjectItem(root, "joint_speed");
if((result != NULL) && (result->type == cJSON_Array))
{
int size = cJSON_GetArraySize(result);
for(i=0;i
{
json_sub=cJSON_GetArrayitem(result,i);
data[i]=json_sub->valueint;
RM_Joint.joint[i] = data[i];
RM_Joint.joint[i] = RM_Joint.joint[i] / 1000;
}
result = cJSON_GetObjectItem(root, "arm_err");
if (result != NULL && result->type == cJSON_Number)
{
arm_err = result->valueint;
}
cJSON_Delete(root);
return 0;
}
else
{
cJSON_Delete(root);
ROS_ERROR("Parser_Joint_Max_Speed Failed!!!");
return -1;
}
}
代码解析：
在如上代码中我们对机械臂传输过来的数组进行了处理，将 JSON 数组中的数据传输到了 RM_Joint 这个结构体对应的数据中，以便于我们在主函数中进行处理。
这里需要注意的是我们用来传递的数据与真实数据是否对应，如 JSON 传输的数据为 int 类型。
但是如图所示：
| 示例 | {"state":"joint_max_speed","joint_speed":[30,30,30,30,30,30]} |
| 说明：依次反馈个关节最大转速均为0.03RPM，单位RPM，分辨率：0.001RPM |
根据手册说明我们可以知道其传输时放大了 1000 倍，我们需要将其还原为真实值需要使用 float 类型进行保存，在数值保存时需要注意，建议可在 RM_Joint 结构体中新建变量或数组保存。 完成以上操作后我们需要在主函数中完成 while(1)大循环中的数据处理。
7. 在 while(1)大循环中的添加数据处理和发布代码
在 rm_driver.cpp 中添加如下代码。
case ARM_JONIT_MAX_SPEED:
Joint_Speed_Max.header.stamp = ros::Time::now();
for (i = 0; i < 6; i++)
{
Joint_Speed_Max.max_velocity[i] = RM_Joint.joint[i];
}
Joint_Max_Speed.publish(Joint_Speed_Max); I
nfo_Arm_Err();
break;
实际代码如下：
代码解析：
在如上代码中，我们将数据从 RM_Joint.joint 数组中传递到 Joint_Speed_Max.max_velocity 数组中之后调用 Joint_Max_Speed 发布器进行了发布。在这里面我们还需要进行 Joint_Speed_Max 这个数据的定义和 Joint_Max_Speed 这个发布器的定义。
8. Joint_Max_Speed 发布器添加
在 rm_driver.h 中添加如下代码。
ros::Publisher Joint_Max_Speed;
实际代码如下：
在 rm_driver.cpp 中添加如下代码。
Joint_Max_Speed = nh_.advertise("/rm_driver/jo int_max_speed",10);
实际代码如下：
代码说明：
通过以上代码我们添加了关节最大速度话题的发布方，发布方为 Joint_Max_Speed，发布话题 为：/rm_driver/joint_max_speed，我们在查看最大速度的时需要订阅该话题。
其消息类型为 rm_msgs::Arm_Joint_Speed_Max，此消息类型非 ROS 原本的消息类型，为新 建的消息类型。
9. Arm_Joint_Speed_Max 数据类型添加
在 rm_msgs 功能包的 msg 文件夹中新建文件 Arm_Joint_Speed_Max.msg，在文件中添加如 下内容。
std_msgs/Header header
string[] name
float64[] max_velocity
实际代码如下：
在 CMakeLists.txt 中添加如下代码:
Arm_Joint_Speed_Max.msg
实际代码如下：
对 rm_msgs 功能包进行编译，我们就可以使用该消息类型了。
在 rm_driver.h 中添加如下代码添加头文件：
#include
实际代码如下:
在 rm_driver.cpp 中添加如下代码:
rm_msgs::Arm_Joint_Speed_Max Joint_Speed_Max;
Joint_Speed_Max.name.resize(6);
Joint_Speed_Max.max_velocity.resize(6);
Joint_Speed_Max.name[0] = "joint1"; Joint_Speed_Max.name[1] = "joint2";
Joint_Speed_Max.name[2] = "joint3"; Joint_Speed_Max.name[3] = "joint4";
Joint_Speed_Max.name[4] = "joint5"; Joint_Speed_Max.name[5] = "joint6";
代码如下：
完成以上操作就可以对 rm_driver 功能包进行编译和运行了。
10. 实现效果
首先在机器人端打开一个终端运行如下指令进入工作空间进行编译。
cd ~/catkin_ws
catkin build rm_driver
在机器人端新打开一个终端，执行如下指令运行 ROS_MASTER。
roscore
在机器人端新打开一个终端，启动 robot_driver 节点。
rosrun rm_driver rm_driver
在机器人端新打开一个终端，订阅最大关节速度的发布话题。
rostopic echo /rm_driver/joint_max_speed
在机器人端新打开一个终端，发布如下话题，使订阅器调用回调函数，获取机械臂关节最大速 度信息。
rostopic echo /rm_driver/joint_max_speed
如下图所示为不同终端的运行指令及提示信息。
到这里，查询流程基本完毕，这里对基本的查询和反馈流程做了比较详细的说明，供大家学习 了解，更加丰富的内容请参考实际代码。