游戏手柄之自定义按钮控制海龟

该文为基础上https://blog.csdn.net/qq_34935373/article/details/87905784

想通过游戏手柄自定义话题，自定义按键实现对海龟的控制。于是有了本文

新建节点/joymsg_pub_node和话题/joycontrol_topic.

在src/目录下新建功能包joy_msg，在joy_msg/src的新建源文件joymsg_pub.cpp。代码如下：：

#include "ros/ros.h"

#include

#include

#include

#include              //perror()

#include            //fork()

#include

#include            //errno, EINTR

#include           //strlen()

#include

#include

#include  

#include  

#include  

#include

#include

#include

 

 

 

#define AXES0 0x00 //Left_Axis from right to left        

#define AXES1 0x01 //Left_Axis from up to down             

#define AXES2 0x02 //Right_Axis from right to left

#define AXES3 0x03 //Right_Axis from down to up

#define AXES4 0x04 //R&L_Key from Right_Key to Left_Key  

#define AXES5 0x05 //D&U_Key from Down_key to Up_key

 

#define BUTTON0 0x00         //X_Botton           

#define BUTTON1 0x01         //A_Botton                   

#define BUTTON2 0x02            //B_Botton       

#define BUTTON3 0x03            //Y_Botton       

#define BUTTON4 0x04            //LB_Botton

#define BUTTON5 0x05            //RB_Botton  

#define BUTTON6 0x06 //LT_Botton

#define BUTTON7 0x07 //RT_Botton

#define BUTTON8 0x08 //BACK_Botton

#define BUTTON9 0x09 //START_Botton

#define BUTTON10 0x10     //Left_Axis__Botton  

#define BUTTON11 0x11    //Right_Axis_Botton

 

float linear_x,linear_y,linear_z ;

float angular_x,angular_y,angular_z, value ;

 

int len, type, number;

int ret0;

pthread_t id0;  

struct js_event js;  

int js_fd;

void * read_js_data(void *);

using namespace std;

 

int main(int argc, char **argv)

{

//Initializing ROS node with a name of joymsg_pub

ros::init(argc, argv,"joymsg_pub");

//Created a nodehandle object

ros::NodeHandle n1;

//Create a publisher object

ros::Publisher number_publisher = n1.advertise("/joycontrol_topic",100);

//Create a rate object

ros::Rate loop_rate(100);

//Variable of the number initializing as zero

 

js_fd = open("/dev/input/js0", O_RDONLY);  

    if (js_fd < 0)  

    {  

        perror("open");  

        return -1;  

    }  

 

    pthread_create(&id0,NULL,read_js_data,NULL);  // 成功返回0，错误返回错误编号

 

    if(ret0!=0)

        {

            printf ("Create pthread1 error!\n");

            exit(1);

        }

    ret0=pthread_detach(id0);

 

while (ros::ok())

{

 

//Created geometry_msgs/Twist 这里可以加入你自定义文件，本文控制的是海龟，所以是这个消息格式，自定义文件，头文件记得添加奥

        geometry_msgs::Twist msg;

 

        //Inserted data to message header

msg.linear.x = linear_x;

msg.linear.y = linear_y;

msg.linear.z = linear_z;

 

        msg.angular.x = angular_x ;

        msg.angular.y = angular_y;

        msg.angular.z = angular_z;

        

 

//Publishing the message

number_publisher.publish(msg);

//Spining once for doing the  all operation once

ros::spinOnce();

//Setting the loop rate

loop_rate.sleep();

}

return 0;

}

 

void * read_js_data(void *)

{

while(1)

{

    len = read(js_fd, &js, sizeof(struct js_event));  

    if (len < 0)  

    {  

        perror("read");  

        return 0 ;  

    }  

    value = js.value;

    type = js.type;

    number = js.number;

 

    if (type == JS_EVENT_BUTTON)  

    {  

        /* if( number == BUTTON0  )

            {

               linear_x=value;  

            }

            if( number == BUTTON5  )

            {

                angular_z=value;  

            }

这里是设置按钮为操作单位，本文采用axis轴作为输入*/

     }

 

    else if (type == JS_EVENT_AXIS)  

    {

        if( number == AXES5 )

        {

            linear_x = value;

        }  

 if( number == AXES3 )

       {

            angular_z= value;

        }

//本文中控制海龟只用到了这两个

    }

} }

 

修改package.xml文件，添加依赖

  catkin

  roscpp

  rospy

  std_msgs

  roscpp

  rospy

  std_msgs

  roscpp

  rospy

  std_msgs

  message_runtime

 

修改CMakeLists.txt

add_executable(joymsg_pub

  src/joymsg_pub.cpp

)

add_dependencies(joymsg_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(joymsg_pub

  ${catkin_LIBRARIES}

)

 

 

本文是基于博客https://blog.csdn.net/qq_34935373/article/details/87905784基础上做的，如果又不懂得，建议看下上文。

进入input_js_control的功能包修改源代码logitech.cpp，如下：

#include

#include

//#include

#include

 

using namespace std;

 

class Teleop

{

public:

    Teleop();

 

private:

    /* data */

    void callback(const geometry_msgs::Twist::ConstPtr& Joy);

    ros::NodeHandle n; //实例化节点

    ros::Subscriber sub ;

    ros::Publisher pub ;

    double vlinear,vangular;//我们控制乌龟的速度，是通过这两个变量调整

  //  int axis_ang,axis_lin; //axes[]的键

};

 

Teleop::Teleop()

{

//我们将这几个变量加上参数，可以在参数服务器方便修改

  // n.param("axis_linear",axis_lin,1); //默认axes[1]接收速度

  // n.param("axis_angular",axis_ang,0);//默认axes[0]接收角度

   n.param("vel_linear",vlinear,1);//默认线速度1 m/s

   n.param("vel_angular",vangular,1);//默认角速度1 单位rad/s

   pub = n.advertise("/turtle1/cmd_vel",1);//将速度发给乌龟

   sub = n.subscribe("joycontrol_topic",10,&Teleop::callback,this);

} //订阅游戏手柄发来的数据}

 

void Teleop::callback(const geometry_msgs::Twist::ConstPtr& Joy)

 {

   geometry_msgs::Twist v;

   v.linear.x =Joy->linear.x*vlinear*0.001; //之后的参数设置在launch文件中输入你的期望值，将游戏手柄的数据乘以你想要的速度，然后发给乌龟

   v.angular.z =Joy->angular.z*vangular*0.001;

   ROS_INFO("linear:%.3lf angular:%.3lf",v.linear.x,v.angular.z);

   pub.publish(v);

}

 

int main(int argc,char** argv)

{

  ros::init(argc, argv, "joy_to_turtle");

  Teleop teleop_turtle;

  ros::spin();

  return 0;

}

 

修改input_js_control.launch文件如下

    

    

    

    

    

       

       

 

    

    

 

    

    

 

 

回到工作目录执行catkin_make 编译节点

然后执行launch文件，就可以了。

最后自己修改按键和axis的轴，确定对应的数字号。我用的是logitech wireless gamepad F710型号.测试结果如下:

 

说明轴0,1和轴4,5在jstest测试和本文测试中顺序相反.