通过ROS控制真实机械臂(2)----单轴运动，手柄控制

创建ROS包，包名redwall_arm ，通过自定义的消息，将手柄的数据发布 msg/ joycontrol.msg，内容如下，分别对应罗技手柄的按钮和遥杆轴。

int32 button1
int32 button2
int32 button3
int32 button4
int32 button5
int32 button6
int32 button7
int32 button8
float32 axis0
float32 axis1
float32 axis2
float32 axis3
float32 axis4
float32 axis5

有了消息类型，接下来就是手柄数据处理和发送ros节点，命名为logitech_pub.cpp，导师要求主从模式，即两个手柄，当主手柄控制的时候，从手柄无法操作，当只插入了一个手柄的usb时，默认为主手柄。文件开头的宏定义为手柄按钮和摇杆轴的地址，可以通过jstest-gtk查看。

#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include "redwall_arm/joycontrol.h"
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <fcntl.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <linux/input.h>
#include <linux/joystick.h>
#include <errno.h>
#include <pthread.h>
#include <math.h>

#define AXES0          0x00
#define AXES1          0x01
#define AXES2          0x02
#define AXES3          0x03
#define AXES4          0x04
#define AXES5          0x05
#define BUTTON1        0x01
#define BUTTON2        0x02
#define BUTTON3        0x03
#define BUTTON4        0x04
#define BUTTON5        0x05
#define BUTTON6        0x06
#define BUTTON7        0x07
#define BUTTON8        0x08

/*手柄模式，单主手柄mode=1，双手柄mode=2*/
int mode;

/* 主手柄 */
int axes0, axes1, axes2, axes3, axes4, axes5;
int button1, button2, button3, button4, button5, button6, button7, button8;
struct js_event js_master;
int js_fd_master;

/* 从手柄 */
int axes0_, axes1_ , axes2_, axes3_, axes4_, axes5_;
int button1_, button2_, button3_, button4_, button5_, button6_, button7_, button8_;
struct js_event js_support;
int js_fd_support;

void * read_js_data_master(void *);
void * read_js_data_support(void *);

using namespace std;

int main(int argc, char **argv)
{
	ros::init(argc, argv,"joymsg_pub");
	ros::NodeHandle n1;
	ros::Publisher number_publisher = n1.advertise<redwall_arm::joycontrol>("/joycontrol_topic",100);
	ros::Rate loop_rate(100);

	js_fd_master = open("/dev/input/js0", O_RDONLY);
    if (js_fd_master < 0)
    {
        perror("打开js0失败,主手柄不可用");
        return -1;
    }
    else
    {
        /* 创建线程并使主线程与子线程分离，子线程结束后，资源自动回收 */
        pthread_t id0;
        pthread_create(&id0,NULL,read_js_data_master,NULL);
        pthread_detach(id0);
    }

    js_fd_support = open("/dev/input/js1", O_RDONLY);
    if (js_fd_support < 0)
    {
        perror("打开js1失败，从手柄不可用");
        mode = 1;
    }
    else
    {
        mode = 2;
        pthread_t id1;
        pthread_create(&id1,NULL,read_js_data_support,NULL);
        pthread_detach(id1);
    }

	while (ros::ok())
	{
	    redwall_arm::joycontrol msg;
	    /* 主从手柄控制 */
	    if(mode==2)
        {
            if ((button1==0) && (button2==0) && (button3==0) && (button4==0) && (button5==0) && (button6==0) && (button7==0) && (button8==0) && (axes0==0) && (axes1==0))
            {
                msg.button1 =  button1_;
                msg.button2 =  button2_;
                msg.button3 =  button3_;
                msg.button4 =  button4_;
                msg.button5 =  button5_;
                msg.button6 =  button6_;
                msg.button7 =  button7_;
                msg.button8 =  button8_;
                msg.axis0 = axes0_;
                msg.axis1 = axes1_;
                msg.axis2 = axes2_;
                msg.axis3 = axes3_;
                msg.axis4 = axes4_;
                msg.axis5 = axes5_;


            }
            else
            {
                msg.button1 =  button1;
                msg.button2 =  button2;
                msg.button3 =  button3;
                msg.button4 =  button4;
                msg.button5 =  button5;
                msg.button6 =  button6;
                msg.button7 =  button7;
                msg.button8 =  button8;
                msg.axis0 = axes0;
                msg.axis1 = axes1;
                msg.axis2 = axes2;
                msg.axis3 = axes3;
                msg.axis4 = axes4;
                msg.axis5 = axes5;

            }
        }

	    /* 单主手柄 */
	    else if(mode==1)
        {
            msg.button1 =  button1;
            msg.button2 =  button2;
            msg.button3 =  button3;
            msg.button4 =  button4;
            msg.button5 =  button5;
            msg.button6 =  button6;
            msg.button7 =  button7;
            msg.button8 =  button8;
            msg.axis0 = axes0;
            msg.axis1 = axes1;
            msg.axis2 = axes2;
            msg.axis3 = axes3;
            msg.axis4 = axes4;
            msg.axis5 = axes5;
        }
        ROS_INFO("%f %f %f %f %f %f",msg.axis0,msg.axis1,msg.axis2,msg.axis3,msg.axis4,msg.axis5);
        /* 发布消息 */
        number_publisher.publish(msg);
        /* 循环所有的操作 */
        ros::spinOnce();
        loop_rate.sleep();
	}
	return 0;
}

void *read_js_data_master(void *)
{
    while(1)
    {
        int len = read(js_fd_master, &js_master, sizeof(struct js_event));
        if (len < 0)
        {
            perror("读取js失败");
            return 0 ;
        }

        int value = js_master.value;
        int type = js_master.type;
        int number = js_master.number;

        if (type == JS_EVENT_BUTTON)
        {
            if( number == BUTTON1  )
            {
                button1=value;
            }
            if( number == BUTTON2  )
            {
                button2=value;
            }

            if( number == BUTTON3  )
            {
                button3=value;
            }

            if( number == BUTTON4  )
            {
                button4=value;
            }

            if( number == BUTTON5  )
            {
                button5=value;
            }

            if( number == BUTTON6  )
            {
                button6=value;
            }

            if( number == BUTTON7  )
            {
                button7=value;

            }

            if( number == BUTTON8  )
            {
                button8=value;

            }
        }

        else if (type == JS_EVENT_AXIS)
        {
            if( number == AXES1 )
            {
                axes1 = value;
            }
            if( number == AXES0 )
            {
                axes0 = value ; // 手柄有误差，可用jstest查看
            }
            if( number == AXES2 )
            {
                axes2 = value;
            }
            if( number == AXES3 )
            {
                axes3 = value ; // 手柄有误差，可用jstest查看
            }
            if( number == AXES4 )
            {
                axes4 = value;
            }
            if( number == AXES5 )
            {
                axes5 = value ; // 手柄有误差，可用jstest查看
            }
        }
    }
}

void *read_js_data_support(void *)
{
    while(1)
    {
        int len = read(js_fd_support, &js_support, sizeof(struct js_event));
        if (len < 0)
        {
            perror("读取js失败");
            return 0 ;
        }

        int value = js_support.value;
        int type = js_support.type;
        int number = js_support.number;

        if (type == JS_EVENT_BUTTON)
        {
            if( number == BUTTON1  )
            {
                button1_=value;
            }
            if( number