ROS 罗技手柄控制机器人（仿真和实体机器人）

20210615更新

测试的时候发现一个问题，之前代码的逻辑是每次按一下摇柄就发一个命令，但是如果一直按着摇柄往一个方向，也是发送一次命令，摇柄转到其他方向，信号发生变化才再次发送一个速度命令，这点其实在仿真里面没有问题。
但是，在实际控制机器人时，就会出现，比如要机器人往前，把摇杆推前，并保持，这时发送一个速度指令可能让机器人走一个loop的时间，然后就停下，这个问题也跟实际机器人的底层控制有关，如果这个机器人是接收到一个命令就一直执行，就不会有这个问题，但是实体机器人一般不会这样做控制，也太危险了。理想情况应该是，当我们控制摇杆往不同方向时，应该一直在发送速度指令，让机器人一直执行不同的指令。
综上所述，改了一下代码，如下：

#include<ros/ros.h>
#include<string>
#include<iostream>
#include<geometry_msgs/Pose.h>
#include<nav_msgs/Odometry.h>
#include<sensor_msgs/Joy.h>
#include<geometry_msgs/Twist.h>
#include<time.h>double vlinear,vangular;
int axis_ang,axis_lin_x,axis_lin_y,ton;
double vx, vy, vtheta_z;
/*
class Teleop
{
public:Teleop();private: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_x,axis_lin_y,ton;
};Teleop::Teleop()
{n.param<int>("axis_linear_x",axis_lin_x,7);n.param<int>("axis_linear_y",axis_lin_y,6);n.param<int>("axis_angular",axis_ang,3);n.param<double>("vel_linear",vlinear,0.15);n.param<double>("vel_angular",vangular,0.25);n.param<int>("button",ton,4);pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",1);sub = n.subscribe<sensor_msgs::Joy>("joy",10,&Teleop::callback,this);
}
*/
void callback(const sensor_msgs::Joy::ConstPtr& Joy)
{geometry_msgs::Twist v;if(Joy->buttons[ton]){vx =(Joy->axes[axis_lin_x])*vlinear;vy =(Joy->axes[axis_lin_y])*vlinear;vtheta_z = (Joy->axes[axis_ang])*vangular;ROS_INFO("linear x y:%.3lf %.3lf  angular:%.3lf",vx,vy,vtheta_z);//  pub.publish(v);}
}int main(int argc,char** argv)
{ros::init(argc, argv, "joy");ros::NodeHandle n;ros::Subscriber sub ;ros::Publisher pub ;ros::Rate r(20);n.param<int>("axis_linear_x",axis_lin_x,7);n.param<int>("axis_linear_y",axis_lin_y,6);n.param<int>("axis_angular",axis_ang,3);n.param<double>("vel_linear",vlinear,0.15);n.param<double>("vel_angular",vangular,0.25);n.param<int>("button",ton,4);sub = n.subscribe<sensor_msgs::Joy>("joy",10,callback);pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",1);while(n.ok()){geometry_msgs::Twist v;// if(Joy->buttons[ton])//     {v.linear.x =vx;v.linear.y =vy;v.angular.z = vtheta_z;// ROS_INFO("linear x y:%.3lf %.3lf  angular:%.3lf",v.linear.x,v.linear.y,v.angular.z);pub.publish(v);// }ros::spinOnce();r.sleep();}return 0;
}

这个就可以满足控制真实机器人时的需求。

机器人在实际使用中，经常需要通过外设来控制它完成一些简单的任务。

可以使用键盘，也可以使用罗技的手柄，手柄比较方便。键盘的代码就不放了，太多了。记录一下手柄控制方法。

要控制的对象是一个全向轮小车，所以会有一个y方向的横移，以及绕z轴自转。不管是holonomic还是non-holonomic的小车，就差一个y方向。

Mehtod：

安装手柄驱动：
sudo apt-get install ros-melodic-joy
sudo apt-get install joystick
根据实际ROS版本，选择安装包
查看手柄串口号
ls -l /dev/input/js0
测试手柄信号
sudo jstest /dev/input/js0
运行ROS节点
rosrun joy joy_node

查看手柄发出的信号
rostopic echo joy
按几下手柄，看看输出的信号对不对

axes和buttons中的数值都在-1到1之间，然后搞清楚每个axes和buttons对应手柄上的按钮就可以开始写代码了，先看下面几张图：
写一个订阅手柄信息和发布/cmd_vel指令的node
先上代码：

#include<ros/ros.h>
#include<string>
#include<iostream>
#include<geometry_msgs/Pose.h>
#include<nav_msgs/Odometry.h>
#include<sensor_msgs/Joy.h>
#include<geometry_msgs/Twist.h>
#include<time.h>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_x,axis_lin_y,ton;
};Teleop::Teleop()
{n.param<int>("axis_linear_x",axis_lin_x,7);n.param<int>("axis_linear_y",axis_lin_y,6);n.param<int>("axis_angular",axis_ang,3);n.param<double>("vel_linear",vlinear,0.15);n.param<double>("vel_angular",vangular,0.25);n.param<int>("button",ton,4);pub = n.advertise<geometry_msgs::Twist>("/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;if(Joy->buttons[ton]){v.linear.x =(Joy->axes[axis_lin_x])*vlinear;v.linear.y =(Joy->axes[axis_lin_y])*vlinear;v.angular.z = (Joy->axes[axis_ang])*vangular;ROS_INFO("linear x y:%.3lf %.3lf  angular:%.3lf",v.linear.x,v.linear.y,v.angular.z);pub.publish(v);}
}int main(int argc,char** argv)
{ros::init(argc, argv, "joy");Teleop telelog;ros::spin();return 0;
}

代码中设定的初始速度比较低，成功测试后可以适当调高vlinear和vangular。

需要用到多少个按钮，就根据实际情况定就好了，这里写的控制逻辑是，每次发送指令时都是按住左上角的LB键，然后在按遥感A和B来发送速度指令，这也是为了安全，防止使用时不小心碰到遥感使机器人撞到其他东西。

完成上述部分后，就可以写一个launch文件和一个shell文件，以后就能一个指令启动，不用一次次打开多个文件。
先写一个名叫handle_control.launch文件：

<?xml version = "1.0"?>
<launch><node name="base_node_serial" pkg="gazebo_mobile_manipulator" type="base_node_serial" />
<node name="handle_control" pkg="gazebo_mobile_manipulator" type="handle_control" /></launch>

其中的pkg名字，根据自己的package修改

然后建一个名为handle_control.sh的shell文件：

#!/bin/bash
# This is our first script.rosnode kill --all;
echo 'Handle control Start!'gnome-terminal --tab --title="joy" --command="bash -c 'rosrun joy joy_node; $SHELL'"gnome-terminal --tab --title="handle control" --command="bash -c 'source catkin_ws/devel/setup.bash; roslaunch gazebo_mobile_manipulator handle_control.launch; $SHELL'"

之后使用时，只需要在terminal中输入bash handle_control.sh即可

测试
1）启动shell后，先rostopic echo /cmd_vel看看，输出的速度大小是否合适。
2）然后就可以启动自己的gazebo模型，当然，需要这个model是速度控制的。
3）同理，真实的机器人也是这样。
以上，都是亲测可行。

REFERENCES

ros使用罗技f710无线控制手柄:
https://www.cnblogs.com/cj2014/p/3989784.html