导师买了个游戏手柄,就研究了一下怎么用游戏手柄控制机器人。想不到有点简单,编写一断代码即可,同时对我这两天学习的复习。
开始教程。
(1)游戏手柄控制乌龟。
rosrun turtlesim turtlesim_noderosrun turtlesim turtle_teleop_key
当我们运行上面的两句,发现键盘的方向键可以乌龟移动,它们俩是通过话题通信。
/turtle1/cmd_vel[geometry_msgs/Twist]
geometry_msgs/Twist是这个话题的类型,即消息的类型,我们看看内部结构
rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linearfloat64 xfloat64 yfloat64 zgeometry_msgs/Vector3 angularfloat64 xfloat64 yfloat64 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: 837stamp: secs: 1500642610nsecs: 656011953frame_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 headeruint32 seqtime stampstring frame_idfloat32[] axesint32[] 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/spub = 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.launchroslaunch turtlebot_teleop keyboard_teleop.launchrosrun 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文件可以重映射哦
如果觉得《ROS之游戏手柄控制乌龟和机器人》对你有帮助,请点赞、收藏,并留下你的观点哦!