睿尔曼6自由度机械臂ROS驱动包功能拓展说明

---

1.主要环境预览

系统：Ubuntu20.04
ROS：noetic
对于系统要求需根据相关手册完成机械臂相关依赖安装，能够运行机械臂本身基本功能，
包括moveit。
准备资料，完成功能拓展我们需要依赖以下资料《睿尔曼6自由度机械臂JSON通信协议
v3.5.3》。

2.添加ROS机械臂驱动包功能说明

机械臂内部有许多的指令，可将其分为三类。
1.首先是设置，通俗的理解就是通过一些设置指令改变机械臂的内部参数。
2.第二是查询，通过一些查询的方式，使机械臂将本身的参数信息反馈出来。
3.第三是反馈，通过对机械臂反馈的指令的解析获取到机械臂的当前配置数据、当前状态或
者我们在配置后是否成功的反馈。

3.ROS机械臂驱动包功能添加

3.1.ROS机械臂驱动包查询指令添加

本节我们主要了解一下查询指令是如何添加的，伴随查询指令不可或缺的就是反馈指令的解析，
所以在这里我们将会为大家直接介绍系统性的查询流程，包括查询指令的发布以及查询后收到反馈
指令的解析，以手册中的第一个查询指令为例“查询关节最大速度”，下面为主要步骤和操作方式。

1.在rm_robot.h文件中声明订阅函数。

该订阅函数的作用是帮助我们时刻检测是否有查询关节最大速度的话题发出，如果收到该话题，
就会订阅其回调函数，向下发布查询指令。

在rm_robot.h中添加如下代码声明订阅方。

ros::Subscriber Sub_get_Joint_Max_Speed_Cmd;

实际代码如下：

 2.在rm_driver.cpp文件中配置订阅方的相关话题和回调函数。

 在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文件中构建回调函数实体

在rm_driver.cpp中添加如下代码构建回调函数。

voidGet_Joint_Max_Speed_Callback(conststd_msgs::Emptymsg)
{ 

ROS_INFO("Get_Joint_Max_Speed_Callback!\n");
intres=0;
res=Get_Joint_Max_Speed();
if(res!=0)
{
ROS_ERROR("Get_Joint_Max_Speed_Callbackcmdfailed!\n");
}
}

实际添加如下：

代码解析：
其中的核心代码为res=Get_Joint_Max_Speed();
Get_Joint_Max_Speed()函数主要包含了与机械臂通信时的通信函数。通信成功返回0，失败
返回其他错误码，并提示错误信息。

4.在rm_robot.h文件中构建通信函数

在rm_robot.h中添加如下代码构建与机械臂的通信函数。 

intGet_Joint_Max_Speed()
{
cJSON*root;
char*data;
charbuffer[200];

intres;
intr=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)
{
return1;
}
return0;
}

实际代码如下：

代码解析：
其中用到了JSON的数据格式进行数据处理，想要理解的话需要多查看一些cJSON的相关资
料，这里主要做使用说明。
在这里就需要结合我们的手册进行通信指令的发布了。 

我们在《睿尔曼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)
{