在線測試手柄的好壞:www.gamepad-
罗技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-joysudo apt-get install ros-$ROS_DISTRO-joystick-drivers
/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文件的数字就是这个表里面来的。
2. 订阅joy并发布cmd_vel
关于这个有两个系统package可以用,第一个代码简单比较直接,第二个用的pimpl实现代码规范但略微复杂。这里选择turtlebot的
turtlebot_teleop - ROS Wiki
teleop_twist_joy - ROS Wiki
cd ~/catkin_ws/srcgit clone /turtlebot/turtlebot.git
官方程序源码地址: turtlebot/turtlebot_joy.cpp at melodic · turtlebot/turtlebot ()
官方的程序两个小问题,第一个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键开启蓝牙配对模式,此时手柄灯闪烁白光。配对成功后白灯常亮。
husky/teleop_ps4.yaml at noetic-devel · husky/husky ()
# 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 x7# Accel y6# Accel z8teleop_twist_joy:axis_linear: 1scale_linear: 0.4scale_linear_turbo: 2.0axis_angular: 0scale_angular: 1.4enable_button: 4enable_turbo_button: 5joy_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;}