From 0567279eefc545b7f079e9ca9645fd1132d9bbe3 Mon Sep 17 00:00:00 2001 From: Kenny Yuan Date: Thu, 23 Nov 2017 14:01:41 +0800 Subject: [PATCH 1/2] make publisher be able to take-in a plain JavaScript object as message to be published --- index.js | 5 +- lib/message-translator.js | 109 ++++++++++++ lib/publisher.js | 17 +- lib/utility.js | 62 +++++++ rosidl_gen/templates/message.dot | 21 +++ test/publisher_array_setup.js | 2 +- test/test-message-translator-complex.js | 196 ++++++++++++++++++++++ test/test-message-translator-primitive.js | 68 ++++++++ 8 files changed, 472 insertions(+), 8 deletions(-) create mode 100644 lib/message-translator.js create mode 100644 lib/utility.js create mode 100644 test/test-message-translator-complex.js create mode 100644 test/test-message-translator-primitive.js diff --git a/index.js b/index.js index 82bb32e1..1e302b70 100644 --- a/index.js +++ b/index.js @@ -25,6 +25,7 @@ const path = require('path'); const QoS = require('./lib/qos.js'); const rclnodejs = require('bindings')('rclnodejs'); const validator = require('./lib/validator.js'); +const util = require('./lib/utility.js'); function inherits(target, source) { let properties = Object.getOwnPropertyNames(source.prototype); @@ -224,7 +225,9 @@ let rcl = { */ expandTopicName(topicName, nodeName, nodeNamespace) { return rclnodejs.expandTopicName(topicName, nodeName, nodeNamespace); - } + }, + + util: util, }; module.exports = rcl; diff --git a/lib/message-translator.js b/lib/message-translator.js new file mode 100644 index 00000000..47221eaf --- /dev/null +++ b/lib/message-translator.js @@ -0,0 +1,109 @@ +// Copyright (c) 2017 Intel Corporation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +'use strict'; + +const debug = require('debug')('rclnodejs:message-translator'); + +/* eslint-disable max-depth */ +function copyMsgObject(msg, obj) { + if (typeof obj === 'object') { + for (let i in obj) { + if (typeof msg[i] !== 'undefined') { + const type = typeof obj[i]; + if (type === 'string' || type === 'number' || type === 'boolean') { + // A primitive-type value + msg[i] = obj[i]; + } else if (Array.isArray(obj[i])) { // TODO(Kenny): deal with TypedArray + // It's an array + if (typeof obj[i][0] === 'object') { + // It's an array of objects: converting to ROS message objects + + // 1. Extract the element-type first + const t = new MessageTranslator(msg[i].classType.elementType); + // 2. Build the array by translate every elements + let msgArray = []; + obj[i].forEach((o) => { + msgArray.push(t.toROSMessage(o)); + }); + // 3. Assign + msg[i].fill(msgArray); + } else { + // It's an array of primitive-type elements + msg[i] = obj[i]; + } + } else { + // Proceed further of this object + copyMsgObject(msg[i], obj[i]); // TODO(Kenny): deal with TypedArray + } + } else { + // Extra fields in obj (but not in msg) + } + } // for + } +} +/* eslint-enable max-depth */ + +class MessageTranslator { + constructor(typeClass) { + this._typeClass = typeClass; + } + + static toPlainObject(message) { + // TODO(Kenny): deal with primitive-type array (convert to TypedArray) + + if (message.isROSArray) { + // It's a ROS message array + // Note: there won't be any JavaScript array in message + let array = []; + const data = message.data; + data.forEach((e) => { + // Translate every elements + array.push(MessageTranslator.toPlainObject(e)); + }); + return array; + // eslint-disable-next-line no-else-return + } else { + // It's a ROS message + const def = message.classType.ROSMessageDef; + let obj = {}; + for (let i in def.fields) { + const name = def.fields[i].name; + if (def.fields[i].type.isPrimitiveType) { + // Direct assignment + obj[name] = message[name]; + } else { + // Proceed further + obj[name] = MessageTranslator.toPlainObject(message[name]); + } + } + return obj; + } + } + + toROSMessage(obj) { + let msg = new this._typeClass(); + const type = typeof obj; + if (type === 'string' || type === 'number' || type === 'boolean') { + msg.data = obj; + } else if (type === 'object') { + copyMsgObject(msg, obj); + } + return msg; + } +} + +module.exports = { + MessageTranslator: MessageTranslator +}; diff --git a/lib/publisher.js b/lib/publisher.js index 2016614e..5962cc13 100644 --- a/lib/publisher.js +++ b/lib/publisher.js @@ -17,6 +17,7 @@ const rclnodejs = require('bindings')('rclnodejs'); const debug = require('debug')('rclnodejs:publisher'); const Entity = require('./entity.js'); +const {MessageTranslator} = require('./message-translator.js'); /** * @class - Class representing a Publisher in ROS @@ -27,6 +28,7 @@ class Publisher extends Entity { constructor(handle, nodeHandle, typeClass, topic, qos) { super(handle, typeClass, qos); this._topic = topic; + this.translator = new MessageTranslator(typeClass); } /** @@ -42,12 +44,15 @@ class Publisher extends Entity { * @return {undefined} */ publish(message) { - // TODO(minggang): Support to convert a plain JavaScript value/object to a ROS message, - // thus we can invoke this function like: publisher.publish('hello world'). - let rclMessage = message; - if (!(message instanceof this._typeClass)) { - rclMessage = new this._typeClass(); - rclMessage.data = message; + let rclMessage; + if (message instanceof this._typeClass) { + rclMessage = message; + } else { + // Use translator to enable call by plain object/number/string argument + // e.g. publisher.publish(3.14); + // publisher.publish('The quick brown fox...'); + // publisher.publish({linear: {x: 0, y: 1, z: 2}, ...}); + rclMessage = this.translator.toROSMessage(message); } let rawRosMessage = rclMessage.serialize(); diff --git a/lib/utility.js b/lib/utility.js new file mode 100644 index 00000000..1b721ed6 --- /dev/null +++ b/lib/utility.js @@ -0,0 +1,62 @@ +// Copyright (c) 2017 Intel Corporation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +'use strict'; + +/* eslint-disable max-depth */ +function deepEqual(msg, obj) { + if (typeof obj !== 'object') { + return msg === obj; + } + + for (let i in obj) { + const m = msg[i]; + const o = obj[i]; + + if (typeof m === 'undefined') { + return false; + } + + const t = typeof o; + if (t === 'string' || t === 'number' || t === 'boolean') { + // A primitive value + if (m !== o) { + return false; + } + } else if (m.isROSArray) { + // An array of message objects + if (!Array.isArray(o)) return false; + + const data = m.data; + for (let j in data) { + if (!deepEqual(data[j], o[j])) { + return false; + } + } + } else { + // A regular message object + const equal = deepEqual(m, o); + if (!equal) { + return false; + } + } + } + + return true; +} +/* eslint-enable max-depth */ + +module.exports = { + deepEqual: deepEqual, +}; diff --git a/rosidl_gen/templates/message.dot b/rosidl_gen/templates/message.dot index f2eb56ce..c724dfed 100644 --- a/rosidl_gen/templates/message.dot +++ b/rosidl_gen/templates/message.dot @@ -18,6 +18,7 @@ let arrayWrapper = it.spec.msgName + 'ArrayWrapper'; let refObjectType = it.spec.msgName + 'RefStruct'; let refObjectArrayType = it.spec.msgName + 'RefStructArray'; let refArrayType = it.spec.msgName + 'RefArray'; +const compactMsgDefJSON = JSON.stringify(JSON.parse(it.json)); if (it.spec.fields.length === 0) { /* Following rosidl_generator_c style, put a bool member named '_dummy' */ @@ -326,6 +327,14 @@ class {{=objectWrapper}} { {{?}} {{~}} } + + get classType() { + return {{=objectWrapper}}; + } + + static get ROSMessageDef() { + return {{=compactMsgDefJSON}}; + } } // Define the wrapper of array class. @@ -445,6 +454,18 @@ class {{=arrayWrapper}} { {{=objectWrapper}}.freeStruct(refObjectArray[index]); } } + + static get elementType() { + return {{=objectWrapper}}; + } + + get isROSArray() { + return true; + } + + get classType() { + return {{=arrayWrapper}}; + } } {{? it.spec.constants != undefined && it.spec.constants.length}} diff --git a/test/publisher_array_setup.js b/test/publisher_array_setup.js index bdbcf721..532fc7e3 100644 --- a/test/publisher_array_setup.js +++ b/test/publisher_array_setup.js @@ -48,7 +48,7 @@ rclnodejs.init().then(function() { node.destroy(); rclnodejs.shutdown(); process.exit(0); - }); + }); }).catch(function(err) { console.log(err); }); diff --git a/test/test-message-translator-complex.js b/test/test-message-translator-complex.js new file mode 100644 index 00000000..3a93288f --- /dev/null +++ b/test/test-message-translator-complex.js @@ -0,0 +1,196 @@ +// Copyright (c) 2017 Intel Corporation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +'use strict'; + +const assert = require('assert'); +const rclnodejs = require('../index.js'); + +describe('Rclnodejs message translation: complex types', function() { + this.timeout(60 * 1000); + + before(function() { + return rclnodejs.init(); + }); + + after(function() { + rclnodejs.shutdown(); + }); + + /* eslint-disable camelcase */ + /* eslint-disable key-spacing */ + /* eslint-disable comma-spacing */ + + [ + { + pkg: 'std_msgs', type: 'MultiArrayDimension', + values: [ + {label: 'label name 0', size: 256, stride: 4, }, + {label: 'label name 1', size: 48, stride: 8, }, + ] + }, + { + pkg: 'geometry_msgs', type: 'Point', + values: [ + {x: 1.5, y: 2.75, z: 3.0, }, + {x: -1.5, y: 2.75, z: -6.0, }, + ] + }, + { + pkg: 'geometry_msgs', type: 'Point32', + values: [ + {x: 1.5, y: 2.75, z: 3.0, }, + {x: -1.5, y: 2.75, z: -6.0, }, + ] + }, + { + pkg: 'geometry_msgs', type: 'Quaternion', + values: [ + {x: 1.5, y: 2.75, z: 3.0, w: 1.0, }, + {x: -1.5, y: 2.75, z: -6.0, w: -1.0, }, + ] + }, + { + pkg: 'geometry_msgs', type: 'Pose', + values: [ + { + position: {x: 1.5, y: 2.75, z: 3.0, }, + orientation: {x: 1.5, y: 2.75, z: 3.0, w: 1.0, }, + }, + { + position: {x: 11.5, y: 112.75, z: 9.0, }, + orientation: {x: 31.5, y: 21.5, z: 7.5, w: 1.5, }, + }, + ] + }, + { + pkg: 'geometry_msgs', type: 'Transform', + values: [ + { + translation: {x: 1.5, y: 2.75, z: 3.0, }, + rotation: {x: 1.5, y: 2.75, z: 3.0, w: 1.0, }, + }, + { + translation: {x: 11.5, y: 112.75, z: 9.0, }, + rotation: {x: 31.5, y: 21.5, z: 7.5, w: 1.5, }, + }, + ] + }, + { + pkg: 'sensor_msgs', type: 'JointState', + values: [ + { + header: { + stamp: {sec: 11223, nanosec: 44556}, + frame_id: '1234567x', + }, + name: ['Willy', 'Tacky'], + position: [1, 7, 3, 4, 2, 2, 8], + velocity: [8, 9, 6, 4], + effort: [1, 0, 2, 6, 7], + }, + { + header: { + stamp: {sec: 11223, nanosec: 44556}, + frame_id: '0001234567x', + }, + name: ['Goodly', 'Lovely', 'Angel', 'Neatly', 'Perfect', 'Tacky'], + position: [1, 23, 7, 3, 4, 2, 2, 8], + velocity: [1, 9, 8, 9, 6, 4], + effort: [2, 1, 1, 0, 2, 6, 7], + }, + ] + }, + { + pkg: 'std_msgs', type: 'Float32MultiArray', + values: [ + { + layout: { + dim: [ + {label: 'height', size: 480, stride: 921600}, + {label: 'width', size: 640, stride: 1920}, + {label: 'channel', size: 3, stride: 8}, + ], + data_offset: 1024, + }, + data: [1.0, 2.0, 3.0, 8.5, 6.75, 0.5, -0.25], + }, + ] + }, + { + pkg: 'std_msgs', type: 'Int32MultiArray', + values: [ + { + layout: { + dim: [ + {label: 'height', size: 10, stride: 600}, + {label: 'width', size: 20, stride: 60}, + {label: 'channel', size: 3, stride: 4}, + ], + data_offset: 0, + }, + data: [-10, 1, 2, 3, 8, 6, 0, -25], + }, + ] + }, + { + pkg: 'sensor_msgs', type: 'PointCloud', + values: [ + { + header: { + stamp: {sec: 11223, nanosec: 44556}, + frame_id: 'f001', + }, + points: [ + {x:0, y:1, z:3}, {x:0, y:1, z:3}, {x:0, y:1, z:3}, {x:0, y:1, z:3}, + ], + channels: [ + { + name: 'rgb', + values: [0.0, 1.5, 2.0, 3.75], + }, + { + name: 'intensity', + values: [10.0, 21.5, 2.0, 3.75], + }, + ], + }, + ] + }, + /* eslint-enable camelcase */ + /* eslint-enable key-spacing */ + /* eslint-enable comma-spacing */ + ].forEach((testData) => { + const topic = testData.topic || 'topic' + testData.type; + testData.values.forEach((v, i) => { + it('Test translation of ' + testData.type + ' msg, case ' + i, function() { + const node = rclnodejs.createNode('test_message_translation_node'); + const MessageType = rclnodejs.require(testData.pkg).msg[testData.type]; + const publisher = node.createPublisher(MessageType, topic); + return new Promise((resolve, reject) => { + const sub = node.createSubscription(MessageType, topic, (value) => { + if (rclnodejs.util.deepEqual(value, v)) { + node.destroy(); + resolve(); + } else { + reject('case ' + i + '. Expected: ' + v + ', Got: ' + value); + } + }); + publisher.publish(v); + rclnodejs.spin(node); + }); + }); + }); + }); +}); diff --git a/test/test-message-translator-primitive.js b/test/test-message-translator-primitive.js new file mode 100644 index 00000000..2b45d1b8 --- /dev/null +++ b/test/test-message-translator-primitive.js @@ -0,0 +1,68 @@ +// Copyright (c) 2017 Intel Corporation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +'use strict'; + +const rclnodejs = require('../index.js'); + +describe('Rclnodejs message translation: primitive types', function() { + this.timeout(60 * 1000); + + before(function() { + return rclnodejs.init(); + }); + + after(function() { + rclnodejs.shutdown(); + }); + + [ + {type: 'Bool', values: [true, false]}, + {type: 'Byte', values: [0, 1, 2, 3, 255]}, + {type: 'Char', values: [-128, -127, -2, -1, 0, 1, 2, 3, 127]}, + {type: 'Float32', values: [-5, 0, 1.25, 89.75, 72.50, 3.141592e8]}, + {type: 'Float64', values: [-5, 0, 1.25, 89.75, 72.50, 3.141592e8]}, + {type: 'Int16', values: [-32768, -2, -1, 0, 1, 2, 3, 32767]}, + {type: 'Int32', values: [-32768, -2, -1, 0, 1, 2, 3, 32767]}, + {type: 'Int64', values: [-32768, -2, -1, 0, 1, 2, 3, 32767]}, + {type: 'Int8', values: [-128, -127, -2, -1, 0, 1, 2, 3, 127]}, + {type: 'String', values: ['', 'A String', ' ', '<>']}, + {type: 'UInt16', values: [0, 1, 2, 3, 32767, 65535]}, + {type: 'UInt32', values: [0, 1, 2, 3, 32767, 65535]}, + {type: 'UInt64', values: [0, 1, 2, 3, 32767, 65535]}, + {type: 'UInt8', values: [0, 1, 2, 3, 127, 255]}, + ].forEach((testData) => { + const topic = testData.topic || 'topic' + testData.type; + testData.values.forEach((v, i) => { + it('Test translation of ' + testData.type + ' msg, value ' + v, function() { + const node = rclnodejs.createNode('test_message_translation_node'); + const MessageType = rclnodejs.require('std_msgs').msg[testData.type]; + const publisher = node.createPublisher(MessageType, topic); + return new Promise((resolve, reject) => { + const sub = node.createSubscription(MessageType, topic, (value) => { + // For primitive types, msgs are defined as a single `.data` field + if (value.data === v) { + node.destroy(); + resolve(); + } else { + reject('case ' + i + '. Expected: ' + v + ', Got: ' + value.data); + } + }); + publisher.publish(v); + rclnodejs.spin(node); + }); + }); + }); + }); +}); From 32ab91b536be0641301731bb41f67569757c04cc Mon Sep 17 00:00:00 2001 From: Kenny Yuan Date: Thu, 23 Nov 2017 15:06:12 +0800 Subject: [PATCH 2/2] Fixing file naming convention issue, use underbar instead of hyphen for files under /lib dir --- lib/{message-translator.js => message_translator.js} | 2 +- lib/publisher.js | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) rename lib/{message-translator.js => message_translator.js} (98%) diff --git a/lib/message-translator.js b/lib/message_translator.js similarity index 98% rename from lib/message-translator.js rename to lib/message_translator.js index 47221eaf..c6cb31e4 100644 --- a/lib/message-translator.js +++ b/lib/message_translator.js @@ -14,7 +14,7 @@ 'use strict'; -const debug = require('debug')('rclnodejs:message-translator'); +const debug = require('debug')('rclnodejs:message_translator'); /* eslint-disable max-depth */ function copyMsgObject(msg, obj) { diff --git a/lib/publisher.js b/lib/publisher.js index 5962cc13..2e2122bf 100644 --- a/lib/publisher.js +++ b/lib/publisher.js @@ -17,7 +17,7 @@ const rclnodejs = require('bindings')('rclnodejs'); const debug = require('debug')('rclnodejs:publisher'); const Entity = require('./entity.js'); -const {MessageTranslator} = require('./message-translator.js'); +const {MessageTranslator} = require('./message_translator.js'); /** * @class - Class representing a Publisher in ROS