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

97
0
2020年11月10日 09时07分

本文介绍手柄控制模块的代码。模块对应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,如下:

 

1

 

其中js0即表示是手柄,默认的就是js0,如果不是,如为js1则需要通过指令修改:

 

rosparam set joy_node/dev "/dev/input/js1"

 

启动手柄

 

rosrun joy joy_node

 

监听其发布的信息rostopic echo /joy。

当我们按下手柄的A键时,如下图:

 

2

 

显然,buttons[0]状态发生改变,A键对应buttons[0]。

我们再上下拨动左摇杆时,如下图:

 

3

 

显然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);

 

附上当初测试的一些情况吧:

 

458

 

在实际测试中,基本上很难实现效果很好的控制,甚至极大概率前进运动时是歪的。模型文件数据与实际机器人终究不可能完全一致。如何改善效果?自然是依靠传感器,如IMU,DVL等,然后在控制上添加闭环了。实际上ROS无论对传感器模块的添加,还是运动控制算法的开发都非常友好方便,开源资料丰富,故虽然这部分demo并没有开发,但实际上我认为软件框架在实际应用中是可期的。这也是我在前文中我提及的测试效果基本实现目标的原因,毕竟这只是一个验证性的demo。根据测试结果也给了我未来对此基于软件架构升级开发的信心。

发表评论

后才能评论