通过ROS控制真实机械臂(12) --- joint_states 消息的发布，更新机械臂位姿

底层的电机控制已经基本完成，还需要解决的最后一个问题就是根据机械臂的运动，将机械臂的位姿状态信息发回到上位机的ROS，让RVIZ中的机械臂和现实中的机械臂保持一致。

目前没有反馈的信息发回到上位机，所以每当点击 Update ，然后Plan and Execute之后，机械臂都是从初始位置重新规划，而不是接着上一次运动到的位置作为起点规划。

在修改move_group配置文件的时候，做过如下修改，意思就是使用真实机械臂发布的关节信息 /joint_states 更新机械臂位姿，而不是使用虚拟的机械臂。

  
  
    
    
    [/joint_states]
  

运行程序之后 通过 rostopic echo 命令查看信息如下：

$ rostopic echo /joint_states
---
header:
  seq: 1333
  stamp:
    secs: 1573198571
    nsecs: 845865011
  frame_id: ''
name: ['Link1', 'Link2', 'Link3', 'Link4', 'Link5']
position: [0.0, 0.0, 0.0, 0.0, 0.0]
velocity: []
effort: []
---
header:
  seq: 1334
  stamp:
    secs: 1573198571
    nsecs: 945868015
  frame_id: ''
name: ['Link1', 'Link2', 'Link3', 'Link4', 'Link5']
position: [0.0, 0.0, 0.0, 0.0, 0.0]
velocity: []
effort: []
---

可以看到各个关节的数据流，很明显就能看出每个关节的position一直是0，这就解释了为什么每次RVIZ中的规划都是从0开始。所以只要将机械臂各个关节运动的position信息用 /joint_states 消息发布即可。

既然要发布各个关节的位姿信息，那么我们首先需要在机械臂上安装编码器，用编码器来检测机械臂位姿的变化。为了方便测试，先对一个关节安装编码器，然后将编码器的AB信号线连接至beaglebone,编码器的具体使用方法参考文章https://blog.csdn.net/qq_34935373/article/details/88735024

然后需要修改之前的客户端和服务器的代码，客户端代码需要增加一个编码器数据采集线程，并将数据通过套接字发送至客户端，而客户端需要增加一个数据接收线程，用来处理发送过来的编码器数据，并转换成关节的位姿信息，通过/joint_states这个话题在ROS上发布。

以下只呈现了主要修改部分，还有一些小细节没有列出，具体参考后面的完整详细代码！

对于服务器代码redwall_arm_client.cpp 增加一个设备树加载的函数：

/* 加载 PRU 设备树函数 */
int write_tree(void)
{
    int fd = open(pwm_path,O_WRONLY);
    if(fd < 0){
        printf("打开slots失败\n");
    }
    write(fd,"am33xx_pwm",10);
    write(fd,"EBB-PRU-Example",15);
    write(fd,"bone_eqep1",10);
    close(fd);

    return 0;
}

增加一个编码器处理线程：

/* 发送的结构体 p3*/
struct pos_data
{
    char lumbar_pos[10];
    int big_arm_pos;        // 因为现在就一个板子和编码器测试，
    int small_arm_pos;      // 所以只有第一个lumbar关节数是真宗的
    int wrist_pos;
    int hand_pos;
};

/* 编码器数据子线程 */
void *send_pos(void *socketCon)
{
    while(1)
    {
        /* 获取编码器数据 */
        int _socketCon = *((int *)socketCon);
        while(p_lumbar.size()!=0)
        {
            FILE *stream = fopen(speed_encoder"position","r+");
            fgets(p3.lumbar_pos, 10, stream);
            // 先用仅有的一个编码器数据模拟一下！！
            p3.big_arm_pos = atoi(p3.lumbar_pos)/2;
            p3.small_arm_pos = atoi(p3.lumbar_pos)/3;
            p3.wrist_pos = atoi(p3.lumbar_pos)/4;
            p3.hand_pos = atoi(p3.lumbar_pos)/5;
            fclose(stream);
            bzero(writebuf, sizeof(p3));           // 清空缓冲区
            memcpy(writebuf,&p3,sizeof(p3));       // 复制到缓冲区
            int sendMsg_len = write(_socketCon,writebuf, sizeof(p3));
            if (sendMsg_len > 0) {
                //ROS_INFO("Send Msg_len %d successful!",sendMsg_len);
            } else {
                printf("向%s:%d发送失败\n");
            }
        }
        usleep(1000);
    }
}

接下来修改redwall_arm_server.cpp代码：

/* 发布joint_states数据 */
#include 
#include 

/* 发送的结构体 p3*/
struct pos_data
{
    char lumbar_pos[10];
    int big_arm_pos;
    int small_arm_pos;
    int wrist_pos;
    int hand_pos;
};

/*SOCKET服务器端接受数据子线程*/
void *fun_thrReceiveHandler(void *socketInfo)
{
    int buffer_length;
    _MySocketInfo _socketInfo = *((_MySocketInfo *)socketInfo);
    // 定义一个joint_states消息发布节点
    ros::NodeHandle nj;
    ros::Publisher joint_pub = nj.advertise("/joint_states", 1);
    while(1)
    {
        bzero(recvbuf,sizeof(p4));   //对recvbuf清零
        buffer_length = read(_socketInfo.socketCon,recvbuf,sizeof(p4)); // 返回读取的数据长度
        if(buffer_length == 0){
            printf("%s:%d 客户端在创建孙子线程的过程中意外关闭了\n",_socketInfo.ipaddr,_socketInfo.port);
            conClientCount--;
            break;
        }else if(buffer_length < 0){
            printf("接受客户端数据失败\n");
            break;
        }
        /* 处理数据 */
        recvbuf[buffer_length] = '\0';
        memcpy(&p4,recvbuf,sizeof(recvbuf));
        int lumbar_encode = atoi(p4.lumbar_pos);
        
        // 同样，先用模拟数据测试一下
        int big_arm_encode = p4.big_arm_pos;
        int small_arm_encode = p4.small_arm_pos;
        int wrist_encode = p4.wrist_pos;
        int hand_encode = p4.hand_pos;

        /******************* 发布消息 ***************/
        sensor_msgs::JointState joint_state;
        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(5);
        joint_state.position.resize(5);
        joint_state.name[0] ="Link1";
        joint_state.position[0] = lumbar_encode*2*3.14/2500;      // 编码器使用的是2500线
        joint_state.name[1] ="Link2";
        joint_state.position[1] = big_arm_encode*2*3.14/2500;
        joint_state.name[2] ="Link3";
        joint_state.position[2] = small_arm_encode*2*3.14/2500;
        joint_state.name[3] ="Link4";
        joint_state.position[3] = wrist_encode*2*3.14/2500;
        joint_state.name[4] ="Link5";
        joint_state.position[4] = hand_encode*2*3.14/2500;
        joint_pub.publish(joint_state);
    }
    printf("接受数据线程结束了\n");
    return NULL;
}

程序修改完毕，运行之后再次通过 rostopic echo 命令查看信息如下：

$ rostopic echo /joint_states
---
header:
  seq: 25208
  stamp:
    secs: 1573298912
    nsecs: 974132974
  frame_id: ''
name: ['Link1', 'Link2', 'Link3', 'Link4', 'Link5']
position: [-0.3768, -0.1884, -0.1256, -0.092944, -0.07536]
velocity: []
effort: []
---
header:
  seq: 25224
  stamp:
    secs: 1573298912
    nsecs: 974790147
  frame_id: ''
name: ['Link1', 'Link2', 'Link3', 'Link4', 'Link5']
position: [-0.3768, -0.1884, -0.1256, -0.092944, -0.07536]
velocity: []
effort: []
---

可以看到现在的/joint_states是有数据的，和编码器的数据是对应的。观察RVIZ中的机械臂，现在每次规划机械臂的初始位置都是根据编码器反馈的数据得到的新位置。