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

Posted 2022-11-30 yaked19

在線測試手柄的好壞：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          8

teleop_twist_joy:
  axis_linear: 1
  scale_linear: 0.4
  scale_linear_turbo: 2.0
  axis_angular: 0
  scale_angular: 1.4
  enable_button: 4
  enable_turbo_button: 5
joy_node:
  deadzone: 0.1
  autorepeat_rate: 20
  dev: /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;