ROS之手柄控制

手柄连接测试

首先，测试下手柄是否能正常连接电脑。手柄主要有有线手柄和无线手柄（蓝牙、2.4G）这几类，与ROS交互时没有什么区别，这里分别测试了手头上的微软xbox手柄、北通阿修罗手柄、小米手柄，使用时都一样，但这几个手柄都有一些按键的映射不一致。实际项目中需要留意这一点，如果更换了手柄，注意修改按键映射的配置文件，否则控制机器人时可能会出错

$ ls /dev/input/

手柄正常连接的话，会显示 js0

使用jstest命令检查下手柄是否能正常工作，拨动摇杆或按下按钮都会有数据刷新

$ sudo jstest /dev/input/js0

从打印的信息可以看出当前的手柄有8个轴向输入（axes）和15个按钮（buttons）

手柄消息通信过程

运行手柄节点，在此之前先执行下roscore

$ roscore
$ rosrun joy joy_node

再来打印下/joy话题上的消息，此时，拨动手柄上的摇杆或者按下按钮时可以看到有数据刷新，其中axes对应手柄的操作轴，buttons对应按下的按钮

$ rostopic echo /joy

使用rqt_graph来看下计算图，可以看到，手柄操作时的数据由/joy_node节点发布到了/joy话题上

使用命令来看一下话题的消息类型

$ rostopic type /joy

输出结果为：sensor_msgs/Joy，这是一个标准的ROS消息，参考：http://wiki.ros.org/sensor_msgs?distro=electric

使用手柄控制小乌龟运动

使用键盘控制小乌龟运动

刚开始学习话题通信的时候基本都会用这个例子，参考http://wiki.ros.org/cn/ROS/Tutorials/UnderstandingTopics

打开一个终端，启动主节点

$ roscore

打开一个终端，运行小乌龟节点

$ rosrun turtlesim turtlesim_node

打开一个终端，运行键盘控制节点

$ rosrun turtlesim turtle_teleop_key

选中键盘控制的终端窗口，就可以使用键盘来控制小乌龟运动了

小乌龟示例通信过程分析

先使用rqt_graph来查看下计算图

$ rqt_graph

可以看出，一个名为/teleop_turtle的节点在名为/turtle1/cmd_vel的话题上发布消息，一个名为/turtlesim的节点从/turtle1/cmd_vel订阅消息

使用命令来看一下话题的消息类型

$ rostopic type /turtle1/cmd_vel

输出结果为：geometry_msgs/Twist，这是一个ROS提供的标准消息类型，参考：http://wiki.ros.org/geometry_msgs?distro=electric


使用命令来看一下消息的详细输出

$ rosmsg show geometry_msgs/Twist

使用命令来打印一下节点通信的内容

$ rostopic echo /turtle1/cmd_vel

下面的输出分别对应上、下、左、右四个方向按键，可以看到只用到了liner中的x值和angular中的z值。

linear(线速度) 下的xyz分别对应在x、y和z方向上的速度(单位是 m/s)；
angular(角速度)下的xyz分别对应x轴上的翻滚、y轴上俯仰和z轴上偏航的速度(单位是rad/s)。

PS：ROS使用右手坐标系，x为朝前方向，y为朝左方向，z为朝上方向

旋转的方向使用右手法则，绕Z轴旋转，称之为航向角，使用yaw表示；绕X轴旋转，称之为横滚角，使用roll表示；绕Y轴旋转，称之为俯仰角，使用pitch表示

编写代码实现手柄控制小乌龟运动

通过上面的分析，我们编写节点来实现手柄控制小乌龟运动主要分为两步：①订阅/joy话题，获取手柄输入的数据；②通过在/turtle1/cmd_vel发布消息来控制小乌龟。

手柄控制代码（teleop_turtle.cpp）

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>

class TeleopTurtle
{
public:
    TeleopTurtle();

private:
    // 处理手柄发送过来的信息
    void callback(const sensor_msgs::Joy::ConstPtr &joy);
    // 实例化ROS句柄
    ros::NodeHandle nh;
    // 定义订阅者对象，用来订阅手柄发送的数据
    ros::Subscriber sub;
    // 定义发布者对象，用来将手柄数据发布到乌龟控制话题上
    ros::Publisher pub;
    // 用来接收launch文件中设置的参数，绑定手柄摇杆、轴的映射
    int axis_linear, axis_angular;
};

TeleopTurtle::TeleopTurtle()
{
    // 从参数服务器读取的参数
    nh.param<int>("axis_linear", axis_linear, 1);
    nh.param<int>("axis_angular", axis_angular, 2);

    pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1);
    sub = nh.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopTurtle::callback, this);
}

void TeleopTurtle::callback(const sensor_msgs::Joy::ConstPtr &joy)
{
    geometry_msgs::Twist vel;
    // 将手柄摇杆轴拨动时值的输出赋值给乌龟的线速度和角速度
    vel.linear.x = joy->axes[axis_linear];
    vel.angular.z = joy->axes[axis_angular];
    ROS_INFO("当前线速度为:%.3lf ； 角速度为:%.3lf", vel.linear.x, vel.angular.z);
    pub.publish(vel);
}

int main(int argc, char **argv)
{
    // 设置编码
    setlocale(LC_ALL, "");
    // 初始化ROS节点
    ros::init(argc, argv, "teleop_turtle");
    TeleopTurtle teleopTurtle;
    ros::spin();
    return 0;
}

配置 CMakeLists.txt

# 节点构建选项，配置可执行文件
add_executable(teleop_turtle src/teleop_turtle.cpp)

# 节点构建选项，配置目标链接库
target_link_libraries(teleop_turtle
  ${catkin_LIBRARIES}
)

编写launch文件

<launch>
    <!-- 启动乌龟节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node"/>
    
    <!-- 启动我们创建的手柄控制乌龟节点 -->
    <node pkg="teleop" type="teleop_turtle" name="teleop_turtle" output="screen"/> 
    
    <!-- 向参数服务器写入参数 -->
    <param name="axis_linear" value="1" type="int"/>
    <param name="axis_angular" value="3" type="int"/>
    
    <!-- 启动手柄节点，respawn="true"表示节点挂掉时会自动重启 -->
    <node respawn="true" pkg="joy" type="joy_node" name="joystick" />
</launch>

编译和运行

使用Ctrl+Shift+B进行编译（单独修改launch文件时无须编译），然后source下环境变量，执行launch文件后即可使用手柄摇杆控制小乌龟运动。

$ roslaunch teleop teleop_turtle.launch

问题扩展

1. 手柄摇杆的值范围是-1到1，默认能发布的速度最大值为1m/s，实际使用过程中可根据需要添加比例系数来增加速度的范围
2. 实际项目中，使用手柄控制机器人时，一般会添加使能键，比如当LT轴按下时，摇杆和其他按键才有用，这样可以避免误操作
3. 上面的示例中，只有在摇杆的值有变化时，才会有速度的刷新，小乌龟才会移动。实际项目中，需要添加一个线程来持续检测摇杆的变化情况，只要摇杆的值是非0的，即使摇杆的值不变也要持续发布速度指令

☝ ★★★ — 返回 《ROS机器人开发笔记汇总》总目录 — ★★★ ☝