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)
{
cJSON_Delete(root);
returnARM_JONIT_MAX_SPEED;
}
else
{
cJSON_Delete(root);
return-2;}
}
}
实际代码如下:
还需要在rm_driver.h的宏定义中,添加获取关节最大速度的宏定义说明,代码如下。
#defineARM_JONIT_MAX_SPEED0x20
实际代码:
代码解析:
在该段代码中用到了cJSON对读取到的代码的处理,包括获取和值的比较,在这里我们也需
要用到《睿尔曼6自由度机械臂JSON通信协议v3.5.3》手册进行数据和通信协议的比对。
如上图所示,反馈关节最大速度的指令格式为{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文件中添加如下代码,处理反馈程序。
intParser_Joint_Max_Speed(char*msg)
{
cJSON*root=NULL,*result,*json_sub;
/*
解码函数,解析接收到的msg信息,解析为JSON格式,用以后面进行分析。
*/
root=cJSON_Parse(msg);
intdata[6];
inti=0;
root=cJSON_Parse(msg);
result=cJSON_GetObjectItem(root,"joint_speed");
if((result!=NULL)&&(result->type==cJSON_Array))
{
intsize=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);
return0;
}
else
{cJSON_Delete(root);
ROS_ERROR("Parser_Joint_Max_SpeedFailed!!!");
return-1;
}
}
实际代码如下:
代码解析:
在如上代码中我们对机械臂传输过来的数组进行了处理,将JSON数组中的数据传输到了
RM_Joint这个结构体对应的数据中,以便于我们在主函数中进行处理。
这里需要注意的是我们用来传递的数据与真实数据是否对应,如JSON传输的数据为int类型。
但是如图所示:
根据手册说明我们可以知道其传输时放大了1000倍,我们需要将其还原为真实值需要使用
float类型进行保存,在数值保存时需要注意,建议可在RM_Joint结构体中新建变量或数组保存。
完成以上操作后我们需要在主函数中完成while(1)大循环中的数据处理和发布。
7.在while(1)大循环中的添加数据处理和发布代码
在rm_driver.cpp中添加如下代码。
caseARM_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);
Info_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::PublisherJoint_Max_Speed;
实际代码如下:
在rm_driver.cpp中添加如下代码。
Joint_Max_Speed=nh_.advertise<rm_msgs::Arm_Joint_Speed_Max>("/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/Headerheader
string[]name
float64[]max_velocity
实际代码如下:
在CMakeLists.txt中添加如下代码:
Arm_Joint_Speed_Max.msg
实际代码如下 :
对rm_msgs功能包进行编译,我们就可以使用该消息类型了。
在rm_driver.h中添加如下代码添加头文件:
#include<rm_msgs/Arm_Joint_Speed_Max.h>
实际代码如下:
在rm_driver.cpp中添加如下代码:
rm_msgs::Arm_Joint_Speed_MaxJoint_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
catkinbuildrm_driver
在机器人端新打开一个终端,执行如下指令运行ROS_MASTER。
roscore
在机器人端新打开一个终端,启动robot_driver节点。
rosrunrm_driverrm_driver
在机器人端新打开一个终端,订阅最大关节速度的发布话题。
rostopicecho/rm_driver/joint_max_speed
在机器人端新打开一个终端,发布如下话题,使订阅器调用回调函数,获取机械臂关节最大速
度信息。
rostopicecho/rm_driver/joint_max_speed
如下图所示为不同终端的运行指令及提示信息。
到这里,查询流程基本完毕,这里对基本的查询和反馈流程做了比较详细的说明,供大家学习
了解,更加丰富的内容请参考实际代码。
3.2.ROS机械臂驱动包设置指令添加
本节我们主要了解一下设置指令是如何添加的,伴随设置指令我们也需要反馈指令的解析,所
以在这里我们将会为大家直接介绍系统性的设置流程,包括设置指令的发布以及查询后收到反馈指
令的解析,以手册中的第一个设置指令为例“设置关节最大速度”,下面为主要步骤和操作方式。
1.在rm_robot.h文件中声明订阅函数。
该订阅函数的作用是帮助我们时刻检测是否有设置关节最大速度的话题发出,如果收到该话题,
就会订阅其回调函数,向下发布设置指令。
在rm_robot.h中添加如下代码声明订阅方。
ros::SubscriberSub_set_Joint_Max_Speed_Cmd;
实际代码如下:
2.在rm_driver.cpp文件中配置订阅方的相关话题和回调函数
在rm_driver.cpp中添加如下代码确定订阅方话题和回调函数。
Sub_set_Joint_Max_Speed_Cmd=nh_.subscribe("/rm_driver/SetJointMaxSpeedCm
d",10,Set_Joint_Max_Speed_Callback);
实际代码如下:
代码中主要参数解析:
/rm_driver/SetJointMaxSpeedCmd为订阅方所接收的话题。
10为缓存队列
Set_Joint_Max_Speed_Callback为接受到话题后需要执行的回调函数。
3.在rm_driver.cpp文件中构建回调函数实体
在rm_driver.cpp中添加如下代码构建回调函数。
voidSet_Joint_Max_Speed_Callback(constrm_msgs::Joint_Max_Speedmsg)
{
intres=0;
uint8_tnum;
uint32_tspeed;
num=msg.joint_num;
speed=(uint32_t)(msg.joint_max_speed*1000);
res=Set_Joint_Max_Speed(num,speed);
if(res==0)
{
ROS_INFO("SetJointMaxSpeedsuccess!\n");}
else
{
ROS_ERROR("SetJointMaxSpeedfailed!\n");
}
}
实际代码如下:
代码解析:
该回调函数会导入两个参数,num代表我们要设置的机械臂的编号取值范围1-6,
joint_max_speed代表我们要设置的关节速度。
其中的核心代码为res=Set_Joint_Max_Speed(num,speed);
Set_Joint_Max_Speed(num,speed);函数主要包含了与机械臂通信时的通信函数。通信成功
返回0,失败返回其他错误码,并提示错误信息。
函数参数constrm_msgs::Joint_Max_Speedmsg为我们自定义的参数,这部分也需要在
rm_msgs功能包中进行构建。
4.在rm_msgs功能包中新建消息
在rm_msgs功能包的msg文件夹下新建Joint_Max_Speed.msg文件,在其中添加如下内容:
uint8joint_num
float32joint_max_speed
实际代码如下
之后我们还需要在CMakeLists.txt文件中添加如下内容:
需要对rm_msgs功能包进行编译。
cd~/catkin_ws
catkinbuildrm_msgs
编译完成后我们就可以使用该消息了,在rm_driver.h文件中添加如下头文件。
#include<rm_msgs/Joint_Max_Speed.h>
实际代码如下:
完成如上操作,我们的消息文件就配置完成了。
5.在rm_driver.h文件中添加通信函数
在rm_robot.h中添加如下代码。
intSet_Joint_Max_Speed(uint8_tnum,uint32_tv)
{
cJSON*root,*array;
char*data;
charbuffer[200];intres;
intr=0;
//创建根节点对象
root=cJSON_CreateObject();
array=cJSON_CreateArray();
//数组加入数据
cJSON_AddNumberToObject(array,"test",num);
cJSON_AddNumberToObject(array,"test",v);
//加入字符串对象
cJSON_AddStringToObject(root,"command","set_joint_max_speed");
cJSON_AddItemToObject(root,"joint_max_speed",array);
data=cJSON_Print(root);
sprintf(buffer,"%s\r\n",data);
res=package_send(Arm_Socket,buffer,strlen(buffer),0);
if(res<0)
{
return1;
}
return0;
}
实际代码如下:
在这里有两个参数,num代表我们要设置的机械臂的编号取值范围1-6,v代表我们要设置的关节速度,由Set_Joint_Max_Speed(uint8_t num, uint32_t v)函数传递过来。
之后我们将数据转为JSON形式然后package_send函数发送给机械臂端,该部分要参考我们的手册进行数据对应。
如{"command":"set_joint_max_speed","joint_max_speed":[2,30000]}
我们在实际填充时"command":"set_joint_max_speed","joint_max_speed"这些值都是固定的,后面数组中的数据是可以改变的代表我们要设置的关节1~6和要设置的速度,对应手册内容如下图。
以上完成了参数设置的设置过程,若发送正常,那机械臂当前的参数就会被改变了,但是我们
还是需要接受机械臂返回的信息,确保我们的修改是生效的。
同样我们需要在主函数中添加处理代码。
6.在rm_driver.cpp中添加设置最大速度反馈处理代码
在rm_driver.cpp中添加如下代码。
caseARM_SET_JONIT_MAX_SPEED:
state.data=RM_Joint.state;
Set_Joint_Max_Speed_Result.publish(state);
break;
实际代码如下:
该代码中我们使state.data=RM_Joint.state;需要在rm_driver.h中进行接受数据的处理,将
对应值传递到RM_Joint.state中。
7.在rm_driver.h中添加反馈数据处理代码
在rm_driver.h中的Parser_Msg函数中添加如下代码。
json_state=cJSON_GetObjectItem(root,"command");
if(json_state!=NULL)
{
if((json_state!=NULL)&&!strcmp("set_joint_max_speed",json_state->val
uestring))
{
res=Parser_Set_Joint_Max_Speed(msg);
if(res==0)
{
cJSON_Delete(root);
returnARM_SET_JONIT_MAX_SPEED;
}
else
{
cJSON_Delete(root);
return-2;
}
}}
实际代码如下:
在该部分代码中我们对接收到的反馈数据进行解析,如果与设置最大速度的反馈值的数据对应
的话,我们就执行与之对应的数据分析函数Parser_Set_Joint_Max_Speed(msg)。
有关于数据对应,我们需要参照《睿尔曼6自由度机械臂JSON通信协议v3.5.3》中的相关数
据介绍,如其中的set_joint_max_speed参数就是根据我们手册的返回值介绍得到的。
在rm_driver.h中添加Parser_Set_Joint_Max_Speed(msg)数据分析函数。
intParser_Set_Joint_Max_Speed(char*msg)
{
cJSON*root=NULL,*result,*json_sub;
/*解码函数,解析接收到的msg信息,解析为JSON格式,用以后面进行分析。
*/
root=cJSON_Parse(msg);
if(root==NULL)
{
cJSON_Delete(root);
return2;
}
result=cJSON_GetObjectItem(root,"joint_max_speed");
if((result->type==cJSON_True)||(result->type==cJSON_False))
{
RM_Joint.state=result->valueint;
ROS_INFO("RM_Joint.Write_Single_Register:%d",RM_Joint.state);
cJSON_Delete(root);
return0;
}
else
{
cJSON_Delete(root);
return1;
}
}
实际代码如下:
在该函数中我们对其实际数据进行来处理,在处理过程中我们将反馈的值给到了
RM_Joint.state这个变量,以便于主函数的调用,在实际进行返回值的处理时也是参照《睿尔曼6
自由度机械臂JSON通信协议v3.5.3》中的相关数据介绍。
8.Set_Joint_Max_Speed_Result发布器添加
在rm_driver.h中添加如下代码。
ros::PublisherSet_Joint_Max_Speed_Result;
实际代码如下:
在rm_driver.cpp中添加如下代码。
Set_Joint_Max_Speed_Result = nh_.advertise<std_msgs::Bool>("/rm_driver/SetJointMaxSpeed_Result", 10);
实际代码如下:
代码说明:
通过以上代码我们添加了关节最大速度反馈话题的发布方,发布方为
Set_Joint_Max_Speed_Result,发布话题为:/rm_driver/SetJointMaxSpeed_Result,我们在查
看最大速度的时需要订阅该话题。
其消息类型为std_msgs::Bool,此消息类型为ROS原本的消息类型。
到此我们需要添加的代码已基本添加完毕,可以进行编译测试。
9.实现效果
首先在机器人端打开一个终端运行如下指令进入工作空间进行编译。
cd~/catkin_ws
catkinbuildrm_driver
在机器人端新打开一个终端,执行如下指令运行ROS_MASTER。
roscore
在机器人端新打开一个终端,启动robot_driver节点。
rosrunrm_driverrm_driver
在机器人端新打开一个终端,订阅最大关节速度反馈的发布话题。
rostopicecho/rm_driver/SetJointMaxSpeed_Result
在机器人端新打开一个终端,发布如下话题,使订阅器调用回调函数,设置机械臂关节最大速
度。
rostopicpub/rm_driver/SetJointMaxSpeedCmdrm_msgs/Joint_Max_Speed
"joint_num:1
joint_max_speed:31.0"
如下图所示为不同终端的运行指令及提示信息。
此时我们的机械臂会处于鸣叫状态,这是由于我们修改最大速度之后需要进行使能,我们新打
开一个机械臂的终端,发布如下话题。
rostopicpub/rm_driver/Joint_Enablerm_msgs/Joint_Enable"joint_num:1
state:true"
之后机械臂就停止鸣叫了,代表使能成功。
可以看到左上角的终端返回True代表我们的配置生效,我们将关节1的最大速度设置为31,
我们需要查看一下是否正确,我们使用之前编写的查询话题。
如上图所示关节1的最大速度由30变为31,在实际使用时不建议大家修改机械臂的最大关节
速度,实在需要修改时应严格参照手册《睿尔曼6自由度机械臂JSON通信协议v3.5.3》进行修改,
如设置关节最大速度的参数说明如下。