基于ROS搭建简易软件框架实现ROV水下目标跟踪（十）--程序解析之手动控制

        项目链接：https://github.com/cabinx/cabin_auv_ws

        本文介绍手柄控制模块的代码。模块对应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。根据测试结果也给了我未来对此基于软件架构升级开发的信心。