基于 Node.js 的 Logitech-g29 方向盘控制

引言

我们项目组有一部分工作是关于人机共驾的。一个典型场景是，当车驶入复杂路况时，自动驾驶功能无法应对，需要由人来接管。目前这部分工作我们主要基于仿真平台展开。为了接管过程流畅，项目要求： 仿真平台中外接方向盘 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 安装方式如下：

1. 下载编译好的 node.js 文件 并解压，bin 文件夹中就是我们要用到的 node 和 npm 可执行文件
2. 通过 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 的转角控制

1. 新建文件夹存放程序

   mkdir nodejs_g29
   

2. 安装 g29 package

   cd nodejs_g29
   npm install logitech-g29
   

3. 修改权限文件，避免每次都要 sudo，才能操控方向盘。
   在 /etc/udev/rules.d/ 目录下新建文件 99-hidraw-permissions.rules，内容如下：

   KERNEL=="hidraw*", SUBSYSTEM=="hidraw", MODE="0664", GROUP="plugdev"
   

   重启。

4. 测试。在文件夹 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 灯会有反映，说明安装成功。

5. 实现 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.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
   }
   
   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
   

包装成 ROS node 的形式

上述程序都是手动设定期望的转角。我们的目标是让 g29 按照 Autoware 发布的控制量自动转动。这里就涉及到 node.js 程序与 ROS 的交互。幸运的是，目前很多主流语言都提供了 ROS 相关的库（client library），其中就包括 node.js 对应的库——rosnodejs。
我们下面将借用 method_2.js 中的控制方式，编写 ROS node 程序。

1. 创建 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
   

2. 编写 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 接受命令，将其中的转角命令对应到方向盘的转动。