From e6cd0376f3d5ef3d8f5d1ca225ccfb7af7f169e2 Mon Sep 17 00:00:00 2001 From: Christian Fritz Date: Mon, 12 Sep 2016 21:11:27 -0700 Subject: [PATCH 1/2] Further unifying the interfaces Implemented uint64 and int64 support Now generating all messages and services on the fly on start (if requested) - message.getAll works and is very fast (takes less than a second) - Fixes #14. Corrected implemention for [U]Int64 in on-the-fly approach Using bignum for [u]int64 if available, otherwise usual javascript numbers are used but a warning is printed if numbers are sent or received that are outside of the precise range of JS for integers (>= 1e15). --- example_turtle.js | 82 +++++++++++++++++++---------- index.js | 82 +++++------------------------ package.json | 2 +- utils/fields.js | 131 ++++++++++++++++++++++++++++++++++++++++------ utils/messages.js | 114 ++++++++++++++++++++++++++++------------ utils/packages.js | 73 ++++++++++++++++++++++---- 6 files changed, 328 insertions(+), 156 deletions(-) diff --git a/example_turtle.js b/example_turtle.js index f8c3889..c730704 100644 --- a/example_turtle.js +++ b/example_turtle.js @@ -1,4 +1,4 @@ -/** +/** An example of using rosnodejs with turtlesim, incl. services, pub/sub, and actionlib. This example uses the on-demand generated messages. @@ -9,16 +9,7 @@ let rosnodejs = require('./index.js'); const ActionClient = require('./lib/ActionClient.js'); -rosnodejs.initNode('/my_node', { - messages: [ - 'turtlesim/Pose', - 'turtle_actionlib/ShapeActionGoal', - 'turtle_actionlib/ShapeActionFeedback', - 'turtle_actionlib/ShapeActionResult', - 'geometry_msgs/Twist', - ], - services: ["turtlesim/TeleportRelative"] -}).then((rosNode) => { +rosnodejs.initNode('/my_node', {onTheFly: true}).then((rosNode) => { // get list of existing publishers, subscribers, and services rosNode._node._masterApi.getSystemState("/my_node").then((data) => { @@ -30,11 +21,11 @@ rosnodejs.initNode('/my_node', { const TeleportRelative = rosnodejs.require('turtlesim').srv.TeleportRelative; const teleport_request = new TeleportRelative.Request({ - linear: -1, + linear: -1, angular: 0.0 }); - let serviceClient = rosNode.serviceClient("/turtle1/teleport_relative", + let serviceClient = rosNode.serviceClient("/turtle1/teleport_relative", "turtlesim/TeleportRelative"); rosNode.waitForService(serviceClient.getService(), 2000) @@ -52,17 +43,17 @@ rosnodejs.initNode('/my_node', { // --------------------------------------------------------- // Subscribe rosNode.subscribe( - '/turtle1/pose', + '/turtle1/pose', 'turtlesim/Pose', (data) => { console.log('pose', data); }, {queueSize: 1, - throttleMs: 1000}); + throttleMs: 1000}); // --------------------------------------------------------- // Publish - // equivalent to: + // equivalent to: // rostopic pub /turtle1/cmd_vel geometry_msgs/Twist '[1, 0, 0]' '[0, 0, 0]' let cmd_vel = rosNode.advertise('/turtle1/cmd_vel','geometry_msgs/Twist', { queueSize: 1, @@ -85,17 +76,54 @@ rosnodejs.initNode('/my_node', { // wait two seconds for previous example to complete setTimeout(function() { - let shapeActionGoal = rosnodejs.require('turtle_actionlib').msg.ShapeActionGoal; - let ac = new ActionClient({ - type: "turtle_actionlib/ShapeAction", - actionServer: "/turtle_shape" + let shapeActionGoal = rosnodejs.require('turtle_actionlib').msg.ShapeActionGoal; + let ac = new ActionClient({ + type: "turtle_actionlib/ShapeAction", + actionServer: "/turtle_shape" + }); + ac.sendGoal(new shapeActionGoal({ + goal: { + edges: 3, + radius: 1 + } + })); + }, 2000); + + // --------------------------------------------------------- + // test int64 + uint64 + + rosNode.subscribe( + '/int64', + 'std_msgs/Int64', + (data) => { + console.log('int64', data); + }, + {queueSize: 1, + throttleMs: 1000}); + + let int64pub = rosNode.advertise('/int64','std_msgs/Int64', { + queueSize: 1, + latching: true, + throttleMs: 9 }); - ac.sendGoal(new shapeActionGoal({ - goal: { - edges: 3, - radius: 1 - } - })); - }, 2000); + const Int64 = rosnodejs.require('std_msgs').msg.Int64; + int64pub.publish(new Int64({ data: "429496729456789012" })); + + rosNode.subscribe( + '/uint64', + 'std_msgs/UInt64', + (data) => { + console.log('uint64', data); + }, + {queueSize: 1, + throttleMs: 1000}); + + let uint64pub = rosNode.advertise('/uint64','std_msgs/UInt64', { + queueSize: 1, + latching: true, + throttleMs: 9 + }); + const UInt64 = rosnodejs.require('std_msgs').msg.UInt64; + uint64pub.publish(new UInt64({ data: "9223372036854775807" })); }); diff --git a/index.js b/index.js index 05be817..791378e 100644 --- a/index.js +++ b/index.js @@ -133,15 +133,19 @@ let Rosnodejs = { rosNode = new RosNode(nodeName, rosMasterUri); return new Promise((resolve, reject) => { - this.use(options.messages, options.services).then(() => { - - const connectedToMasterCallback = () => { - Logging.initializeOptions(this, options.logging); - resolve(this.getNodeHandle()); - }; - + const connectedToMasterCallback = () => { + Logging.initializeOptions(this, options.logging); + resolve(this.getNodeHandle()); + }; + + if (options.onTheFly) { + // generate definitions for all messages and services + messages.getAll(function() { + _checkMasterHelper(connectedToMasterCallback, 0); + }); + } else { _checkMasterHelper(connectedToMasterCallback, 0); - }); + } }) .catch((err) => { log.error('Error: ' + err); @@ -188,68 +192,6 @@ let Rosnodejs = { return rtv; }, - /** create message classes and services classes for all the given - * types before calling callback */ - use(messages, services) { - const self = this; - return new Promise((resolve, reject) => { - self._useMessages(messages) - .then(() => { return self._useServices(services); }) - .then(() => { resolve(); }); - }); - }, - - /** create message classes for all the given types */ - _useMessages(types) { - const self = this; - - // make sure required types are available - [ - // for action lib: - 'actionlib_msgs/GoalStatusArray', - 'actionlib_msgs/GoalID', - // for logging: - 'rosgraph_msgs/Log', - ].forEach(function(type) { - if (!self.checkMessage(type)) { - // required message definition not available yet, load it - // on-demand - types.unshift(type); - } - }); - - if (!types || types.length == 0) { - return Promise.resolve(); - } - var count = types.length; - return new Promise((resolve, reject) => { - types.forEach(function(type) { - messages.getMessage(type, function(error, Message) { - if (--count == 0) { - resolve(); - } - }); - }); - }); - }, - - /** create service classes for all the given types */ - _useServices(types) { - if (!types || types.length == 0) { - return Promise.resolve(); - } - var count = types.length; - return new Promise((resolve, reject) => { - types.forEach(function(type) { - messages.getService(type, function() { - if (--count == 0) { - resolve(); - } - }); - }); - }); - }, - /** * @return {NodeHandle} for initialized node */ diff --git a/package.json b/package.json index 1141920..c3561a7 100644 --- a/package.json +++ b/package.json @@ -27,7 +27,7 @@ "xmlrpc": "chfritz/node-xmlrpc", "walker" : "1.0.7", "md5" : "2.1.0", - "async" : "0.1.22", + "async" : "2.0.1", "bunyan": "1.8.1" } } diff --git a/utils/fields.js b/utils/fields.js index 6a991b7..f1c84e9 100644 --- a/utils/fields.js +++ b/utils/fields.js @@ -2,6 +2,17 @@ var fields = exports; +// Try to require bignum package. If available use it for int64 values. +var bignum; +try { + bignum = require('bignum'); +} catch (e) {} + +var BIGNUM_WARNING = `\n** (rosnodejs) WARNING: Using int64 messages without the \ +bignum package is not recommended. You seem to be dealing with numbers larger \ +than 1e15 so precision will be lost!\n`; + + /* map of all primitive types and their default values */ var map = { 'char': '', @@ -40,7 +51,7 @@ fields.isPrimitive = function(fieldType) { return (fields.primitiveTypes.indexOf(fieldType) >= 0); }; -var isArrayRegex = /.*\[*\]$/; +var isArrayRegex = /\[*\]$/; fields.isArray = function(fieldType) { return (fieldType.match(isArrayRegex) !== null); }; @@ -82,10 +93,20 @@ fields.parsePrimitive = function(fieldType, fieldValue) { parsedValue = Math.abs(parsedValue); } else if (fieldType === 'int64') { - throwUnsupportedInt64Exception(); + if (bignum) { + parsedValue = bignum(fieldValue); + } else { + parsedValue = parseInt(fieldValue); + } } else if (fieldType === 'uint64') { - throwUnsupportedInt64Exception(); + if (bignum) { + parsedValue = bignum(fieldValue); + parsedValue = parsedValue.abs(); + } else { + parsedValue = parseInt(fieldValue); + parsedValue = Math.abs(parsedValue); + } } else if (fieldType === 'float32') { parsedValue = parseFloat(fieldValue); @@ -108,7 +129,7 @@ fields.parsePrimitive = function(fieldType, fieldValue) { } var secs = parseInt(now/1000); var nsecs = (now % 1000) * 1000; - + parsedValue.secs = secs; parsedValue.nsecs = nsecs; } @@ -117,7 +138,7 @@ fields.parsePrimitive = function(fieldType, fieldValue) { return parsedValue; }; -fields.serializePrimitive = +fields.serializePrimitive = function(fieldType, fieldValue, buffer, bufferOffset) { if (fieldType === 'bool') { buffer.writeUInt8(fieldValue, bufferOffset); @@ -141,10 +162,56 @@ fields.serializePrimitive = buffer.writeUInt32LE(fieldValue, bufferOffset); } else if (fieldType === 'int64') { - throwUnsupportedInt64Exception(); + if (bignum) { + if (!bignum.isBigNum(fieldValue)) { + fieldValue = bignum(fieldValue); + } + var buf = fieldValue.toBuffer({ + endian: "little", + size: 8 + }); + buf.copy(buffer, bufferOffset); + } else { + var high = Math.trunc(fieldValue / 0x100000000); + var low = fieldValue % 0x100000000; + if (fieldValue < 0) { + // constructing two's complement representation + // fieldValue = 0x10000000000000000 + fieldValue; + // ^ doesn't work, because JS precision is only 53 bits + // we'll need to work directly on the parts: + // both high and low are negative + high = 0x100000000 + high; + if (low != 0) { + high -= 1; + } + low = 0x100000000 + low; + } + // yes, UInt, we've already constructed the two's complement + buffer.writeUInt32LE(low, bufferOffset); + buffer.writeUInt32LE(high, bufferOffset+4); + if (fieldValue >= 1e15) { + console.warn(BIGNUM_WARNING); + } + } } else if (fieldType === 'uint64') { - throwUnsupportedInt64Exception(); + if (bignum) { + if (!bignum.isBigNum(fieldValue)) { + fieldValue = bignum(fieldValue); + } + var high = fieldValue.div(0x100000000).toNumber(); + var low = fieldValue.mod(0x100000000).toNumber(); + buffer.writeUInt32LE(low, bufferOffset); + buffer.writeUInt32LE(high, bufferOffset+4); + } else { + var high = Math.trunc(fieldValue / 0x100000000); + var low = fieldValue % 0x100000000; + buffer.writeUInt32LE(low, bufferOffset); + buffer.writeUInt32LE(high, bufferOffset+4); + if (fieldValue >= 1e15) { + console.warn(BIGNUM_WARNING); + } + } } else if (fieldType === 'float32') { buffer.writeFloatLE(fieldValue, bufferOffset); @@ -161,7 +228,7 @@ fields.serializePrimitive = buffer.writeUInt32LE(fieldValue.secs, bufferOffset); buffer.writeUInt32LE(fieldValue.nsecs, bufferOffset+4); } - + } fields.deserializePrimitive = function(fieldType, buffer, bufferOffset) { @@ -189,10 +256,48 @@ fields.deserializePrimitive = function(fieldType, buffer, bufferOffset) { fieldValue = buffer.readUInt32LE(bufferOffset); } else if (fieldType === 'int64') { - throwUnsupportedInt64Exception(); + if (bignum) { + // using bignum package + var buf = new Buffer(8); + buffer.copy(buf, 0, bufferOffset, bufferOffset+8); + fieldValue = bignum.fromBuffer(buf, { + endian: "little", + size: 8 + }); + } else { + // no luck, returning an inaccurate 53bit js number + var low = buffer.readUInt32LE(bufferOffset); + var high = buffer.readUInt32LE(bufferOffset+4); + if (high >= 0x10000000) { + // the value is negative + if (low != 0) { + high += 1; + } + high = high - 0x100000000; + low = low - 0x100000000; + } + fieldValue = high * 0x100000000 + low; + if (fieldValue >= 1e15) { + console.warn(BIGNUM_WARNING); + } + } } else if (fieldType === 'uint64') { - throwUnsupportedInt64Exception(); + if (bignum) { + var buf = new Buffer(8); + buffer.copy(buf, 0, bufferOffset, bufferOffset+8); + fieldValue = bignum.fromBuffer(buf, { + endian: "little", + size: 8 + }); + } else { + var low = buffer.readUInt32LE(bufferOffset); + var high = buffer.readUInt32LE(bufferOffset+4); + fieldValue = high * 0x100000000 + low; + if (fieldValue >= 1e15) { + console.warn(BIGNUM_WARNING); + } + } } else if (fieldType === 'float32') { fieldValue = buffer.readFloatLE(bufferOffset); @@ -311,9 +416,3 @@ fields.getMessageSize = function(message) { return messageSize; } - -function throwUnsupportedInt64Exception() { - var error = new Error('int64 and uint64 are currently unsupported field types. See https://github.com/baalexander/rosnodejs/issues/2'); - throw error; -} - diff --git a/utils/messages.js b/utils/messages.js index 14baf9a..4fefcc0 100644 --- a/utils/messages.js +++ b/utils/messages.js @@ -23,7 +23,12 @@ messages.getPackageFromRegistry = function(packagename) { /** ensure the handler for this message type is in the registry, * create it if it doesn't exist */ messages.getMessage = function(messageType, callback) { - getMessageFromPackage(messageType, "msg", callback); + var fromRegistry = getMessageFromRegistry(messageType, ["msg"]); + if (fromRegistry) { + callback(null, fromRegistry); + } else { + getMessageFromPackage(messageType, "msg", callback); + } } /** ensure the handler for requests for this service type is in the @@ -32,25 +37,65 @@ messages.getService = function(messageType, callback) { getMessageFromPackage(messageType, "srv", callback); } +/** get all message and service definitions, from all packages */ +messages.getAll = function(callback) { + packages.getAllPackages(function(err, packageDirectories) { + // for each found package: + async.eachSeries(packageDirectories, function(directory, packageCallback) { + var packageName = path.basename(directory); + // for both msgs and srvs: + async.eachSeries(["msg", "srv"], function(type, typeCallback) { + // read the package's respective sub directory + var files = []; + fs.readdir(path.join(directory, type), function(err, files) { + // add each found msg/srv definition + async.eachSeries(files, function(file, fileCallback) { + var fileName = path.basename(file, "." + type); + var messageType = packageName + "/" + fileName; + + // check whether we already computed it due to dependencies: + var cachehit = false; + if (type == "msg") { + cachehit = (getMessageFromRegistry(messageType, [type]) != undefined); + } else { + cachehit = (getMessageFromRegistry(messageType, [type, "Response"]) != undefined + && getMessageFromRegistry(messageType, [type, "Request"]) != undefined); + } + if (cachehit) { + fileCallback(); + } else { + var filePath = path.join(directory, type, file); + getMessageFromFile(messageType, filePath, type, function(err, message) { + fileCallback(); + }); + } + }, typeCallback); + }); + }, packageCallback); + }, callback); + }); +}; + + // --------------------------------------------------------- // Registry var registry = {}; -/* +/* registry looks like: - { 'packagename': + { 'packagename': { - msg: { + msg: { 'String': classdef, 'Pose': classdef, ... }, - srv: { Request: + srv: { Request: { 'SetBool': classdef, ... }, - Response: + Response: { 'SetBool': classdef, ... @@ -64,8 +109,8 @@ var registry = {}; /** @param messageType is the ROS message or service type, e.g. 'std_msgs/String' - @param type is from the set - [["msg"], ["srv","Request"], ["srv","Response"] + @param type is from the set + [["msg"], ["srv","Request"], ["srv","Response"] */ function getMessageFromRegistry(messageType, type) { var packageName = getPackageNameFromMessageType(messageType); @@ -90,10 +135,10 @@ function getMessageFromRegistry(messageType, type) { } } -/** +/** @param messageType is the ROS message or service type, e.g. 'std_msgs/String' - @param message is the message class definition + @param message is the message class definition @param type is from the set "msg", "srv" @param (optional) subtype \in { "Request", "Response" } */ @@ -116,7 +161,7 @@ function setMessageInRegistry(messageType, message, type, subtype) { } var serviceType = subtype; // "Request" or "Response" - registry[packageName][type][messageName][serviceType] = message; + registry[packageName][type][messageName][serviceType] = message; } } @@ -138,7 +183,7 @@ function getMessageFromPackage(messageType, type, callback) { function getMessageFromFile(messageType, filePath, type, callback) { var packageName = getPackageNameFromMessageType(messageType) , messageName = getMessageNameFromMessageType(messageType); - + var details = { messageType : messageType , messageName : messageName @@ -159,7 +204,9 @@ function getMessageFromFile(messageType, filePath, type, callback) { response = buildMessageClass(details.response); setMessageInRegistry(messageType, request, type, "Request"); setMessageInRegistry(messageType, response, type, "Response"); - callback(null, message); + callback(); + // ^ no value needed for services, since they cannot appear nested + // still pretty hacky } else { console.log("unknown service", type); } @@ -175,7 +222,7 @@ function parseMessageFile(fileName, details, type, callback) { } else { extractFields( - content, details, type, function(error, aggregate) { + content, details, function(error, aggregate) { if (error) { callback(error); } else { @@ -195,8 +242,14 @@ function parseMessageFile(fileName, details, type, callback) { }; rtv.request.constants = aggregate[0].constants; rtv.request.fields = aggregate[0].fields; - rtv.response.constants = aggregate[1].constants; - rtv.response.fields = aggregate[1].fields; + if (aggregate.length > 1) { + // if there is a response: + rtv.response.constants = aggregate[1].constants; + rtv.response.fields = aggregate[1].fields; + } else { + rtv.response.constants = []; + rtv.response.fields = []; + } rtv.request.md5 = rtv.response.md5 = calculateMD5(rtv, "srv"); callback(null, rtv); } else { @@ -221,7 +274,7 @@ function calculateMD5(details, type) { var constants = part.constants.map(function(field) { return field.type + ' ' + field.name + '=' + field.value; }).join('\n'); - + var fields = part.fields.map(function(field) { if (field.messageType) { return field.messageType.md5 + ' ' + field.name; @@ -230,7 +283,7 @@ function calculateMD5(details, type) { return field.type + ' ' + field.name; } }).join('\n'); - + message += constants; if (message.length > 0 && fields.length > 0) { message += "\n"; @@ -247,7 +300,7 @@ function calculateMD5(details, type) { text = getMD5text(details); } else if (type == "srv") { text = getMD5text(details.request); - text += getMD5text(details.response); + text += getMD5text(details.response); } else { console.log("calculateMD5: Unknown type", type); return null; @@ -256,7 +309,7 @@ function calculateMD5(details, type) { return md5(text); } -function extractFields(content, details, type, callback) { +function extractFields(content, details, callback) { function parsePart(lines, callback) { var constants = [] , fields = [] @@ -335,7 +388,7 @@ function extractFields(content, details, type, callback) { callback(); } } - else if (fieldsUtil.isMessage(fieldType)) { + else { fieldType = normalizeMessageType(fieldType, details.packageName); messages.getMessage(fieldType, function(error, messageType) { fields.push({ @@ -351,7 +404,7 @@ function extractFields(content, details, type, callback) { } } - async.forEachSeries(lines, parseLine, function(error) { + async.eachSeries(lines, parseLine, function(error) { if (error) { callback(error); } @@ -360,9 +413,6 @@ function extractFields(content, details, type, callback) { } }); } - - - var lines = content.split('\n'); @@ -376,7 +426,7 @@ function extractFields(content, details, type, callback) { } return memo; }, [[]]); - + async.map(parts, parsePart, function(err, aggregate) { callback(err, aggregate); }); @@ -446,14 +496,14 @@ function buildMessageClass(details) { } if (details.fields) { - details.fields.forEach(function(field) { + details.fields.forEach(function(field) { if (field.messageType) { // sub-message class - that[field.name] = + that[field.name] = new (field.messageType)(values ? values[field.name] : undefined); } else { // simple value - that[field.name] = values ? values[field.name] : + that[field.name] = values ? values[field.name] : (field.value || fieldsUtil.getDefaultValue(field.type)); } }); @@ -467,7 +517,7 @@ function buildMessageClass(details) { Message.md5sum = Message.prototype.md5sum = function() { return this.md5; }; - Message.Constants = Message.constants + Message.Constants = Message.constants = Message.prototype.constants = details.constants; Message.fields = Message.prototype.fields = details.fields; Message.serialize = Message.prototype.serialize = @@ -495,15 +545,13 @@ function getPackageNameFromMessageType(messageType) { : ''; } -var isNormalizedMessageType = /.*\/.*$/; function normalizeMessageType(messageType, packageName) { var normalizedMessageType = messageType; if (messageType == "Header") { normalizedMessageType = getMessageType("std_msgs", messageType); - } else if (messageType.match(isNormalizedMessageType) === null) { + } else if (messageType.indexOf("/") < 0) { normalizedMessageType = getMessageType(packageName, messageType); } - return normalizedMessageType; } diff --git a/utils/packages.js b/utils/packages.js index 45c0383..cd7506d 100644 --- a/utils/packages.js +++ b/utils/packages.js @@ -1,6 +1,15 @@ -var fs = require('fs') - , path = require('path') - , walker = require('walker'); +var fs = require('fs'); +var path = require('path'); +var walker = require('walker'); +var async = require('async'); + +// TODO: make this sync, e.g., using: +// https://www.npmjs.com/package/fs-walker +// https://www.npmjs.com/package/walk +// +// OR: just load all packages incl. messages and services upfront. We don't want +// to hold things up in the middle of production by being sync. So maybe it's +// better to just load everything up front without asking. function walk(directory, symlinks) { @@ -76,7 +85,7 @@ function findPackageInDirectory(directory, packageName, callback) { }) .on('end', function() { if (!found) { - var error = + var error = new Error('ENOTFOUND - Package ' + packageName + ' not found'); error.name = 'PackageNotFoundError'; callback(error); @@ -86,7 +95,7 @@ function findPackageInDirectory(directory, packageName, callback) { function findPackageInDirectoryChain(directories, packageName, callback) { if (directories.length < 1) { - var error = + var error = new Error('ENOTFOUND - Package ' + packageName + ' not found'); error.name = 'PackageNotFoundError'; callback(error); @@ -97,7 +106,7 @@ function findPackageInDirectoryChain(directories, packageName, callback) { if (error) { if (error.name === 'PackageNotFoundError') { // Recursive call, try in next directory - return findPackageInDirectoryChain(directories, + return findPackageInDirectoryChain(directories, packageName, callback); } else { @@ -113,14 +122,60 @@ function findPackageInDirectoryChain(directories, packageName, callback) { // --------------------------------------------------------- -// Implements the same crawling algorithm as rospack find -// See http://ros.org/doc/api/rospkg/html/rospack.html +var cache = {}; + +// Implements the same crawling algorithm as rospack find. See +// http://docs.ros.org/independent/api/rospkg/html/rospack.html#crawling-algorithm // packages = {}; exports.findPackage = function(packageName, callback) { + var directory = cache[packageName]; + if (directory) { + callback(null, directory); + return; + } var rosRoot = process.env.ROS_ROOT; var packagePath = process.env.ROS_PACKAGE_PATH var rosPackagePaths = packagePath.split(':') var directories = [rosRoot].concat(rosPackagePaths); - return findPackageInDirectoryChain(directories, packageName, callback); + return findPackageInDirectoryChain(directories, packageName, + function(err, directory) { + cache[packageName] = directory; + callback(err, directory); + }); } +// --------------------------------------------------------- +// Logic for iterating over *all* packages + +function forEachPackageInDirectory(directory, list, onEnd) { + fs.access(directory, fs.R_OK, (err) => { + if (!err) { + walk(directory) + .on('package', function(name, dir) { + list.push(dir); + }) + .on('end', onEnd); + } else { + onEnd(); + } + }); +} + +/** get list of package directories */ +exports.getAllPackages = function(done) { + var rosRoot = process.env.ROS_ROOT; + var packagePath = process.env.ROS_PACKAGE_PATH + var rosPackagePaths = packagePath.split(':') + var directories = [rosRoot].concat(rosPackagePaths); + async.reduce(directories, [], function(memo, directory, callback) { + forEachPackageInDirectory(directory, memo, function() { + callback(null, memo); + }); + }, function(err, directories) { + directories.forEach(function(directory) { + var packageName = path.basename(directory); + cache[packageName] = directory; + }); + done(err, directories); + }); +} From 1a22dd6ec7b44d6df11dd9f68b1a076e5d502d3f Mon Sep 17 00:00:00 2001 From: Christian Fritz Date: Sat, 24 Sep 2016 15:48:26 -0700 Subject: [PATCH 2/2] ActionClient: export it and added cancel - Added getActionClient to Rosnodejs (previously the action client wasn't exported in any way) - ActionClient: added "cancel" function --- example_turtle.js | 7 ++++--- index.js | 24 ++++++++++++++++++++++++ lib/ActionClient.js | 27 ++++++++++++++++++++------- 3 files changed, 48 insertions(+), 10 deletions(-) diff --git a/example_turtle.js b/example_turtle.js index c730704..157e32f 100644 --- a/example_turtle.js +++ b/example_turtle.js @@ -7,8 +7,6 @@ 'use strict'; let rosnodejs = require('./index.js'); -const ActionClient = require('./lib/ActionClient.js'); - rosnodejs.initNode('/my_node', {onTheFly: true}).then((rosNode) => { // get list of existing publishers, subscribers, and services @@ -77,7 +75,7 @@ rosnodejs.initNode('/my_node', {onTheFly: true}).then((rosNode) => { // wait two seconds for previous example to complete setTimeout(function() { let shapeActionGoal = rosnodejs.require('turtle_actionlib').msg.ShapeActionGoal; - let ac = new ActionClient({ + let ac = rosnodejs.getActionClient({ type: "turtle_actionlib/ShapeAction", actionServer: "/turtle_shape" }); @@ -87,6 +85,9 @@ rosnodejs.initNode('/my_node', {onTheFly: true}).then((rosNode) => { radius: 1 } })); + setTimeout(function() { + ac.cancel(); + }, 1000); }, 2000); // --------------------------------------------------------- diff --git a/index.js b/index.js index 791378e..edf336a 100644 --- a/index.js +++ b/index.js @@ -29,6 +29,7 @@ const LogFormatter = require('./utils/log/LogFormatter.js'); const RosNode = require('./lib/RosNode.js'); const NodeHandle = require('./lib/NodeHandle.js'); const Logging = require('./lib/Logging.js'); +const ActionClient = require('./lib/ActionClient.js'); msgUtils.findMessageFiles(); @@ -216,6 +217,29 @@ let Rosnodejs = { console: ConsoleLogStream, ros: RosLogStream } + }, + + + //------------------------------------------------------------------ + // ActionLib + //------------------------------------------------------------------ + + /** + Get an action client for a given type and action server. + + Example: + let ac = rosNode.getActionClient({ + type: "turtle_actionlib/ShapeAction", + actionServer: "/turtle_shape" + }); + let shapeActionGoal = + rosnodejs.require('turtle_actionlib').msg.ShapeActionGoal; + ac.sendGoal(new shapeActionGoal({ + goal: { edges: 3, radius: 1 } })); + */ + getActionClient(options) { + options.rosnodejs = Rosnodejs; + return new ActionClient(options); } } diff --git a/lib/ActionClient.js b/lib/ActionClient.js index 29bd73c..3911ebd 100644 --- a/lib/ActionClient.js +++ b/lib/ActionClient.js @@ -17,7 +17,6 @@ 'use strict'; -const rosnodejs = require('../index.js'); const timeUtils = require('../utils/time_utils.js'); let EventEmitter = require('events'); @@ -29,28 +28,29 @@ class ActionClient extends EventEmitter { this._actionServer = options.actionServer; - const nh = rosnodejs.nh; + this._rosnodejs = options.rosnodejs; + const nh = this._rosnodejs.getNodeHandle(); // FIXME: support user options for these parameters - this._goalPub = nh.advertise(this._actionServer + '/goal', + this._goalPub = nh.advertise(this._actionServer + '/goal', this._actionType + 'Goal', { queueSize: 1, latching: true }); - this._cancelPub = nh.advertise(this._actionServer + '/cancel', + this._cancelPub = nh.advertise(this._actionServer + '/cancel', 'actionlib_msgs/GoalID', { queueSize: 1, latching: true }); - this._statusSub = nh.subscribe(this._actionServer + '/status', + this._statusSub = nh.subscribe(this._actionServer + '/status', 'actionlib_msgs/GoalStatusArray', (msg) => { this._handleStatus(msg); }, { queueSize: 1 } ); - this._feedbackSub = nh.subscribe(this._actionServer + '/feedback', + this._feedbackSub = nh.subscribe(this._actionServer + '/feedback', this._actionType + 'Feedback', (msg) => { this._handleFeedback(msg); }, { queueSize: 1 } ); - this._statusSub = nh.subscribe(this._actionServer + '/result', + this._statusSub = nh.subscribe(this._actionServer + '/result', this._actionType + 'Result', (msg) => { this._handleResult(msg); }, { queueSize: 1 } ); @@ -97,6 +97,19 @@ class ActionClient extends EventEmitter { this._goals[goalId] = goal; this._goalPub.publish(goal); + return goal; + } + + /** Cancel the given goal. If none is given, send an empty goal message, + i.e. cancel all goals. See + http://wiki.ros.org/actionlib/DetailedDescription#The_Messages + */ + cancel(goal) { + if (!goal) { + let GoalID = this._rosnodejs.require('actionlib_msgs').msg.GoalID; + goal = new GoalID(); + } + this._cancelPub.publish(goal); } _generateGoalId() {