基于 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 接受命令,将其中的转角命令对应到方向盘的转动。

推荐阅读更多精彩内容