给SLAM小车添加手柄遥控功能罗技F710和PS4 手柄

在線測試手柄的好壞：www.gamepad-tester.com

罗技Logetic F710 手柄

手柄正面背后上有D和X拨档，D表示Direct 模式，X表示Xbox 模式，带有陀螺仪数据，一般是游戏中手柄当做鼠标选择或进行打字用。一般遥控机器人的时候没有用到这个信息，所以直接选择D档。

正面小按钮Mode灯亮和不亮的时候，会影响摇杆的数据输出。mode灯亮的模式下，只有右手边摇杆可以在+1和-1之间无极调节（精确到小数点后18位），而其他摇杆只有三种状态输出：+1.0，0.0，-1.0。因此直接选择mode灯不亮的模式，按mode按钮可以来回切换模式。

1. 安装驱动

将硬件设备的信息转为ROS中可以识别的/joy topic

sudo apt-get install ros-$ROS_DISTRO-joy
sudo apt-get install ros-$ROS_DISTRO-joystick-drivers

http://wiki.ros.org/joy 官方帮助在这里

插入F710 USB接收器到电脑，系统如果识别到，输入以下指令 ls /dev/input/js*，可以看到一个名为js0的设备接入系统了。

rosrun joy joy_node _dev:=/dev/input/js0

默认是打开 /dev/input/js0 这个设备, 如果是其他设备使用下面设置不同的手柄

rosparam list

rosparam set joy_node/dev "/dev/input/js1"

用 rostopic echo /joy 然后拨弄遥控手柄，就可以查看信息交互信息了。上面的mode 模式选择的无极调节和非无极调节就是通过这个debug看出来的。后文的launch文件的数字就是这个表里面来的。

罗技F710 手柄摇杆键值对应关系（注意此时mode灯处于熄灭状态）
axis	0	1	2	3	4	5
	左(左右)	左(前后)	右(左右)	右(前后)	十字键左右	十字键前后

罗技F710 按钮键值对应关系表
button	0	1	2	3	4	5	6	7	8	9	10	11
	X(左)	A(下)	B(右)	Y(上)	LT	RT	LB	RB	Back	Start	L3	R3

2. 订阅joy并发布cmd_vel

关于这个有两个系统package可以用，第一个代码简单比较直接，第二个用的pimpl实现代码规范但略微复杂。这里选择turtlebot的

turtlebot_teleop - ROS Wiki

teleop_twist_joy - ROS Wiki

cd ~/catkin_ws/src
git clone https://github.com/turtlebot/turtlebot.git

官方程序源码地址： turtlebot/turtlebot_joy.cpp at melodic · turtlebot/turtlebot (github.com)

官方的程序两个小问题，第一个vel的topic名字没有设成变量，默认为cmd_vel，不利于后面设置多输入速度参数配置，要修改的话必须改源码。 joy名字也是如此。

第二个如果速度不合理，（比如一开始scale_angular和scale_linear设置低了）需要先结束launch程序找到并修改launch文件，再重启程序。手柄上还空有很多键，可以加个步进按钮调整(这里利用手柄上的X和Y键，每次按下按钮速度就上下调整0.05)和Reset 按钮（Button序号5，查表知道为RT），按下此按钮，小车速度复位。也即为小车设置了低速档和高速档。

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
#include "boost/thread/mutex.hpp"
#include "boost/thread/thread.hpp"
#include "ros/console.h"
#include <iostream>
using namespace std;class Teleop
{
public:Teleop();private:void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);void publish();ros::NodeHandle ph_, nh_;int linear_, angular_, deadman_axis_,l_up,l_down;double l_scale_, a_scale_;ros::Publisher vel_pub_;ros::Subscriber joy_sub_;geometry_msgs::Twist last_published_;boost::mutex publish_mutex_;bool deadman_pressed_;bool zero_twist_published_;ros::Timer timer_;int reset_;double l_reset_, a_reset_;std::string vel_name_;std::string joy_name_;};Teleop::Teleop():ph_("~"),linear_(1),angular_(2),deadman_axis_(4),l_up(3),l_down(1),reset_(5),l_scale_(0.2),a_scale_(0.4),l_reset_(0.2),a_reset_(0.4)	
{ph_.param("axis_linear", linear_, linear_);ph_.param("axis_angular", angular_, angular_);ph_.param("axis_deadman", deadman_axis_, deadman_axis_);ph_.param("scale_angular", a_scale_, a_scale_);ph_.param("scale_linear", l_scale_, l_scale_);ph_.param("reset_btn", reset_, reset_);ph_.param("angular_reset", a_reset_, a_reset_);ph_.param("linear_reset", l_reset_, l_reset_);ph_.param<std::string>("vel_name", vel_name_, std::string("/turtle1/cmd_vel"));ph_.param<std::string>("joy_name", joy_name_, std::string("/joy"));deadman_pressed_ = false;zero_twist_published_ = false;cout<<"vel topic name: "<<vel_name_<<endl;cout<<"joy topic name: "<<joy_name_<<endl;vel_pub_ = nh_.advertise<geometry_msgs::Twist>(vel_name_, 1, true);joy_sub_ = nh_.subscribe<sensor_msgs::Joy>(joy_name_, 10, &Teleop::joyCallback, this);timer_ = nh_.createTimer(ros::Duration(0.1), boost::bind(&Teleop::publish, this));
}void Teleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{geometry_msgs::Twist vel;if(joy->axes[angular_])a_scale_ = a_scale_ + 0.05 * joy->buttons[l_up] - 0.05 * joy->buttons[l_down];if(joy->axes[linear_])l_scale_ = l_scale_ + 0.05 * joy->buttons[l_up] - 0.05 * joy->buttons[l_down];if(joy->buttons[reset_]){l_scale_ = l_reset_;a_scale_ = a_reset_;}vel.angular.z = a_scale_*joy->axes[angular_];vel.linear.x = l_scale_*joy->axes[linear_];last_published_ = vel;cout<<"cmd_cel value: x "<<vel.linear.x<< " angular "<<vel.angular.z<<endl;deadman_pressed_ = joy->buttons[deadman_axis_];
}void Teleop::publish()
{boost::mutex::scoped_lock lock(publish_mutex_);if (deadman_pressed_){vel_pub_.publish(last_published_);zero_twist_published_=false;}else if(!deadman_pressed_ && !zero_twist_published_){vel_pub_.publish(*new geometry_msgs::Twist());zero_twist_published_=true;}
}int main(int argc, char** argv)
{ros::init(argc, argv, "joy_car");Teleop robot_teleop;ros::spin();
}

在自己的一个ROS package工作目录下建一个logetic_joy_teleop.launch，内容如下。

直线速度最大值(scale_linear)和角速度最大值(scale_angular)，油门离合键(axis_deadman 4)、前进后退轴(axis_linear,1 ，查文章前面的对应表知道对应手柄左摇杆的前后位)、左右转轴(axis_angular，2，对应手柄右摇杆的左右位)

<launch><node pkg="turtlebot_teleop" type="turtlebot_teleop_joy" name="turtlebot_teleop_joy" output="screen" clear_params="true"><param name="scale_angular" value="0.2"/><param name="scale_linear" value="0.2"/><param name="linear_reset" value="0.2"/><param name="angular_reset" value="0.4"/><!-- Left up down --><param name="axis_linear" value="1"/><!-- Right left right --><param name="axis_angular" value="2"/><!-- L1 --><param name="axis_deadman" value="4"/><!-- R1 --><param name="reset_btn" value="5"/><param name="vel_name" value="/joy_cmd_vel" type="string" /><param name="joy_name" value="/joy" type="string" /></node><node pkg="joy" type="joy_node" name="joystick"><param name="dev" value="/dev/input/js0" type="string" /></node></launch>

PS4 手柄长按share和PS键开启蓝牙配对模式，此时手柄灯闪烁白光。配对成功后白灯常亮。

PS4 手柄摇杆键值对应关系
axis	0	1	2	3	4	5
	左 (左右)	左 (前后)	L2 默认为1, 按下递减到-1	右 (左右)	右 (前后)	R2 默认为1, 按下递减到-1

PS4 手柄按键键值对应关系
button	0	1	2	3	4	5	6	7	8	9	10	11	12	13	14
	× (下)	○ (右)	□ (左)	△ (上)	L1	R1	share	options	PS 键	L3	R3	十字左	十字右	十字上	十字下

husky/teleop_ps4.yaml at noetic-devel · husky/husky (github.com)

# Teleop configuration for PS4 joystick using the x-pad configuration.
# Left thumb-stick up/down for velocity, left/right for twist
# Left shoulder button for enable
# Right shoulder button for enable-turbo
#
#          L1                                       R1
#          L2                                       R2
#       _=====_                                  _=====_
#      / _____ \                                / _____ \
#    +.-'_____'-.------------------------------.-'_____'-.+
#   /   |     |  '.        S O N Y           .'  |  _  |   \
#  / ___| /|\ |___ \                        / ___| /_\ |___ \      (Y)
# / |      |      | ;                      ; | _         _ ||
# | | <---   ---> | |                      | ||_|       (_)||  (X)     (B)
# | |___   |   ___| ;                      ; |___       ___||
# |\    | \|/ |    /  _      ____      _   \    | (X) |    /|      (A)
# | \   |_____|  .','" "',  (_PS_)  ,'" "', '.  |_____|  .' |
# |  '-.______.-' /       \        /       \  '-._____.-'   |
# |               |  LJ   |--------|  RJ   |                |
# |              /\       /        \       /\               |
# |             /  '.___.'          '.___.'  \              |
# |            /                              \             |
#  \          /                                \           /
#   \________/                                  \_________/
#
#          ^ x
#          |
#          |
#  y <-----+      Accelerometer axes
#           \
#            \
#             > z (out)
#
# BUTTON         Value
#   L1             4
#   L2             6
#   R1             5
#   R2             7
#   A              1
#   B              2
#   X              0
#   Y              3
#
#    AXIS        Value
# Left Horiz.      0
# Left Vert.       1
# Right Horiz.     2
# Right Vert.      5
#    L2            3
#    R2            4
# D-pad Horiz.     9
# D-pad Vert.      10
# Accel x          7
# Accel y          6
# Accel z          8teleop_twist_joy:axis_linear: 1scale_linear: 0.4scale_linear_turbo: 2.0axis_angular: 0scale_angular: 1.4enable_button: 4enable_turbo_button: 5
joy_node:deadzone: 0.1autorepeat_rate: 20dev: /dev/input/ps4

一个简要的ROS下手柄控制程序发布cmd_vel

#include <ros/ros.h>
#include<queue>
#include<sensor_msgs/Joy.h>
#include<geometry_msgs/Twist.h>struct Velocity{double linear = 0;double angle = 0;
};Velocity joy_vel;
static bool tag = false;
const int max_vel = 1;void joy_callback(const sensor_msgs::JoyConstPtr& msg)
{joy_vel.linear = msg->axes[1] * max_vel*1.0;joy_vel.angle = msg->axes[2] * max_vel*1.5; 
}int main(int argc,char** argv)
{ros::init(argc,argv,"joy_control_node");ros::NodeHandle n;ros::NodeHandle p;ros::Subscriber joy_sub;ros::Publisher cmd_pub;joy_sub = n.subscribe("/joy",1,&joy_callback);cmd_pub = p.advertise<geometry_msgs::Twist>("/cmd_vel",1);geometry_msgs::Twist car_vel;ROS_INFO("start to joy control");ros::Rate r(100);while(ros::ok()){car_vel.linear.x = joy_vel.linear;car_vel.linear.y = 0.0;car_vel.linear.z = 0.0;car_vel.angular.z = joy_vel.angle;car_vel.angular.y = 0.0;car_vel.angular.x = 0.0;cmd_pub.publish(car_vel);ros::spinOnce();r.sleep();}ros::shutdown();return 0;}