如何实现rviz中的机械臂和实际的机械臂同步----ROS机械臂学习笔记(三)

利用网络和Joint States话题消息，实现仿真机械臂和实际的机械臂实现同步运动。
下位机：把和实际机械臂相连的电脑称为下位机。
上位机：装有ubuntu的电脑称为上位机。
在一篇的博客中利用joint_states发布消息，去控制机械臂， 我们利用joint states话题去发布关节状态的消息，然后rviz中的机械臂接受到相应的消息之后就会运动。现在我们在下位机中用Socket写一个通信程序，把读取到的实际机械臂的关节角度消息发送给上位机中，然后在上位机中写一个节点，用来接受消息，并且把消息填充到joint states中，最后发布出去。这样就可以实现rviz中的机械臂和实际的机械臂实现同步运动了。
上位机中的节点程序如下：

// receive the news of real joint angle form the TCP/IP,and use the topic JointState to publishe the news.

#include <unistd.h>
#include <stdio.h>
#include<iostream>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/types.h>
#include <stdlib.h>
#include <string.h>
#include <arpa/inet.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>

#define SERVER_PORT 17999
#define LENGTH_OF_LISTEN_QUEUE 20
#define BUFFER_SIZE 10

double b=1;

int main(int argc, char* argv[]) // (int argc, char* argv[])
{

    ros::init(argc, argv, "send_denso_date");
    ros::NodeHandle n;
    ros::Rate loop_rate(1000); //下位机中的发送频率是1000HZ ，不相同的话，不能实现同步。
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1000);    //设置一个发布者，将消息发布出去,发布到相应的节点中去
    sensor_msgs::JointState joint_state;


    //网络设置部分
    struct sockaddr_in server_addr;
    int server_socket;
    int opt = 1;
     double a=90;
    bzero(&server_addr, sizeof(server_addr)); // 置字节字符串前n个字节为0，包括'\0'
    server_addr.sin_family = AF_INET;
    server_addr.sin_addr.s_addr = htons(INADDR_ANY); // 转小端,INADDR_ANY就是指定地址为0.0.0.0的地址
    server_addr.sin_port = htons(SERVER_PORT);  

    // 创建一个Socket
    server_socket = socket(PF_INET, SOCK_STREAM, 0);

    if (server_socket < 0)
    {
        printf("Create Socket Failed!\n");
        exit(1);
    }


    // bind a socket
    setsockopt(server_socket, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
    if(bind(server_socket, (struct sockaddr*)&server_addr, sizeof(server_addr)))
    {
        printf("Server Bind Port: %d Failed!\n", SERVER_PORT);
        exit(1);
    }

    // 监听Socket
    if (listen(server_socket, LENGTH_OF_LISTEN_QUEUE))
    {
        printf("Server Listen Failed!\n");
        exit(1);
    }      

    while(1)
    {

        struct sockaddr_in client_addr;
        int client_socket;
        socklen_t length;
        double Buffer[6];

        // 连接客户端Socket
        length = sizeof(client_addr);
        client_socket = accept(server_socket, (struct sockaddr*)&client_addr, &length);
        if (client_socket < 0)
        {
            printf("Server Accept Failed!\n");
            break;
        }
         // 从客户端接收数据
        while(1)
        {
            bzero(Buffer, BUFFER_SIZE);
            length = recv(client_socket, (char*)&Buffer, sizeof(Buffer), 0);      //接受数据

            joint_state.header.stamp = ros::Time::now();                          //创建相应的消息
            joint_state.name.resize(6);
            joint_state.position.resize(6);
          //joint_state.name={"joint1","joint2""joint3""joint4""joint5""joint6"}; //不知道这个为什么不可以。
        joint_state.name[0]="joint1";
            joint_state.position[0] = Buffer[0];
        joint_state.name[1] ="joint2";
            joint_state.position[1] = Buffer[1];
        joint_state.name[2] ="joint3";
            joint_state.position[2] = Buffer[2];
            joint_state.name[3] ="joint4";
            joint_state.position[3] = Buffer[3];
        joint_state.name[4] ="joint5";
            joint_state.position[4] = Buffer[4];
        joint_state.name[5] ="joint6";
            joint_state.position[5] =Buffer[5];
            joint_pub.publish(joint_state);

            loop_rate.sleep();
            send(client_socket, (char*)&b, sizeof(b), 0); 


            if (length < 0)
            {
                printf("Server Recieve Data Failed!\n");
                break;
            }           

            if ('q' == Buffer[0])
            {
                printf("Quit!\n");
                break;
            }

            //printf("%s\n", Buffer);
            std::cout<<Buffer[0]<<std::endl;  //读取数据，只有一个数据
            break;

        }

        close(client_socket);
    }

    close(server_socket);
    return 0;
}

在这种方法中，所有的控制和路径点都是在下位机中实现的和给定的。有一些需要注意的地方就是两者的发送和接受的频率要一致，如果不一致的话，rviz中的模型就会有一种幻影，不能实现同步。

还有一种方法是把moveit中运动规划的轨迹点下发给下位机，下位机接受到相应的轨迹点后进行运动。剩下的依然是在下位机中用Socket写一个通信程序，把读取到的实际机械臂的关节角度消息发送给上位机中，然后在上位机中写一个节点，用来接受消息，并且把消息填充到joint states中，最后发布出去。这样就可以实现rviz中的机械臂和实际的机械臂实现同步运动了。**
在第二种方法中，控制是在下位机中实现的，但是路径点是在上位机中给定的。这种方法目前还在尝试中。