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

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

开始教程。

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

rosrun turtlesim turtlesim_node
rosrun 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 z
geometry_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_id
float32[] axes
int32[] 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.launch
roslaunch turtlebot_teleop keyboard_teleop.launch
rosrun 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文件可以重映射哦

来源：CSDN

作者：小菜虎

链接：https://blog.csdn.net/u014587147/article/details/75673110