本文介绍手柄控制模块的代码。模块对应cabin_teleop。结合前几篇关于基础运动模块代码的解析，显然我们只需要操控手柄发布力和力矩的信息[Fx，Fy，Fz，Mx，My，Mz]即可实现对机器人的控制。如发布[10,0,0,0,0,0]即表示对机器人施加x轴方向10N的力，机器人x轴方向前进；发布[0,0,0,0,0,10]即表示对机器人施加z轴方向10N*m的力矩，机器人偏航方向逆时针转动。 一、joystick工具包 ros下使用手柄非常方便，教程也非常多。我也粗略的介绍一下。借助joystick工具包（sudo apt-get install ros-melodic-joystick-drivers）。当然我推荐源码安装http://wiki.ros.org/joystick_drivers。 我们来看一下手柄的数据格式sensor_msgs/Joy（http://docs.ros.org/en/api/sensor_msgs/html/msg/Joy.html）。
# Reports the state of a joysticks axes and buttons.
Header header # timestamp in the header is the time the data is received from the joystick
float32[] axes # the axes measurements from a joystick
int32[] buttons # the buttons measurements from a joystick
Axes表示摇杆，buttons表示按键。 安装完成后，接入手柄，可以看一下其端口号ls /dev/input，如下：
其中js0即表示是手柄，默认的就是js0，如果不是，如为js1则需要通过指令修改：
rosparam set joy_node/dev "/dev/input/js1"
启动手柄
rosrun joy joy_node
监听其发布的信息rostopic echo /joy。 当我们按下手柄的A键时，如下图：
显然，buttons[0]状态发生改变，A键对应buttons[0]。 我们再上下拨动左摇杆时，如下图：
显然axes[1]状态发生改变，左摇杆上下方向对应axes[1]。 二、头文件 头文件反映按键的映射关系，不同型号的手柄间可能会不一致，只用修改头文件的映射关系可以适应手柄的变化。 以测试时使用的logicool手柄为例,代码对应logicool_button_mapping.h。。
#define AXES_STICK_LEFT_UD 1
#define BUTTON_SHAPE_A 0
AXES_STICK_LEFT_UD为左摇杆上下方向，BUTTON_SHAPE_A为按键A，如此就与上文中联系起来了。当需要使用不同型号的手柄时，按照上文的方法，每个按键都测试一次，然后修改头文件即可。 三、变量初始化
double joy_force[3]; //The force input along the x, y, z axis
double joy_moment[3]; //The moment input around the x, y, axis
作用于机器人上的力与力矩。 四、JoyCallback（）函数 监听按键信息，对按键事件做出响应。 （1）左十字上下方向
if(1 == msg.axes[AXES_CROSS_UD]){
joy_force[0] += 5.0;
}
else if(-1 == msg.axes[AXES_CROSS_UD]){
joy_force[0] -= 5.0;
}
else{
joy_force[0] += 0.0;
}
每按一次上方向，增加5；每按一次下方向，减少5；demo里此时机器人持续前进与后退。 （2）左摇杆左右方向
if(msg.axes[AXES_STICK_LEFT_LR]){
current_axes_factor[1] = msg.axes[AXES_STICK_LEFT_LR];
if(current_axes_factor[1] > 0){
joy_moment[2] = current_axes_factor[1] * max_forward_thrust;
}
else{
joy_moment[2] = current_axes_factor[1] * max_backward_thrust;
}
}
根据摇杆的幅度确定输出大小，demo里此时机器人随摇杆做偏航运动。 （3）A键
if(1 == msg.buttons[BUTTON_SHAPE_A]){
joy_switch = true;
joy_switch_state = !joy_switch_state;
}
改变锁死joy_switch状态； （4）B键
if(1 == msg.buttons[BUTTON_SHAPE_B]){
joy_reset = true;
}
激活reset的flag。 五、Main（）函数 （1）响应锁死
if(joy_switch){
output_switch_state.kill = joy_switch_state;
joy_swtich_state_pub.publish(output_switch_state);
joy_switch = false;
}
实际上发布了/state/switches。在pwm计算模块pwm_controller监听该信号，若为信号值为false，则锁死pwm波为初始值；若信号值为true，则解锁。控制时可以随时通过A键开解锁。当处于锁死状态时，各推进器PWM波值锁定为初始值，只有按下A键解锁，才能继续控制。 注意：为了安全保险，程序启动时默认是锁死状态的，启动后需要按下A键解锁才能通过摇杆控制机器人运动。 （2）响应reset
if(joy_reset){
output_reset_state.reset_pwm = joy_reset_state;
joy_reset_state_pub.publish(output_reset_state);
for(int i = 0; i < 3; i++){
joy_force[i] = 0.0;
joy_moment[i] = 0.0;
}
joy_reset = false;
}
按下B键时，发布的力与力矩为0；同时发布/command/reset，信息内容为true，在pwm计算模块pwm_controller监听该信号，信息内容为ture时，将pwm波置为初始值，再改变reset状态。 实际上可以将B键理解成急停，按下时机器人立即回归初始状态。 （3）发布力和力矩
output_netLoad.header.stamp = ros::Time::now();
//Move forward
output_netLoad.force.x = joy_force[0];
output_netLoad.force.y = joy_force[1];
output_netLoad.force.z = joy_force[2];
//Rotate around z axis (yaw)
output_netLoad.moment.x = joy_moment[0];
output_netLoad.moment.y = joy_moment[1];
output_netLoad.moment.z = joy_moment[2];
joy_netLoad_pub.publish(output_netLoad);
附上当初测试的一些情况吧：
在实际测试中，基本上很难实现效果很好的控制，甚至极大概率前进运动时是歪的。模型文件数据与实际机器人终究不可能完全一致。如何改善效果？自然是依靠传感器，如IMU，DVL等，然后在控制上添加闭环了。实际上ROS无论对传感器模块的添加，还是运动控制算法的开发都非常友好方便，开源资料丰富，故虽然这部分demo并没有开发，但实际上我认为软件框架在实际应用中是可期的。这也是我在前文中我提及的测试效果基本实现目标的原因，毕竟这只是一个验证性的demo。根据测试结果也给了我未来对此基于软件架构升级开发的信心。
