通過ROS控制真實機械臂(12) --- joint_states 消息的發佈，更新機械臂位姿

底層的電機控制已經基本完成，還需要解決的最後一個問題就是根據機械臂的運動，將機械臂的位姿狀態信息發回到上位機的ROS，讓RVIZ中的機械臂和現實中的機械臂保持一致。

目前沒有反饋的信息發回到上位機，所以每當點擊 Update ，然後Plan and Execute之後，機械臂都是從初始位置重新規劃，而不是接着上一次運動到的位置作爲起點規劃。

在修改move_group配置文件的時候，做過如下修改，意思就是使用真實機械臂發佈的關節信息 /joint_states 更新機械臂位姿，而不是使用虛擬的機械臂。

  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="$(arg use_gui)"/>
    <!--rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam -->
    <rosparam param="/source_list">[/joint_states]</rosparam >
  </node>

運行程序之後 通過 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 <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

/* 發送的結構體 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<sensor_msgs::JointState>("/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中的機械臂，現在每次規劃機械臂的初始位置都是根據編碼器反饋的數據得到的新位置。