引言
我们项目组有一部分工作是关于人机共驾的。一个典型场景是，当车驶入复杂路况时，自动驾驶功能无法应对，需要由人来接管。目前这部分工作我们主要基于仿真平台展开。为了接管过程流畅，项目要求： 仿真平台中外接方向盘 Logitech-g29 在自动驾驶状态下能够跟随 Autoware 的控制命令转动，这样在接管时，人类驾驶员更容易把握当前车辆状态，尤其是当前车辆转角。
根据我们的经验，对于实车，在自动驾驶状态下方向盘一般是跟随车轮一起转动的。但在我们的仿真平台上，g29 方向盘默认情况下并不能依照 Autoware 的指令转动，因此需要额外设置一下。
在查找资料过程中，我们发现已经有人在 github 上分享了基于 Node.js 实现的 g29 方向盘控制程序。我们进一步将程序包装成 ROS node 的形式，以便接受 Autoware 相关 topic 发送过来的控制指令。
参考1：基于 Node.js 实现的 g29 方向盘控制程序。
参考2：rosnodejs API
我们用的平台
- Ubuntu 16.04
- Node.js v6.17 (官网要求 4.0 及以上)
- ROS Kinetic
- Logitech g29 (PS3 mode)
比较简单的 node.js 安装方式如下：
- 下载编译好的 node.js 文件 并解压，
bin文件夹中就是我们要用到的 node 和 npm 可执行文件 - 通过
ln -s的方式将上述两个文件添加到/usr/local/bin文件夹下即可sudo ln -s <原文件路径> <目标路径> # 例如 sudo ln -s /home/my_name/Downloads/node-v7.6.0-linux-x64/bin/node /usr/local/bin sudo ln -s /home/my_name/Downloads/node-v7.6.0-linux-x64/bin/npm /usr/local/bin
通过 Node.js 实现对 g29 的转角控制
-
新建文件夹存放程序
mkdir nodejs_g29 -
安装 g29 package
cd nodejs_g29 npm install logitech-g29 -
修改权限文件，避免每次都要 sudo，才能操控方向盘。
在/etc/udev/rules.d/目录下新建文件99-hidraw-permissions.rules，内容如下：KERNEL=="hidraw*", SUBSYSTEM=="hidraw", MODE="0664", GROUP="plugdev"重启。
-
测试。在文件夹 nodejs_g29 中新建
test.js文件内容如下：const g = require('logitech-g29') g.connect(function(err) { g.on('pedals-gas', function(val) { g.leds(val) }) })连接上 g29 方向盘，通过如下命令执行程序：
node test.js此时踩下油门踏板，方向盘上 led 灯会有反映，说明安装成功。
-
实现 g29 方向盘转角控制。在 github 的 issue 中有人提出了控制方向盘角度的问题，在回答中提供了两种方式，亲测都有效。
- 程序1:
method_1.js：
var g = require('logitech-g29') var options = { autocenter: false, // set to false so the wheel will not fight itself when we rotate it debug: false, range: 900 } var wheel = { currentPos: 0, // initial value does not matter moveToPos: 0, // initial value does not matter moved: true } function moveToDegree(deg) { /* @param {Number} deg Degree can be anywhere from 0 (far left) to 450 (center) to 900 (far right). */ deg = deg / options.range * 100 wheel.moveToPos = deg wheel.moved = false if (deg < wheel.currentPos) { g.forceConstant(0.3) } else { g.forceConstant(0.7) } } g.connect(options, function(err) { if (err) { console.log('Oops -> ' + err) } g.forceFriction(0.6) // without friction the wheel will tend to overshoot a move command moveToDegree(options.range / 2) // center g.on('wheel-turn', function(val) { wheel.currentPos = val if (wheel.moved === false) { console.log('wheel at position ' + val) var min = wheel.moveToPos - 1 var max = wheel.moveToPos + 1 if (wheel.currentPos >= min && wheel.currentPos <= max) { console.log('--- move complete, turning off force') g.forceConstant() // turn off force wheel.moved = true } } }) })要运行上述程序，首先进入 node 环境：
node在 node 环境中加载上述程序
.load method_1.js用
.load方式运行程序相当于逐行输入程序。通过函数moveToDegree( deg )可以控制 g29 方向盘到任意位置，其中 deg : 0～900，例如输入moveToDegree(0)方向盘会转到最左边。
程序运行过程中，屏幕上会显示转角值，这个转角值来自wheel.currentPos，其取值范围为 0～100。 程序中 0～900 和 0～100 这两个转角范围之间是通过deg = deg / options.range * 100转换的。程序2：
method_2.jsvar g = require('logitech-g29') var options = { autocenter: false, // set to false so the wheel will not fight itself when we rotate it debug: false, range: 900 } var wheel = { currentPos: 0, // initial value does not matter moveToPos: 0, // initial value does not matter moved: true } var connected = false; var setpoint = 50; setInterval(function() { if (!connected) return; var seconds = new Date().getTime() / 1000; setpoint = 50 + 10 * Math.sin(seconds); var error = setpoint - wheel.currentPos; console.log("Setpoint:", setpoint); console.log("Position:", wheel.currentPos); console.log("Error:", error); var p = 0.1; var u = p * error; // u may range from -0.5 to 0.5 var u_clipped = Math.max(-0.5, Math.min(0.5, u)); console.log("U, U clipped", u, u_clipped); g.forceConstant(0.5 + u_clipped); }, 100); g.connect(options, function(err) { if (err) { console.log('Oops -> ' + err) } connected = true; g.forceFriction(0.8) // without friction the wheel will tend to overshoot a move command g.on('wheel-turn', function(val) { wheel.currentPos = val; }) })该程序采用了 P 控制器，基于当前位置 wheel.currentPos 与目标位置 setpoint 的偏差，成比例地调整施加的控制力。程序中令目标点 setpoint 按照时间的正弦函数变化
setpoint = 50 + 10 * Math.sin(seconds);，所以在运行程序时，方向盘会不停的左右转动。直接通过如下命令运行程序即可node method2.js - 程序1:
包装成 ROS node 的形式
上述程序都是手动设定期望的转角。我们的目标是让 g29 按照 Autoware 发布的控制量自动转动。这里就涉及到 node.js 程序与 ROS 的交互。幸运的是，目前很多主流语言都提供了 ROS 相关的库（client library），其中就包括 node.js 对应的库——rosnodejs。
我们下面将借用 method_2.js 中的控制方式，编写 ROS node 程序。
-
创建 ROS 工作空间和 package
mkdir -p ~/rosnodejs_ws/src cd ~/rosnodejs_ws/src git clone https://github.com/RethinkRobotics-opensource/rosnodejs_examples.git cd rosnodejs_examples npm install npm install logitech-g29 cd ~/rosnodejs_ws catkin_make echo "source ~/rosnodejs_ws/devel/setup.bash" >> ~/.bashrc -
编写 ROS node 程序
在上述 rosnodejs_examples package 中已经有两个示例程序了，通过它们基本上可以理解如何创建节点、接受和发送 msg 等关键步骤。官网 也提供了基本介绍。最终，我们的程序(基本框架)如下：#!/usr/bin/env node 'use strict'; const rosnodejs = require('rosnodejs'); const g = require('logitech-g29'); const TwistStamped = rosnodejs.require('geometry_msgs').msg.TwistStamped; const options = { autocenter: false, debug: false, range: 900 } const wheel = { currentPos: 0, moveToPos: 0, moved: true } var connected = false; / / once g29 is connected, run this function g.connect(options, function() { connected = true; g.forceFriction(0.4); g.on('wheel-turn', function(val) { wheel.currentPos = val; }) }) var setpoint = 50; var error = 0; var p = 0.1; var u = 0.0; var u_clipped = 0.0; // once a new msg is received, run the callback function var callback = function(msg){ if (!connected) return; setpoint = (-msg.twist.angular.z + 1) * 50; // 在实际项目中，angular.z 与方向盘转角的对应关系应该更加精细 error = setpoint - wheel.currentPos u = p * error; // u may range from -0.5 to 0.5 u_clipped = Math.max(-0.5, Math.min(0.5, u)); g.forceConstant(0.5 + u_clipped); } function twist_cmd_raw_receiver() { rosnodejs.initNode('/g29_feedback') .then((rosNode) => { // Create ROS subscriber var sub = rosNode.subscribe('/twist_cmd_raw', TwistStamped, callback); }); } if (require.main === module) { // Invoke Main Function twist_cmd_raw_receiver(); }运行该程序就可以让 g29 方向盘从 ROS topic
twist_cmd_raw接受命令，将其中的转角命令对应到方向盘的转动。
