【睿尔曼-RealMan】睿尔曼超轻量仿人机械臂--ROS功能包添加写寄存器操作

一、 package介绍

本文为在rm_driver功能包中增加写单个寄存器功能流程。

二、 添加写单个寄存器功能

1.拼接JSON字符串发送指令

在rm_robot.h文件中创建Write_Single_Register_Cmd函数，拼接json字符串给机械臂发送指令。

//写单个寄存器
int Write_Single_Register_Cmd(int port, int address, int wdata, int device)
{
    cJSON *root;
    char *data;
    char buffer[200];
    int res;
    //创建根结点对象
    root = cJSON_CreateObject();
 
    //加入字符串对象
    cJSON_AddStringToObject(root, "command", "write_single_register");
    cJSON_AddNumberToObject(root, "port", port);
    cJSON_AddNumberToObject(root, "address", address);
    cJSON_AddNumberToObject(root, "data", wdata);
    cJSON_AddNumberToObject(root, "device", device);
 
    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;
}

 2. 自定义话题消息 write_single_register 

（1）在rm_msgs文件夹中创建write_single_register.msg 文件，文件中消息类型定义如下：

int8 port
int32 address
int16 data
int16 device

 （2）在CMakeList.txt中，设置需要编译的msg文件

 add_message_files(
    FILES
    write_registers.msg
 )

（3）保存后在终端运行以下命令，编译msg文件。

cd catkin_ws
catkin build rm_msgs

编译完成后可使用如下命令查看自定义的消息类型

rosmsg show write_single_register 

（4）在rm_robot.h文件中包含该类型的头文件

#include <rm_msgs/write_registers.h>

 3. 接收消息并处理

创建Subscriber订阅/rm_driver/Write_Single_Register_Cmd话题，通过回调函数接收处理返回的消息。

ros::Subscriber sub_WriteSingleRegister;
sub_WriteSingleRegister = nh_.subscribe("/rm_driver/Write_Single_Register_Cmd", 10, Write_Single_Register_Cmd_Callback);

// 回调函数 
void Write_Single_Register_Cmd_Callback(const rm_msgs::write_single_register msg)
{
    int res = 0;
    int port = msg.port;
    int address = msg.address; 
    int wdata = msg.data;
    int device = msg.device;
 
    res = Write_Single_Register_Cmd(port,address,wdata, device);
    if (res != 0)
    {
        ROS_ERROR("Write_Single_Register_Cmd failed!\n");
    }
    else
    {
        ROS_INFO("Write_Single_Register_Cmd send success!\n");
    }
}

4. 解析返回值

定义标识符 WRITE_SINGLE_REGISTER，进解析函数Parser_Msg()，解析返回值，解析成功返回WRITE_SINGLE_REGISTER。

该指令的返回值为{"command":"write_single_register","write_state":true}，则解析函数如下:

#define WRITE_SINGLE_REGISTER 0x22
 
int Parser_Write_Single_Register_Cmd(char *msg)
{
    cJSON *root = NULL, *result;
    root = cJSON_Parse(msg);
 
    if(root == NULL)
    {
        cJSON_Delete(root);
        return 2;
    }
 
    result = cJSON_GetObjectItem(root, "write_state");
    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);
        return 0;
    }
    else
    {
        cJSON_Delete(root);
        return 1;
    }
}
 
 
int Parser_Msg(char *msg)
{
    cJSON *root = NULL, *json_state;
    root = cJSON_Parse(msg);
    // ROS_INFO("all msg data is:%s", msg);
    int res = 0;
 
    if (root == NULL)
    {
        cJSON_Delete(root);
        return -1;
    }
    json_state = cJSON_GetObjectItem(root, "command");
    if (json_state != NULL)
    {
        if (!strcmp("write_single_register", json_state->valuestring))
            {
                json_state = cJSON_GetObjectItem(root, "write_state");
                if (json_state != NULL)
                {
                    res = Parser_Write_Single_Register_Cmd(msg);
                    if (res == 0)
                    {
                        cJSON_Delete(root);
                        return WRITE_SINGLE_REGISTER;
                    }
                    else
                    {
                        cJSON_Delete(root);
                        return -7;
                    }
                }
            }
    }
}

5. 创建publisher，发布topic

创建1个pbulisher，发布名为/rm_driver/WriteSingleRegister_result的topic， 消息类型为std_msgs::Bool

ros::Publisher pub_WriteSingleRegister_result；

pub_WriteSingleRegister_result = nh_.advertise("/rm_driver/WriteSingleRegister_result", 10);

6. 接收到解析函数的返回值后，发布消息state。

 7. 保存文件，终端执行以下命令编译rm_driver功能包

cd catkin_ws
catkin build rm_driver​

到这里，写寄存器功能已经添加完成。可发布订阅话题测试该功能。

三、 测试

首先打开终端，执行以下命令运行roscore：

roscore

然后打开一个新终端，执行以下命令运行机器人驱动节点rm_driver：

cd catkin_ws
source devel/setup.bash
rosrun rm_driver rm_driver

打开一个新终端执行以下命令，等待打印写寄存器返回值。

cd catkin_ws
source devel/setup.bash
rostopic echo /rm_driver/WriteSingleRegister_result

 打开一个新终端执行以下命令，发布写单个寄存器消息（可按tab键补全命令消息参数）

rostopic pub -1 /rm_driver/Write_Single_Register_Cmd rm_msgs/write_single_register "port: 0
address: 0
data: 0
device: 0"

此时查看打印的窗口，可以看到打印的返回值信息。