GazeboJs hydra publisher


Introduction

Hydra publisher using GazeboJs and node-sixense modules. It publishes the hydra messages over the ~/hydra gazebo topic. Similar tutorial cand be found here. The main difference being that the later requires gazebo being installed from source.

Project setup

Make sure gazebo is running

gazebo

Install the publisher:

npm install hydra_gzjs_pub

Run the publisher:

cd node_modules/hydra_gzjs_pub
sudo node hydra_pub.js

For publishing the hydra controller without root privileges please look at this tutorial.

Test that the publisher works:

gz topic -l
gz topic -e /gazebo/default/hydra

Code

The publisher code is located in the hydra_pub.js file

(hydra_pub.js):

var hydra =  require('node-sixense');  
var gazebojs = require('gazebojs');

var gazebo = new gazebojs.Gazebo();

var type = "gazebo.msgs.Hydra"

var topic = "~/hydra"


hydra.sixenseInit();

hydra.sixenseSetActiveBase(0);

hydra.sixenseGetAllNewestDataAsync(function (error, allData)
{    
    var msgS = '{ ' + 
                '"right" : {' +
                    '"pose" : {' +
                        '"position" : {' +
                            '"x" : ' + allData[1].position.z * -0.001 + ',' +
                            '"y" : ' + allData[1].position.x * -0.001 + ',' + 
                            '"z" : ' + allData[1].position.y * 0.001 + 
                            '},' +
                        '"orientation" : {' +
                            '"x" : ' + - allData[1].rotationQuaternion.z + ',' + 
                            '"y" : ' + - allData[1].rotationQuaternion.x + ',' + 
                            '"z" : ' + allData[1].rotationQuaternion.y + ',' + 
                            '"w" : ' + allData[1].rotationQuaternion.w +
                            '}' +
                    '},'+
                    '"button_bumper" : '+ allData[1].buttons.bumper + ',' +
                    '"button_1" : '+ allData[1].buttons.button1 + ',' +
                    '"button_2" : '+ allData[1].buttons.button2 + ',' +
                    '"button_3" : '+ allData[1].buttons.button3 + ',' +
                    '"button_4" : '+ allData[1].buttons.button4 + ',' +
                    '"button_joy" : '+ allData[1].buttons.joystick + ',' +
                    '"button_center" : '+ allData[1].buttons.start + ',' +
                    '"joy_x" : ' + allData[1].joystick.y + ',' +
                    '"joy_y" : ' + allData[1].joystick.x + ',' +
                    '"trigger" : ' + allData[1].trigger  +
                '},' +

                '"left" : {' +
                    '"pose" : {' +
                        '"position" : {' +
                            '"x" : ' + allData[0].position.z * -0.001 + ',' +
                            '"y" : ' + allData[0].position.x * -0.001 + ',' + 
                            '"z" : ' + allData[0].position.y * 0.001 + 
                            '},' +
                        '"orientation" : {' +
                            '"x" : ' + allData[0].rotationQuaternion.x + ',' + 
                            '"y" : ' + allData[0].rotationQuaternion.y + ',' + 
                            '"z" : ' + allData[0].rotationQuaternion.z + ',' + 
                            '"w" : ' + allData[0].rotationQuaternion.w +
                            '}' +
                    '},'+
                    '"button_bumper" : '+ allData[0].buttons.bumper + ',' +
                    '"button_1" : '+ allData[0].buttons.button1 + ',' +
                    '"button_2" : '+ allData[0].buttons.button2 + ',' +
                    '"button_3" : '+ allData[0].buttons.button3 + ',' +
                    '"button_4" : '+ allData[0].buttons.button4 + ',' +
                    '"button_joy" : '+ allData[0].buttons.joystick + ',' +
                    '"button_center" : '+ allData[0].buttons.start + ',' +
                    '"joy_x" : ' + allData[0].joystick.y + ',' +
                    '"joy_y" : ' + allData[0].joystick.x + ',' +
                    '"trigger" : ' + allData[0].trigger  +
                '}' +        
            '}';

    var msg = JSON.parse(msgS);


    gazebo.publish(type, topic , msg);

})

Code explained

The first lines load the Gazebo C++ and the node-sixense modules into the Node V8 script engine, and creates and instance of the Gazebo class.

var hydra =  require('node-sixense');  
var gazebojs = require('gazebojs');
var gazebo = new gazebojs.Gazebo();

We then set the message type and the topic:

var type = "gazebo.msgs.Hydra"
var topic = "~/hydra"

We then initialize sixense and for every new data we call the sixenseGetAllNewestDataAsync callback function which publishes the hydra message.

hydra.sixenseInit();
hydra.sixenseSetActiveBase(0);
hydra.sixenseGetAllNewestDataAsync(function (error, allData)
{    
    var msgS =

...

    var msg = JSON.parse(msgS);
    gazebo.publish(type, topic , msg);
});

Troubleshooting

In case of offsets, the axis of the controller have been switched in multiple places in order to be similar to the output of the hydra plugin version. If needed otherwise these can be easily changed from the source code.