ROS之游戏手柄控制乌龟和机器人

导师买了个游戏手柄，就研究了一下怎么用游戏手柄控制机器人。想不到有点简单，编写一断代码即可，同时对我这两天学习的复习。

开始教程。

（1）游戏手柄控制乌龟。

rosrun turtlesim turtlesim_noderosrun turtlesim turtle_teleop_key

当我们运行上面的两句，发现键盘的方向键可以乌龟移动，它们俩是通过话题通信。

/turtle1/cmd_vel[geometry_msgs/Twist]

geometry_msgs/Twist是这个话题的类型，即消息的类型，我们看看内部结构

rosmsg show geometry_msgs/Twist

geometry_msgs/Vector3 linearfloat64 xfloat64 yfloat64 zgeometry_msgs/Vector3 angularfloat64 xfloat64 yfloat64 z

我们只用linear.x这个来控制速度，angular.z来控制方向。

思路：我们知道乌龟会订阅上面这个话题，因此我们要写这样一个代码，有订阅器，发布器。订阅游戏手柄的指令，然后发布速度到/turtle1/cmd_vel话题上，这样乌龟就可以订阅了。

现在我们要了解游戏手柄的发送的数据类型，然后经过订阅处理，发送自己想要的速度。

首先在终端运行：

ls /dev/input/

看看有哪些端口，其中会有js0这个端口，我们就用这个端口接收游戏手柄发来的数据。然后

sudo jstest /dev/input/js0

会发现

axes: 0: 0 1:0 2: 0...buttons: 0: 0 1:0 2:0 3:0 4:0...

接下来，我们运行接收游戏手柄数据的节点

rosrun joy joy_node

这个节点与手柄传过来的数据是通过/joy这个话题通信的。我们要知道哪些键控制着什么，于是我们移动手柄，看看这个话题接收的数据

rostopic echo /joy

会看到

---header: seq: 837stamp: secs: 1500642610nsecs: 656011953frame_id: ''axes: [-0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, 0.0]buttons: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]---

左右移动摇杆1，发现axes[0]变化-1到1，我们把它当成角度赋值给angualr.z，前后移动摇杆1，发现axes[1]变化-1到1，我们把它的当成速度赋值给linear.x。 也许你已经看到了/joy话题的类型

rostopic type /joy

显示

sensor_msgs/Joy

$rosmsg show sensor_msgs/Joy

std_msgs/Header headeruint32 seqtime stampstring frame_idfloat32[] axesint32[] buttons

好，现在我也知道了/joy话题和其类型。我们开始编写代码：

#include<ros/ros.h>#include<geometry_msgs/Twist.h>#include <sensor_msgs/Joy.h>#include<iostream>using namespace std;class Teleop{public:Teleop();private:/* data */void callback(const sensor_msgs::Joy::ConstPtr& Joy);ros::NodeHandle n; //实例化节点ros::Subscriber sub ;ros::Publisher pub ;double vlinear,vangular;//我们控制乌龟的速度，是通过这两个变量调整int axis_ang,axis_lin;//axes[]的键};Teleop::Teleop(){ //我们将这几个变量加上参数，可以在参数服务器方便修改n.param<int>("axis_linear",axis_lin,1); //默认axes[1]接收速度 n.param<int>("axis_angular",axis_ang,0);//默认axes[0]接收角度 n.param<double>("vel_linear",vlinear,1);//默认线速度1 m/s n.param<double>("vel_angular",vangular,1);//默认角速度1 单位rad/spub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1);//将速度发给乌龟 sub = n.subscribe<sensor_msgs::Joy>("joy",10,&Teleop::callback,this); //订阅游戏手柄发来的数据}void Teleop::callback(const sensor_msgs::Joy::ConstPtr& Joy){ geometry_msgs::Twist v; v.linear.x =Joy->axes[axis_lin]*vlinear; //将游戏手柄的数据乘以你想要的速度，然后发给乌龟v.angular.z =Joy->axes[axis_ang]*vangular;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, "logteleop");Teleop telelog; ros::spin();return 0;}

之后，运行此节点，乌龟节点，手柄节点，即可控制乌龟了。现在我们将这三个节点编程launch文件，并设置控制按钮和速度：

<launch><node pkg="turtlesim" type="turtlesim_node" name="sim"></node><node pkg="action_tutorials" type="logteleop" name="logteleop" output="screen"> </node><!--input axis --><param name="axis_linear" value="4" type="int"/><param name="axis_angular" value="3" type="int"/><!--input vel --><param name="vel_linear" value="2" type="double"/><param name="vel_angular" value="1.5" type="double"/><node respawn="true" pkg="joy" type="joy_node" name="joystick" ></node></launch>

在启动这三个节点的同时，可以设置速度和游戏手柄的摇杆。我用的是罗技游戏摇杆

摇杆1控制着axes[0]和axes[1],摇杆2控制着axes[3]axes[4]。所以在启动文件时候我可以随意设置是哪个摇杆，如果不设置就采取默认。这就是参数的好处。

（2）接下来，用游戏手柄控制机器人，我们先启动机和

roslaunch turtlebot_bringup minimal.launchroslaunch turtlebot_teleop keyboard_teleop.launchrosrun rqt_graph rqt_graph

其中第一个是启动总机器人的节点，第二个是启动键盘控制机器人走的节点，第三个是查看他们的通信，如下图：

键盘发布的话题是

/cmd_vel_mux/input/teleop

接下来，我们把上面的代码稍微修改，将发布的话题改为这个话题即可。

上代码：

#include<ros/ros.h>#include<geometry_msgs/Twist.h>#include <sensor_msgs/Joy.h>#include<iostream>using namespace std;class Teleop{public:Teleop();private:/* data */void callback(const sensor_msgs::Joy::ConstPtr& Joy);ros::NodeHandle n;ros::Subscriber sub ;ros::Publisher pub ;double vlinear,vangular;int axis_ang,axis_lin,ton;};Teleop::Teleop(){n.param<int>("axis_linear",axis_lin,1);n.param<int>("axis_angular",axis_ang,0);n.param<double>("vel_linear",vlinear,1);n.param<double>("vel_angular",vangular,1);n.param<int>("button",ton,5);pub = n.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/teleop",1);sub = n.subscribe<sensor_msgs::Joy>("joy",10,&Teleop::callback,this);}void Teleop::callback(const sensor_msgs::Joy::ConstPtr& Joy){geometry_msgs::Twist v;//v.linear.x =Joy->axes[axis_lin]*vlinear;// v.angular.z =Joy->axes[axis_ang]*vangular;if(Joy->buttons[ton]){v.linear.x =(Joy->axes[axis_lin]+Joy->axes[1])*vlinear;v.angular.z =(Joy->axes[axis_ang]+Joy->axes[0])*vangular;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, "log");Teleop telelog;ros::spin();return 0;}

执行的时候，我发现手误轻轻碰到摇杆机器人就会走，我不想这样，于是我又多加了键，buttons[5]按键，它会传过来0或1，当我按着的时候会是1。这样，当且仅当我按着这个按钮的时候，并且移动摇杆，机器人才会走。至于按哪个键，我可以设置参数，默认是buttons[5]。

v.linear.x =(Joy->axes[axis_lin]+Joy->axes[1])*vlinear;v.angular.z =(Joy->axes[axis_ang]+Joy->axes[0])*vangular;

这句我可以通过参数将摇杆2的axes[3]和axes[4]也能控制机器人，让他们与摇杆1一起控制。控制过程随意，可以让摇杆1前后移动控制速度，摇杆2左右移动控制速度，也可以同时前后，速度翻倍哦，也可以不移动摇杆2，只移动摇杆1控制，这样摇杆2控制的速度数据为0哦，等于是只有摇杆1在控制。双手控制摇杆更刺激哦

突然发现，我也挺聪明的。

附上launch文件

<launch><include file="$(find turtlebot_bringup)/launch/minimal.launch"/><node pkg="action_tutorials" type="logrobt" name="loghandle" output="screen"></node><!--input axis --><param name="axis_linear" value="4" type="int"/><param name="axis_angular" value="3" type="int"/><!--input vel --><param name="vel_linear" value="0.5" type="double"/><param name="vel_angular" value="1.5" type="double"/><param name="button" value="5" type="int"/><node respawn="true" pkg="joy" type="joy_node" name="joystick" output="screen"><param name="dev" value="/dev/input/js0" type="string"/><param name="deadzone" value="0.12" /></node></launch>

最后通过图，看下订阅关系

有没有发现，节点名都变了？哈哈，通过launch文件可以重映射哦

如果觉得《ROS之游戏手柄控制乌龟和机器人》对你有帮助，请点赞、收藏，并留下你的观点哦！