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 linear  float64 x  float64 y  float64 zgeometry_msgs/Vector3 angular  float64 x  float64 y  float64 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: 837  stamp:     secs: 1500642610    nsecs: 656011953  frame_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 header  uint32 seq  time stamp  string 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/s pub = 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文件可以重映射哦