ROS实战|游戏手柄之自定义按钮控制海龟

该文为基础上https://blog.csdn.net/qq_34935373/article/details/87905784
想通过游戏手柄自定义话题,自定义按键实现对海龟的控制。于是有了本文
ROS实战|游戏手柄之自定义按钮控制海龟
文章图片

新建节点/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实战|游戏手柄之自定义按钮控制海龟】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 theall 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 = https://www.it610.com/article/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型号.测试结果如下:

ROS实战|游戏手柄之自定义按钮控制海龟
文章图片

说明轴0,1和轴4,5在jstest测试和本文测试中顺序相反.ROS实战|游戏手柄之自定义按钮控制海龟
文章图片

    推荐阅读