睿尔曼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<size;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 这个发布器的定义。