├── .babelrc ├── .gitignore ├── .npmignore ├── .travis.yml ├── LICENSE ├── README.md ├── package.json ├── src ├── actions │ ├── ActionClient.js │ ├── ActionServer.js │ ├── ClientGoalHandle.js │ ├── ClientStates.js │ ├── GoalHandle.js │ ├── GoalIdGenerator.js │ ├── SimpleActionClient.js │ └── SimpleActionServer.js ├── examples │ ├── fibonacciAction.js │ ├── on_the_fly.js │ ├── standard.js │ └── turtle.js ├── index.js ├── lib │ ├── ActionClientInterface.js │ ├── ActionServerInterface.js │ ├── Logging.js │ ├── MasterApiClient.js │ ├── Names.js │ ├── NodeHandle.js │ ├── ParamServerApiClient.js │ ├── Publisher.js │ ├── RosNode.js │ ├── ServiceClient.js │ ├── ServiceServer.js │ ├── SlaveApiClient.js │ ├── Subscriber.js │ ├── ThisNode.js │ ├── Time.js │ └── impl │ │ ├── PublisherImpl.js │ │ └── SubscriberImpl.js ├── ros_msg_utils │ ├── LICENSE │ ├── index.js │ ├── lib │ │ ├── base_deserialize.js │ │ ├── base_serialize.js │ │ ├── encoding_utils.js │ │ └── message_cache.js │ ├── package.json │ └── test │ │ ├── deserialization_test.js │ │ ├── directory.js │ │ └── serialization_test.js ├── tools │ ├── flatten.js │ └── generateMessages.js └── utils │ ├── ClientStates.js │ ├── XmlrpcClient.js │ ├── event_utils.js │ ├── log │ ├── ConsoleLogStream.js │ ├── LogFormatter.js │ ├── Logger.js │ └── RosLogStream.js │ ├── messageGeneration │ ├── IndentedWriter.js │ ├── MessageLoader.js │ ├── MessageSpec.js │ ├── MessageWriter.js │ ├── fields.js │ ├── messages.js │ └── packages.js │ ├── message_utils.js │ ├── network_utils.js │ ├── remapping_utils.js │ ├── serialization_utils.js │ ├── spinners │ ├── ClientQueue.js │ └── GlobalSpinner.js │ ├── tcpros_utils.js │ ├── time_utils.js │ ├── udpros_utils.js │ └── xmlrpc_utils.js └── test ├── DeserializeStream.js ├── Log.js ├── SpinnerTest.js ├── directory.js ├── gennodejsTest.js ├── messages.js ├── namespaceTest.js ├── onTheFly.js ├── onTheFlyMessages.js ├── perfTest.js ├── services.js ├── stress.js ├── utils └── MasterStub.js └── xmlrpcTest.js /.babelrc: -------------------------------------------------------------------------------- 1 | { 2 | "presets": [ 3 | [ 4 | "env", 5 | { 6 | "targets": { 7 | "node": 4 8 | }, 9 | "exclude": [ 10 | "transform-es2015-classes", 11 | "transform-es2015-block-scoping", 12 | "transform-es2015-arrow-functions" 13 | ] 14 | } 15 | ] 16 | ] 17 | } -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Logs 2 | logs 3 | *.log 4 | npm-debug.log* 5 | 6 | # Runtime data 7 | pids 8 | *.pid 9 | *.seed 10 | 11 | # Directory for instrumented libs generated by jscoverage/JSCover 12 | lib-cov 13 | 14 | # Coverage directory used by tools like istanbul 15 | coverage 16 | 17 | # Grunt intermediate storage (http://gruntjs.com/creating-plugins#storing-task-files) 18 | .grunt 19 | 20 | # node-waf configuration 21 | .lock-wscript 22 | 23 | # Compiled binary addons (http://nodejs.org/api/addons.html) 24 | build/Release 25 | 26 | # Dependency directory 27 | node_modules 28 | 29 | # Optional npm cache directory 30 | .npm 31 | 32 | # Optional REPL history 33 | .node_repl_history 34 | 35 | # ignore the build repo 36 | dist/ 37 | -------------------------------------------------------------------------------- /.npmignore: -------------------------------------------------------------------------------- 1 | # Logs 2 | logs 3 | *.log 4 | npm-debug.log* 5 | 6 | # Runtime data 7 | pids 8 | *.pid 9 | *.seed 10 | 11 | # Directory for instrumented libs generated by jscoverage/JSCover 12 | lib-cov 13 | 14 | # Coverage directory used by tools like istanbul 15 | coverage 16 | 17 | # Grunt intermediate storage (http://gruntjs.com/creating-plugins#storing-task-files) 18 | .grunt 19 | 20 | # node-waf configuration 21 | .lock-wscript 22 | 23 | # Compiled binary addons (http://nodejs.org/api/addons.html) 24 | build/Release 25 | 26 | # Dependency directory 27 | node_modules 28 | 29 | # Optional npm cache directory 30 | .npm 31 | 32 | # Optional REPL history 33 | .node_repl_history 34 | 35 | # ignore the src repo 36 | src/ 37 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: xenial 2 | sudo: required 3 | compiler: 4 | - gcc 5 | language: node_js 6 | node_js: 7 | - "6" 8 | - "7" 9 | - "8" 10 | - "10" 11 | env: 12 | global: 13 | - ROS_DISTRO=kinetic 14 | - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [precise|trusty|...] 15 | - CI_SOURCE_PATH=$(pwd) 16 | - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall 17 | - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options 18 | - ROS_PARALLEL_JOBS='-j8 -l6' 19 | # Set the python path manually to include /usr/-/python2.7/dist-packages 20 | # as this is where apt-get installs python packages. 21 | - PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages 22 | branches: 23 | only: 24 | - kinetic-devel 25 | 26 | # Install system dependencies, namely a very barebones ROS setup. 27 | before_install: 28 | - sudo apt-get install -y dpkg # to upgrade to dpkg >= 1.17.5ubuntu5.8, which fixes https://bugs.launchpad.net/ubuntu/+source/dpkg/+bug/1730627 29 | - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 30 | - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 31 | - sudo apt-get update -qq 32 | - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-ros-base 33 | - source /opt/ros/$ROS_DISTRO/setup.bash 34 | # Prepare rosdep to install dependencies. 35 | - sudo rosdep init 36 | - rosdep update 37 | 38 | # Create a catkin workspace with the package under integration. 39 | install: 40 | - mkdir -p ~/catkin_ws/src 41 | - cd ~/catkin_ws/src 42 | - catkin_init_workspace 43 | # Create the devel/setup.bash (run catkin_make with an empty workspace) and 44 | # source it to set the path variables. 45 | - cd ~/catkin_ws 46 | - catkin_make 47 | - source devel/setup.bash 48 | 49 | # Install required message packages 50 | - git clone https://github.com/ros/std_msgs.git src/std_msgs 51 | - git clone https://github.com/ros/common_msgs src/common_msgs 52 | - git clone https://github.com/RethinkRobotics-opensource/test_msgs.git src/test_msgs 53 | - git clone https://github.com/ros/ros_comm_msgs.git src/ros_comm_msgs 54 | # Install all dependencies, using wstool first and rosdep second. 55 | # wstool looks for a ROSINSTALL_FILE defined in the environment variables. 56 | - cd ~/catkin_ws/src 57 | - wstool init 58 | - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi 59 | - wstool up 60 | # package depdencies: install using rosdep. 61 | - cd ~/catkin_ws 62 | - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO 63 | - source devel/setup.bash 64 | 65 | script: 66 | # FIXME: enable testing with messages generated by gennodejs 67 | # once unicode fixes are merged. 68 | # We should test with gennodejs and rosnodejs messages 69 | # - cd $CI_SOURCE_PATH 70 | # - touch CATKIN_IGNORE 71 | # - cd ~/catkin_ws 72 | # - catkin_make 73 | - cd $CI_SOURCE_PATH 74 | - npm install 75 | - npm run compile 76 | - npm run generate 77 | - npm test 78 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rosnodejs [![Build Status](https://travis-ci.org/RethinkRobotics-opensource/rosnodejs.svg)](https://travis-ci.org/RethinkRobotics-opensource/rosnodejs) 2 | 3 | ## Install 4 | `npm install rosnodejs` 5 | 6 | ## Start a node 7 | ```js 8 | const rosnodejs = require('rosnodejs'); 9 | rosnodejs.initNode('/my_node') 10 | .then(() => { 11 | // do stuff 12 | }); 13 | ``` 14 | 15 | ## Publish/Subscribe 16 | ```js 17 | const nh = rosnodejs.nh; 18 | const sub = nh.subscribe('/chatter', 'std_msgs/String', (msg) => { 19 | console.log('Got msg on chatter: %j', msg); 20 | }); 21 | 22 | const pub = nh.advertise('/chatter', 'std_msgs/String'); 23 | pub.publish({ data: "hi" }); 24 | ``` 25 | 26 | The `advertise` function takes an options object as optional third argument: 27 | ```js 28 | { 29 | queueSize: 1, 30 | latching: false // 31 | throttleMs: 0 32 | } 33 | ``` 34 | If you plan to publish messages right after another in the same function, set `throttleMs: -1`. Otherwise only the last message will be sent. 35 | 36 | ### Udp transport (Experimental) 37 | 38 | ```js 39 | const nh = rosnodejs.nh; 40 | const sub = nh.subscribe('/chatter', 'std_msgs/String', (msg) => { 41 | console.log('Got msg on chatter: %j', msg); 42 | }, { 43 | transports: ["TCPROS", "UDPROS"], // specify transports, default ["TCPROS"] 44 | dgramSize: 1500 // optional: datagram packet size, default: 1500 bytes 45 | }); 46 | 47 | const pub = nh.advertise('/chatter', 'std_msgs/String'); 48 | pub.publish({ data: "hi" }); 49 | ``` 50 | ## Services 51 | ```js 52 | const service = nh.advertiseService('/add_two_ints', 'beginner_tutorials/AddTwoInts', (req, res) => { 53 | res.sum = req.a + req.b; 54 | return true; 55 | }); 56 | 57 | const client = nh.serviceClient('/add_two_ints', 'beginner_tutorials/AddTwoInts'); 58 | client.call({a: 1, b: 2}); 59 | ``` 60 | 61 | ## Params 62 | ```js 63 | nh.setParam('val', 2); 64 | nh.getParam('val') 65 | .then((val) => { 66 | // do stuff 67 | }); 68 | ``` 69 | ## Generating Messages 70 | 71 | Messages can be generated in a number of ways depending on the versions of ROS and Node.js you're using. 72 | - catkin - works in ROS Kinetic and later for Node.js v6+. 73 | ```sh 74 | $ catkin_make 75 | OR 76 | $ catkin build 77 | ``` 78 | - `loadAllPackages()` - One-time "build" call available through `rosnodejs` for versions of ROS before Kinetic and Node.js v6+. Should be called separately and in advance of processes attempting to use messages. 79 | ```js 80 | const rosnodejs = require('rosnodejs'); 81 | rosnodejs.loadAllPackages(); 82 | ``` 83 | - On the fly - all versions of ROS and Node.js 4.5+. When generating on the fly, messages can not be required until the node has initialized. 84 | ```js 85 | const rosnodejs = require('rosnodejs'); 86 | rosnodejs.initNode('my_node', { onTheFly: true }).then(() => { 87 | const stdMsgs = rosnodejs.require('std_msgs'); 88 | ... 89 | } 90 | ``` 91 | 92 | | |Pre-Kinetic|Kinetic & Later| 93 | |:---:|:---:|:---:| 94 | |Node.js >= v6|`loadAllPackages()`, on the fly|catkin, `loadAllPackages()`, on the fly| 95 | |Node.js < v6|on the fly|on the fly| 96 | 97 | ## Using Messages 98 | ```js 99 | const sensorMsgs = rosnodejs.require('sensor_msgs'); 100 | 101 | const image = new sensorMsgs.msg.Image(); 102 | const temperature = new sensorMsgs.msg.Temperature({ temperature: 32 }); 103 | 104 | const SetCameraInfo = sensorMsgs.srv.SetCameraInfo; 105 | const setRequest = new SetCameraInfo.Request(); 106 | 107 | // messages can be used when advertising/subscribing 108 | const nh = rosnodejs.nh; 109 | const StringMsg = rosnodejs.require('std_msgs').msg.String; 110 | const sub = nh.subscribe('/chatter', StringMsg, (msg) => { ... }); 111 | const pub = nh.advertise('/chatter', StringMsg); 112 | 113 | const AddTwoInts = rosnodejs.require('beginner_tutorials').srv.AddTwoInts; 114 | const service = nh.advertiseService('/add_two_ints', AddTwoInts, (req, res) => { ... }); 115 | const client = nh.serviceClient('/add_two_ints', AddTwoInts); 116 | ``` 117 | ## Actions (Experimental) 118 | ```js 119 | const nh = rosnodejs.nh; 120 | const as = new rosnodejs.ActionServer({ 121 | nh, 122 | type: 'turtle_actionlib/Shape', 123 | actionServer: '/turtle_shape' 124 | }); 125 | 126 | as.on('goal', function (goal) { 127 | goal.setAccepted(); 128 | }); 129 | 130 | as.start(); 131 | 132 | const ac = new rosnodejs.ActionClient({ 133 | nh, 134 | type: 'turtle_actionlib/Shape', 135 | actionServer:'/turtle_shape' 136 | }); 137 | 138 | ac.sendGoal({edges: 3, radius: 1}); 139 | ``` 140 | ## Run the turtlesim example 141 | 142 | Start: 143 | 144 | ```sh 145 | roscore 146 | rosrun turtlesim turtlesim_node 147 | rosrun turtle_actionlib shape_server 148 | ``` 149 | 150 | Then run 151 | ```sh 152 | node src/examples/turtle.js 153 | ``` 154 | 155 | or, if you are running an older version of node: 156 | 157 | ```sh 158 | npm run compile 159 | node dist/examples/turtle.js 160 | ``` 161 | 162 | ## Catkin-Like 163 | Checkout [`rosnodejs_examples`](https://github.com/RethinkRobotics-opensource/rosnodejs_examples) for a more catkin-inspired take on using `rosnodejs`. 164 | 165 | ## Inspired By 166 | `rosnodejs` was inspired by other work that you can learn more about here 167 | - [roscpp & rospy](https://github.com/ros/ros_comm) 168 | - [Robots as web services](http://ieeexplore.ieee.org/document/5980464/?tp=&arnumber=5980464&url=http:%2F%2Fieeexplore.ieee.org%2Fxpls%2Fabs_all.jsp%3Farnumber%3D5980464) 169 | - [ROSBridge](https://github.com/RobotWebTools/rosbridge_suite) 170 | - [roslibjs](https://github.com/RobotWebTools/roslibjs) 171 | 172 | -------------------------------------------------------------------------------- /package.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "rosnodejs", 3 | "version": "3.1.0", 4 | "description": "Native ROS for nodejs", 5 | "main": "dist/index.js", 6 | "keywords": [ 7 | "ros" 8 | ], 9 | "scripts": { 10 | "test": "mocha --exit test/directory.js", 11 | "gennodejsTest": "mocha test/gennodejsTest.js test/onTheFlyMessages.js", 12 | "stressTest": "mocha test/stress.js", 13 | "serviceTest": "mocha --exit test/services.js", 14 | "flatten": "node tools/flatten.js", 15 | "generate": "node dist/tools/generateMessages.js", 16 | "compile": "babel src/ -d dist/", 17 | "prepublish": "npm run compile" 18 | }, 19 | "author": "chris smith", 20 | "license": "Apache-2.0", 21 | "repository": { 22 | "type": "git", 23 | "url": "git://github.com/RethinkRobotics-opensource/rosnodejs.git" 24 | }, 25 | "devDependencies": { 26 | "babel-cli": "^6.18.0", 27 | "babel-preset-env": "^1.7.0", 28 | "chai": "^4.1.2", 29 | "mocha": "^5.2.0" 30 | }, 31 | "dependencies": { 32 | "argparse": "1.0.10", 33 | "async": "2.6.4", 34 | "bn.js": "^4.11.6", 35 | "bunyan": "1.8.12", 36 | "md5": "2.2.1", 37 | "ultron": "1.1.1", 38 | "walker": "1.0.7", 39 | "xmlrpc-rosnodejs": "1.4.0" 40 | }, 41 | "bugs": { 42 | "url": "https://github.com/RethinkRobotics-opensource/rosnodejs/issues" 43 | }, 44 | "homepage": "https://github.com/RethinkRobotics-opensource/rosnodejs#readme", 45 | "directories": { 46 | "test": "test" 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /src/actions/ActionClient.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Rethink Robotics 3 | * 4 | * Copyright 2017 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const msgUtils = require('../utils/message_utils.js'); 21 | 22 | const ActionClientInterface = require('../lib/ActionClientInterface.js'); 23 | 24 | const EventEmitter = require('events'); 25 | const Ultron = require('ultron'); 26 | 27 | const ClientGoalHandle = require('./ClientGoalHandle.js'); 28 | const Time = require('../lib/Time.js'); 29 | 30 | const log = require('../lib/Logging.js').getLogger('actionlib_nodejs'); 31 | const ThisNode = require('../lib/ThisNode.js'); 32 | const GoalIdGenerator = require('./GoalIdGenerator.js'); 33 | 34 | /** 35 | * @class ActionClient 36 | * EXPERIMENTAL 37 | * 38 | */ 39 | class ActionClient extends EventEmitter { 40 | constructor(options) { 41 | super(); 42 | 43 | this._acInterface = new ActionClientInterface(options); 44 | 45 | this._acInterface.on('status', this._handleStatus.bind(this)); 46 | this._acInterface.on('feedback', this._handleFeedback.bind(this)); 47 | this._acInterface.on('result', this._handleResult.bind(this)); 48 | 49 | const actionType = this._acInterface.getType(); 50 | this._messageTypes = this._messageTypes = { 51 | result: msgUtils.getHandlerForMsgType(actionType + 'Result'), 52 | feedback: msgUtils.getHandlerForMsgType(actionType + 'Feedback'), 53 | goal: msgUtils.getHandlerForMsgType(actionType + 'Goal'), 54 | actionResult: msgUtils.getHandlerForMsgType(actionType + 'ActionResult'), 55 | actionFeedback: msgUtils.getHandlerForMsgType(actionType + 'ActionFeedback'), 56 | actionGoal: msgUtils.getHandlerForMsgType(actionType + 'ActionGoal') 57 | }; 58 | 59 | this._goalLookup = {}; 60 | 61 | this._shutdown = false; 62 | this._ultron = new Ultron(ThisNode); 63 | 64 | // FIXME: how to handle shutdown? Should user be responsible? 65 | // should we check for shutdown in interval instead of listening 66 | // to events here? 67 | this._ultron.once('shutdown', () => { this.shutdown(); }); 68 | } 69 | 70 | shutdown() { 71 | if (!this._shutdown) { 72 | this._shutdown = true; 73 | 74 | this._ultron.destroy(); 75 | this._ultron = null; 76 | 77 | return this._acInterface.shutdown(); 78 | } 79 | // else 80 | return Promise.resolve(); 81 | } 82 | 83 | sendGoal(goal, transitionCb = null, feedbackCb = null) { 84 | const actionGoal = new this._messageTypes.actionGoal(); 85 | 86 | const now = Time.now(); 87 | actionGoal.header.stamp = now; 88 | actionGoal.goal_id.stamp = now; 89 | const goalIdStr = GoalIdGenerator(now); 90 | actionGoal.goal_id.id = goalIdStr; 91 | actionGoal.goal = goal; 92 | 93 | this._acInterface.sendGoal(actionGoal); 94 | 95 | const handle = new ClientGoalHandle(actionGoal, this._acInterface); 96 | 97 | if (transitionCb && typeof transitionCb === 'function') { 98 | handle.on('transition', transitionCb); 99 | } 100 | if (feedbackCb && typeof feedbackCb === 'function') { 101 | handle.on('feedback', feedbackCb); 102 | } 103 | 104 | this._goalLookup[goalIdStr] = handle; 105 | 106 | return handle; 107 | } 108 | 109 | cancelAllGoals() { 110 | this._acInterface.cancel("", { secs: 0, nsecs: 0}); 111 | } 112 | 113 | cancelGoalsAtAndBeforeTime(stamp) { 114 | this._acInterface.cancel("", stamp); 115 | } 116 | 117 | waitForActionServerToStart(timeout) { 118 | return this._acInterface.waitForActionServerToStart(timeout); 119 | } 120 | 121 | isServerConnected() { 122 | return this._acInterface.isServerConnected(); 123 | } 124 | 125 | _handleStatus(msg) { 126 | const list = msg.status_list; 127 | const len = list.length; 128 | 129 | const statusMap = {}; 130 | 131 | for (let i = 0; i < len; ++i) { 132 | const entry = list[i]; 133 | const goalId = entry.goal_id.id; 134 | 135 | statusMap[goalId] = entry; 136 | } 137 | 138 | for (let goalId in this._goalLookup) { 139 | const goalHandle = this._goalLookup[goalId]; 140 | goalHandle.updateStatus(statusMap[goalId]); 141 | } 142 | } 143 | 144 | _handleFeedback(msg) { 145 | const goalId = msg.status.goal_id.id; 146 | const goalHandle = this._goalLookup[goalId]; 147 | if (goalHandle) { 148 | goalHandle.updateFeedback(msg); 149 | } 150 | } 151 | 152 | _handleResult(msg) { 153 | const goalId = msg.status.goal_id.id; 154 | const goalHandle = this._goalLookup[goalId]; 155 | if (goalHandle) { 156 | delete this._goalLookup[goalId]; 157 | goalHandle.updateResult(msg); 158 | } 159 | } 160 | } 161 | 162 | module.exports = ActionClient; 163 | -------------------------------------------------------------------------------- /src/actions/ActionServer.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Rethink Robotics 3 | * 4 | * Copyright 2017 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const msgUtils = require('../utils/message_utils.js'); 21 | const EventEmitter = require('events'); 22 | const Ultron = require('ultron'); 23 | 24 | const ActionServerInterface = require('../lib/ActionServerInterface.js'); 25 | const GoalHandle = require('./GoalHandle.js'); 26 | const Time = require('../lib/Time.js'); 27 | 28 | const log = require('../lib/Logging.js').getLogger('actionlib_nodejs'); 29 | const ThisNode = require('../lib/ThisNode.js'); 30 | 31 | let GoalIdMsg = null; 32 | let GoalStatusMsg = null; 33 | let GoalStatusArrayMsg = null; 34 | let GoalStatuses = null; 35 | let goalCount = 0; 36 | 37 | /** 38 | * @class ActionServer 39 | * EXPERIMENTAL 40 | * 41 | */ 42 | class ActionServer extends EventEmitter { 43 | constructor(options) { 44 | super(); 45 | 46 | if (GoalStatusMsg === null) { 47 | GoalStatusMsg = msgUtils.requireMsgPackage('actionlib_msgs').msg.GoalStatus; 48 | GoalStatuses = GoalStatusMsg.Constants; 49 | } 50 | 51 | if (GoalStatusArrayMsg === null) { 52 | GoalStatusArrayMsg = msgUtils.requireMsgPackage('actionlib_msgs').msg.GoalStatusArray; 53 | } 54 | 55 | this._options = options; 56 | 57 | this._pubSeqs = { 58 | result: 0, 59 | feedback: 0, 60 | status: 0 61 | }; 62 | 63 | this._goalHandleList = []; 64 | this._goalHandleCache = {}; 65 | 66 | this._lastCancelStamp = Time.epoch(); 67 | 68 | this._statusListTimeout = { secs: 5, nsecs: 0 }; 69 | this._shutdown = false; 70 | this._ultron = new Ultron(ThisNode); 71 | } 72 | 73 | start() { 74 | this._started = true; 75 | this._asInterface = new ActionServerInterface(this._options); 76 | 77 | this._asInterface.on('goal', this._handleGoal.bind(this)); 78 | this._asInterface.on('cancel', this._handleCancel.bind(this)); 79 | 80 | const actionType = this._asInterface.getType(); 81 | 82 | this._messageTypes = { 83 | result: msgUtils.getHandlerForMsgType(actionType + 'Result'), 84 | feedback: msgUtils.getHandlerForMsgType(actionType + 'Feedback'), 85 | actionResult: msgUtils.getHandlerForMsgType(actionType + 'ActionResult'), 86 | actionFeedback: msgUtils.getHandlerForMsgType(actionType + 'ActionFeedback') 87 | }; 88 | 89 | this.publishStatus(); 90 | 91 | let statusFreq = 5; 92 | if (this._options.statusFrequency !== undefined) { 93 | if (typeof this._options.statusFrequency !== 'number') { 94 | throw new Error(`Invalid value (${this._options.statusFrequency}) for statusFrequency - expected number`); 95 | } 96 | 97 | statusFreq = this._options.statusFrequency; 98 | } 99 | 100 | if (statusFreq > 0) { 101 | this._statusFreqInt = setInterval(() => { 102 | this.publishStatus(); 103 | }, 1000 / statusFreq); 104 | } 105 | 106 | // FIXME: how to handle shutdown? Should user be responsible? 107 | // should we check for shutdown in interval instead of listening 108 | // to events here? 109 | this._ultron.once('shutdown', () => { this.shutdown(); }); 110 | } 111 | 112 | generateGoalId() { 113 | return this._asInterface.generateGoalId(); 114 | } 115 | 116 | shutdown() { 117 | if (!this._shutdown) { 118 | this._shutdown = true; 119 | this.removeAllListeners(); 120 | 121 | clearInterval(this._statusFreqInt); 122 | this._statusFreqInt = null; 123 | 124 | this._ultron.destroy(); 125 | this._ultron = null; 126 | 127 | if (this._asInterface) { 128 | return this._asInterface.shutdown(); 129 | } 130 | } 131 | // else 132 | return Promise.resolve(); 133 | } 134 | 135 | _getGoalHandle(id) { 136 | return this._goalHandleCache[id]; 137 | } 138 | 139 | _handleGoal(msg) { 140 | if (!this._started) { 141 | return; 142 | } 143 | 144 | const newGoalId = msg.goal_id.id; 145 | 146 | let handle = this._getGoalHandle(newGoalId); 147 | 148 | if (handle) { 149 | // check if we already received a request to cancel this goal 150 | if (handle.getStatusId() === GoalStatuses.RECALLING) { 151 | handle.setCancelled(this._createMessage('result')); 152 | } 153 | 154 | handle._destructionTime = msg.goal_id.stamp; 155 | return false; 156 | } 157 | 158 | handle = new GoalHandle(msg.goal_id, this, GoalStatuses.PENDING, msg.goal); 159 | this._goalHandleList.push(handle); 160 | this._goalHandleCache[handle.id] = handle; 161 | 162 | const goalStamp = msg.goal_id.stamp; 163 | // check if this goal has already been cancelled based on its timestamp 164 | if (!Time.isZeroTime(goalStamp) && 165 | Time.timeComp(goalStamp, this._lastCancelStamp) < 0) { 166 | handle.setCancelled(this._createMessage('result')); 167 | return false; 168 | } 169 | else { 170 | // track goal, I guess 171 | this.emit('goal', handle); 172 | } 173 | 174 | return true; 175 | } 176 | 177 | _handleCancel(msg) { 178 | if (!this._started) { 179 | return; 180 | } 181 | 182 | const cancelId = msg.id; 183 | const cancelStamp = msg.stamp; 184 | const cancelStampIsZero = Time.isZeroTime(cancelStamp); 185 | 186 | const shouldCancelEverything = (cancelId === '' && cancelStampIsZero); 187 | 188 | let goalIdFound = false; 189 | 190 | for (let i = 0, len = this._goalHandleList.length; i < len; ++i) { 191 | const handle = this._goalHandleList[i]; 192 | const handleId = handle.id; 193 | const handleStamp = handle.getStatus().goal_id.stamp; 194 | 195 | if (shouldCancelEverything || 196 | cancelId === handleId || 197 | (!Time.isZeroTime(handleStamp) && 198 | Time.timeComp(handleStamp, cancelStamp) < 0)) 199 | { 200 | if (cancelId === handleId) { 201 | goalIdFound = true; 202 | } 203 | 204 | if (handle.setCancelRequested()) { 205 | this.emit('cancel', handle); 206 | } 207 | } 208 | } 209 | 210 | // if the requested goal_id was not found and it is not empty, 211 | // then we need to store the cancel request 212 | if (cancelId !== '' && !goalIdFound) { 213 | const handle = new GoalHandle(msg, this, GoalStatuses.RECALLING); 214 | this._goalHandleList.push(handle); 215 | this._goalHandleCache[handle.id] = handle; 216 | } 217 | 218 | // update the last cancel stamp if new one occurred later 219 | if (Time.timeComp(cancelStamp, this._lastCancelStamp) > 0) { 220 | this._lastCancelStamp = cancelStamp; 221 | } 222 | } 223 | 224 | publishResult(status, result) { 225 | const msg = this._createMessage('actionResult', { status, result }); 226 | msg.header.stamp = Time.now(); 227 | msg.header.seq = this._getAndIncrementSeq('actionResult'); 228 | this._asInterface.publishResult(msg); 229 | this.publishStatus(); 230 | } 231 | 232 | publishFeedback(status, feedback) { 233 | const msg = this._createMessage('actionFeedback', { status, feedback }); 234 | msg.header.stamp = Time.now(); 235 | msg.header.seq = this._getAndIncrementSeq('actionFeedback'); 236 | this._asInterface.publishFeedback(msg); 237 | this.publishStatus(); 238 | } 239 | 240 | publishStatus() { 241 | const msg = new GoalStatusArrayMsg(); 242 | msg.header.stamp = Time.now(); 243 | msg.header.seq = this._getAndIncrementSeq('status'); 244 | 245 | let goalsToRemove = new Set(); 246 | 247 | const now = Time.now(); 248 | 249 | for (let i = 0, len = this._goalHandleList.length; i < len; ++i) { 250 | const goalHandle = this._goalHandleList[i]; 251 | msg.status_list.push(goalHandle.getGoalStatus()); 252 | 253 | const t = goalHandle._destructionTime; 254 | if (!Time.isZeroTime(t) && 255 | Time.lt(Time.add(t, this._statusListTimeout), now)) 256 | { 257 | goalsToRemove.add(goalHandle); 258 | } 259 | } 260 | 261 | // clear out any old goal handles 262 | this._goalHandleList = this._goalHandleList.filter((goal) => { 263 | // kind of funky to remove from another object in this filter... 264 | if (goalsToRemove.has(goal)) { 265 | delete this._goalHandleCache[goal.id]; 266 | return false; 267 | } 268 | return true; 269 | }); 270 | 271 | this._asInterface.publishStatus(msg); 272 | } 273 | 274 | _getAndIncrementSeq(type) { 275 | return this._pubSeqs[type]++ 276 | } 277 | 278 | _createMessage(type, args = {}) { 279 | return new this._messageTypes[type](args); 280 | } 281 | } 282 | 283 | module.exports = ActionServer; 284 | -------------------------------------------------------------------------------- /src/actions/ClientStates.js: -------------------------------------------------------------------------------- 1 | const SimpleGoalState = { 2 | PENDING: 'PENDING', 3 | ACTIVE: 'ACTIVE', 4 | DONE: 'DONE' 5 | }; 6 | 7 | const SimpleClientGoalState = { 8 | PENDING: 'PENDING', 9 | ACTIVE: 'ACTIVE', 10 | RECALLED: 'RECALLED', 11 | REJECTED: 'REJECTED', 12 | PREEMPTED: 'PREEMPTED', 13 | ABORTED: 'ABORTED', 14 | SUCCEEDED: 'SUCCEEDED', 15 | LOST: 'LOST' 16 | }; 17 | 18 | const CommState = { 19 | WAITING_FOR_GOAL_ACK: 0, 20 | PENDING: 1, 21 | ACTIVE: 2, 22 | WAITING_FOR_RESULT: 3, 23 | WAITING_FOR_CANCEL_ACK: 4, 24 | RECALLING: 5, 25 | PREEMPTING: 6, 26 | DONE: 7 27 | }; 28 | 29 | module.exports = { 30 | CommState, 31 | SimpleGoalState, 32 | SimpleClientGoalState 33 | }; 34 | -------------------------------------------------------------------------------- /src/actions/GoalHandle.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Rethink Robotics 3 | * 4 | * Copyright 2017 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const msgUtils = require('../utils/message_utils.js'); 21 | const timeUtils = require('../lib/Time.js'); 22 | const log = require('../lib/Logging.js').getLogger('ros.rosnodejs'); 23 | 24 | let GoalStatus = null; 25 | let GoalStatuses = null; 26 | 27 | class GoalHandle { 28 | /** 29 | * goalId: An actionlib_msgs/GoalID. 30 | * actionServer: The ActionServer processing this goal 31 | * status: A number from actionlib_msgs/GoalStatus, like GoalStatuses.PENDING. 32 | * goal: The goal message, e.g., a FibonacciGoal. May be left undefined if 33 | * this goal is used to represent a cancellation. 34 | */ 35 | constructor(goalId, actionServer, status, goal) { 36 | if (goalId.id === '') { 37 | goalId = actionServer.generateGoalId(); 38 | } 39 | 40 | if (timeUtils.isZeroTime(goalId.stamp)) { 41 | goalId.stamp = timeUtils.now(); 42 | } 43 | 44 | this.id = goalId.id; 45 | 46 | this._as = actionServer; 47 | 48 | if (GoalStatus === null) { 49 | GoalStatus = msgUtils.requireMsgPackage('actionlib_msgs').msg.GoalStatus; 50 | GoalStatuses = GoalStatus.Constants; 51 | } 52 | 53 | this._status = new GoalStatus({ 54 | status: status || GoalStatuses.PENDING, 55 | goal_id: goalId 56 | }); 57 | 58 | this._goal = goal; 59 | 60 | this._destructionTime = timeUtils.epoch(); 61 | } 62 | 63 | getGoal() { 64 | return this._goal; 65 | } 66 | 67 | getStatusId() { 68 | return this._status.status; 69 | } 70 | 71 | getGoalId() { 72 | return this._status.goal_id; 73 | } 74 | 75 | getGoalStatus() { 76 | return this._status; 77 | } 78 | 79 | publishFeedback(feedback) { 80 | this._as.publishFeedback(this._status, feedback); 81 | } 82 | 83 | _setStatus(status, text) { 84 | this._status.status = status; 85 | if (text) { 86 | this._status.text = text; 87 | } 88 | 89 | // FIXME: just guessing about setting destruction time 90 | if (this._isTerminalState()) { 91 | this._destructionTime = timeUtils.now(); 92 | } 93 | 94 | this._as.publishStatus(); 95 | } 96 | 97 | _publishResult(result) { 98 | this._as.publishResult(this._status, result); 99 | } 100 | 101 | // For Goal State transitions, See 102 | // http://wiki.ros.org/actionlib/DetailedDescription#Server_Description 103 | 104 | setCanceled(result, text = '') { 105 | const status = this.getStatusId(); 106 | switch (status) { 107 | case GoalStatuses.PENDING: 108 | case GoalStatuses.RECALLING: 109 | this._setStatus(GoalStatuses.RECALLED, text); 110 | this._publishResult(result); 111 | break; 112 | case GoalStatuses.ACTIVE: 113 | case GoalStatuses.PREEMPTING: 114 | this._setStatus(GoalStatuses.PREEMPTED, text); 115 | this._publishResult(result); 116 | break; 117 | default: 118 | this._logInvalidTransition('setCancelled', status); 119 | break; 120 | } 121 | } 122 | 123 | setCancelled(result, text = '') { 124 | return this.setCanceled(result, text); 125 | } 126 | 127 | setRejected(result, text = '') { 128 | const status = this.getStatusId(); 129 | switch (status) { 130 | case GoalStatuses.PENDING: 131 | case GoalStatuses.RECALLING: 132 | this._setStatus(GoalStatuses.REJECTED, text); 133 | this._publishResult(result); 134 | break; 135 | default: 136 | this._logInvalidTransition('setRejected', status); 137 | break; 138 | } 139 | } 140 | 141 | setAccepted(text = '') { 142 | const status = this.getStatusId(); 143 | switch (status) { 144 | case GoalStatuses.PENDING: 145 | this._setStatus(GoalStatuses.ACTIVE, text); 146 | break; 147 | case GoalStatuses.RECALLING: 148 | this._setStatus(GoalStatuses.PREEMPTING, text); 149 | break; 150 | default: 151 | this._logInvalidTransition('setAccepted', status); 152 | break; 153 | } 154 | } 155 | 156 | setAborted(result, text = '') { 157 | const status = this.getStatusId(); 158 | switch (status) { 159 | case GoalStatuses.PREEMPTING: 160 | case GoalStatuses.ACTIVE: 161 | this._setStatus(GoalStatuses.ABORTED, text); 162 | this._publishResult(result); 163 | break; 164 | default: 165 | this._logInvalidTransition('setAborted', status); 166 | break; 167 | } 168 | } 169 | 170 | setSucceeded(result, text = '') { 171 | const status = this.getStatusId(); 172 | switch (status) { 173 | case GoalStatuses.PREEMPTING: 174 | case GoalStatuses.ACTIVE: 175 | this._setStatus(GoalStatuses.SUCCEEDED, text); 176 | this._publishResult(result); 177 | break; 178 | default: 179 | this._logInvalidTransition('setSucceeded', status); 180 | break; 181 | } 182 | } 183 | 184 | setCancelRequested() { 185 | const status = this.getStatusId(); 186 | switch (status) { 187 | case GoalStatuses.PENDING: 188 | this._setStatus(GoalStatuses.RECALLING); 189 | return true; 190 | case GoalStatuses.ACTIVE: 191 | this._setStatus(GoalStatuses.PREEMPTING); 192 | return true; 193 | default: 194 | this._logInvalidTransition('setCancelRequested', status); 195 | return false; 196 | } 197 | } 198 | 199 | _logInvalidTransition(transition, currentStatus) { 200 | log.warn('Unable to %s from status %s for goal %s', transition, currentStatus, this.id); 201 | } 202 | 203 | _isTerminalState() { 204 | return [ 205 | GoalStatuses.REJECTED, 206 | GoalStatuses.RECALLED, 207 | GoalStatuses.PREEMPTED, 208 | GoalStatuses.ABORTED, 209 | GoalStatuses.SUCCEEDED 210 | ].includes(this._status.status); 211 | } 212 | } 213 | 214 | module.exports = GoalHandle; 215 | -------------------------------------------------------------------------------- /src/actions/GoalIdGenerator.js: -------------------------------------------------------------------------------- 1 | const ThisNode = require('../lib/ThisNode.js'); 2 | const Time = require('../lib/Time.js'); 3 | 4 | let GOAL_COUNT = 0; 5 | 6 | module.exports = function(now) { 7 | if (!now || now.secs === undefined || now.nsecs === undefined) { 8 | now = Time.now(); 9 | } 10 | 11 | ++GOAL_COUNT; 12 | if (GOAL_COUNT > Number.MAX_SAFE_INTEGER) { 13 | GOAL_COUNT = 0; 14 | } 15 | 16 | return `${ThisNode.getNodeName()}-${GOAL_COUNT}-${now.secs}.${now.nsecs}`; 17 | } 18 | -------------------------------------------------------------------------------- /src/actions/SimpleActionServer.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Rethink Robotics 3 | * 4 | * Copyright 2017 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const ActionServer = require('./ActionServer.js'); 21 | const Ultron = require('ultron'); 22 | const EventEmitter = require('events'); 23 | 24 | const Time = require('../lib/Time.js'); 25 | 26 | const log = require('../lib/Logging.js').getLogger('actionlib_nodejs'); 27 | const msgUtils = require('../utils/message_utils.js'); 28 | const ThisNode = require('../lib/ThisNode.js'); 29 | 30 | let GoalStatuses = null; 31 | 32 | class SimpleActionServer extends EventEmitter { 33 | constructor(options) { 34 | super(); 35 | 36 | this._as = new ActionServer(options); 37 | 38 | if (GoalStatuses === null) { 39 | GoalStatuses = msgUtils.requireMsgPackage('actionlib_msgs').msg.GoalStatus.Constants; 40 | } 41 | 42 | this._executeCallback = options.executeCallback; 43 | 44 | this._currentGoal = null; 45 | this._nextGoal = null; 46 | 47 | this._preemptRequested = false; 48 | this._newGoalPreemptRequest = false; 49 | 50 | this._shutdown = false; 51 | this._ultron = new Ultron(ThisNode); 52 | } 53 | 54 | start() { 55 | this._as.start(); 56 | 57 | this._as.on('goal', this._handleGoal.bind(this)); 58 | this._as.on('cancel', this._handleCancel.bind(this)); 59 | 60 | if (this._executeCallback) { 61 | this._runExecuteLoop(); 62 | } 63 | 64 | // FIXME: how to handle shutdown? Should user be responsible? 65 | // should we check for shutdown in interval instead of listening 66 | // to events here? 67 | this._ultron.once('shutdown', () => { this.shutdown(); }); 68 | } 69 | 70 | isActive() { 71 | if (this._currentGoal) { 72 | const status = this._currentGoal.getStatusId(); 73 | return status === GoalStatuses.ACTIVE || status === GoalStatuses.PREEMPTING; 74 | } 75 | return false; 76 | } 77 | 78 | isNewGoalAvailable() { 79 | return !!this._nextGoal; 80 | } 81 | 82 | isPreemptRequested() { 83 | return this._preemptRequested; 84 | } 85 | 86 | shutdown() { 87 | if (!this._shutdown) { 88 | this._shutdown = true; 89 | this.removeAllListeners(); 90 | 91 | this._currentGoal = null; 92 | this._nextGoal = null; 93 | clearTimeout(this._executeLoopTimer); 94 | 95 | this._ultron.destroy(); 96 | this._ultron = null; 97 | 98 | return this._as.shutdown(); 99 | } 100 | // else 101 | return Promise.resolve(); 102 | } 103 | 104 | acceptNewGoal() { 105 | if (!this._nextGoal) { 106 | log.error('Attempting to accept the next goal when a new goal is not available'); 107 | return; 108 | } 109 | 110 | if (this.isActive()) { 111 | const result = this._as._createMessage('result'); 112 | 113 | this._currentGoal.setCancelled( 114 | result, 115 | 'This goal was canceled because another goal was received by the simple action server' 116 | ); 117 | } 118 | 119 | this._currentGoal = this._nextGoal; 120 | this._nextGoal = null; 121 | 122 | this._preemptRequested = this._newGoalPreemptRequest; 123 | this._newGoalPreemptRequest = false; 124 | 125 | this._currentGoal.setAccepted('This goal has been accepted by the simple action server'); 126 | 127 | return this._currentGoal.getGoal(); 128 | } 129 | 130 | publishFeedback(feedback) { 131 | if (this._currentGoal) { 132 | this._currentGoal.publishFeedback(feedback); 133 | } 134 | } 135 | 136 | setAborted(result, text) { 137 | if (this._currentGoal) { 138 | if (!result) { 139 | result = this._as._createMessage('result'); 140 | } 141 | 142 | this._currentGoal.setAborted(result, text); 143 | } 144 | } 145 | 146 | setPreempted(result, text) { 147 | if (this._currentGoal) { 148 | if (!result) { 149 | result = this._as._createMessage('result'); 150 | } 151 | 152 | this._currentGoal.setCanceled(result, text); 153 | } 154 | } 155 | 156 | setSucceeded(result, text) { 157 | if (this._currentGoal) { 158 | if (!result) { 159 | result = this._as._createMessage('result'); 160 | } 161 | 162 | this._currentGoal.setSucceeded(result, text); 163 | } 164 | } 165 | 166 | _handleGoal(newGoal) { 167 | const hasGoal = this.isActive(); 168 | let acceptGoal = false; 169 | if (!hasGoal) { 170 | acceptGoal = true; 171 | } 172 | else { 173 | let stamp = this._nextGoal ? this._nextGoal.getGoalId().stamp 174 | : this._currentGoal.getGoalId().stamp; 175 | let newStamp = newGoal.getGoalId().stamp; 176 | 177 | acceptGoal = Time.timeComp(stamp, newStamp) <= 0; 178 | } 179 | 180 | if (acceptGoal) { 181 | if (this._nextGoal) { 182 | const result = this._as._createMessage('result'); 183 | this._nextGoal.setCancelled( 184 | result, 185 | 'This goal was canceled because another goal was received by the simple action server' 186 | ); 187 | } 188 | 189 | this._nextGoal = newGoal; 190 | this._newGoalPreemptRequest = false; 191 | 192 | if (hasGoal) { 193 | this._preemptRequested = true; 194 | this.emit('preempt'); 195 | } 196 | 197 | this.emit('goal'); 198 | } 199 | else { 200 | // FIXME: make debug 201 | log.warn('Not accepting new goal'); 202 | } 203 | } 204 | 205 | _handleCancel(goal) { 206 | if (this._currentGoal && this._currentGoal.id === goal.id) { 207 | this._preemptRequested = true; 208 | this.emit('preempt'); 209 | } 210 | else if (this._nextGoal && this._nextGoal.id === goal.id) { 211 | this._newGoalPreemptRequest = true; 212 | } 213 | } 214 | 215 | _runExecuteLoop(timeoutMs = 100) { 216 | this._executeLoopTimer = setTimeout(() => { 217 | if (this._shutdown) { 218 | return; 219 | } 220 | log.infoThrottle(1000, 'execute loop'); 221 | if (this.isActive()) { 222 | log.error('Should never reach this code with an active goal!'); 223 | } 224 | else if (this.isNewGoalAvailable()) { 225 | const goal = this.acceptNewGoal(); 226 | this._executeCallback(goal) 227 | .then(() => { 228 | if (this.isActive()) { 229 | log.warn('%s\n%s\n%s', 230 | 'Your executeCallback did not set the goal to a terminate status', 231 | 'This is a bug in your ActionServer implementation. Fix your code!', 232 | 'For now, the ActionServer will set this goal to aborted' 233 | ); 234 | 235 | this.setAborted( 236 | this._as._createMessage('result'), 237 | 'This goal was aborted by the simple action server. The user should have set a terminal status on this goal and did not' 238 | ); 239 | } 240 | 241 | this._runExecuteLoop(0); 242 | }); 243 | } 244 | else { 245 | this._runExecuteLoop(100); 246 | } 247 | }, timeoutMs); 248 | } 249 | } 250 | 251 | module.exports = SimpleActionServer; 252 | -------------------------------------------------------------------------------- /src/examples/fibonacciAction.js: -------------------------------------------------------------------------------- 1 | const rosnodejs = require('../index.js'); 2 | const { FibonacciGoal, FibonacciResult, FibonacciFeedback } = rosnodejs.require('actionlib_tutorials').msg; 3 | 4 | rosnodejs.initNode('fibonacci') 5 | .then(() => { 6 | function executeCallback(goal) { 7 | const feedback = new FibonacciFeedback(); 8 | 9 | rosnodejs.log.info('got goal!'); 10 | feedback.sequence.push(0, 1); 11 | rosnodejs.log.info('Executing fibonacci sequence %d', goal.order); 12 | 13 | function _exec(iter, done) { 14 | if (iter <= goal.order) { 15 | if (as.isPreemptRequested() || !rosnodejs.ok()) { 16 | rosnodejs.log.warn('PREEMPTED!'); 17 | as.setPreempted(); 18 | done(); 19 | } 20 | else { 21 | feedback.sequence.push(feedback.sequence[iter] + feedback.sequence[iter-1]); 22 | rosnodejs.log.info('Update: %j', feedback.sequence); 23 | as.publishFeedback(feedback); 24 | setTimeout(_exec, 100, iter+1, done); 25 | } 26 | } 27 | else { 28 | // done! 29 | const result = new FibonacciResult(); 30 | result.sequence = feedback.sequence; 31 | rosnodejs.log.info('Succeeded!'); 32 | as.setSucceeded(result); 33 | done(); 34 | } 35 | } 36 | 37 | return new Promise(function(resolve) { 38 | _exec(1, resolve); 39 | }); 40 | } 41 | 42 | const as = new rosnodejs.SimpleActionServer({ 43 | nh: rosnodejs.nh, 44 | type: 'actionlib_tutorials/Fibonacci', 45 | actionServer: '/fibonacci', 46 | executeCallback 47 | }); 48 | 49 | as.start(); 50 | 51 | const ac = new rosnodejs.SimpleActionClient({ 52 | nh: rosnodejs.nh, 53 | type: 'actionlib_tutorials/Fibonacci', 54 | actionServer: '/fibonacci' 55 | }); 56 | 57 | ac.waitForServer() 58 | .then(() => { 59 | rosnodejs.log.info('Connected to action server!'); 60 | 61 | const goal = { order: 20 }; 62 | ac.sendGoal(goal, 63 | function(result) { rosnodejs.log.info('Result: %j', result) }, 64 | function() { console.log('ACTIVE'); }, 65 | function(feedback) { rosnodejs.log.info('%j', feedback); } 66 | ); 67 | 68 | ac.waitForResult({secs: 30, nsecs: 0}) 69 | .then((finished) => { 70 | if (finished) { 71 | rosnodejs.log.info('Action finished: %s', ac.getState()); 72 | } 73 | else { 74 | rosnodejs.log.info('Action did not finish before the time out'); 75 | } 76 | 77 | rosnodejs.shutdown() 78 | .then(() => { 79 | rosnodejs.log.info('Shutdown complete!'); 80 | rosnodejs.reset(); 81 | }); 82 | }); 83 | }); 84 | }) 85 | .catch(function(err) { 86 | console.error(err.stack); 87 | }); 88 | -------------------------------------------------------------------------------- /src/examples/on_the_fly.js: -------------------------------------------------------------------------------- 1 | 'use strict'; 2 | 3 | let rosnodejs = require('../index.js'); 4 | 5 | rosnodejs.initNode('/my_node', { onTheFly: true}) 6 | .then((rosNode) => { 7 | 8 | const std_msgs = rosnodejs.require('std_msgs').msg; 9 | const msg = new std_msgs.String(); 10 | 11 | const SetBool = rosnodejs.require('std_srvs').srv.SetBool; 12 | const request = new SetBool.Request(); 13 | 14 | // EXP 1) Service Server 15 | let service = rosNode.advertiseService( 16 | '/set_bool','std_srvs/SetBool', 17 | (req, resp) => { 18 | console.log('Handling request! ' + JSON.stringify(req)); 19 | resp.success = !req.data; 20 | resp.message = 'Inverted!'; 21 | return true; 22 | }); 23 | 24 | // EXP 2) Service Client 25 | setTimeout(function() { 26 | let serviceClient = rosNode.serviceClient('/set_bool','std_srvs/SetBool'); 27 | rosNode.waitForService(serviceClient.getService(), 2000) 28 | .then((available) => { 29 | if (available) { 30 | const request = new SetBool.Request(); 31 | request.data = true; 32 | serviceClient.call(request).then((resp) => { 33 | console.log('Service response ' + JSON.stringify(resp)); 34 | }); 35 | } else { 36 | console.log('Service not available'); 37 | } 38 | }); 39 | }, 1000); // wait a second before calling our service 40 | 41 | // EXP 3) Params 42 | rosNode.setParam('~junk', {'hi': 2}).then(() => { 43 | rosNode.getParam('~junk').then((val) => { 44 | console.log('Got Param!!! ' + JSON.stringify(val)); 45 | }); 46 | }); 47 | 48 | // // EXP 4) Publisher 49 | let pub = rosNode.advertise('/my_topic','std_msgs/String', { 50 | queueSize: 1, 51 | latching: true, 52 | throttleMs: 9 53 | }); 54 | 55 | // EXP 5) Subscriber 56 | let sub = rosNode.subscribe( 57 | '/my_topic', 58 | 'std_msgs/String', 59 | (data) => { 60 | console.log('SUB DATA ', data, data.data); 61 | }, 62 | {queueSize: 1, 63 | throttleMs: 1000}); 64 | 65 | let msgStart = 'my message '; 66 | let iter = 0; 67 | setInterval(() => { 68 | msg.data = msgStart + iter 69 | pub.publish(msg); 70 | ++iter; 71 | if (iter > 200) { 72 | iter = 0; 73 | } 74 | }, 5); 75 | }); 76 | -------------------------------------------------------------------------------- /src/examples/standard.js: -------------------------------------------------------------------------------- 1 | 'use strict'; 2 | 3 | let rosnodejs = require('../index.js'); 4 | const std_msgs = rosnodejs.require('std_msgs').msg; 5 | const SetBool = rosnodejs.require('std_srvs').srv.SetBool; 6 | 7 | rosnodejs.initNode('/test_node', { node: { forceExit: true }}) 8 | .then((rosNode) => { 9 | // EXP 1) Service Server 10 | let service = rosNode.advertiseService('set_bool', SetBool, 11 | (req, resp) => { 12 | rosnodejs.log.info('Handling request! ' + JSON.stringify(req)); 13 | resp.success = !req.data; 14 | resp.message = 'Inverted!'; 15 | return true; 16 | }); 17 | 18 | // EXP 2) Service Client 19 | let serviceClient = rosNode.serviceClient('set_bool', 'std_srvs/SetBool', {persist: true}); 20 | rosNode.waitForService(serviceClient.getService(), 2000) 21 | .then((available) => { 22 | if (available) { 23 | const request = new SetBool.Request(); 24 | request.data = true; 25 | serviceClient.call(request).then((resp) => { 26 | rosnodejs.log.info('Service response ' + JSON.stringify(resp)); 27 | }) 28 | .then(() => { 29 | request.data = false; 30 | serviceClient.call(request).then((resp) => { 31 | rosnodejs.log.info('Service response 2 ' + JSON.stringify(resp)); 32 | }); 33 | }) 34 | .then(() => { 35 | let serviceClient2 = rosNode.serviceClient('set_bool', 'std_srvs/SetBool'); 36 | serviceClient2.call(request).then((resp) => { 37 | rosnodejs.log.info('Non persistent response ' + JSON.stringify(resp)); 38 | }) 39 | }); 40 | } 41 | }); 42 | 43 | // EXP 3) Params 44 | rosNode.setParam('junk', {'hi': 2}).then(() => { 45 | rosNode.getParam('junk').then((val) => { rosnodejs.log.info('Got Param!!! ' + JSON.stringify(val)); }); 46 | }); 47 | 48 | // EXP 4) Publisher 49 | let pub = rosNode.advertise( 'my_topic', std_msgs.String, 50 | { 51 | queueSize: 1, 52 | latching: true, 53 | throttleMs: 9 54 | } 55 | ); 56 | 57 | let msgStart = 'my message '; 58 | let iter = 0; 59 | const msg = new std_msgs.String(); 60 | setInterval(() => { 61 | msg.data = msgStart + iter; 62 | pub.publish(msg); 63 | ++iter; 64 | if (iter > 200) { 65 | iter = 0; 66 | } 67 | }, 5); 68 | 69 | // EXP 5) Subscriber 70 | let sub = rosNode.subscribe('my_topic', 'std_msgs/String', 71 | (data) => { 72 | rosnodejs.log.info('SUB DATA ' + data.data); 73 | }, 74 | { 75 | queueSize: 1, 76 | throttleMs: 1000 77 | } 78 | ); 79 | }) 80 | .catch((err) => { 81 | rosnodejs.log.error(err.stack); 82 | }); 83 | -------------------------------------------------------------------------------- /src/examples/turtle.js: -------------------------------------------------------------------------------- 1 | /** 2 | An example of using rosnodejs with turtlesim, incl. services, 3 | pub/sub, and actionlib. This example uses the on-demand generated 4 | messages. 5 | */ 6 | 7 | 'use strict'; 8 | 9 | let rosnodejs = require('../index.js'); 10 | rosnodejs.initNode('/my_node', {onTheFly: true}).then((rosNode) => { 11 | 12 | // get list of existing publishers, subscribers, and services 13 | rosNode._node._masterApi.getSystemState("/my_node").then((data) => { 14 | console.log("getSystemState, result", data, data.publishers[0]); 15 | }); 16 | 17 | // --------------------------------------------------------- 18 | // Service Call 19 | 20 | const TeleportRelative = rosnodejs.require('turtlesim').srv.TeleportRelative; 21 | const teleport_request = new TeleportRelative.Request({ 22 | linear: -1, 23 | angular: 0.0 24 | }); 25 | 26 | let serviceClient = rosNode.serviceClient("/turtle1/teleport_relative", 27 | "turtlesim/TeleportRelative"); 28 | 29 | rosNode.waitForService(serviceClient.getService(), 2000) 30 | .then((available) => { 31 | if (available) { 32 | serviceClient.call(teleport_request, (resp) => { 33 | console.log('Service response ' + JSON.stringify(resp)); 34 | }); 35 | } else { 36 | console.log('Service not available'); 37 | } 38 | }); 39 | 40 | 41 | // --------------------------------------------------------- 42 | // Subscribe 43 | rosNode.subscribe( 44 | '/turtle1/pose', 45 | 'turtlesim/Pose', 46 | (data) => { 47 | console.log('pose', data); 48 | }, 49 | {queueSize: 1, 50 | throttleMs: 1000}); 51 | 52 | // --------------------------------------------------------- 53 | // Publish 54 | // equivalent to: 55 | // rostopic pub /turtle1/cmd_vel geometry_msgs/Twist '[1, 0, 0]' '[0, 0, 0]' 56 | let cmd_vel = rosNode.advertise('/turtle1/cmd_vel','geometry_msgs/Twist', { 57 | queueSize: 1, 58 | latching: true, 59 | throttleMs: 9 60 | }); 61 | 62 | const Twist = rosnodejs.require('geometry_msgs').msg.Twist; 63 | const msgTwist = new Twist({ 64 | linear: { x: 1, y: 0, z: 0 }, 65 | angular: { x: 0, y: 0, z: 1 } 66 | }); 67 | cmd_vel.publish(msgTwist); 68 | 69 | 70 | // --------------------------------------------------------- 71 | // test actionlib 72 | // rosrun turtlesim turtlesim_node 73 | // rosrun turtle_actionlib shape_server 74 | 75 | // wait two seconds for previous example to complete 76 | setTimeout(function() { 77 | const shapeActionGoal = rosnodejs.require('turtle_actionlib').msg.ShapeActionGoal; 78 | const ac = rosnodejs.nh.actionClient( 79 | "/turtle_shape", 80 | "turtle_actionlib/Shape" 81 | ); 82 | ac.sendGoal(new shapeActionGoal({ 83 | goal: { 84 | edges: 3, 85 | radius: 1 86 | } 87 | })); 88 | setTimeout(function() { 89 | ac.cancel(); 90 | }, 1000); 91 | }, 2000); 92 | 93 | // --------------------------------------------------------- 94 | // test int64 + uint64 95 | 96 | rosNode.subscribe( 97 | '/int64', 98 | 'std_msgs/Int64', 99 | (data) => { 100 | console.log('int64', data); 101 | }, 102 | {queueSize: 1, 103 | throttleMs: 1000}); 104 | 105 | let int64pub = rosNode.advertise('/int64','std_msgs/Int64', { 106 | queueSize: 1, 107 | latching: true, 108 | throttleMs: 9 109 | }); 110 | const Int64 = rosnodejs.require('std_msgs').msg.Int64; 111 | int64pub.publish(new Int64({ data: "429496729456789012" })); 112 | 113 | rosNode.subscribe( 114 | '/uint64', 115 | 'std_msgs/UInt64', 116 | (data) => { 117 | console.log('uint64', data); 118 | }, 119 | {queueSize: 1, 120 | throttleMs: 1000}); 121 | 122 | let uint64pub = rosNode.advertise('/uint64','std_msgs/UInt64', { 123 | queueSize: 1, 124 | latching: true, 125 | throttleMs: 9 126 | }); 127 | const UInt64 = rosnodejs.require('std_msgs').msg.UInt64; 128 | uint64pub.publish(new UInt64({ data: "9223372036854775807" })); 129 | 130 | }); 131 | -------------------------------------------------------------------------------- /src/lib/ActionClientInterface.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const msgUtils = require('../utils/message_utils.js'); 21 | const EventEmitter = require('events'); 22 | const Time = require('./Time.js'); 23 | const GoalIdGenerator = require('../actions/GoalIdGenerator.js'); 24 | let GoalID = null; 25 | let Header = null; 26 | 27 | class ActionClientInterface extends EventEmitter { 28 | constructor(options) { 29 | super(); 30 | 31 | if (GoalID === null) { 32 | GoalID = msgUtils.requireMsgPackage('actionlib_msgs').msg.GoalID; 33 | } 34 | 35 | if (Header === null) { 36 | Header = msgUtils.requireMsgPackage('std_msgs').msg.Header; 37 | } 38 | 39 | this._actionType = options.type; 40 | 41 | this._actionServer = options.actionServer; 42 | 43 | const nh = options.nh; 44 | 45 | const goalOptions = Object.assign({ queueSize: 10, latching: false }, options.goal); 46 | this._goalPub = nh.advertise(this._actionServer + '/goal', 47 | this._actionType + 'ActionGoal', 48 | goalOptions); 49 | 50 | const cancelOptions = Object.assign({ queueSize: 10, latching: false }, options.cancel); 51 | this._cancelPub = nh.advertise(this._actionServer + '/cancel', 52 | 'actionlib_msgs/GoalID', 53 | cancelOptions); 54 | 55 | const statusOptions = Object.assign({ queueSize: 1 }, options.status); 56 | this._statusSub = nh.subscribe(this._actionServer + '/status', 57 | 'actionlib_msgs/GoalStatusArray', 58 | (msg) => { this._handleStatus(msg); }, 59 | statusOptions); 60 | 61 | const feedbackOptions = Object.assign({ queueSize: 1 }, options.feedback); 62 | this._feedbackSub = nh.subscribe(this._actionServer + '/feedback', 63 | this._actionType + 'ActionFeedback', 64 | (msg) => { this._handleFeedback(msg); }, 65 | feedbackOptions); 66 | 67 | const resultOptions = Object.assign({ queueSize: 1 }, options.result); 68 | this._resultSub = nh.subscribe(this._actionServer + '/result', 69 | this._actionType + 'ActionResult', 70 | (msg) => { this._handleResult(msg); }, 71 | resultOptions); 72 | 73 | this._hasStatus = false; 74 | } 75 | 76 | getType() { 77 | return this._actionType; 78 | } 79 | 80 | /** 81 | * Cancel the given goal. If none is given, send an empty goal message, 82 | * i.e. cancel all goals. See 83 | * http://wiki.ros.org/actionlib/DetailedDescription#The_Messages 84 | * @param [goalId] {string} id of the goal to cancel 85 | */ 86 | cancel(goalId, stamp = null) { 87 | if (!stamp) { 88 | stamp = Time.now(); 89 | } 90 | 91 | const cancelGoal = new GoalID({ stamp }); 92 | if (!goalId) { 93 | this._cancelPub.publish(cancelGoal); 94 | } 95 | else { 96 | cancelGoal.id = goalId; 97 | this._cancelPub.publish(cancelGoal); 98 | } 99 | } 100 | 101 | sendGoal(goal) { 102 | this._goalPub.publish(goal); 103 | } 104 | 105 | waitForActionServerToStart(timeoutMs) { 106 | let isConnected = this.isServerConnected(); 107 | if (isConnected) { 108 | return Promise.resolve(true); 109 | } 110 | else { 111 | if (typeof timeoutMs !== 'number') { 112 | timeoutMs = 0; 113 | } 114 | 115 | return this._waitForActionServerToStart(timeoutMs, Date.now()); 116 | } 117 | } 118 | 119 | _waitForActionServerToStart(timeoutMs, start) { 120 | return setTimeoutP(100) 121 | .then(() => { 122 | if (this.isServerConnected()) { 123 | return true; 124 | } 125 | else if (timeoutMs > 0 && start + timeoutMs > Date.now()) { 126 | return false; 127 | } 128 | else { 129 | return this._waitForActionServerToStart(timeoutMs, start); 130 | } 131 | }); 132 | } 133 | 134 | generateGoalId(now) { 135 | return GoalIdGenerator(now); 136 | } 137 | 138 | isServerConnected() { 139 | return this._hasStatus && 140 | this._goalPub.getNumSubscribers() > 0 && 141 | this._cancelPub.getNumSubscribers() > 0 && 142 | this._statusSub.getNumPublishers() > 0 && 143 | this._feedbackSub.getNumPublishers() > 0 && 144 | this._resultSub.getNumPublishers() > 0; 145 | } 146 | 147 | /** 148 | * Shuts down this ActionClient. It shuts down publishers, subscriptions 149 | * and removes all attached event listeners. 150 | * @returns {Promise} 151 | */ 152 | 153 | shutdown() { 154 | this.removeAllListeners(); 155 | 156 | return Promise.all([ 157 | this._goalPub.shutdown(), 158 | this._cancelPub.shutdown(), 159 | this._statusSub.shutdown(), 160 | this._feedbackSub.shutdown(), 161 | this._resultSub.shutdown() 162 | ]); 163 | } 164 | 165 | _handleStatus(msg) { 166 | this._hasStatus = true; 167 | this.emit('status', msg); 168 | } 169 | 170 | _handleFeedback(msg) { 171 | this.emit('feedback', msg); 172 | } 173 | 174 | _handleResult(msg) { 175 | this.emit('result', msg); 176 | } 177 | } 178 | 179 | function setTimeoutP(timeoutMs) { 180 | return new Promise(function(resolve) { 181 | setTimeout(resolve, timeoutMs); 182 | }); 183 | } 184 | 185 | module.exports = ActionClientInterface; 186 | -------------------------------------------------------------------------------- /src/lib/ActionServerInterface.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017 Rethink Robotics 3 | * 4 | * Copyright 2017 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const timeUtils = require('../utils/time_utils.js'); 21 | const msgUtils = require('../utils/message_utils.js'); 22 | const EventEmitter = require('events'); 23 | 24 | let GoalID = null; 25 | let goalCount = 0; 26 | 27 | class ActionServerInterface extends EventEmitter { 28 | constructor(options) { 29 | super(); 30 | 31 | if (GoalID === null) { 32 | GoalID = msgUtils.requireMsgPackage('actionlib_msgs').msg.GoalID; 33 | } 34 | 35 | this._actionType = options.type; 36 | 37 | this._actionServer = options.actionServer; 38 | 39 | const nh = options.nh; 40 | this._nh = nh; 41 | 42 | const goalOptions = Object.assign({ queueSize: 50 }, options.goal); 43 | this._goalSub = nh.subscribe(this._actionServer + '/goal', 44 | this._actionType + 'ActionGoal', 45 | (msg) => { this._handleGoal(msg); }, 46 | goalOptions); 47 | 48 | const cancelOptions = Object.assign({ queueSize: 50 }, options.cancel); 49 | this._cancelSub = nh.subscribe(this._actionServer + '/cancel', 50 | 'actionlib_msgs/GoalID', 51 | (msg) => { this._handleCancel(msg); }, 52 | cancelOptions); 53 | 54 | const statusOptions = Object.assign({ queueSize: 50 }, options.status); 55 | this._statusPub = nh.advertise(this._actionServer + '/status', 56 | 'actionlib_msgs/GoalStatusArray', 57 | statusOptions); 58 | 59 | const feedbackOptions = Object.assign({ queueSize: 50 }, options.feedback); 60 | this._feedbackPub = nh.advertise(this._actionServer + '/feedback', 61 | this._actionType + 'ActionFeedback', 62 | feedbackOptions); 63 | 64 | const resultOptions = Object.assign({ queueSize: 50 }, options.result); 65 | this._resultPub = nh.advertise(this._actionServer + '/result', 66 | this._actionType + 'ActionResult', 67 | resultOptions); 68 | } 69 | 70 | getType() { 71 | return this._actionType; 72 | } 73 | 74 | generateGoalId() { 75 | const now = timeUtils.now(); 76 | return new GoalID({ 77 | id: `${this._nh.getNodeName()}-${goalCount++}-${now.sec}.${now.nsec}`, 78 | stamp: now 79 | }); 80 | } 81 | 82 | shutdown() { 83 | return Promise.all([ 84 | this._goalSub.shutdown(), 85 | this._cancelSub.shutdown(), 86 | this._statusPub.shutdown(), 87 | this._feedbackPub.shutdown(), 88 | this._resultPub.shutdown(), 89 | ]); 90 | } 91 | 92 | _handleGoal(msg) { 93 | this.emit('goal', msg); 94 | } 95 | 96 | _handleCancel(msg) { 97 | this.emit('cancel', msg); 98 | } 99 | 100 | publishResult(resultMsg) { 101 | this._resultPub.publish(resultMsg); 102 | } 103 | 104 | publishFeedback(feedbackMsg) { 105 | this._feedbackPub.publish(feedbackMsg); 106 | } 107 | 108 | publishStatus(statusMsg) { 109 | this._statusPub.publish(statusMsg); 110 | } 111 | } 112 | 113 | module.exports = ActionServerInterface; 114 | -------------------------------------------------------------------------------- /src/lib/MasterApiClient.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | "use strict"; 19 | 20 | let xmlrpc = require('xmlrpc-rosnodejs'); 21 | let networkUtils = require('../utils/network_utils.js'); 22 | let Logging = require('./Logging.js'); 23 | const XmlrpcClient = require('../utils/XmlrpcClient.js'); 24 | 25 | //----------------------------------------------------------------------- 26 | 27 | class MasterApiClient { 28 | 29 | constructor(rosMasterUri, logName) { 30 | this._log = Logging.getLogger(Logging.DEFAULT_LOGGER_NAME + '.masterapi'); 31 | this._log.info('Connecting to ROS Master at ' + rosMasterUri); 32 | this._xmlrpcClient = new XmlrpcClient(networkUtils.getAddressAndPortFromUri(rosMasterUri), this._log); 33 | }; 34 | 35 | getXmlrpcClient() { 36 | return this._xmlrpcClient; 37 | } 38 | 39 | _call(method, data, resolve, reject, options = {}) { 40 | this._xmlrpcClient.call(method, data, resolve, reject, options); 41 | } 42 | 43 | registerService(callerId, service, serviceUri, uri, options) { 44 | let data = [ 45 | callerId, 46 | service, 47 | serviceUri, 48 | uri 49 | ]; 50 | 51 | return new Promise((resolve, reject) => { 52 | this._call('registerService', data, resolve, reject, options); 53 | }); 54 | } 55 | 56 | unregisterService(callerId, service, serviceUri, options) { 57 | let data = [ 58 | callerId, 59 | service, 60 | serviceUri 61 | ]; 62 | 63 | return new Promise((resolve, reject) => { 64 | this._call('unregisterService', data, resolve, reject, options); 65 | }); 66 | } 67 | 68 | registerSubscriber(callerId, topic, topicType, uri, options) { 69 | let data = [ 70 | callerId, 71 | topic, 72 | topicType, 73 | uri 74 | ]; 75 | return new Promise((resolve, reject) => { 76 | this._call('registerSubscriber', data, resolve, reject, options); 77 | }); 78 | } 79 | 80 | unregisterSubscriber(callerId, topic, uri, options) { 81 | let data = [ 82 | callerId, 83 | topic, 84 | uri 85 | ]; 86 | return new Promise((resolve, reject) => { 87 | this._call('unregisterSubscriber', data, resolve, reject, options); 88 | }); 89 | } 90 | 91 | registerPublisher(callerId, topic, topicType, uri, options) { 92 | let data = [ 93 | callerId, 94 | topic, 95 | topicType, 96 | uri 97 | ]; 98 | return new Promise((resolve, reject) => { 99 | this._call('registerPublisher', data, resolve, reject, options); 100 | }); 101 | } 102 | 103 | unregisterPublisher(callerId, topic, uri, options) { 104 | let data = [ 105 | callerId, 106 | topic, 107 | uri 108 | ]; 109 | return new Promise((resolve, reject) => { 110 | this._call('unregisterPublisher', data, resolve, reject, options); 111 | }); 112 | } 113 | 114 | lookupNode(callerId, nodeName, options) { 115 | let data = [callerId, nodeName]; 116 | return new Promise((resolve, reject) => { 117 | this._call('lookupNode', data, resolve, reject, options); 118 | }); 119 | } 120 | 121 | getPublishedTopics(callerId, subgraph, options) { 122 | let data = [callerId, subgraph]; 123 | return new Promise((resolve,reject)=>{ 124 | this._call( 125 | 'getPublishedTopics', 126 | data, 127 | function(data) { 128 | return resolve({ 129 | topics: data[2].map((topic) => { return { 130 | name: topic[0], type: topic[1] 131 | }}) 132 | }) 133 | }, 134 | reject, 135 | options); 136 | }) 137 | } 138 | 139 | getTopicTypes(callerId, options) { 140 | let data = [callerId]; 141 | return new Promise((resolve,reject)=>{ 142 | this._call( 143 | 'getTopicTypes', 144 | data, 145 | function(data) { 146 | return resolve({ 147 | topics: data[2].map((topic) => { return { 148 | name: topic[0], type: topic[1] 149 | }}) 150 | }) 151 | }, 152 | reject, 153 | options); 154 | }) 155 | } 156 | 157 | /** return an object containing all current publishers (by topic), 158 | subscribers (by topic), and services (by name) */ 159 | getSystemState(callerId, options) { 160 | function toObject(memo, sublist) { 161 | memo[sublist[0]] = sublist[1]; 162 | return memo; 163 | } 164 | 165 | let data = [callerId]; 166 | return new Promise((resolve, reject) => { 167 | this._call( 168 | 'getSystemState', 169 | data, 170 | function(data) { 171 | return resolve({ 172 | publishers: data[2][0].reduce(toObject, {}), 173 | subscribers: data[2][1].reduce(toObject, {}), 174 | services: data[2][2].reduce(toObject, {}) 175 | }); 176 | }, 177 | reject, 178 | options 179 | ); 180 | }); 181 | } 182 | 183 | getUri(callerId, options) { 184 | let data = [callerId]; 185 | return new Promise((resolve, reject) => { 186 | this._call('getUri', data, resolve, reject, options); 187 | }); 188 | } 189 | 190 | lookupService(callerId, service, options) { 191 | let data = [callerId, service]; 192 | return new Promise((resolve, reject) => { 193 | this._call('lookupService', data, resolve, reject, options); 194 | }); 195 | } 196 | }; 197 | 198 | //----------------------------------------------------------------------- 199 | 200 | module.exports = MasterApiClient; 201 | -------------------------------------------------------------------------------- /src/lib/Names.js: -------------------------------------------------------------------------------- 1 | const CLEAN_REGEX = /\/\//g; 2 | 3 | // http://wiki.ros.org/Names 4 | class Names { 5 | constructor() { 6 | this._remappings = {}; 7 | this._namespace = ''; 8 | } 9 | 10 | init(remappings, namespace) { 11 | this._namespace = namespace; 12 | this._remappings = {}; 13 | 14 | Object.keys(remappings).forEach((left) => { 15 | if (left && !left.startsWith('_')) { 16 | const right = remappings[left]; 17 | 18 | const resolvedLeft = this.resolve(left, false); 19 | const resolvedRight = this.resolve(right, false); 20 | 21 | this._remappings[resolvedLeft] = resolvedRight; 22 | } 23 | }); 24 | } 25 | 26 | validate(name, throwError = false) { 27 | if (typeof name !== 'string') { 28 | if (throwError) { 29 | throw new Error('Unable to validate non-string name'); 30 | } 31 | return false; 32 | } 33 | 34 | const len = name.length; 35 | if (len === 0) { 36 | return true; 37 | } 38 | // else 39 | // First character must be alphanumeric, '/', or '~' 40 | const c = name[0]; 41 | if (!isAlpha(c) && c !== '/' && c !== '~') { 42 | if (throwError) { 43 | throw new Error(`Character [${c}] is not valid as the first character in Graph Resource Name [${name}]. Valid characters are a-z, A-Z, / and in some cases ~.`); 44 | } 45 | // else 46 | return false; 47 | } 48 | 49 | for (let i = 1; i < len; ++i) { 50 | if (!isValidCharInName(name[i])) { 51 | if (throwError) { 52 | throw new Error(`Character [${name[i]}] at element [${i}] is not valid in Graph Resource Name [${name}]. Valid characters are a-z, A-Z, 0-9, / and _.`); 53 | } 54 | // else 55 | return false; 56 | } 57 | } 58 | 59 | return true; 60 | } 61 | 62 | clean(name) { 63 | name = name.replace(CLEAN_REGEX, '/'); 64 | 65 | if (name.endsWith('/')) { 66 | return name.substr(0, -1); 67 | } 68 | // else 69 | return name; 70 | } 71 | 72 | append(left, right) { 73 | return this.clean(left + '/' + right); 74 | } 75 | 76 | remap(name) { 77 | return this.resolve(name, true); 78 | } 79 | 80 | /** 81 | * @param [namespace] {string} namespace to resolve name to. If not provided, node's namespace will be used 82 | * @param name {string} name to resolve 83 | * @param [remap] {bool} flag indicating if we should also attempt to remap the name 84 | */ 85 | resolve(...args) { 86 | let [namespace, name, remap] = this._parseResolveArgs(args); 87 | 88 | this.validate(name, true); 89 | 90 | if (name.length === 0) { 91 | if (namespace.length === 0) { 92 | return '/'; 93 | } 94 | else if (namespace[0] === '/') { 95 | return namespace 96 | } 97 | // else 98 | return '/' + namespace; 99 | } 100 | 101 | if (name.startsWith('~')) { 102 | name = name.replace('~', this._namespace + '/'); 103 | } 104 | 105 | if (!name.startsWith('/')) { 106 | name = namespace + '/' + name; 107 | } 108 | 109 | name = this.clean(name); 110 | 111 | if (remap) { 112 | name = this._remap(name); 113 | } 114 | 115 | return name; 116 | } 117 | 118 | parentNamespace(name) { 119 | this.validate(name, true); 120 | 121 | if (name.length === 0) { 122 | return ''; 123 | } 124 | else if (name === '/') { 125 | return '/'; 126 | } 127 | 128 | let p = name.lastIndexOf('/'); 129 | if (p === name.length - 1) { 130 | p = name.lastIndexOf('/', p - 1); 131 | } 132 | 133 | if (p < 0) { 134 | return ''; 135 | } 136 | else if (p === 0) { 137 | return '/'; 138 | } 139 | // else 140 | return name.substring(0, p); 141 | } 142 | 143 | _remap(name) { 144 | return this._remappings[name] || name; 145 | } 146 | 147 | _parseResolveArgs(args) { 148 | let name, namespace = this._namespace, remap = true; 149 | switch (args.length) { 150 | case 0: 151 | name = ''; 152 | break; 153 | case 1: 154 | name = args[0]; 155 | break; 156 | case 2: 157 | if (typeof args[1] === 'string') { 158 | [namespace, name] = args; 159 | } 160 | else { 161 | [name, remap] = args; 162 | } 163 | break; 164 | default: 165 | [namespace, name, remap] = args; 166 | break; 167 | } 168 | 169 | return [namespace, name, remap]; 170 | } 171 | } 172 | 173 | module.exports = new Names(); 174 | 175 | //------------------------------------------------------------------ 176 | // Local Helper functions 177 | //------------------------------------------------------------------ 178 | 179 | 180 | 181 | function isAlpha(char) { 182 | if (char >= 'A' && char <= 'Z') { 183 | return true; 184 | } 185 | else if (char >= 'a' && char <= 'z') { 186 | return true; 187 | } 188 | // else 189 | return false; 190 | } 191 | 192 | function isAlnum(char) { 193 | if (isAlpha(char)) { 194 | return true; 195 | } 196 | else if (char >= '0' && char <= '9') { 197 | return true; 198 | } 199 | // else 200 | return false; 201 | } 202 | 203 | function isValidCharInName(char) { 204 | return (isAlnum(char) || char == '/' || char == '_'); 205 | } 206 | -------------------------------------------------------------------------------- /src/lib/ParamServerApiClient.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | const Logging = require('./Logging.js'); 20 | const XmlrpcClient = require('../utils/XmlrpcClient.js'); 21 | 22 | //----------------------------------------------------------------------- 23 | 24 | class ParamServerApiClient { 25 | constructor(xmlrpcClient) { 26 | this._log = Logging.getLogger(Logging.DEFAULT_LOGGER_NAME + '.params'); 27 | 28 | this._xmlrpcClient = xmlrpcClient; 29 | } 30 | 31 | _call(method, data, resolve, reject) { 32 | this._xmlrpcClient.call(method, data, resolve, reject); 33 | } 34 | 35 | deleteParam(callerId, key) { 36 | let data = [ 37 | callerId, 38 | key 39 | ]; 40 | 41 | return new Promise((resolve, reject) => { 42 | this._call('deleteParam', data, resolve, reject); 43 | }); 44 | } 45 | 46 | setParam(callerId, key, value) { 47 | let data = [ 48 | callerId, 49 | key, 50 | value 51 | ]; 52 | 53 | return new Promise((resolve, reject) => { 54 | this._call('setParam', data, resolve, reject); 55 | }); 56 | } 57 | 58 | getParam(callerId, key) { 59 | let data = [ 60 | callerId, 61 | key 62 | ]; 63 | 64 | return new Promise((resolve, reject) => { 65 | this._call('getParam', data, (resp) => { 66 | // resp[2] is the actual parameter value, and presumably all anyone cares about 67 | resolve(resp[2]); 68 | }, reject); 69 | }); 70 | } 71 | 72 | searchParam(callerId, key) { 73 | throw new Error('NOT IMPLEMENTED'); 74 | } 75 | 76 | subscribeParam(callerId, key) { 77 | throw new Error('NOT IMPLEMENTED'); 78 | } 79 | 80 | unsubscribeParam(callerId, key) { 81 | throw new Error('NOT IMPLEMENTED'); 82 | } 83 | 84 | hasParam(callerId, key) { 85 | let data = [ 86 | callerId, 87 | key 88 | ]; 89 | 90 | return new Promise((resolve, reject) => { 91 | this._call('hasParam', data, (resp) => { 92 | // resp[2] is whether it actually has param and presumably all anyone cares about 93 | resolve(resp[2]); 94 | }, reject); 95 | }); 96 | } 97 | 98 | getParamNames(callerId) { 99 | let data = [ 100 | callerId 101 | ]; 102 | 103 | return new Promise((resolve, reject) => { 104 | this._call('getParamNames', data, (resp) => { 105 | // resp[2] is parameter name list and presumably all anyone cares about 106 | resolve(resp[2]); 107 | }, reject); 108 | }); 109 | } 110 | } 111 | 112 | //----------------------------------------------------------------------- 113 | 114 | module.exports = ParamServerApiClient; 115 | -------------------------------------------------------------------------------- /src/lib/Publisher.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, 12 | * software distributed under the License is distributed on an "AS 13 | * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 14 | * express or implied. See the License for the specific language 15 | * governing permissions and limitations under the License. 16 | */ 17 | 18 | "use strict"; 19 | 20 | const EventEmitter = require('events'); 21 | const Ultron = require('ultron'); 22 | const {rebroadcast} = require('../utils/event_utils.js'); 23 | 24 | /** 25 | * @class Publisher 26 | * Public facing publishers class. Allows users to send messages to subscribers 27 | * on a given topic. 28 | */ 29 | class Publisher extends EventEmitter { 30 | constructor(impl) { 31 | super(); 32 | 33 | ++impl.count; 34 | this._impl = impl; 35 | this._ultron = new Ultron(impl); 36 | 37 | this._topic = impl.getTopic(); 38 | this._type = impl.getType(); 39 | 40 | rebroadcast('registered', this._ultron, this); 41 | rebroadcast('connection', this._ultron, this); 42 | rebroadcast('disconnect', this._ultron, this); 43 | rebroadcast('error', this._ultron, this); 44 | } 45 | 46 | /** 47 | * Get the topic this publisher is publishing on 48 | * @returns {string} 49 | */ 50 | getTopic() { 51 | return this._topic; 52 | } 53 | 54 | /** 55 | * Get the type of message this publisher is sending 56 | * (e.g. std_msgs/String) 57 | * @returns {string} 58 | */ 59 | getType() { 60 | return this._type; 61 | } 62 | 63 | /** 64 | * Check if this publisher is latching 65 | * @returns {boolean} 66 | */ 67 | getLatching() { 68 | if (this._impl) { 69 | return this._impl.getLatching(); 70 | } 71 | // else 72 | return false; 73 | } 74 | 75 | /** 76 | * Get the numbber of subscribers currently connected to this publisher 77 | * @returns {number} 78 | */ 79 | getNumSubscribers() { 80 | if (this._impl) { 81 | return this._impl.getNumSubscribers(); 82 | } 83 | // else 84 | return 0; 85 | } 86 | 87 | /** 88 | * Shuts down this publisher. If this is the last publisher on this topic 89 | * for this node, closes the publisher and unregisters the topic from Master 90 | * @returns {Promise} 91 | */ 92 | shutdown() { 93 | const topic= this.getTopic(); 94 | if (this._impl) { 95 | const impl = this._impl 96 | this._impl = null; 97 | this._ultron.destroy(); 98 | this._ultron = null; 99 | 100 | --impl.count; 101 | if (impl.count <= 0) { 102 | return impl.getNode().unadvertise(impl.getTopic()); 103 | } 104 | 105 | this.removeAllListeners(); 106 | } 107 | // else 108 | return Promise.resolve(); 109 | } 110 | 111 | /** 112 | * Check if this publisher has been shutdown 113 | * @returns {boolean} 114 | */ 115 | isShutdown() { 116 | return !!this._impl; 117 | } 118 | 119 | /** 120 | * Schedule the msg for publishing - or publish immediately if we're 121 | * supposed to 122 | * @param msg {object} object type matching this._type 123 | * @param [throttleMs] {number} optional override for publisher setting 124 | */ 125 | publish(msg, throttleMs) { 126 | this._impl.publish(msg, throttleMs); 127 | } 128 | } 129 | 130 | module.exports = Publisher; 131 | -------------------------------------------------------------------------------- /src/lib/ServiceServer.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | "use strict"; 19 | 20 | const net = require('net'); 21 | const NetworkUtils = require('../utils/network_utils.js'); 22 | const ros_msg_utils = require('../ros_msg_utils'); 23 | const base_serializers = ros_msg_utils.Serialize; 24 | const SerializationUtils = require('../utils/serialization_utils.js'); 25 | const DeserializeStream = SerializationUtils.DeserializeStream; 26 | const Deserialize = SerializationUtils.Deserialize; 27 | const Serialize = SerializationUtils.Serialize; 28 | const TcprosUtils = require('../utils/tcpros_utils.js'); 29 | const EventEmitter = require('events'); 30 | const Logging = require('./Logging.js'); 31 | const {REGISTERING, REGISTERED, SHUTDOWN} = require('../utils/ClientStates.js'); 32 | 33 | class ServiceServer extends EventEmitter { 34 | constructor(options, callback, nodeHandle) { 35 | super(); 36 | this._service = options.service; 37 | 38 | this._type = options.type; 39 | 40 | this._port = null; 41 | 42 | this._nodeHandle = nodeHandle; 43 | 44 | this._log = Logging.getLogger('ros.rosnodejs'); 45 | 46 | this._requestCallback = callback; 47 | 48 | if (!options.typeClass) { 49 | throw new Error(`Unable to load service for service ${this.getService()} with type ${this.getType()}`); 50 | } 51 | this._messageHandler = options.typeClass; 52 | 53 | this._clients = {}; 54 | 55 | this._state = REGISTERING; 56 | 57 | this._register(); 58 | }; 59 | 60 | getService() { 61 | return this._service; 62 | } 63 | 64 | getType() { 65 | return this._type; 66 | } 67 | 68 | getPersist() { 69 | return this._persist; 70 | } 71 | 72 | isCallInProgress() { 73 | return this._calling; 74 | } 75 | 76 | getServiceUri() { 77 | return NetworkUtils.formatServiceUri(this._port); 78 | } 79 | 80 | getClientUris() { 81 | return Object.keys(this._clients); 82 | } 83 | 84 | /** 85 | * The ROS client shutdown code is a little noodly. Users can close a client through 86 | * the ROS node or the client itself and both are correct. Either through a node.unadvertise() 87 | * call or a client.shutdown() call - in both instances a call needs to be made to the ROS master 88 | * and the client needs to tear itself down. 89 | */ 90 | shutdown() { 91 | this._nodeHandle.unadvertiseService(this.getService()); 92 | } 93 | 94 | isShutdown() { 95 | return this._state === SHUTDOWN; 96 | } 97 | 98 | disconnect() { 99 | this._state = SHUTDOWN; 100 | 101 | Object.keys(this._clients).forEach((clientId) => { 102 | const client = this._clients[clientId]; 103 | 104 | client.$deserializeStream.removeAllListeners(); 105 | 106 | client.end(); 107 | client.destroy(); 108 | }); 109 | 110 | this._clients = {}; 111 | } 112 | 113 | handleClientConnection(client, header) { 114 | if (this.isShutdown()) { 115 | return; 116 | } 117 | // else 118 | // TODO: verify header data 119 | this._log.debug('Service %s handling new client connection ', this.getService()); 120 | 121 | const error = TcprosUtils.validateServiceClientHeader(header, this.getService(), this._messageHandler.md5sum()); 122 | if (error) { 123 | this._log.error('Error while validating service %s connection header: %s', this.getService(), error); 124 | client.end(Serialize(TcprosUtils.createTcpRosError(error))); 125 | return; 126 | } 127 | 128 | let respHeader = 129 | TcprosUtils.createServiceServerHeader( 130 | this._nodeHandle.getNodeName(), 131 | this._messageHandler.md5sum(), 132 | this.getType()); 133 | client.write(respHeader); 134 | 135 | client.$persist = (header['persistent'] === '1'); 136 | 137 | // bind to message handler 138 | client.$messageHandler = this._handleMessage.bind(this, client); 139 | client.$deserializeStream.on('message', client.$messageHandler); 140 | 141 | client.on('close', () => { 142 | delete this._clients[client.name]; 143 | this._log.debug('Service client %s disconnected!', client.name); 144 | }); 145 | 146 | this._clients[client.name] = client; 147 | this.emit('connection', header, client.name); 148 | } 149 | 150 | _handleMessage(client, data) { 151 | this._log.trace('Service ' + this.getService() + ' got message! ' + data.toString('hex')); 152 | // deserialize msg 153 | const req = this._messageHandler.Request.deserialize(data); 154 | 155 | // call service callback 156 | let resp = new this._messageHandler.Response(); 157 | let result = this._requestCallback(req, resp); 158 | Promise.resolve(result) 159 | .then((success) => { 160 | // client should already have been closed, so if we got here just cut out early 161 | if (this.isShutdown()) { 162 | return; 163 | } 164 | 165 | const serializeResponse = TcprosUtils.serializeServiceResponse( 166 | this._messageHandler.Response, 167 | resp, 168 | success 169 | ); 170 | 171 | // send service response 172 | client.write(serializeResponse); 173 | 174 | if (!client.$persist) { 175 | this._log.debug('Closing non-persistent client'); 176 | client.end(); 177 | delete this._clients[client.name]; 178 | } 179 | }); 180 | } 181 | 182 | _register() { 183 | this._nodeHandle.registerService(this.getService()) 184 | .then((resp) => { 185 | // if we were shutdown between the starting the registration and now, bail 186 | if (this.isShutdown()) { 187 | return; 188 | } 189 | 190 | this._state = REGISTERED; 191 | this.emit('registered'); 192 | }); 193 | } 194 | } 195 | 196 | module.exports = ServiceServer; 197 | -------------------------------------------------------------------------------- /src/lib/SlaveApiClient.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | "use strict"; 19 | 20 | let xmlrpc = require('xmlrpc-rosnodejs'); 21 | 22 | //----------------------------------------------------------------------- 23 | 24 | class SlaveApiClient { 25 | 26 | constructor(host, port) { 27 | this._xmlrpcClient = xmlrpc.createClient({host: host, port: port}); 28 | }; 29 | 30 | requestTopic(callerId, topic, protocols) { 31 | let data = [callerId, topic, protocols]; 32 | return new Promise((resolve, reject) => { 33 | this._xmlrpcClient.methodCall('requestTopic', data, (err, resp) => { 34 | if (err || resp[0] !== 1) { 35 | reject(err, resp); 36 | } 37 | else { 38 | resolve(resp); 39 | } 40 | }); 41 | }); 42 | }; 43 | }; 44 | 45 | //----------------------------------------------------------------------- 46 | 47 | module.exports = SlaveApiClient; 48 | -------------------------------------------------------------------------------- /src/lib/Subscriber.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | "use strict"; 19 | 20 | const EventEmitter = require('events'); 21 | const Ultron = require('ultron'); 22 | const {rebroadcast} = require('../utils/event_utils.js'); 23 | 24 | //----------------------------------------------------------------------- 25 | 26 | /** 27 | * @class Subscriber 28 | * Public facing subscriber class. Allows users to listen to messages from 29 | * publishers on a given topic. 30 | */ 31 | class Subscriber extends EventEmitter { 32 | constructor(impl) { 33 | super(); 34 | 35 | ++impl.count; 36 | this._impl = impl; 37 | this._ultron = new Ultron(impl); 38 | 39 | this._topic = impl.getTopic(); 40 | this._type = impl.getType(); 41 | 42 | rebroadcast('registered', this._ultron, this); 43 | rebroadcast('connection', this._ultron, this); 44 | rebroadcast('disconnect', this._ultron, this); 45 | rebroadcast('error', this._ultron, this); 46 | rebroadcast('message', this._ultron, this); 47 | } 48 | 49 | /** 50 | * Get the topic this publisher is publishing on 51 | * @returns {string} 52 | */ 53 | getTopic() { 54 | return this._topic; 55 | } 56 | 57 | /** 58 | * Get the type of message this publisher is sending 59 | * (e.g. std_msgs/String) 60 | * @returns {string} 61 | */ 62 | getType() { 63 | return this._type; 64 | } 65 | 66 | /** 67 | * Get the number of publishers currently connected to this subscriber 68 | * @returns {number} 69 | */ 70 | getNumPublishers() { 71 | if (this._impl) { 72 | return this._impl.getNumPublishers(); 73 | } 74 | // else 75 | return 0; 76 | } 77 | 78 | /** 79 | * Shuts down this subscriber. If this is the last subscriber on this topic 80 | * for this node, closes the subscriber and unregisters the topic from Master 81 | * @returns {Promise} 82 | */ 83 | shutdown() { 84 | if (this._impl) { 85 | const impl = this._impl 86 | this._impl = null; 87 | this._ultron.destroy(); 88 | this._ultron = null; 89 | 90 | --impl.count; 91 | if (impl.count <= 0) { 92 | return impl.getNode().unsubscribe(impl.getTopic()); 93 | } 94 | 95 | this.removeAllListeners(); 96 | } 97 | // else 98 | return Promise.resolve(); 99 | } 100 | 101 | /** 102 | * Check if this publisher has been shutdown 103 | * @returns {boolean} 104 | */ 105 | isShutdown() { 106 | return !!this._impl; 107 | } 108 | } 109 | 110 | //----------------------------------------------------------------------- 111 | 112 | module.exports = Subscriber; 113 | -------------------------------------------------------------------------------- /src/lib/ThisNode.js: -------------------------------------------------------------------------------- 1 | module.exports = { 2 | node: null, 3 | 4 | getNodeName() { 5 | if (this.node) { 6 | return this.node.getNodeName(); 7 | } 8 | // else 9 | return null; 10 | }, 11 | 12 | ok() { 13 | return this.node && !this.node.isShutdown(); 14 | }, 15 | 16 | shutdown() { 17 | if (this.ok()) { 18 | return this.node.shutdown(); 19 | } 20 | // else 21 | return Promise.resolve(); 22 | }, 23 | 24 | on(evt, handler) { 25 | if (this.ok()) { 26 | return this.node.on(evt, handler); 27 | } 28 | }, 29 | 30 | once(evt, handler) { 31 | if (this.ok()) { 32 | return this.node.once(evt, handler); 33 | } 34 | }, 35 | 36 | removeListener(evt, handler) { 37 | if (this.ok()) { 38 | return this.node.removeListener(evt, handler); 39 | } 40 | } 41 | }; 42 | -------------------------------------------------------------------------------- /src/lib/Time.js: -------------------------------------------------------------------------------- 1 | const timeUtils = require('../utils/time_utils.js'); 2 | 3 | let simTimeSub = null; 4 | let simTime = timeUtils.dateToRosTime(0); 5 | 6 | function handleSimTimeMessage(msg) { 7 | simTime = msg.clock; 8 | } 9 | 10 | const Time = { 11 | useSimTime: false, 12 | 13 | _initializeRosTime(rosnodejs, notime) { 14 | //Only for testing purposes! 15 | if (notime) { 16 | return Promise.resolve(); 17 | } 18 | const nh = rosnodejs.nh; 19 | return nh.getParam('/use_sim_time') 20 | .then((val) => { 21 | this.useSimTime = val; 22 | 23 | if (val) { 24 | simTimeSub = nh.subscribe('/clock', 'rosgraph_msgs/Clock', handleSimTimeMessage, {throttleMs: -1}); 25 | } 26 | }) 27 | .catch((err) => { 28 | if (err.statusCode === undefined) { 29 | throw err; 30 | } 31 | }); 32 | }, 33 | 34 | now() { 35 | if (this.useSimTime) { 36 | return simTime; 37 | } 38 | // else 39 | return timeUtils.now(); 40 | }, 41 | 42 | rosTimeToDate: timeUtils.rosTimeToDate, 43 | dateToRosTime: timeUtils.dateToRosTime, 44 | epoch: timeUtils.epoch, 45 | isZeroTime: timeUtils.isZeroTime, 46 | toNumber: timeUtils.toNumber, 47 | toSeconds: timeUtils.toSeconds, 48 | timeComp: timeUtils.timeComp, 49 | add: timeUtils.add, 50 | lt: timeUtils.lt, 51 | }; 52 | 53 | module.exports = Time; 54 | -------------------------------------------------------------------------------- /src/ros_msg_utils/index.js: -------------------------------------------------------------------------------- 1 | const MessageCache = require('./lib/message_cache.js'); 2 | 3 | module.exports = { 4 | Serialize: require('./lib/base_serialize.js'), 5 | Deserialize: require('./lib/base_deserialize.js'), 6 | getByteLength: require('./lib/encoding_utils.js').getByteLength, 7 | Find: MessageCache.Find, 8 | CMAKE_PREFIX_PATH: MessageCache.CMAKE_PREFIX_PATH, 9 | CMAKE_PATHS: MessageCache.CMAKE_PATHS, 10 | MESSAGE_PATH: MessageCache.MESSAGE_PATH, 11 | packageMap: MessageCache.packageMap 12 | }; 13 | -------------------------------------------------------------------------------- /src/ros_msg_utils/lib/base_deserialize.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const BN = require('bn.js'); 21 | 22 | /*----------------------------------------------------------------------------- 23 | * Primitive Deserialization Functions 24 | * 25 | * Each primitive type deserialization function has an identical signature 26 | * 27 | * @param buffer {Buffer} buffer to deserialize value from 28 | * @param bufferOffset {Array.Number} array of length 1 with the current bufferOffset 29 | * stored at index 0. This value should be incremented according to the size of 30 | * the value pulled from the buffer. 31 | * @returns {*} the specified value from the buffer 32 | * DeserializeFunc(buffer, bufferOffset) 33 | *-----------------------------------------------------------------------------*/ 34 | 35 | function StringDeserializer(buffer, bufferOffset) { 36 | const len = UInt32Deserializer(buffer, bufferOffset); 37 | const str = buffer.toString('utf8', bufferOffset[0], bufferOffset[0] + len) 38 | bufferOffset[0] += len; 39 | return str; 40 | } 41 | 42 | function UInt8Deserializer(buffer, bufferOffset) { 43 | const val = buffer.readUInt8(bufferOffset[0], true); 44 | bufferOffset[0] += 1; 45 | return val; 46 | } 47 | 48 | function UInt16Deserializer(buffer, bufferOffset) { 49 | let val = buffer.readUInt16LE(bufferOffset[0], true); 50 | bufferOffset[0] += 2; 51 | return val; 52 | } 53 | 54 | function UInt32Deserializer(buffer, bufferOffset) { 55 | let val = buffer.readUInt32LE(bufferOffset[0], true); 56 | bufferOffset[0] += 4; 57 | return val; 58 | } 59 | 60 | function UInt64Deserializer(buffer, bufferOffset) { 61 | let slice = buffer.slice(bufferOffset[0], bufferOffset[0] + 8); 62 | let val = new BN(slice, 'le'); 63 | bufferOffset[0] += 8; 64 | return val; 65 | } 66 | 67 | function Int8Deserializer(buffer, bufferOffset) { 68 | let val = buffer.readInt8(bufferOffset[0], true); 69 | bufferOffset[0] += 1; 70 | return val; 71 | } 72 | 73 | function Int16Deserializer(buffer, bufferOffset) { 74 | let val = buffer.readInt16LE(bufferOffset[0], true); 75 | bufferOffset[0] += 2; 76 | return val; 77 | } 78 | 79 | function Int32Deserializer(buffer, bufferOffset) { 80 | let val = buffer.readInt32LE(bufferOffset[0], true); 81 | bufferOffset[0] += 4; 82 | return val; 83 | } 84 | 85 | function Int64Deserializer(buffer, bufferOffset) { 86 | let slice = buffer.slice(bufferOffset[0], bufferOffset[0] + 8); 87 | let val = new BN(slice, 'le').fromTwos(64); 88 | bufferOffset[0] += 8; 89 | return val; 90 | } 91 | 92 | function Float32Deserializer(buffer, bufferOffset) { 93 | let val = buffer.readFloatLE(bufferOffset[0], true); 94 | bufferOffset[0] += 4; 95 | return val; 96 | } 97 | 98 | function Float64Deserializer(buffer, bufferOffset) { 99 | let val = buffer.readDoubleLE(bufferOffset[0], true); 100 | bufferOffset[0] += 8; 101 | return val; 102 | } 103 | 104 | function TimeDeserializer(buffer, bufferOffset) { 105 | const secs = Int32Deserializer(buffer, bufferOffset); 106 | const nsecs = Int32Deserializer(buffer, bufferOffset); 107 | return {secs: secs, nsecs: nsecs}; 108 | } 109 | 110 | function BoolDeserializer(buffer, bufferOffset) { 111 | const val = !!buffer.readInt8(bufferOffset[0], true); 112 | bufferOffset[0] += 1; 113 | return val; 114 | } 115 | 116 | /*----------------------------------------------------------------------------- 117 | * Primitive Array Deserialization Functions 118 | * 119 | * Each primitive type array deserialization function has an identical signature 120 | * 121 | * @param buffer {Buffer} buffer to deserialize value from 122 | * @param bufferOffset {Array.Number} array of length 1 with the current bufferOffset 123 | * stored at index 0. This value should be incremented according to the size of 124 | * the value pulled from the buffer. 125 | * @param [arrayLen] {Number|null} 126 | * a negative number or null means to deserialize a variable length array from the buffer 127 | * a positive number means to deserialize a constant length array from the buffer 128 | * @returns {*} the specified value from the buffer 129 | * DeserializeArrayFunc(buffer, bufferOffset, arrayLen) 130 | *-----------------------------------------------------------------------------*/ 131 | 132 | /** 133 | * @function getArrayLen helper function to deserialize the length of an array 134 | * when necessary. Array length is just encoded as a uint32. 135 | * @returns {Number} 136 | */ 137 | const getArrayLen = UInt32Deserializer; 138 | 139 | /** 140 | * Template for most primitive array deserializers which are bound to this function and provide 141 | * the deserializeFunc param 142 | * @param deserializeFunc {function} function to deserialize a single instance of the type. Typically hidden 143 | * from users by binding. 144 | * @param buffer {Buffer} buffer to deserialize data from 145 | * @param bufferOffset {Array.number} 146 | * @param arrayLen {null|number} 147 | * @returns {Array} 148 | * @constructor 149 | */ 150 | function DefaultArrayDeserializer(deserializeFunc, buffer, bufferOffset, arrayLen=null) { 151 | // interpret a negative array len as a variable length array 152 | // so we need to parse its length ourselves 153 | if (arrayLen === null || arrayLen < 0) { 154 | arrayLen = getArrayLen(buffer, bufferOffset); 155 | } 156 | const array = new Array(arrayLen); 157 | for (let i = 0; i < arrayLen; ++i) { 158 | array[i] = deserializeFunc(buffer, bufferOffset, null); 159 | } 160 | return array; 161 | } 162 | 163 | /** 164 | * Specialized array deserialization for UInt8 Arrays 165 | * We return the raw buffer when deserializing uint8 arrays because it's much faster 166 | */ 167 | function UInt8ArrayDeserializer(buffer, bufferOffset, arrayLen=null) { 168 | if (arrayLen === null || arrayLen < 0) { 169 | arrayLen = getArrayLen(buffer, bufferOffset); 170 | } 171 | 172 | const array = buffer.slice(bufferOffset[0], bufferOffset[0] + arrayLen); 173 | bufferOffset[0] += arrayLen; 174 | return array; 175 | } 176 | 177 | //----------------------------------------------------------------------------- 178 | 179 | const PrimitiveDeserializers = { 180 | string: StringDeserializer, 181 | float32: Float32Deserializer, 182 | float64: Float64Deserializer, 183 | bool: BoolDeserializer, 184 | int8: Int8Deserializer, 185 | int16: Int16Deserializer, 186 | int32: Int32Deserializer, 187 | int64: Int64Deserializer, 188 | uint8: UInt8Deserializer, 189 | uint16: UInt16Deserializer, 190 | uint32: UInt32Deserializer, 191 | uint64: UInt64Deserializer, 192 | char: UInt8Deserializer, 193 | byte: Int8Deserializer, 194 | time: TimeDeserializer, 195 | duration: TimeDeserializer 196 | }; 197 | 198 | const ArrayDeserializers = { 199 | string: DefaultArrayDeserializer.bind(null, StringDeserializer), 200 | float32: DefaultArrayDeserializer.bind(null, Float32Deserializer), 201 | float64: DefaultArrayDeserializer.bind(null, Float64Deserializer), 202 | bool: DefaultArrayDeserializer.bind(null, BoolDeserializer), 203 | int8: DefaultArrayDeserializer.bind(null, Int8Deserializer), 204 | int16: DefaultArrayDeserializer.bind(null, Int16Deserializer), 205 | int32: DefaultArrayDeserializer.bind(null, Int32Deserializer), 206 | int64: DefaultArrayDeserializer.bind(null, Int64Deserializer), 207 | uint8: UInt8ArrayDeserializer, 208 | uint16: DefaultArrayDeserializer.bind(null, UInt16Deserializer), 209 | uint32: DefaultArrayDeserializer.bind(null, UInt32Deserializer), 210 | uint64: DefaultArrayDeserializer.bind(null, UInt64Deserializer), 211 | char: UInt8ArrayDeserializer, 212 | byte: DefaultArrayDeserializer.bind(null, Int8Deserializer), 213 | time: DefaultArrayDeserializer.bind(null, TimeDeserializer), 214 | duration: DefaultArrayDeserializer.bind(null, TimeDeserializer) 215 | }; 216 | 217 | //----------------------------------------------------------------------------- 218 | 219 | module.exports = Object.assign({}, PrimitiveDeserializers, {Array: ArrayDeserializers}); 220 | -------------------------------------------------------------------------------- /src/ros_msg_utils/lib/base_serialize.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const BN = require('bn.js'); 21 | const getByteLength = require('./encoding_utils.js').getByteLength; 22 | 23 | /*----------------------------------------------------------------------------- 24 | * Primitive Serialization Functions 25 | * 26 | * Each primitive type serialization function has an identical signature 27 | * 28 | * @param value {*} value to serialize as determined by function name 29 | * @param buffer {Buffer} buffer to serialize value into 30 | * @param bufferOffset {Number} offset from buffer start to store value 31 | * @returns {Number} new buffer offset after serializing value 32 | * SerializeFunc(value, buffer, bufferOffset) 33 | *-----------------------------------------------------------------------------*/ 34 | 35 | function StringSerializer(value, buffer, bufferOffset) { 36 | let len = getByteLength(value); 37 | bufferOffset = buffer.writeUInt32LE(len, bufferOffset); 38 | return bufferOffset + buffer.write(value, bufferOffset, len, 'utf8'); 39 | } 40 | 41 | function UInt8Serializer(value, buffer, bufferOffset) { 42 | return buffer.writeUInt8(value, bufferOffset, true); 43 | } 44 | 45 | function UInt16Serializer(value, buffer, bufferOffset) { 46 | return buffer.writeUInt16LE(value, bufferOffset, true); 47 | } 48 | 49 | function UInt32Serializer(value, buffer, bufferOffset) { 50 | return buffer.writeUInt32LE(value, bufferOffset, true); 51 | } 52 | 53 | function UInt64Serializer(value, buffer, bufferOffset) { 54 | if (!BN.isBN(value)) { 55 | value = new BN(value); 56 | } 57 | 58 | const buf = value.toBuffer('le', 8); 59 | buffer.set(buf, bufferOffset); 60 | 61 | return bufferOffset + 8; 62 | } 63 | 64 | function Int8Serializer(value, buffer, bufferOffset) { 65 | return buffer.writeInt8(value, bufferOffset, true); 66 | } 67 | 68 | function Int16Serializer(value, buffer, bufferOffset) { 69 | return buffer.writeInt16LE(value, bufferOffset, true); 70 | } 71 | 72 | function Int32Serializer(value, buffer, bufferOffset) { 73 | return buffer.writeInt32LE(value, bufferOffset, true); 74 | } 75 | 76 | function Int64Serializer(value, buffer, bufferOffset) { 77 | if (!BN.isBN(value)) { 78 | value = new BN(value); 79 | } 80 | 81 | value = value.toTwos(64); 82 | 83 | const buf = value.toBuffer('le', 8); 84 | buffer.set(buf, bufferOffset); 85 | 86 | return bufferOffset + 8; 87 | } 88 | 89 | function Float32Serializer(value, buffer, bufferOffset) { 90 | return buffer.writeFloatLE(value, bufferOffset, true); 91 | } 92 | 93 | function Float64Serializer(value, buffer, bufferOffset) { 94 | return buffer.writeDoubleLE(value, bufferOffset, true); 95 | } 96 | 97 | function TimeSerializer(value, buffer, bufferOffset) { 98 | bufferOffset = buffer.writeInt32LE(value.secs, bufferOffset, true); 99 | return buffer.writeInt32LE(value.nsecs, bufferOffset, true); 100 | } 101 | 102 | function BoolSerializer(value, buffer, bufferOffset) { 103 | return buffer.writeInt8(value ? 1 : 0, bufferOffset, true); 104 | } 105 | 106 | /*----------------------------------------------------------------------------- 107 | * Primitive Array Serialization Functions 108 | * 109 | * Each primitive type array serialization function has an identical signature 110 | * 111 | * @param value {*} value to serialize as determined by function name 112 | * @param buffer {Buffer} buffer to serialize value into 113 | * @param bufferOffset {Number} offset from buffer start to store value 114 | * @param [specArrayLen] {Number|null} array length desired by message specification 115 | * a negative number or null means to serialize a variable length array into the buffer 116 | * a positive number means to serialize a constant length array from the buffer 117 | * @returns {Number} new buffer offset after serializing value 118 | * SerializeFunc(value, buffer, bufferOffset, specArrayLen) 119 | *-----------------------------------------------------------------------------*/ 120 | 121 | /** 122 | * Template for most primitive array serializers which are bound to this function and provide 123 | * the serializeFunc param 124 | * @param serializeFunc {function} function to serialize a single instance of the type. Typically hidden 125 | * from users by binding. 126 | * @param array {Array} array of values of the desired type 127 | * @param buffer {Buffer} buffer to serialize data into 128 | * @param bufferOffset {Array.number} 129 | * @param specArrayLen {null|number} 130 | * @returns {Number} buffer offset 131 | * @constructor 132 | */ 133 | function DefaultArraySerializer(serializeFunc, array, buffer, bufferOffset, specArrayLen=null) { 134 | const arrLen = array.length; 135 | 136 | if (specArrayLen === null || specArrayLen < 0) { 137 | bufferOffset = buffer.writeUInt32LE(arrLen, bufferOffset, true); 138 | } 139 | 140 | for (let i = 0; i < arrLen; ++i) { 141 | bufferOffset = serializeFunc(array[i], buffer, bufferOffset); 142 | } 143 | 144 | return bufferOffset; 145 | } 146 | 147 | /** 148 | * Specialized array serialization for UInt8 Arrays 149 | */ 150 | function UInt8ArraySerializer(array, buffer, bufferOffset, specArrayLen=null) { 151 | const arrLen = array.length; 152 | 153 | if (specArrayLen === null || specArrayLen < 0) { 154 | bufferOffset = buffer.writeUInt32LE(arrLen, bufferOffset, true); 155 | } 156 | 157 | buffer.set(array, bufferOffset); 158 | return bufferOffset + arrLen; 159 | } 160 | 161 | //----------------------------------------------------------------------------- 162 | 163 | const PrimitiveSerializers = { 164 | string: StringSerializer, 165 | float32: Float32Serializer, 166 | float64: Float64Serializer, 167 | bool: BoolSerializer, 168 | int8: Int8Serializer, 169 | int16: Int16Serializer, 170 | int32: Int32Serializer, 171 | int64: Int64Serializer, 172 | uint8: UInt8Serializer, 173 | uint16: UInt16Serializer, 174 | uint32: UInt32Serializer, 175 | uint64: UInt64Serializer, 176 | char: UInt8Serializer, 177 | byte: Int8Serializer, 178 | time: TimeSerializer, 179 | duration: TimeSerializer 180 | }; 181 | 182 | const ArraySerializers = { 183 | string: DefaultArraySerializer.bind(null, StringSerializer), 184 | float32: DefaultArraySerializer.bind(null, Float32Serializer), 185 | float64: DefaultArraySerializer.bind(null, Float64Serializer), 186 | bool: DefaultArraySerializer.bind(null, BoolSerializer), 187 | int8: DefaultArraySerializer.bind(null, Int8Serializer), 188 | int16: DefaultArraySerializer.bind(null, Int16Serializer), 189 | int32: DefaultArraySerializer.bind(null, Int32Serializer), 190 | int64: DefaultArraySerializer.bind(null, Int64Serializer), 191 | uint8: UInt8ArraySerializer, 192 | uint16: DefaultArraySerializer.bind(null, UInt16Serializer), 193 | uint32: DefaultArraySerializer.bind(null, UInt32Serializer), 194 | uint64: DefaultArraySerializer.bind(null, UInt64Serializer), 195 | char: UInt8ArraySerializer, 196 | byte: DefaultArraySerializer.bind(null, Int8Serializer), 197 | time: DefaultArraySerializer.bind(null, TimeSerializer), 198 | duration: DefaultArraySerializer.bind(null, TimeSerializer) 199 | }; 200 | 201 | //----------------------------------------------------------------------------- 202 | 203 | module.exports = Object.assign({}, PrimitiveSerializers, {Array: ArraySerializers}); 204 | -------------------------------------------------------------------------------- /src/ros_msg_utils/lib/encoding_utils.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | module.exports = { 21 | 22 | /** 23 | * Returns the number of bytes in a string (using utf8 encoding), 24 | * in order to know the length needed to serialize it into a Buffer 25 | * @param strValue 26 | * @return {Number} 27 | */ 28 | getByteLength(strValue) { 29 | return Buffer.byteLength(strValue, 'utf8'); 30 | } 31 | 32 | }; -------------------------------------------------------------------------------- /src/ros_msg_utils/lib/message_cache.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const fs = require('fs'); 21 | const path = require('path'); 22 | 23 | const CMAKE_PREFIX_PATH = process.env.CMAKE_PREFIX_PATH; 24 | if (!CMAKE_PREFIX_PATH) { 25 | throw new Error('Unable to find CMAKE_PREFIX_PATH environment variable. Did you source setup.bash?') 26 | } 27 | const cmakePaths = CMAKE_PREFIX_PATH.split(path.delimiter); 28 | const jsMsgPath = path.join('share', 'gennodejs', 'ros'); 29 | 30 | //----------------------------------------------------------------------- 31 | // Search through the CMAKE_PREFIX_PATH for generated javascript messages. 32 | // Caches paths for any of the packages found in the desired location. 33 | // rosnodejs and generated message files will consult this cache to require 34 | // message packages 35 | //----------------------------------------------------------------------- 36 | const packagePaths = {}; 37 | cmakePaths.forEach((cmakePath) => { 38 | const dirPath = path.join(cmakePath, jsMsgPath); 39 | try { 40 | let msgPackages = fs.readdirSync(dirPath); 41 | msgPackages.forEach((msgPackage) => { 42 | // If the message package has been found in a previous workspace, 43 | // don't overwrite it now. This is critical to enabling ws overlays. 44 | if (!packagePaths.hasOwnProperty(msgPackage)) { 45 | packagePaths[msgPackage] = path.join(dirPath, msgPackage, '_index.js'); 46 | } 47 | }); 48 | } 49 | catch(err) { 50 | // pass 51 | } 52 | }); 53 | 54 | module.exports = { 55 | Find(messagePackage) { 56 | if (packagePaths.hasOwnProperty(messagePackage)) { 57 | return require(packagePaths[messagePackage]); 58 | } 59 | // else 60 | throw new Error(`Unable to find message package ${messagePackage} from CMAKE_PREFIX_PATH`); 61 | }, 62 | 63 | CMAKE_PREFIX_PATH, 64 | CMAKE_PATHS: cmakePaths, 65 | MESSAGE_PATH: jsMsgPath, 66 | packageMap: Object.assign({}, packagePaths) 67 | }; 68 | -------------------------------------------------------------------------------- /src/ros_msg_utils/package.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "ros_msg_utils", 3 | "version": "1.0.2", 4 | "description": "Provides Helper functions for ros messages", 5 | "main": "index.js", 6 | "scripts": { 7 | "test": "mocha test/directory.js", 8 | "compile": "babel --presets es2015 index.js -d dist/ && babel --presets es2015 lib -d dist/lib/", 9 | "prepublish": "npm run compile" 10 | }, 11 | "keywords": [ 12 | "ros", 13 | "messages" 14 | ], 15 | "author": "chris smith", 16 | "license": "Apache-2.0", 17 | "repository": { 18 | "type": "git", 19 | "url": "git://github.com/RethinkRobotics-opensource/ros_msg_utils" 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /src/ros_msg_utils/test/directory.js: -------------------------------------------------------------------------------- 1 | require('./deserialization_test.js'); 2 | require('./serialization_test.js'); -------------------------------------------------------------------------------- /src/tools/flatten.js: -------------------------------------------------------------------------------- 1 | 'use strict' 2 | 3 | // a utility script that will flatten the directory structure for the message 4 | // files generated by genjs. 5 | 6 | let message_utils = require('./utils/message_utils.js'); 7 | 8 | let args = process.argv; 9 | console.log(args); 10 | 11 | if (args.length < 3) { 12 | throw new Error('Must include desired output directory in calls to flatten!'); 13 | } 14 | 15 | message_utils.findMessageFiles(); 16 | message_utils.flatten(args[2]); 17 | -------------------------------------------------------------------------------- /src/tools/generateMessages.js: -------------------------------------------------------------------------------- 1 | 'use strict'; 2 | const path = require('path'); 3 | const rosnodejs = require('../index.js'); 4 | const ArgumentParser = require('argparse').ArgumentParser; 5 | 6 | const parser = new ArgumentParser({ 7 | addHelp: true, 8 | description: 'Utility script to generate ROS messages' 9 | }); 10 | 11 | parser.addArgument( 12 | ['-p', '--pkg'], 13 | { 14 | help: 'Message package to build (e.g. std_msgs). Also builds dependencies' 15 | } 16 | ); 17 | parser.addArgument( 18 | ['-o', '--output'], 19 | { 20 | help: 'Directory to output message into (e.g. /tmp). Messages are built to devel space by default' 21 | } 22 | ); 23 | parser.addArgument( 24 | ['-v', '--verbose'], 25 | { 26 | action: 'storeTrue' 27 | } 28 | ); 29 | 30 | const args = parser.parseArgs(); 31 | 32 | if (args.output) { 33 | args.output = path.resolve(args.output); 34 | } 35 | 36 | if (args.pkg !== null) { 37 | rosnodejs.loadPackage(args.pkg, args.output, args.verbose); 38 | } 39 | else { 40 | rosnodejs.loadAllPackages(args.output, args.verbose) 41 | .then(() => { 42 | console.log('Message generation complete!'); 43 | process.exit(); 44 | }) 45 | .catch((err) => { 46 | console.error('Error while generating messages!'); 47 | process.exit(1) 48 | }); 49 | } -------------------------------------------------------------------------------- /src/utils/ClientStates.js: -------------------------------------------------------------------------------- 1 | module.exports = { 2 | REGISTERING: Symbol('rosnodejs.clientRegistering'), 3 | REGISTERED: Symbol('rosnodejs.clientRegistered'), 4 | SHUTDOWN: Symbol('rosnodejs.clientShutdown') 5 | }; -------------------------------------------------------------------------------- /src/utils/XmlrpcClient.js: -------------------------------------------------------------------------------- 1 | 'use strict'; 2 | const EventEmitter = require('events'); 3 | const xmlrpc = require('xmlrpc-rosnodejs'); 4 | 5 | const CONNECTION_REFUSED='ECONNREFUSED'; 6 | const TRY_AGAIN_LIST = [1, 2, 2, 4, 4, 4, 4, 8, 8, 8, 8, 16, 16, 32, 64, 128, 256, 512, 1024, 2048]; 7 | 8 | class XmlrpcCall { 9 | constructor(method, data, resolve, reject, options={}) { 10 | this.method = method; 11 | this.data = data; 12 | this.resolve = resolve; 13 | this.reject = reject; 14 | 15 | this.maxAttempts = options.maxAttempts || Infinity; 16 | } 17 | 18 | call(client) { 19 | return new Promise((resolve, reject) => { 20 | client.methodCall(this.method, this.data, (err, resp) => { 21 | if (err) { 22 | reject(err); 23 | } 24 | else if (resp[0] !== 1) { 25 | const msg = resp[1]; 26 | const error = new Error(`ROS XMLRPC Error: ${msg}`); 27 | error.code = 'EROSAPIERROR'; 28 | error.statusCode = resp[0]; 29 | error.statusMessage = msg; 30 | error.value = resp[2]; 31 | reject(error); 32 | } 33 | else { 34 | resolve(resp); 35 | } 36 | }); 37 | }); 38 | } 39 | } 40 | 41 | class XmlrpcClient extends EventEmitter { 42 | constructor(clientAddressInfo, log) { 43 | super(); 44 | 45 | this._xmlrpcClient = xmlrpc.createClient(clientAddressInfo); 46 | 47 | this._log = log; 48 | 49 | this._callQueue = []; 50 | 51 | this._timeout = 0; 52 | this._timeoutId = null; 53 | 54 | this._failedAttempts = 0; 55 | } 56 | 57 | getClient() { 58 | return this._xmlrpcClient; 59 | } 60 | 61 | call(method, data, resolve, reject, options) { 62 | const newCall = new XmlrpcCall(method, data, resolve, reject, options); 63 | const numCalls = this._callQueue.length; 64 | this._callQueue.push(newCall); 65 | // if nothing else was on the queue, try executing the call now 66 | if (numCalls === 0) { 67 | this._tryExecuteCall(); 68 | } 69 | } 70 | 71 | clear() { 72 | this._log.info('Clearing xmlrpc client queue...'); 73 | if (this._callQueue.length !== 0) { 74 | this._callQueue[0].reject(new Error('Clearing call queue - probably shutting down...')); 75 | } 76 | clearTimeout(this._timeoutId); 77 | this._callQueue = []; 78 | } 79 | 80 | _tryExecuteCall() { 81 | if (this._callQueue.length === 0) { 82 | this._log.warn('Tried executing xmlprc call on empty queue'); 83 | return; 84 | } 85 | // else 86 | const call = this._callQueue[0]; 87 | this._log.info('Try execute call %s: %j', call.method, call.data); 88 | call.call(this._xmlrpcClient) 89 | .then((resp) => { 90 | // call succeeded, clean up and call its handler 91 | this._log.info('Call %s %j succeeded! %j', call.method, call.data, resp); 92 | this._shiftQueue(); 93 | this._resetTimeout(); 94 | call.resolve(resp); 95 | }) 96 | .catch((err) => { 97 | ++this._failedAttempts; 98 | this._log.info('Call %s %j failed! %s', call.method, call.data, err); 99 | if (err instanceof Error && 100 | err.code === CONNECTION_REFUSED && 101 | this._failedAttempts < call.maxAttempts) { 102 | // Call failed to connect - try to connect again. 103 | // All future calls would have same error since they're 104 | // directed at the same xmlrpc server. 105 | this._log.info('Trying call again on attempt %d of %d', this._failedAttempts, call.maxAttempts); 106 | this._scheduleTryAgain(); 107 | this.emit(CONNECTION_REFUSED, err, this._failedAttempts); 108 | } 109 | else { 110 | // call failed - move on. 111 | this._shiftQueue(); 112 | this._resetTimeout(); 113 | call.reject(err); 114 | } 115 | }) 116 | .then(() => { 117 | if (this._timeoutId === null && this._callQueue.length > 0) { 118 | this._tryExecuteCall(); 119 | } 120 | }); 121 | } 122 | 123 | _shiftQueue() { 124 | this._callQueue.shift(); 125 | } 126 | 127 | _resetTimeout() { 128 | this._timeout = 0; 129 | this._timeoutId = null; 130 | this._failedAttempts = 0; 131 | } 132 | 133 | _scheduleTryAgain() { 134 | const timeout = TRY_AGAIN_LIST[this._timeout]; 135 | if (this._timeout + 1 < TRY_AGAIN_LIST.length) { 136 | ++this._timeout; 137 | } 138 | this._log.info('Scheduling call again in %dms', timeout); 139 | this._timeoutId = setTimeout(this._tryExecuteCall.bind(this), timeout); 140 | } 141 | } 142 | 143 | module.exports = XmlrpcClient; 144 | -------------------------------------------------------------------------------- /src/utils/event_utils.js: -------------------------------------------------------------------------------- 1 | module.exports = { 2 | rebroadcast(evt, emitter, rebroadcaster) { 3 | emitter.on(evt, rebroadcaster.emit.bind(rebroadcaster, evt)); 4 | } 5 | }; 6 | -------------------------------------------------------------------------------- /src/utils/log/ConsoleLogStream.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const bunyan = require('bunyan'); 21 | 22 | class ConsoleLogStream { 23 | constructor(options) { 24 | if (options.hasOwnProperty('formatter')) { 25 | this._formatter = options.formatter; 26 | } 27 | else { 28 | this._formatter = (rec) => { return rec.msg; }; 29 | } 30 | } 31 | 32 | write(rec) { 33 | let msg; 34 | if (typeof rec === 'string' || rec instanceof String) { 35 | console.log(rec); 36 | return; 37 | } 38 | else if (typeof rec === 'object') { 39 | const formattedMsg = this._formatter(rec); 40 | if (typeof formattedMsg === 'string' || formattedMsg instanceof String) { 41 | msg = formattedMsg; 42 | } 43 | else { 44 | console.error('Unable to format message %j', rec); 45 | return; 46 | } 47 | 48 | const logLevel = rec.level; 49 | if (logLevel <= bunyan.INFO) { 50 | console.info(msg); 51 | } 52 | else if (logLevel <= bunyan.WARN) { 53 | console.warn(msg); 54 | } 55 | else { // logLevel === bunyan.ERROR || bunyan.FATAL 56 | console.error(msg); 57 | } 58 | } 59 | } 60 | }; 61 | 62 | module.exports = ConsoleLogStream; 63 | -------------------------------------------------------------------------------- /src/utils/log/LogFormatter.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const moment= require('moment'); 21 | const bunyan = require('bunyan'); 22 | const timeUtils = require('../time_utils.js'); 23 | 24 | const DEFAULT_FORMAT = '[${severity}] [${time}] (${logger}): ${message}'; 25 | const CONSOLE_FORMAT = process.env.ROSCONSOLE_JS_FORMAT || DEFAULT_FORMAT; 26 | const CONSOLE_TOKEN_REGEX = /\${([a-z|A-Z]+)}/g; 27 | 28 | class LogFormatter { 29 | constructor() { 30 | this._tokens = []; 31 | 32 | this._parseFormat(); 33 | this._numTokens = this._tokens.length; 34 | } 35 | 36 | _parseFormat() { 37 | let match; 38 | let lastMatchIndex = 0; 39 | while ((match = CONSOLE_TOKEN_REGEX.exec(CONSOLE_FORMAT)) !== null) { 40 | const preToken = CONSOLE_FORMAT.substr(lastMatchIndex, match.index - lastMatchIndex); 41 | if (preToken.length > 0) { 42 | this._tokens.push(new DefaultToken(preToken)); 43 | } 44 | this._tokens.push(this._getTokenizer(match[1])); 45 | lastMatchIndex = match.index + match[0].length; 46 | } 47 | const postToken = CONSOLE_FORMAT.substr(lastMatchIndex); 48 | if (postToken.length > 0) { 49 | this._tokens.push(new DefaultToken(postToken)); 50 | } 51 | } 52 | 53 | _getTokenizer(token) { 54 | switch(token) { 55 | case 'severity': 56 | return new SeverityToken(); 57 | case 'message': 58 | return new MessageToken(); 59 | case 'time': 60 | return new TimeToken(); 61 | case 'logger': 62 | return new LoggerToken(); 63 | case 'isodate': 64 | return new IsoDateToken(); 65 | default: 66 | return new DefaultToken(token); 67 | } 68 | } 69 | 70 | format(rec) { 71 | const fields = this._tokens.map((token) => { return token.format(rec); }); 72 | return fields.join(''); 73 | } 74 | } 75 | 76 | // ---------------------------------------------------------------------------------------- 77 | // Tokens used for log formatting 78 | 79 | class DefaultToken { 80 | constructor(val) { 81 | this.val = val; 82 | } 83 | 84 | format() { 85 | return this.val; 86 | } 87 | } 88 | 89 | class SeverityToken { 90 | constructor() { } 91 | 92 | format(rec) { 93 | return bunyan.nameFromLevel[rec.level].toUpperCase(); 94 | } 95 | } 96 | 97 | class MessageToken { 98 | constructor() { } 99 | 100 | format(rec) { 101 | return rec.msg; 102 | } 103 | } 104 | 105 | class TimeToken { 106 | constructor() { } 107 | 108 | format(rec) { 109 | const recTime = rec.time; 110 | return `${(recTime / 1000).toFixed(3)}`; 111 | } 112 | } 113 | 114 | class LoggerToken { 115 | constructor() { } 116 | 117 | format(rec) { 118 | return rec.scope || rec.name; 119 | } 120 | } 121 | 122 | class IsoDateToken { 123 | constructor() { } 124 | 125 | format(rec) { 126 | return moment(rec.time).format('YYYY-MM-DDTHH:mm:ss.SSSZZ') 127 | } 128 | } 129 | 130 | const logFormatter = new LogFormatter(); 131 | module.exports = logFormatter.format.bind(logFormatter); 132 | -------------------------------------------------------------------------------- /src/utils/log/Logger.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | const bunyan = require('bunyan'); 20 | const crypto = require('crypto'); 21 | 22 | //------------------------------------------------------------------------ 23 | 24 | /** 25 | * Logger is a minimal wrapper around a bunyan logger. It adds useful methods 26 | * to throttle/limit logging. 27 | * @class Logger 28 | */ 29 | class Logger { 30 | constructor(options) { 31 | options = options || {}; 32 | 33 | this._name = options.name; 34 | 35 | if (options.$parent) { 36 | this._logger = options.$parent.child(options.childOptions); 37 | } 38 | else { 39 | this._logger = bunyan.createLogger({ 40 | name: this._name, 41 | level: options.level || bunyan.INFO, 42 | streams: options.streams 43 | }); 44 | } 45 | 46 | this._throttledLogs = new Map(); 47 | this._onceLogs = new Set(); 48 | 49 | const logMethods = new Set(['trace', 'debug', 'info', 'warn', 'error', 'fatal']); 50 | this._createLogMethods(logMethods); 51 | this._createThrottleLogMethods(logMethods); 52 | this._createOnceLogMethods(logMethods); 53 | } 54 | 55 | getStreams() { 56 | return this._logger.streams; 57 | } 58 | 59 | child(childOptions) { 60 | // setup options 61 | const name = childOptions.name; 62 | delete childOptions.name; 63 | const options = { 64 | childOptions: childOptions, 65 | $parent: this._logger, 66 | name 67 | }; 68 | 69 | // create logger 70 | return new Logger(options); 71 | } 72 | 73 | level(level=null) { 74 | this._logger.level(level); 75 | } 76 | 77 | setLevel(level) { 78 | this._logger.level(level); 79 | } 80 | 81 | getLevel() { 82 | return this._logger.level(); 83 | } 84 | 85 | getName() { 86 | return this._name; 87 | } 88 | 89 | addStream(stream) { 90 | this._logger.addStream(stream, this.getLevel()); 91 | } 92 | 93 | clearStreams() { 94 | this._logger.streams = []; 95 | } 96 | 97 | /** 98 | * Binds to bunyan logger's method for each method (info, debug, etc) 99 | * @param methods {Set.} 100 | */ 101 | _createLogMethods(methods) { 102 | methods.forEach((method) => { 103 | if (this.hasOwnProperty(method)) { 104 | throw new Error('Unable to create method %s', method); 105 | } 106 | this[method] = this._logger[method].bind(this._logger); 107 | }); 108 | } 109 | 110 | /** 111 | * Attaches throttled logging functions to this object for each level method 112 | * (info, debug, etc) 113 | * e.g. 114 | * logger.infoThrottle(1000, 'Hi'); 115 | * logger.debugThrottle(1000, 'Hi'); 116 | * Logs are throttled by a String key taken from the second argument (first 117 | * should always be throttle time in ms). So if you're logging something with 118 | * variable values, using format strings should be preferred since it will 119 | * throttle appropriately while composition will not. 120 | * e.g. 121 | * let i = 0; 122 | * setInterval(() => { 123 | * logger.infoThrottle(1000, 'Counter: %d', i); // prints once second 124 | * logger.infoThrottle(1000, 'Counter: ' + i); // prints twice a second 125 | * ++i; 126 | * }, 500); 127 | * 128 | * @param methods {Set.} 129 | */ 130 | _createThrottleLogMethods(methods) { 131 | methods.forEach((method) => { 132 | let throttleMethod = method + 'Throttle'; 133 | if (this.hasOwnProperty(throttleMethod)) { 134 | throw new Error('Unable to create method %s', throttleMethod); 135 | } 136 | 137 | // there's currently a bug using arguments in a () => {} function 138 | this[throttleMethod] = function(throttleTimeMs, args) { 139 | // If the desired log level is enabled and the message 140 | // isn't being throttled, then log the message. 141 | if (this[method]() && !this._throttle(...arguments)) { 142 | return this[method].apply(this, Array.from(arguments).slice(1)); 143 | } 144 | return false; 145 | }.bind(this); 146 | }); 147 | } 148 | 149 | _createOnceLogMethods(methods) { 150 | methods.forEach((method) => { 151 | let onceMethod = method + 'Once'; 152 | if (this.hasOwnProperty(onceMethod)) { 153 | throw new Error('Unable to create method %s', onceMethod); 154 | } 155 | 156 | // there's currently a bug using arguments in a () => {} function 157 | this[onceMethod] = function(args) { 158 | if (this[method]() && this._once(arguments)) { 159 | return this[method].apply(this, arguments); 160 | } 161 | return false; 162 | }.bind(this); 163 | }); 164 | } 165 | 166 | //-------------------------------------------------------------- 167 | // Throttled loggers 168 | // These will generally be slower. Performance will also degrade the more 169 | // places where you throttle your logs. Keep this in mind. Make child loggers. 170 | //-------------------------------------------------------------- 171 | 172 | /** 173 | * Handles throttling logic for each log statement. Throttles logs by attempting 174 | * to create a string log 'key' from the arguments. 175 | * @param throttleTimeMs {number} 176 | * @param args {Array} arguments provided to calling function 177 | * @return {boolean} should this log be throttled (if true, the log should not be written) 178 | */ 179 | _throttle(throttleTimeMs, ...args) { 180 | const now = Date.now(); 181 | const throttlingMsg = this._getThrottleMsg(args); 182 | if (throttlingMsg === null) { 183 | // we couldn't get a msg to hash - fall through and log the message 184 | return false; 185 | } 186 | // else 187 | const msgHash = hashMessage(throttlingMsg); 188 | 189 | const throttledLog = this._throttledLogs.get(msgHash); 190 | 191 | if (throttledLog === undefined || now + 1 - throttledLog.getStartTime() >= throttledLog.getThrottleTime()) { 192 | const newThrottledLog = new ThrottledLog(now, throttleTimeMs); 193 | this._throttledLogs.set(msgHash, newThrottledLog); 194 | return false; 195 | } 196 | return true; 197 | } 198 | 199 | //-------------------------------------------------------------- 200 | // Throttled loggers 201 | // These will generally be slower. Performance will also degrade the more 202 | // places where you throttle your logs. Keep this in mind. Make child loggers. 203 | //-------------------------------------------------------------- 204 | 205 | /** 206 | * Handles once logic for each log statement. Throttles logs by attempting 207 | * to create a string log 'key' from the arguments. 208 | * @param args {Array} arguments provided to calling function 209 | * @return {boolean} should this be written 210 | */ 211 | _once(args) { 212 | const throttleMsg = this._getThrottleMsg(args); 213 | if (throttleMsg === null) { 214 | // we couldn't get a msg to hash - fall through and log the message 215 | return true; 216 | } 217 | 218 | const logKey = hashMessage(throttleMsg); 219 | 220 | if (!this._onceLogs.has(logKey)) { 221 | this._onceLogs.add(logKey); 222 | return true; 223 | } 224 | return false; 225 | } 226 | 227 | _getThrottleMsg(args) { 228 | const firstArg = args[0]; 229 | if (typeof firstArg === 'string' || firstArg instanceof String) { 230 | return firstArg; 231 | } 232 | else if (typeof firstArg === 'object') { 233 | // bunyan supports passing an object as the first argument with 234 | // optional fields to add to the log record - the second argument 235 | // is the actual string 'log message' in this case, so just return that 236 | return args[1]; 237 | } 238 | // fall through *womp womp* 239 | return null; 240 | } 241 | 242 | /** 243 | * Remove old throttled logs (logs that were throttled whose throttling time has passed) from the throttling map 244 | * @returns {Number} number of logs that were cleaned out 245 | */ 246 | clearExpiredThrottledLogs() { 247 | const logsToRemove = []; 248 | const now = Date.now(); 249 | this._throttledLogs.forEach((log, key) => { 250 | if (now - log.getStartTime() >= log.getThrottleTime()) { 251 | logsToRemove.push(key); 252 | } 253 | }); 254 | 255 | logsToRemove.forEach((logKey) => { 256 | this._throttledLogs.delete(logKey); 257 | }); 258 | 259 | return logsToRemove.length; 260 | } 261 | 262 | getThrottledLogSize() { 263 | return this._throttledLogs.size; 264 | } 265 | } 266 | 267 | //----------------------------------------------------------------------- 268 | 269 | /** 270 | * @class ThrottledLog 271 | * Small utility class implementation for ThrottledLogger 272 | */ 273 | class ThrottledLog { 274 | constructor(timeThrottleStarted, throttlingTime) { 275 | this.logThrottleStartTime = timeThrottleStarted; 276 | this.logthrottleTimeMs = throttlingTime; 277 | } 278 | 279 | getStartTime() { 280 | return this.logThrottleStartTime; 281 | } 282 | 283 | getThrottleTime() { 284 | return this.logthrottleTimeMs; 285 | } 286 | } 287 | 288 | // Utility function to help hash messages when we throttle them. 289 | function hashMessage(msg) { 290 | const sha1 = crypto.createHash('sha1'); 291 | sha1.update(msg); 292 | return sha1.digest('hex'); 293 | } 294 | 295 | //----------------------------------------------------------------------- 296 | 297 | module.exports = Logger; 298 | -------------------------------------------------------------------------------- /src/utils/log/RosLogStream.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | const bunyan = require('bunyan'); 20 | const timeUtils = require('../time_utils.js'); 21 | 22 | let RosgraphLogMsg; 23 | 24 | class RosLogStream { 25 | constructor(nh, rosgraphLogMsg, options) { 26 | RosgraphLogMsg = rosgraphLogMsg; 27 | options = options || {}; 28 | 29 | let queueSize = 200; 30 | if (options.hasOwnProperty('queueSize')) { 31 | queueSize = options.queueSize; 32 | } 33 | 34 | if (options.hasOwnProperty('formatter')) { 35 | this._formatter = options.formatter; 36 | } 37 | else { 38 | this._formatter = (rec) => { return rec.msg; }; 39 | } 40 | 41 | this._nodeName = nh.getNodeName(); 42 | 43 | this._rosoutPub = nh.advertise('/rosout', 'rosgraph_msgs/Log', 44 | {queueSize: queueSize, latching: true}); 45 | } 46 | 47 | getPub() { 48 | return this._rosoutPub; 49 | } 50 | 51 | _getRosLogLevel(bunyanLogLevel) { 52 | // ROS log levels defined in rosgraph_msgs/Log 53 | // bunyan trace and debug levels will both map to ROS debug 54 | if (bunyanLogLevel === bunyan.TRACE) { 55 | return RosgraphLogMsg.Constants.DEBUG; 56 | } 57 | // else 58 | return RosgraphLogMsg.Constants[bunyan.nameFromLevel[bunyanLogLevel].toUpperCase()]; 59 | } 60 | 61 | write(rec) { 62 | if (this._rosoutPub !== null) { 63 | const msg = new RosgraphLogMsg(); 64 | 65 | if (typeof rec === 'string' || rec instanceof String) { 66 | // if user sets this stream to be type 'stream' instead of 'raw', we'll 67 | // get the formatted string and nothing else - just send it as is. 68 | msg.msg = rec; 69 | } 70 | else if (typeof rec === 'object') { 71 | // being used as a raw stream 72 | msg.header.stamp = timeUtils.dateToRosTime(rec.time); 73 | 74 | msg.name = this._nodeName; 75 | msg.file = rec.scope || rec.name; 76 | msg.level = this._getRosLogLevel(rec.level); 77 | const formattedMsg = this._formatter(rec); 78 | if (typeof formattedMsg === 'string' || formattedMsg instanceof String) { 79 | msg.msg = formattedMsg; 80 | } 81 | else { 82 | console.error('Unable to format message %j', rec); 83 | return; 84 | } 85 | } 86 | // console.log('ROSOUT: %j', msg); 87 | this._rosoutPub.publish(msg); 88 | } 89 | } 90 | }; 91 | 92 | module.exports = RosLogStream; 93 | -------------------------------------------------------------------------------- /src/utils/messageGeneration/IndentedWriter.js: -------------------------------------------------------------------------------- 1 | 'use strict'; 2 | const util = require('util'); 3 | 4 | class IndentedWriter { 5 | constructor() { 6 | this._str = ''; 7 | this._indentation = 0; 8 | } 9 | 10 | write(args) { 11 | let formattedStr = util.format.apply(this,arguments); 12 | if (this.isIndented()) { 13 | for (let i = 0; i < this._indentation; ++i) { 14 | this._str += ' '; 15 | } 16 | } 17 | this._str += formattedStr; 18 | return this.newline(); 19 | } 20 | 21 | newline(indentDir=undefined) { 22 | this._str += '\n'; 23 | if (indentDir === undefined) { 24 | return this; 25 | } 26 | else if (indentDir > 0) { 27 | return this.indent(); 28 | } 29 | else if (indentDir < 0) { 30 | return this.dedent(); 31 | } 32 | // else 33 | return this; 34 | } 35 | 36 | indent() { 37 | ++this._indentation; 38 | if (arguments.length > 0) { 39 | return this.write(...arguments); 40 | } 41 | // else 42 | return this; 43 | } 44 | 45 | isIndented() { 46 | return this._indentation > 0; 47 | } 48 | 49 | dedent() { 50 | --this._indentation; 51 | if (this._indentation < 0) { 52 | this.resetIndent(); 53 | } 54 | if (arguments.length > 0) { 55 | return this.write(...arguments); 56 | } 57 | // else 58 | return this; 59 | } 60 | 61 | resetIndent() { 62 | this._indentation = 0; 63 | return this; 64 | } 65 | 66 | dividingLine() { 67 | return this.write('//-----------------------------------------------------------'); 68 | } 69 | 70 | get() { 71 | return this._str; 72 | } 73 | } 74 | 75 | module.exports = IndentedWriter; 76 | -------------------------------------------------------------------------------- /src/utils/messageGeneration/fields.js: -------------------------------------------------------------------------------- 1 | 'use strict'; 2 | 3 | var fields = exports; 4 | 5 | const ros_msg_utils = require('../../ros_msg_utils'); 6 | const BN = require('bn.js'); 7 | 8 | /* map of all primitive types and their default values */ 9 | var map = { 10 | 'char': 0, 11 | 'byte': 0, 12 | 'bool': false, 13 | 'int8': 0, 14 | 'uint8': 0, 15 | 'int16': 0, 16 | 'uint16': 0, 17 | 'int32': 0, 18 | 'uint32': 0, 19 | 'int64': 0, 20 | 'uint64': 0, 21 | 'float32': 0, 22 | 'float64': 0, 23 | 'string': '', 24 | 'time': {secs: 0, nsecs: 0}, 25 | 'duration': {secs: 0, nsecs: 0} 26 | }; 27 | 28 | fields.primitiveTypes = Object.keys(map); 29 | 30 | fields.getDefaultValue = function(type) { 31 | let match = type.match(/(.*)\[(\d*)\]/); 32 | if (match) { 33 | // it's an array 34 | const basetype = match[1]; 35 | const length = (match[2].length > 0 ? parseInt(match[2]) : 0); 36 | return new Array(length).fill(fields.getDefaultValue(basetype)); 37 | } else { 38 | return map[type]; 39 | } 40 | }; 41 | 42 | fields.isString = function(type) { 43 | return type === 'string'; 44 | } 45 | 46 | fields.isTime = function(type) { 47 | return type === 'time' || type === 'duration'; 48 | } 49 | 50 | fields.isBool = function(type) { 51 | return type === 'bool'; 52 | } 53 | 54 | fields.isFloat = function(type) { 55 | return type === 'float32' || type === 'float64'; 56 | } 57 | 58 | fields.isInteger = function(type) { 59 | return (['byte', 'char', 'int8', 'uint8', 'int16', 'uint16', 60 | 'int32', 'uint32', 'int64', 'uint64'].indexOf('type') >= 0); 61 | } 62 | 63 | fields.isPrimitive = function(fieldType) { 64 | return (fields.primitiveTypes.indexOf(fieldType) >= 0); 65 | }; 66 | 67 | var isArrayRegex = /\[(\d*)\]$/; 68 | fields.isArray = function(fieldType, details) { 69 | var match = fieldType.match(isArrayRegex); 70 | if (match) { 71 | if (match[1] && details) { 72 | details.length = match[1]; 73 | } 74 | return true; 75 | } else { 76 | return false; 77 | } 78 | }; 79 | 80 | fields.isMessage = function(fieldType) { 81 | return !this.isPrimitive(fieldType) && !this.isArray(fieldType); 82 | }; 83 | 84 | fields.getTypeOfArray = function(arrayType) { 85 | return this.isArray(arrayType) ? arrayType.split('[')[0] 86 | : false; 87 | } 88 | 89 | fields.getLengthOfArray = function(arrayType) { 90 | var match = arrayType.match(/.*\[(\d*)\]$/); 91 | if (match[1] === '') { 92 | return null; 93 | } 94 | return parseInt(match[1]); 95 | } 96 | 97 | function parseType(msgType, field) { 98 | if (!msgType) { 99 | throw new Error(`Invalid empty type ${JSON.stringify(field)}`); 100 | } 101 | // else 102 | if (fields.isArray(msgType)) { 103 | field.isArray = true; 104 | var constantLength = msgType.endsWith('[]'); 105 | var splits = msgType.split('['); 106 | if (splits.length > 2) { 107 | throw new Error(`Only support 1-dimensional array types: ${msgType}`); 108 | } 109 | field.baseType = splits[0]; 110 | if (constantLength) { 111 | field.arrayLen = fields.getLengthOfArray(msgType); 112 | } 113 | else { 114 | field.arrayLen = null; 115 | } 116 | } 117 | else { 118 | field.baseType= msgType; 119 | field.isArray = false; 120 | field.arrayLen = null; 121 | } 122 | } 123 | 124 | function isHeader(type) { 125 | return (['Header', 'std_msgs/Header', 'roslib/Header'].indexOf(type) >= 0); 126 | } 127 | 128 | fields.Constant = function(name, type, val, valText) { 129 | this.name = name; 130 | this.type = type; 131 | this.val = val; 132 | this.valText = valText; 133 | }; 134 | 135 | fields.Constant.prototype.equals = function(other) { 136 | return other instanceof fields.Constant && 137 | other.name === this.name && other.type === this.type && other.val === this.val; 138 | }; 139 | 140 | fields.Field = function(name, type) { 141 | this.name = name; 142 | this.type = type; 143 | parseType(type, this); 144 | this.isHeader = isHeader(type); 145 | this.isBuiltin = fields.isPrimitive(this.baseType); 146 | }; 147 | 148 | fields.Field.isHeader = function(type) { 149 | return isHeader(type); 150 | } 151 | 152 | fields.Field.isBuiltin = function(type) { 153 | return fields.isPrimitive(type); 154 | }; 155 | 156 | 157 | fields.parsePrimitive = function(fieldType, fieldValue) { 158 | var parsedValue = fieldValue; 159 | 160 | if (fieldType === 'bool') { 161 | parsedValue = (fieldValue === '1') 162 | } 163 | else if (fieldType === 'int8' || fieldType === 'byte') { 164 | parsedValue = parseInt(fieldValue); 165 | } 166 | else if (fieldType === 'uint8' || fieldType === 'char') { 167 | parsedValue = parseInt(fieldValue); 168 | parsedValue = Math.abs(parsedValue); 169 | } 170 | else if (fieldType === 'int16') { 171 | parsedValue = parseInt(fieldValue); 172 | } 173 | else if (fieldType === 'uint16') { 174 | parsedValue = parseInt(fieldValue); 175 | parsedValue = Math.abs(parsedValue); 176 | } 177 | else if (fieldType === 'int32') { 178 | parsedValue = parseInt(fieldValue); 179 | } 180 | else if (fieldType === 'uint32') { 181 | parsedValue = parseInt(fieldValue); 182 | parsedValue = Math.abs(parsedValue); 183 | } 184 | else if (fieldType === 'int64') { 185 | parsedValue = new BN(fieldValue); 186 | } 187 | else if (fieldType === 'uint64') { 188 | parsedValue = new BN(fieldValue); 189 | } 190 | else if (fieldType === 'float32') { 191 | parsedValue = parseFloat(fieldValue); 192 | } 193 | else if (fieldType === 'float64') { 194 | parsedValue = parseFloat(fieldValue); 195 | } 196 | else if (fieldType === 'time') { 197 | var now; 198 | if (fieldValue.secs && fieldValue.nsecs) { 199 | parsedValue.secs = fieldValue.secs; 200 | parsedValue.nsecs = fieldValue.nsecs; 201 | } else { 202 | if (fieldValue instanceof Date) { 203 | now = fieldValue.getTime(); 204 | } else if (typeof fieldValue == "number") { 205 | now = fieldValue; 206 | } else { 207 | now = Date.now(); 208 | } 209 | var secs = parseInt(now/1000); 210 | var nsecs = (now % 1000) * 1000; 211 | 212 | parsedValue.secs = secs; 213 | parsedValue.nsecs = nsecs; 214 | } 215 | } 216 | 217 | return parsedValue; 218 | }; 219 | 220 | fields.serializePrimitive = 221 | function(fieldType, fieldValue, buffer, bufferOffset) { 222 | 223 | const serializer = ros_msg_utils.Serialize[fieldType]; 224 | if (!serializer) { 225 | throw new Error(`Unable to get primitive serializer for field type ${fieldType}`); 226 | } 227 | // else 228 | 229 | return serializer(fieldValue, buffer, bufferOffset); 230 | }; 231 | 232 | fields.deserializePrimitive = function(fieldType, buffer, bufferOffset) { 233 | const deserializer = ros_msg_utils.Deserialize[fieldType]; 234 | if (!deserializer) { 235 | throw new Error(`Unable to get primitive deserializer for field type ${fieldType}`); 236 | } 237 | // else 238 | 239 | return deserializer(buffer, bufferOffset); 240 | }; 241 | 242 | fields.getPrimitiveSize = function(fieldType, fieldValue) { 243 | var fieldSize = 0; 244 | 245 | if (fieldType === 'char') { 246 | fieldSize = 1; 247 | } 248 | else if (fieldType === 'byte') { 249 | fieldSize = 1; 250 | } 251 | else if (fieldType === 'bool') { 252 | fieldSize = 1; 253 | } 254 | else if (fieldType === 'int8') { 255 | fieldSize = 1; 256 | } 257 | else if (fieldType === 'uint8') { 258 | fieldSize = 1; 259 | } 260 | else if (fieldType === 'int16') { 261 | fieldSize = 2; 262 | } 263 | else if (fieldType === 'uint16') { 264 | fieldSize = 2; 265 | } 266 | else if (fieldType === 'int32') { 267 | fieldSize = 4; 268 | } 269 | else if (fieldType === 'uint32') { 270 | fieldSize = 4; 271 | } 272 | else if (fieldType === 'int64') { 273 | fieldSize = 8; 274 | } 275 | else if (fieldType === 'uint64') { 276 | fieldSize = 8; 277 | } 278 | else if (fieldType === 'float32') { 279 | fieldSize = 4; 280 | } 281 | else if (fieldType === 'float64') { 282 | fieldSize = 8; 283 | } 284 | else if (fieldType === 'string') { 285 | if (fieldValue !== undefined) { 286 | fieldSize = Buffer.byteLength(fieldValue, 'utf8') + 4; 287 | } 288 | } 289 | else if (fieldType === 'time') { 290 | fieldSize = 8; 291 | } 292 | else if (fieldType === 'duration') { 293 | fieldSize = 8; 294 | } 295 | 296 | return fieldSize; 297 | } 298 | 299 | fields.getArraySize = function(field, array, msgSpec) { 300 | var that = this 301 | , arraySize = 0; 302 | 303 | // if this is a variable length array it has a 4 byte length field at the beginning 304 | if (field.arrayLen === null) { 305 | arraySize = 4; 306 | } 307 | 308 | array.forEach(function(value) { 309 | if (field.isBuiltin) { 310 | arraySize += that.getPrimitiveSize(field.baseType, value); 311 | } 312 | else { 313 | arraySize += that.getMessageSize(value, msgSpec.getMsgSpecForType(field.baseType)); 314 | } 315 | }); 316 | 317 | return arraySize; 318 | }; 319 | 320 | fields.getMessageSize = function(message, msgSpec) { 321 | var that = this 322 | , messageSize = 0 323 | , innerfields = msgSpec.fields 324 | ; 325 | 326 | innerfields.forEach(function(field) { 327 | var fieldValue = message[field.name]; 328 | if (field.isArray) { 329 | messageSize += that.getArraySize(field, fieldValue, msgSpec); 330 | } 331 | else if (field.isBuiltin) { 332 | messageSize += that.getPrimitiveSize(field.type, fieldValue); 333 | } 334 | else { // it's a message 335 | messageSize += that.getMessageSize(fieldValue, msgSpec.getMsgSpecForType(field.baseType)); 336 | } 337 | }); 338 | 339 | return messageSize; 340 | }; 341 | 342 | fields.getMessageNameFromMessageType = function(messageType) { 343 | return messageType.indexOf('/') !== -1 ? messageType.split('/')[1] 344 | : messageType; 345 | }; 346 | 347 | fields.getPackageNameFromMessageType = function(messageType) { 348 | return messageType.indexOf('/') !== -1 ? messageType.split('/')[0] 349 | : ''; 350 | }; 351 | -------------------------------------------------------------------------------- /src/utils/message_utils.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const fs = require('fs'); 21 | const path = require('path'); 22 | const utils = require('util'); 23 | const loggingManager = require('../lib/Logging.js'); 24 | const messages = require('./messageGeneration/messages.js'); 25 | const ros_msg_utils = require('../ros_msg_utils'); 26 | 27 | // *grumble grumble* this is unfortunate 28 | // Our ros messages are going to be loaded from all over the place 29 | // They all need access to ros_msg_utils but we can't guarantee that 30 | // they'll be able to find ros_msg_utils without forcing people to 31 | // add ros_msg_utils to their node_path or installing it globally 32 | // or installing it separately for every message package 33 | global._ros_msg_utils = ros_msg_utils; 34 | 35 | let messagePackageMap = {}; 36 | 37 | //----------------------------------------------------------------------- 38 | // Utilities for loading, finding handlers for 39 | // message serialization/deserialization 40 | // 41 | // When rosnodejs starts, it searches through your cmakepath for generated 42 | // javascript messages. It caches paths for any of the packages it finds. 43 | // Then, in rosnodejs when you ask to use a message package we check for it 44 | // in the cache and require it if found. 45 | //----------------------------------------------------------------------- 46 | 47 | function createDirectory(directory) { 48 | let curPath = '/'; 49 | const paths = directory.split(path.sep); 50 | paths.forEach((part, index) => { 51 | if (!part) { 52 | return; 53 | } 54 | curPath = path.join(curPath, part); 55 | const thisPath = curPath; 56 | 57 | try { 58 | fs.mkdirSync(thisPath); 59 | } 60 | catch (err) { 61 | if (err.code !== 'EEXIST') { 62 | throw err; 63 | } 64 | } 65 | }); 66 | } 67 | 68 | function copyFile(from, to, replaceCallback) { 69 | return new Promise((resolve, reject) => { 70 | const readStream = fs.createReadStream(from); 71 | let fileData = ''; 72 | readStream.on('data', (data) => { 73 | fileData += data; 74 | }); 75 | 76 | readStream.on('end', () => { 77 | if (typeof replaceCallback === 'function') { 78 | fileData = replaceCallback(fileData); 79 | } 80 | 81 | // open the output file for writing 82 | const writeStream = fs.createWriteStream(to); 83 | writeStream.on('open', () => { 84 | writeStream.write(fileData); 85 | writeStream.end(); 86 | resolve(); 87 | }); 88 | }); 89 | }); 90 | } 91 | 92 | let MessageUtils = { 93 | getTopLevelMessageDirectory() { 94 | return path.join(ros_msg_utils.CMAKE_PATHS[0], ros_msg_utils.MESSAGE_PATH); 95 | }, 96 | 97 | flatten(outputDir) { 98 | const finderDeclRegex = /^let _finder = require\('\.\.\/\.\.\/\.\.\/find\.js'\);/m; 99 | const finderCallRegex = /^let (\w+) = _finder\(\'\1\'\);/gm; 100 | 101 | const flatten_local = (packageName, startPath, localPath, outputDir) => { 102 | const flattenPath = path.join(startPath, localPath); 103 | fs.readdir(flattenPath, (err, files) => { 104 | if (err) { 105 | // if the path doesn't exist, it just means the package currently 106 | // being flattened doesn't have either msgs or srvs 107 | if (err.code !== 'ENOENT') { 108 | throw new Error('Error while flattening generated messages ' + err); 109 | } 110 | return; 111 | } 112 | // else 113 | const outputPath = path.join(outputDir, packageName, localPath); 114 | createDirectory(outputPath) 115 | 116 | files.forEach((file) => { 117 | const filePath = path.join(flattenPath, file); 118 | const outputFilePath = path.join(outputDir, packageName, localPath, file); 119 | let callback; 120 | if (file !== '_index.js') { 121 | callback = (fileData) => { 122 | fileData = fileData.replace(finderDeclRegex, ''); 123 | let matchData; 124 | while ((matchData = finderCallRegex.exec(fileData)) !== null) { 125 | const matchStr = matchData[0]; 126 | const msgPackage = matchData[1]; 127 | const replaceStr = 128 | utils.format('let %s = require(\'../../%s/_index.js\');', 129 | msgPackage, msgPackage); 130 | fileData = fileData.replace(matchStr, replaceStr); 131 | } 132 | return fileData; 133 | }; 134 | } 135 | copyFile(filePath, outputFilePath, callback); 136 | }); 137 | }); 138 | }; 139 | 140 | outputDir = path.resolve(outputDir); 141 | const messageDirectory = path.join(outputDir, 'ros'); 142 | createDirectory(messageDirectory); 143 | 144 | // find relevant genjs base files and copy to output directory 145 | const filesToCopy = ['base_deserialize.js', 'base_serialize.js']; 146 | cmakePaths.some((cmakePath) => { 147 | const checkPath = path.join(cmakePath, 'share', 'node_js'); 148 | let files = fs.readdirSync(checkPath); 149 | if (!files) { 150 | return false; 151 | } 152 | files.forEach((fileName) => { 153 | if (filesToCopy.indexOf(fileName) !== -1) { 154 | copyFile(path.join(checkPath, fileName), path.join(outputDir, fileName)); 155 | } 156 | }); 157 | return true; 158 | }); 159 | 160 | Object.keys(messagePackagePathMap).forEach((packageName) => { 161 | const messagePackagePath = messagePackagePathMap[packageName]; 162 | const dir = path.dirname(messagePackagePath); 163 | 164 | flatten_local(packageName, dir, 'msg', messageDirectory); 165 | flatten_local(packageName, dir, 'srv', messageDirectory); 166 | // copy the index 167 | copyFile(messagePackagePath, 168 | path.join(messageDirectory, packageName, '_index.js')); 169 | }); 170 | }, 171 | 172 | loadMessagePackage(msgPackage) { 173 | messagePackageMap[msgPackage] = ros_msg_utils.Find(msgPackage); 174 | }, 175 | 176 | getPackage(msgPackage) { 177 | return messagePackageMap[msgPackage]; 178 | }, 179 | 180 | requireMsgPackage(msgPackage) { 181 | // check our registry of on-demand generate message definition 182 | var fromRegistry = messages.getPackageFromRegistry(msgPackage); 183 | if (fromRegistry) { 184 | return fromRegistry; 185 | } 186 | 187 | // if we can't find it in registry, check for gennodejs 188 | // pre-compiled versions 189 | let pack = this.getPackage(msgPackage); 190 | if (!pack) { 191 | this.loadMessagePackage(msgPackage); 192 | return this.getPackage(msgPackage); 193 | } 194 | // else 195 | return pack; 196 | }, 197 | 198 | getAvailableMessagePackages() { 199 | return ros_msg_utils.packageMap; 200 | }, 201 | 202 | getHandlerForMsgType(rosDataType, loadIfMissing=false) { 203 | let type = messages.getFromRegistry(rosDataType, "msg"); 204 | if (type) { 205 | return type; 206 | } else { 207 | const [msgPackage, type] = rosDataType.split('/'); 208 | let messagePackage = this.getPackage(msgPackage); 209 | if (!messagePackage && loadIfMissing) { 210 | this.loadMessagePackage(msgPackage); 211 | messagePackage = this.getPackage(msgPackage); 212 | } 213 | 214 | if (!messagePackage) { 215 | throw new Error('Unable to find message package ' + msgPackage); 216 | } 217 | // else 218 | return messagePackage.msg[type]; 219 | } 220 | }, 221 | 222 | getHandlerForSrvType(rosDataType, loadIfMissing=false) { 223 | let srv = messages.getFromRegistry(rosDataType, "srv"); 224 | if (srv) { 225 | return srv 226 | } else { 227 | const [msgPackage, type] = rosDataType.split('/'); 228 | let messagePackage = this.getPackage(msgPackage); 229 | 230 | if (!messagePackage && loadIfMissing) { 231 | this.loadMessagePackage(msgPackage); 232 | messagePackage = this.getPackage(msgPackage); 233 | } 234 | 235 | if (!messagePackage) { 236 | throw new Error('Unable to find service package ' + msgPackage 237 | + '. Request: ' + !!request + ', Response: ' + !!response); 238 | } 239 | // else 240 | return messagePackage.srv[type]; 241 | } 242 | } 243 | }; 244 | 245 | //----------------------------------------------------------------------- 246 | 247 | module.exports = MessageUtils; 248 | -------------------------------------------------------------------------------- /src/utils/network_utils.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const os = require('os'); 21 | const RemapUtils = require('./remapping_utils'); 22 | 23 | let HOST = null; 24 | 25 | const NetworkUtils = { 26 | 27 | init(remappings) { 28 | const ip = remappings[RemapUtils.SPECIAL_KEYS.ip]; 29 | const host = remappings[RemapUtils.SPECIAL_KEYS.hostname]; 30 | 31 | const ROS_IP = process.env.ROS_IP; 32 | const ROS_HOSTNAME = process.env.ROS_HOSTNAME; 33 | 34 | HOST = ip || host || ROS_IP || ROS_HOSTNAME || os.hostname(); 35 | }, 36 | 37 | /** 38 | * FIXME: should this just return ROS_IP? 39 | * get this computer's (non-internal) ip address 40 | * @param [family] {string} 'IPv4', 'IPv6', ... 'IPv4' default 41 | * @param [networkInterface] {string} network interface to use ('eth0') else finds first match 42 | */ 43 | getIpAddress: function(family, networkInterface) { 44 | family = family || 'IPv4'; 45 | let interfaces = os.networkInterfaces(); 46 | let interfaceNames; 47 | if (networkInterface && !ifaces.hasOwnProperty(networkInterface)) { 48 | return null; 49 | } 50 | else if (networkInterface) { 51 | interfaceNames = [ networkInterface ]; 52 | } 53 | else { 54 | interfaceNames = Object.keys(interfaces); 55 | } 56 | 57 | let ipAddress = null; 58 | interfaceNames.some((ifName) => { 59 | interfaces[ifName].forEach((iface) => { 60 | if (iface.internal || family !== iface.family) { 61 | // skip over internal (i.e. 127.0.0.1) and addresses from different families 62 | return false; 63 | } 64 | 65 | ipAddress = iface.address; 66 | return true; 67 | }); 68 | }); 69 | return ipAddress; 70 | }, 71 | 72 | getHost() { 73 | return HOST; 74 | }, 75 | 76 | getAddressAndPortFromUri(uriString) { 77 | let regexStr = /(?:http:\/\/|rosrpc:\/\/)?([a-zA-Z\d\-_.]+):(\d+)/; 78 | let match = uriString.match(regexStr); 79 | if (match === null) { 80 | throw new Error ('Unable to find host and port from uri ' + uriString + ' with regex ' + regexStr); 81 | } 82 | // else 83 | return { 84 | host: match[1], 85 | port: match[2] 86 | }; 87 | }, 88 | 89 | formatServiceUri(port) { 90 | return 'rosrpc://' + this.getHost() + ':' + port; 91 | } 92 | }; 93 | 94 | //------------------------------------------------------------------ 95 | 96 | module.exports = NetworkUtils; 97 | -------------------------------------------------------------------------------- /src/utils/remapping_utils.js: -------------------------------------------------------------------------------- 1 | 2 | const SPECIAL_KEYS = { 3 | name: '__name', 4 | log: '__log', // I don't think rosnodejs is responsible for changing the log directory 5 | ip: '__ip', 6 | hostname: '__hostname', 7 | master: '__master', 8 | ns: '__ns' 9 | }; 10 | 11 | const RemapUtils = { 12 | SPECIAL_KEYS, 13 | processRemapping(args) { 14 | const len = args.length; 15 | 16 | const remapping = {}; 17 | 18 | for (let i = 0; i < len; ++i) { 19 | const arg = args[i]; 20 | let p = arg.indexOf(':='); 21 | if (p >= 0) { 22 | const local = arg.substring(0, p); 23 | const external = arg.substring(p+2); 24 | 25 | remapping[local] = external; 26 | } 27 | } 28 | 29 | return remapping; 30 | } 31 | } 32 | 33 | module.exports = RemapUtils; 34 | -------------------------------------------------------------------------------- /src/utils/serialization_utils.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | const util = require('util'); 20 | const Transform = require('stream').Transform; 21 | 22 | //----------------------------------------------------------------------- 23 | 24 | /** 25 | * DeserializeStream handles parsing of message chunks for TCPROS 26 | * encoded messages. When a full message has been received, it 27 | * emits 'message' with the data for that message. All socket 28 | * communications should be piped through this. 29 | */ 30 | class DeserializeStream extends Transform { 31 | 32 | constructor(options) { 33 | super(options); 34 | 35 | // Transform.call(this, options); 36 | // true once we've pulled off the message length 37 | // for the next message we'll need to deserialize 38 | this._inBody = false; 39 | 40 | // track how many bytes of this message we've received so far 41 | this._messageConsumed = 0; 42 | 43 | // how long this message will be 44 | this._messageLen = -1; 45 | 46 | // as bytes of this message arrive, store them in this 47 | // buffer until we have the whole thing 48 | this._messageBuffer = []; 49 | 50 | // TODO: These are specific to parsing a service response... 51 | // don't use them everywhere 52 | // the first byte in a service response is true/false service success/fail 53 | this._deserializeServiceResp = false; 54 | 55 | this._serviceRespSuccess = null; 56 | } 57 | 58 | _transform(chunk, encoding, done) { 59 | let pos = 0; 60 | let chunkLen = chunk.length; 61 | 62 | while (pos < chunkLen) { 63 | if (this._inBody) { 64 | let messageRemaining = this._messageLen - this._messageConsumed; 65 | 66 | // if the chunk is longer than the amount of the message we have left 67 | // just pull off what we need 68 | if (chunkLen >= messageRemaining + pos) { 69 | let slice = chunk.slice(pos, pos + messageRemaining); 70 | this._messageBuffer.push(slice); 71 | let concatBuf = Buffer.concat(this._messageBuffer, this._messageLen); 72 | this.emitMessage(concatBuf); 73 | 74 | // message finished, reset 75 | this._messageBuffer = []; 76 | pos += messageRemaining; 77 | this._inBody = false; 78 | this._messageConsumed = 0; 79 | } 80 | else { 81 | // rest of the chunk does not complete the message 82 | // cache it and move on 83 | this._messageBuffer.push(chunk.slice(pos)); 84 | this._messageConsumed += chunkLen - pos; 85 | pos = chunkLen; 86 | } 87 | } 88 | else { 89 | // if we're deserializing a service response, first byte is 'success' 90 | if (this._deserializeServiceResp && 91 | this._serviceRespSuccess === null) { 92 | this._serviceRespSuccess = chunk.readUInt8(pos, true); 93 | ++pos; 94 | } 95 | 96 | let bufLen = 0; 97 | this._messageBuffer.forEach((bufferEntry) => { 98 | bufLen += bufferEntry.length; 99 | }); 100 | 101 | // first 4 bytes of the message are a uint32 length field 102 | if (chunkLen - pos >= 4 - bufLen) { 103 | this._messageBuffer.push(chunk.slice(pos, pos + 4 - bufLen)); 104 | const buffer = Buffer.concat(this._messageBuffer, 4); 105 | this._messageLen = buffer.readUInt32LE(0); 106 | pos += 4 - bufLen; 107 | 108 | this._messageBuffer = [] 109 | // if its an empty message, there won't be any bytes left and message 110 | // will never be emitted -- handle that case here 111 | if (this._messageLen === 0 && pos === chunkLen) { 112 | this.emitMessage(Buffer.from([])); 113 | } 114 | else { 115 | this._inBody = true; 116 | } 117 | } 118 | else { 119 | // the length field is split on a chunk 120 | this._messageBuffer.push(chunk.slice(pos)); 121 | pos = chunkLen; 122 | } 123 | } 124 | } 125 | done(); 126 | } 127 | 128 | emitMessage(buffer) { 129 | if (this._deserializeServiceResp) { 130 | this.emit('message', buffer, this._serviceRespSuccess); 131 | this._serviceRespSuccess = null; 132 | } 133 | else { 134 | this.emit('message', buffer); 135 | } 136 | } 137 | 138 | setServiceRespDeserialize() { 139 | this._deserializeServiceResp = true; 140 | } 141 | }; 142 | 143 | 144 | //----------------------------------------------------------------------- 145 | 146 | function PrependLength(buffer, len) { 147 | let lenBuf = Buffer.allocUnsafe(4); 148 | lenBuf.writeUInt32LE(len, 0); 149 | return Buffer.concat([lenBuf, buffer], buffer.length + 4); 150 | } 151 | 152 | //----------------------------------------------------------------------- 153 | 154 | let SerializationUtils = { 155 | DeserializeStream: DeserializeStream, 156 | 157 | PrependLength: PrependLength, 158 | 159 | Serialize(buffer) { 160 | return PrependLength(buffer, buffer.length); 161 | }, 162 | 163 | Deserialize(buffer) { 164 | let len = buffer.readUInt32LE(0, true); 165 | buffer = buffer.slice(4); 166 | return len; 167 | } 168 | } 169 | 170 | //----------------------------------------------------------------------- 171 | 172 | module.exports = SerializationUtils; 173 | -------------------------------------------------------------------------------- /src/utils/spinners/ClientQueue.js: -------------------------------------------------------------------------------- 1 | const EventEmitter = require('events') 2 | 3 | /** 4 | * @class ClientQueue 5 | * Queue of messages to handle for an individual client (subscriber or publisher) 6 | */ 7 | class ClientQueue extends EventEmitter { 8 | constructor(client, queueSize, throttleMs) { 9 | super(); 10 | 11 | if (queueSize < 1) { 12 | queueSize = Number.POSITIVE_INFINITY; 13 | } 14 | 15 | this._client = client; 16 | 17 | this._queue = []; 18 | this._queueSize = queueSize; 19 | 20 | this.throttleMs = throttleMs; 21 | this._handleTime = null; 22 | } 23 | 24 | destroy() { 25 | this._queue = []; 26 | this._client = null; 27 | this._handleTime = null; 28 | } 29 | 30 | push(item) { 31 | this._queue.push(item); 32 | if (this.length > this._queueSize) { 33 | this._queue.shift(); 34 | } 35 | } 36 | 37 | get length() { 38 | return this._queue.length; 39 | } 40 | 41 | handleClientMessages(time) { 42 | if (this._handleTime === null || time - this._handleTime >= this.throttleMs) { 43 | this._handleTime = time; 44 | this._client._handleMsgQueue(this._queue); 45 | this._queue = []; 46 | return true; 47 | } 48 | // else 49 | return false; 50 | } 51 | } 52 | 53 | module.exports = ClientQueue; 54 | -------------------------------------------------------------------------------- /src/utils/spinners/GlobalSpinner.js: -------------------------------------------------------------------------------- 1 | 'use strict'; 2 | 3 | const DEFAULT_SPIN_RATE_HZ = 200; 4 | const events = require('events'); 5 | const LoggingManager = require('../../lib/Logging.js'); 6 | const log = LoggingManager.getLogger('ros.spinner'); 7 | 8 | const ClientQueue = require('./ClientQueue.js'); 9 | 10 | const PING_OP = 'ping'; 11 | const DELETE_OP = 'delete'; 12 | const ADD_OP = 'add'; 13 | 14 | /** 15 | * @class GlobalSpinner 16 | * Clients (subscribers and publishers) will register themselves with the node's spinner 17 | * when they're created. Clients will disconnect from the spinner whenever they're shutdown. 18 | * Whenever they receive a new message to handle, those clients will "ping" the spinner, 19 | * which will push the new message onto that client's queue and add the client to a list 20 | * of clients to be handled on the next spin. While spinning, the spinner is locked and 21 | * ping and disconnect operations are cached in order to ensure that changes aren't 22 | * made to the spinner during its execution (e.g. subscriber callback publishes a message, 23 | * publisher pings the spinner which queues the new message and adds the client to its callback 24 | * list, the client list is cleared at the end of the spin and this client has a 25 | * message hanging in its queue that will never be handled). Once all of the messages 26 | * received since the last spin are handled the Spinner is unlocked and all cached 27 | * ping and disconnect operations are replayed in order. 28 | */ 29 | class GlobalSpinner extends events { 30 | constructor({spinRate=null, emit=false} = {}) { 31 | super(); 32 | 33 | if (typeof spinRate === 'number') { 34 | this._spinTime = 1 / spinRate; 35 | } 36 | else { 37 | this._spinTime = 0; 38 | } 39 | 40 | this._spinTimer = null; 41 | 42 | this._clientCallQueue = []; 43 | this._clientQueueMap = new Map(); 44 | 45 | /** 46 | * Acts as a mutex while handling messages in _handleQueue 47 | * @type {boolean} 48 | * @private 49 | */ 50 | this._queueLocked = false; 51 | this._lockedOpCache = []; 52 | 53 | // emit is just for testing purposes 54 | this._emit = emit; 55 | } 56 | 57 | clear() { 58 | clearTimeout(this._spinTimer); 59 | this._queueLocked = false; 60 | this._clientQueueMap.forEach((clientQueue) => { 61 | clientQueue.destroy(); 62 | }); 63 | this._clientQueueMap.clear(); 64 | this._clientCallQueue = []; 65 | } 66 | 67 | addClient(client, clientId, queueSize, throttleMs) { 68 | if (this._queueLocked) { 69 | this._lockedOpCache.push({op: ADD_OP, client, clientId, queueSize, throttleMs}); 70 | } 71 | else if (queueSize > 0) { 72 | this._clientQueueMap.set(clientId, new ClientQueue(client, queueSize, throttleMs)); 73 | } 74 | } 75 | 76 | /** 77 | * When subscribers/publishers receive new messages to handle, they will 78 | * "ping" the spinner. 79 | * @param clientId 80 | * @param msg 81 | */ 82 | ping(clientId=null, msg=null) { 83 | if (!clientId || !msg) { 84 | throw new Error('Trying to ping spinner without clientId') 85 | } 86 | 87 | if (this._queueLocked) { 88 | this._lockedOpCache.push({op: PING_OP, clientId, msg}); 89 | } 90 | else { 91 | this._queueMessage(clientId, msg); 92 | this._setTimer(); 93 | } 94 | } 95 | 96 | disconnect(clientId) { 97 | if (this._queueLocked) { 98 | this._lockedOpCache.push({op: DELETE_OP, clientId}); 99 | } 100 | else { 101 | const index = this._clientCallQueue.indexOf(clientId); 102 | if (index !== -1) { 103 | this._clientCallQueue.splice(index, 1); 104 | } 105 | this._clientQueueMap.delete(clientId); 106 | } 107 | } 108 | 109 | _queueMessage(clientId, message) { 110 | const clientQueue = this._clientQueueMap.get(clientId); 111 | if (!clientQueue) { 112 | throw new Error(`Unable to queue message for unknown client ${clientId}`); 113 | } 114 | // else 115 | if (clientQueue.length === 0) { 116 | this._clientCallQueue.push(clientId); 117 | } 118 | 119 | clientQueue.push(message); 120 | } 121 | 122 | _handleLockedOpCache() { 123 | const len = this._lockedOpCache.length; 124 | for (let i = 0; i < len; ++i) { 125 | const {op, clientId, msg, client, queueSize, throttleMs} = this._lockedOpCache[i]; 126 | if (op === PING_OP) { 127 | this.ping(clientId, msg); 128 | } 129 | else if (op === DELETE_OP) { 130 | this.disconnect(clientId); 131 | } 132 | else if (op === ADD_OP) { 133 | this.addClient(client, clientId, queueSize, throttleMs); 134 | } 135 | } 136 | this._lockedOpCache = []; 137 | } 138 | 139 | _setTimer() { 140 | if (this._spinTimer === null) { 141 | if (this._emit) { 142 | this._spinTimer = setTimeout(() => { 143 | this._handleQueue(); 144 | this.emit('tick'); 145 | }, this._spinTime); 146 | } 147 | else { 148 | this._spinTimer = setTimeout(this._handleQueue.bind(this), this._spinTime); 149 | } 150 | } 151 | } 152 | 153 | _handleQueue() { 154 | // lock the queue so that ping and disconnect operations are cached 155 | // while we're running through the call list instead of modifying 156 | // the list beneath us. 157 | this._queueLocked = true; 158 | const now = Date.now(); 159 | const keepOnQueue = []; 160 | let len = this._clientCallQueue.length; 161 | for (let i = 0; i < len; ++i) { 162 | const clientId = this._clientCallQueue[i]; 163 | const clientQueue = this._clientQueueMap.get(clientId); 164 | if (!clientQueue.handleClientMessages(now)) { 165 | keepOnQueue.push(clientId); 166 | } 167 | } 168 | 169 | if (keepOnQueue.length > 0) { 170 | this._clientCallQueue = keepOnQueue; 171 | } 172 | else { 173 | this._clientCallQueue = []; 174 | } 175 | 176 | // unlock the queue now that we've handled everything 177 | this._queueLocked = false; 178 | // handle any operations that occurred while the queue was locked 179 | this._handleLockedOpCache(); 180 | 181 | // TODO: figure out if these clients that are throttling messages are 182 | // consistently keeping the timer running when it otherwise wouldn't be 183 | // and eating up CPU. Consider starting a slower timer if the least-throttled 184 | // client won't be handled for N cycles (e.g N === 5). 185 | this._spinTimer = null; 186 | if (this._clientCallQueue.length > 0) { 187 | this._setTimer(); 188 | } 189 | } 190 | } 191 | 192 | module.exports = GlobalSpinner; 193 | -------------------------------------------------------------------------------- /src/utils/time_utils.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Copyright 2016 Chris Smith 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | 'use strict'; 19 | 20 | const NSEC_TO_SEC = 1e-9; 21 | const USEC_TO_SEC = 1e-6; 22 | const MSEC_TO_SEC = 1e-3; 23 | 24 | module.exports = { 25 | rosTimeToDate(rosTime) { 26 | var date = new Date(); 27 | // setTime takes in ms since epoch 28 | date.setTime(rosTime.secs * 1000 + Math.floor(rosTime.nsecs * USEC_TO_SEC)); 29 | return date; 30 | }, 31 | 32 | dateToRosTime(date) { 33 | var secs = Math.floor(date * MSEC_TO_SEC); 34 | var nsecs = date % 1000 * 1000000; 35 | return {'secs': secs, 'nsecs': nsecs}; 36 | }, 37 | 38 | now() { 39 | return this.dateToRosTime(Date.now()); 40 | }, 41 | 42 | epoch() { 43 | return { 44 | secs: 0, 45 | nsecs: 0 46 | }; 47 | }, 48 | 49 | isZeroTime(t) { 50 | return t.secs === 0 && t.nsecs === 0; 51 | }, 52 | 53 | toNumber(t) { 54 | return this.toSeconds(t); 55 | }, 56 | 57 | add(a, b) { 58 | let nsecs = a.nsecs + b.nsecs; 59 | let secs = a.secs + b.secs; 60 | 61 | // FIXME: we're ignoring negative time here 62 | if (nsecs > 1e9) { 63 | secs += Math.floor(nsecs / 1e9); 64 | nsecs = nsecs % 1e9; 65 | } 66 | 67 | return { 68 | secs, 69 | nsecs 70 | } 71 | }, 72 | 73 | lt(a, b) { 74 | return this.timeComp(a, b) < 0; 75 | }, 76 | 77 | toSeconds(t) { 78 | return t.secs + t.nsecs * NSEC_TO_SEC; 79 | }, 80 | 81 | timeComp(a, b) { 82 | const secDif = a.secs - b.secs; 83 | if (secDif !== 0) { 84 | return Math.sign(secDif); 85 | } 86 | else { 87 | return Math.sign(a.nsecs - b.nsecs); 88 | } 89 | } 90 | }; 91 | -------------------------------------------------------------------------------- /src/utils/xmlrpc_utils.js: -------------------------------------------------------------------------------- 1 | 'use strict'; 2 | 3 | 4 | module.exports = { 5 | call(client, method, data, resolve, reject, log, timeout) { 6 | log.debug('Calling method ' + method +': ' + data); 7 | 8 | if (timeout === undefined) { 9 | timeout = 0; 10 | } 11 | 12 | setTimeout(() => { 13 | client.methodCall(method, data, (err, resp) => { 14 | if (err && err.code === 'ECONNREFUSED') { 15 | if (timeout === 0) { 16 | timeout = 1; 17 | } 18 | else { 19 | timeout *= 2; 20 | } 21 | log.debug('Trying again in ' + timeout + 'ms'); 22 | log.debug('Connection refused during method %s: %j', method, data); 23 | this.call(client, method, data, resolve, reject, log, timeout); 24 | } 25 | else if (err || resp[0] !== 1) { 26 | log.debug('Some other error during %s: %s, %j', method, err, resp); 27 | log.debug((new Error()).stack); 28 | reject(err, resp); 29 | } 30 | else { 31 | resolve(resp); 32 | } 33 | }); 34 | }, timeout); 35 | } 36 | }; 37 | -------------------------------------------------------------------------------- /test/SpinnerTest.js: -------------------------------------------------------------------------------- 1 | 'use strict'; 2 | 3 | const chai = require('chai'); 4 | const expect = chai.expect; 5 | const GlobalSpinner = require('../src/utils/spinners/GlobalSpinner.js'); 6 | 7 | let handleList = []; 8 | let spinner; 9 | 10 | class DummyClient { 11 | constructor() { 12 | this.id = 'xxxxxxxx'.replace(/[x]/g, function(c) { 13 | return (Math.random()*8).toString(16); 14 | }); 15 | } 16 | 17 | _handleMsgQueue(queue) { 18 | handleList.push({id: this.id, queue}); 19 | } 20 | } 21 | 22 | describe('GlobalSpinner', () => { 23 | 24 | beforeEach(() => { 25 | spinner = new GlobalSpinner({emit: true}); 26 | handleList = []; 27 | }); 28 | 29 | it('Ping', (done) => { 30 | const clients = [new DummyClient(), new DummyClient(), new DummyClient(), new DummyClient()]; 31 | 32 | clients.forEach((client) => { 33 | spinner.addClient(client, client.id, 3, 0); 34 | }); 35 | 36 | spinner.ping(clients[0].id, "junk"); 37 | expect(spinner._spinTimer).to.not.be.null; 38 | 39 | spinner.ping(clients[3].id, "junk"); 40 | spinner.ping(clients[2].id, "junk"); 41 | spinner.ping(clients[1].id, "junk"); 42 | 43 | expect(spinner._clientCallQueue.length).to.equal(4); 44 | 45 | spinner.disconnect(clients[3].id); 46 | 47 | expect(spinner._clientCallQueue.length).to.equal(3); 48 | 49 | spinner.once('tick', () => { 50 | expect(handleList.length).to.equal(3); 51 | expect(handleList[0].id).to.equal(clients[0].id); 52 | expect(handleList[1].id).to.equal(clients[2].id); 53 | expect(handleList[2].id).to.equal(clients[1].id); 54 | 55 | done(); 56 | }); 57 | }); 58 | 59 | it('QueueSize', (done) => { 60 | const client = new DummyClient(); 61 | 62 | const messages = ["a", "b", "c", "d", "e"]; 63 | 64 | spinner.addClient(client, client.id, 3, 0); 65 | 66 | messages.forEach((message) => { 67 | spinner.ping(client.id, message); 68 | }); 69 | 70 | spinner.once('tick', () => { 71 | expect(handleList.length).to.equal(1); 72 | const handledQueue = handleList[0].queue; 73 | expect(handledQueue.length).to.equal(3); 74 | expect(handledQueue[0]).to.equal(messages[2]); 75 | expect(handledQueue[1]).to.equal(messages[3]); 76 | expect(handledQueue[2]).to.equal(messages[4]); 77 | 78 | done(); 79 | }) 80 | }); 81 | 82 | it('Locking', (done) => { 83 | const client = new DummyClient(); 84 | 85 | spinner.addClient(client, client.id, 3, 0); 86 | 87 | spinner.ping(client.id, 'junk'); 88 | 89 | // lock the queue so that the next ping is cached 90 | spinner._queueLocked = true; 91 | 92 | spinner.ping(client.id, 'junk'); 93 | 94 | expect(spinner._lockedOpCache.length).to.equal(1); 95 | 96 | spinner.once('tick', () => { 97 | expect(handleList.length).to.equal(1); 98 | expect(handleList[0].queue.length).to.equal(1); 99 | expect(spinner._lockedOpCache.length).to.equal(0); 100 | expect(spinner._clientCallQueue.length).to.equal(1); 101 | expect(spinner._spinTimer).to.not.be.null; 102 | handleList = []; 103 | 104 | spinner.once('tick', () => { 105 | expect(handleList.length).to.equal(1); 106 | expect(handleList[0].queue.length).to.equal(1); 107 | expect(spinner._lockedOpCache.length).to.equal(0); 108 | expect(spinner._clientCallQueue.length).to.equal(0); 109 | handleList = []; 110 | 111 | spinner.ping(client.id, 'junk'); 112 | 113 | // lock the queue so the next disconnect is cached 114 | spinner._queueLocked = true; 115 | 116 | spinner.disconnect(client.id); 117 | expect(spinner._lockedOpCache.length).to.equal(1); 118 | 119 | spinner.once('tick', () => { 120 | expect(handleList.length).to.equal(1); 121 | expect(handleList[0].queue.length).to.equal(1); 122 | expect(spinner._lockedOpCache.length).to.equal(0); 123 | expect(spinner._clientCallQueue.length).to.equal(0); 124 | expect(spinner._clientQueueMap.has(client.id)).to.be.false; 125 | handleList = []; 126 | 127 | spinner.addClient(client, client.id, 3, 0); 128 | spinner.ping(client.id, 'junk'); 129 | 130 | expect(spinner._lockedOpCache.length).to.equal(0); 131 | 132 | // lock the queue so the next disconnect is cached 133 | spinner._queueLocked = true; 134 | 135 | spinner.disconnect(client.id); 136 | spinner.addClient(client, client.id, 3, 0); 137 | spinner.ping(client.id, 'junk'); 138 | spinner.ping(client.id, 'junk'); 139 | 140 | expect(spinner._lockedOpCache.length).to.equal(4); 141 | 142 | spinner.once('tick', () => { 143 | // things that got in before we locked the spinner 144 | expect(handleList.length).to.equal(1); 145 | expect(handleList[0].queue.length).to.equal(1); 146 | expect(spinner._lockedOpCache.length).to.equal(0); 147 | expect(spinner._clientCallQueue.length).to.equal(1); 148 | expect(spinner._clientQueueMap.has(client.id)).to.be.true; 149 | handleList = []; 150 | 151 | // things that got in after we locked the spinner 152 | spinner.once('tick', () => { 153 | expect(handleList.length).to.equal(1); 154 | expect(handleList[0].queue.length).to.equal(2); 155 | expect(spinner._lockedOpCache.length).to.equal(0); 156 | expect(spinner._clientCallQueue.length).to.equal(0); 157 | expect(spinner._clientQueueMap.has(client.id)).to.be.true; 158 | 159 | spinner.disconnect(client.id); 160 | 161 | expect(spinner._clientQueueMap.has(client.id)).to.be.false; 162 | 163 | done(); 164 | }); 165 | }); 166 | }); 167 | }); 168 | }); 169 | }); 170 | 171 | it('Throttling', (done) => { 172 | const client = new DummyClient(); 173 | 174 | const throttleMs = 100; 175 | spinner.addClient(client, client.id, 1, throttleMs); 176 | 177 | spinner.ping(client.id, "junk"); 178 | spinner.once('tick', () => { 179 | let firstTick = Date.now(); 180 | spinner.ping(client.id, "junk"); 181 | 182 | spinner.on('tick', () => { 183 | if (spinner._clientCallQueue.length === 0) { 184 | const lastTick = Date.now(); 185 | const tDiff = lastTick - firstTick + 1; 186 | expect(tDiff).to.be.at.least(throttleMs); 187 | 188 | done(); 189 | } 190 | }); 191 | }); 192 | }); 193 | }); 194 | -------------------------------------------------------------------------------- /test/directory.js: -------------------------------------------------------------------------------- 1 | require('./DeserializeStream.js'); 2 | require('./namespaceTest.js'); 3 | require('./SpinnerTest.js'); 4 | require('./xmlrpcTest.js'); 5 | require('./Log.js'); 6 | require('./onTheFly.js'); 7 | require('./messages.js'); 8 | require('./services.js'); 9 | -------------------------------------------------------------------------------- /test/messages.js: -------------------------------------------------------------------------------- 1 | 'use strict'; 2 | 3 | const chai = require('chai'); 4 | const expect = chai.expect; 5 | const rosnodejs = require('../src/index.js'); 6 | 7 | describe('messages', function () { 8 | 9 | it('real messages', function() { 10 | expect(rosnodejs.require('geometry_msgs')).to.not.be.empty; 11 | expect(rosnodejs.require('geometry_msgs').msg).to.not.be.empty; 12 | expect(rosnodejs.require('geometry_msgs').msg).to.have.keys( 13 | 'Accel','AccelStamped','AccelWithCovariance','AccelWithCovarianceStamped', 14 | 'Inertia','InertiaStamped','Point','Point32','PointStamped', 15 | 'Polygon','PolygonStamped','Pose','Pose2D','PoseArray','PoseStamped', 16 | 'PoseWithCovariance','PoseWithCovarianceStamped','Quaternion', 17 | 'QuaternionStamped','Transform','TransformStamped','Twist','TwistStamped', 18 | 'TwistWithCovariance','TwistWithCovarianceStamped','Vector3', 19 | 'Vector3Stamped','Wrench','WrenchStamped' 20 | ); 21 | expect(rosnodejs.require('geometry_msgs').srv).to.be.empty; 22 | 23 | expect(rosnodejs.require('std_msgs')).to.not.be.empty; 24 | expect(rosnodejs.require('std_msgs').msg).to.not.be.empty; 25 | expect(rosnodejs.require('std_msgs').msg).to.have.keys( 26 | 'Bool','Duration','Float64MultiArray','Int32MultiArray', 27 | 'MultiArrayDimension','UInt16MultiArray','UInt8','Byte','Empty','Header', 28 | 'Int64','MultiArrayLayout','UInt32','UInt8MultiArray','ByteMultiArray', 29 | 'Float32','Int16','Int64MultiArray','String','UInt32MultiArray','Char', 30 | 'Float32MultiArray','Int16MultiArray','Int8','Time','UInt64','ColorRGBA', 31 | 'Float64','Int32','Int8MultiArray','UInt16','UInt64MultiArray' 32 | ) 33 | expect(rosnodejs.require('std_msgs').srv).to.be.empty; 34 | 35 | expect(rosnodejs.require('std_srvs')).to.not.be.empty; 36 | expect(rosnodejs.require('std_srvs').msg).to.be.empty; 37 | expect(rosnodejs.require('std_srvs').srv).to.not.be.empty; 38 | expect(rosnodejs.require('std_srvs').srv).to.have.keys( 39 | 'Empty','SetBool','Trigger' 40 | ); 41 | 42 | expect(rosnodejs.require('test_msgs')).to.not.be.empty; 43 | expect(rosnodejs.require('test_msgs').msg).to.not.be.empty; 44 | expect(rosnodejs.require('test_msgs').msg).to.have.keys( 45 | 'AllTypes','BaseTypeConstantLengthArray','ConstantLengthArray', 46 | 'VariableLengthArray','BaseType','BaseTypeVariableLengthArray','StdMsg', 47 | "TestActionAction","TestActionActionFeedback","TestActionActionGoal", 48 | "TestActionActionResult","TestActionFeedback","TestActionGoal", 49 | "TestActionResult" 50 | ); 51 | expect(rosnodejs.require('test_msgs').srv).to.not.be.empty; 52 | expect(rosnodejs.require('test_msgs').srv).to.have.keys( 53 | 'BasicService','HeaderService','NonSpecServiceDivider','TestService' 54 | ); 55 | }); 56 | 57 | it('non-existant messages', function() { 58 | const nonExistantPkg = 'made_up_message_package'; 59 | expect(rosnodejs.require.bind(rosnodejs, nonExistantPkg)).to.throw( 60 | Error, `Unable to find message package ${nonExistantPkg} from CMAKE_PREFIX_PATH` 61 | ); 62 | }); 63 | }); 64 | -------------------------------------------------------------------------------- /test/onTheFly.js: -------------------------------------------------------------------------------- 1 | 'use strict'; 2 | 3 | const chai = require('chai'); 4 | const expect = chai.expect; 5 | const xmlrpc = require('xmlrpc-rosnodejs'); 6 | const rosnodejs = require('../src/index.js'); 7 | const Master = require('./utils/MasterStub.js'); 8 | 9 | const MASTER_PORT = 11234; 10 | 11 | describe('OnTheFly', function () { 12 | let master; 13 | 14 | before(function() { 15 | this.timeout(0); 16 | 17 | master = new Master('localhost', MASTER_PORT); 18 | master.provideAll(); 19 | 20 | return rosnodejs.initNode('/testNode', { 21 | rosMasterUri: `http://localhost:${MASTER_PORT}`, 22 | onTheFly: true, 23 | notime: true, 24 | logging: {skipRosLogging: true}}) 25 | }); 26 | 27 | after(() => { 28 | rosnodejs.reset(); 29 | return master.shutdown(); 30 | }); 31 | 32 | it('serialize/deserialize PoseWithCovariance', (done) => { 33 | const geometry_msgs = rosnodejs.require('geometry_msgs').msg; 34 | const msg = new geometry_msgs.PoseWithCovariance({ 35 | pose: { 36 | position: {x:0, y:0, z:0}, 37 | orientation: {w:1, x:0, y:0, z:0} 38 | }, 39 | covariance: [ 40 | 0,0,0,0,0,0.123, 41 | 0,2,0,0,0,0, 42 | 0,0,4,0,0,0, 43 | 0,0,0,6,0,0, 44 | 0,0,0,0,8,0, 45 | 0.123,0,0,0,0,0.654321654321 46 | ] 47 | }); 48 | 49 | const size = geometry_msgs.PoseWithCovariance.getMessageSize(msg); 50 | const buffer = new Buffer(size); 51 | geometry_msgs.PoseWithCovariance.serialize(msg, buffer, 0); 52 | 53 | const read = geometry_msgs.PoseWithCovariance.deserialize(buffer); 54 | expect(read.covariance.length == msg.covariance.length 55 | && read.covariance.every((v,i)=> v === msg.covariance[i])).to.be.true; 56 | 57 | done(); 58 | }); 59 | 60 | it('serialize/deserialize String', (done) => { 61 | const std_msgs = rosnodejs.require('std_msgs').msg; 62 | const data = 'sДvΣ τhΣ 子猫'; // Test with multi-byte UTF-8 characters. 63 | // If this test fails, you killed a kitten. 64 | const msg = new std_msgs.String({ data: data }); 65 | 66 | const size = std_msgs.String.getMessageSize(msg); 67 | 68 | const buffer = new Buffer(size); 69 | std_msgs.String.serialize(msg, buffer, 0); 70 | 71 | const read = std_msgs.String.deserialize(buffer); 72 | expect(read.data).to.deep.equal(data); 73 | 74 | done(); 75 | }); 76 | 77 | it('UTF String', (done) => { 78 | const nh = rosnodejs.nh; 79 | const msg = 'Hello, 世界世界世界'; 80 | const topic = '/chatter'; 81 | const pub = nh.advertise(topic, 'std_msgs/String'); 82 | 83 | const sub = nh.subscribe(topic, 'std_msgs/String', (data) => { 84 | expect(data.data).to.equal(msg); 85 | done(); 86 | }); 87 | 88 | pub.on('connection', () => { 89 | pub.publish({data: msg}); 90 | }); 91 | }); 92 | }); 93 | -------------------------------------------------------------------------------- /test/perfTest.js: -------------------------------------------------------------------------------- 1 | 'use strict'; 2 | 3 | /** 4 | * Quick test for serialization, deserialization Performance 5 | */ 6 | 7 | const expect = require('chai').expect; 8 | const rosnodejs = require('../src/index.js'); 9 | const TfStamped = rosnodejs.require('geometry_msgs').msg.TransformStamped; 10 | const TfMessage = rosnodejs.require('tf2_msgs').msg.TFMessage; 11 | const Image = rosnodejs.require('sensor_msgs').msg.Image; 12 | const Header = rosnodejs.require('std_msgs').msg.Header; 13 | 14 | const header = new Header({seq: 100, stamp: {secs: 20, nsecs: 100010}, frame_id: 'test_cam'}); 15 | 16 | function getSeconds(hrTime) { 17 | return hrTime[0] + hrTime[1] / 1e9; 18 | } 19 | 20 | function getMB(bytes) { 21 | return bytes / 1e6; 22 | } 23 | 24 | function getBandwidth(bytes, hrTime) { 25 | return (getMB(bytes) / getSeconds(hrTime)).toFixed(3); 26 | } 27 | 28 | const NUM_CYCLES = 100; 29 | 30 | let hrTime; 31 | let deltaT; 32 | 33 | console.log('=== Serialization Performance Test ==='); 34 | console.log(' =='); 35 | console.log(' == Image Test'); 36 | console.log(' == Cycles: %d', NUM_CYCLES); 37 | console.log(' =='); 38 | 39 | 40 | let image; 41 | console.time('Create Image'); 42 | let width = 1280, 43 | height = 800, 44 | step = width * 3; 45 | for (let i = 0; i < NUM_CYCLES; ++i ) { 46 | image = new Image({ 47 | width: width, 48 | height: height, 49 | encoding: 'bgr8', 50 | step: step, 51 | data: Buffer.allocUnsafe(step * height), 52 | header 53 | }); 54 | } 55 | console.timeEnd('Create Image'); 56 | 57 | let bytesPerCycle; 58 | let bufsize; 59 | console.time('Determine Message Size'); 60 | for (let i = 0; i < NUM_CYCLES; ++i) { 61 | bufsize = Image.getMessageSize(image); 62 | } 63 | console.timeEnd('Determine Message Size'); 64 | bytesPerCycle = bufsize * NUM_CYCLES; 65 | 66 | console.log('Buffer size: %d', bufsize); 67 | 68 | console.time('allocate buffer'); 69 | let buffer; 70 | for (let i = 0; i < NUM_CYCLES; ++i) { 71 | buffer = new Buffer(bufsize); 72 | } 73 | console.timeEnd('allocate buffer'); 74 | 75 | console.time('Serialize'); 76 | hrTime = process.hrtime(); 77 | for (let i = 0; i < NUM_CYCLES; ++i) { 78 | Image.serialize(image, buffer, 0); 79 | } 80 | deltaT = process.hrtime(hrTime); 81 | console.timeEnd('Serialize'); 82 | console.log(`Serialized BW: ${getBandwidth(bytesPerCycle, deltaT)}MB`); 83 | 84 | console.time('Deserialize'); 85 | let deserialized; 86 | hrTime = process.hrtime(); 87 | for (let i = 0; i < NUM_CYCLES; ++i) { 88 | deserialized = Image.deserialize(buffer, [0]); 89 | } 90 | deltaT = process.hrtime(hrTime); 91 | console.timeEnd('Deserialize'); 92 | console.log(`Deserialized BW: ${getBandwidth(bytesPerCycle, deltaT)}MB`); 93 | 94 | // verify equality! 95 | expect(deserialized).to.deep.equal(image); 96 | 97 | const NUM_TFS = 1000; 98 | 99 | console.log(' =='); 100 | console.log(' == TF Test'); 101 | console.log(' == Cycles: %d', NUM_CYCLES); 102 | console.log(' == # of Transforms: %d', NUM_TFS); 103 | console.log(' =='); 104 | 105 | let tfStamped = new TfStamped(); 106 | tfStamped.header.frame_id = 'test_parent_frame'; 107 | tfStamped.child_frame_id = 'test_frame'; 108 | 109 | console.time('Create TfMessage'); 110 | let tfMessage; 111 | for (let i = 0; i < NUM_CYCLES; ++i) { 112 | tfMessage = new TfMessage(); 113 | for (let j = 0; j < NUM_TFS; ++j) { 114 | let tf = new TfStamped(tfStamped); 115 | tfMessage.transforms.push(tf); 116 | } 117 | } 118 | console.timeEnd('Create TfMessage'); 119 | 120 | console.time('Determine Message Size'); 121 | for (let i = 0; i < NUM_CYCLES; ++i) { 122 | bufsize = TfMessage.getMessageSize(tfMessage); 123 | } 124 | console.timeEnd('Determine Message Size'); 125 | bytesPerCycle = bufsize * NUM_CYCLES; 126 | 127 | console.log('Buffer size: %d', bufsize); 128 | 129 | console.time('Allocate buffer'); 130 | for (let i = 0; i < NUM_CYCLES; ++i) { 131 | buffer = new Buffer(bufsize); 132 | } 133 | console.timeEnd('Allocate buffer'); 134 | 135 | console.time('Serialize'); 136 | hrTime = process.hrtime(); 137 | for (let i = 0; i < NUM_CYCLES; ++i) { 138 | TfMessage.serialize(tfMessage, buffer, 0); 139 | } 140 | deltaT = process.hrtime(hrTime); 141 | console.timeEnd('Serialize'); 142 | console.log(`Serialized BW: ${getBandwidth(bytesPerCycle, deltaT)}MB`); 143 | 144 | console.time('Deserialize'); 145 | hrTime = process.hrtime(); 146 | for (let i = 0; i < NUM_CYCLES; ++i) { 147 | deserialized = TfMessage.deserialize(buffer, [0]); 148 | } 149 | deltaT = process.hrtime(hrTime); 150 | console.timeEnd('Deserialize'); 151 | console.log(`Deserialized BW: ${getBandwidth(bytesPerCycle, deltaT)}MB`); 152 | 153 | // verify equality! 154 | expect(deserialized).to.deep.equal(tfMessage); -------------------------------------------------------------------------------- /test/services.js: -------------------------------------------------------------------------------- 1 | 'use strict' 2 | 3 | const chai = require('chai'); 4 | const expect = chai.expect; 5 | const { spawn } = require('child_process'); 6 | 7 | const rosnodejs = require('../src/index.js'); 8 | 9 | const MASTER_PORT = 11235; 10 | 11 | const initArgs = { 12 | rosMasterUri: `http://localhost:${MASTER_PORT}`, 13 | logging: {skipRosLogging: true}, 14 | notime: true 15 | }; 16 | 17 | describe('Services Tests', () => { 18 | 19 | let rn; 20 | let core; 21 | 22 | before((done) => { 23 | // start a ros master 24 | core = spawn('roscore', ['-p', MASTER_PORT]); 25 | setTimeout(() => rosnodejs.initNode('/testNode', initArgs).then((rn_) => { 26 | rn = rn_; 27 | done(); 28 | }), 1000); // allow 1s for ros master to start 29 | }); 30 | 31 | after((done) => { 32 | rosnodejs.shutdown().then(() => { 33 | rosnodejs.reset(); 34 | core && core.kill('SIGINT'); 35 | done(); 36 | }); 37 | }); 38 | 39 | describe('get type', () => { 40 | it('can get the type', function(done) { 41 | rn.getServiceHeader('/rosout/get_loggers').then((p) => { 42 | expect(p.type).to.equal('roscpp/GetLoggers'); 43 | done(); 44 | }); 45 | }); 46 | }); 47 | }); 48 | -------------------------------------------------------------------------------- /test/stress.js: -------------------------------------------------------------------------------- 1 | 2 | const chai = require('chai'); 3 | const xmlrpc = require('xmlrpc-rosnodejs'); 4 | const rosnodejs = require('rosnodejs'); 5 | 6 | const TOPIC = '/topic'; 7 | const TYPE = 'std_msgs/String'; 8 | const SERVICE = '/service'; 9 | const SRV = 'std_srvs/Empty'; 10 | 11 | const MASTER_PORT = 11234; 12 | 13 | // Each Test in this suite simulates rapid fire connection/disconnection 14 | // of TCPROS clients 15 | describe('ClientShutdown', function() { 16 | 17 | this.timeout(10000); 18 | this.slow(10000); 19 | 20 | let sub = null; 21 | let pub = null; 22 | let service = null; 23 | let client = null; 24 | 25 | let interval1; 26 | let interval2; 27 | let interval3; 28 | 29 | let masterStub; 30 | 31 | function startSub(nh) { 32 | sub = nh.subscribe(TOPIC, TYPE, (msg) => { 33 | console.log('%j', msg); 34 | }); 35 | 36 | return sub; 37 | } 38 | 39 | function stopSub() { 40 | if (sub) { 41 | sub.shutdown(); 42 | sub = null; 43 | } 44 | } 45 | 46 | function startPub(nh) { 47 | pub = nh.advertise(TOPIC, TYPE); 48 | return pub; 49 | } 50 | 51 | function stopPub() { 52 | if (pub) { 53 | pub.shutdown(); 54 | pub = null; 55 | } 56 | } 57 | 58 | function startService(nh) { 59 | service = nh.advertiseService(SERVICE, SRV, () => { 60 | console.log('handling service call'); 61 | return true; 62 | }); 63 | return service; 64 | } 65 | 66 | function stopService() { 67 | if (service) { 68 | service.shutdown(); 69 | service = null; 70 | } 71 | } 72 | 73 | function startClient(nh) { 74 | client = nh.serviceClient(SERVICE, SRV); 75 | return client; 76 | } 77 | 78 | function stopClient() { 79 | if (client) { 80 | client.shutdown(); 81 | client = null; 82 | } 83 | } 84 | 85 | before((done) => { 86 | masterStub = xmlrpc.createServer({host: 'localhost', port: MASTER_PORT}, () => { done(); }); 87 | }); 88 | 89 | after((done) => { 90 | masterStub.close(() => { done(); }); 91 | }); 92 | 93 | beforeEach(() => { 94 | let pubInfo = null; 95 | let subInfo = null; 96 | let serviceInfo = null; 97 | 98 | masterStub.on('getUri', (err, params, callback) => { 99 | const resp = [ 1, '', 'localhost:11311/' ]; 100 | callback(null, resp); 101 | }); 102 | 103 | masterStub.on('registerSubscriber', (err, params, callback) => { 104 | subInfo = params[3]; 105 | // console.log('sub reg ' + params); 106 | //console.log(pubInfo); 107 | 108 | const resp = [1, 'You did it!', []]; 109 | if (pubInfo) { 110 | resp[2].push(pubInfo); 111 | } 112 | callback(null, resp); 113 | }); 114 | 115 | masterStub.on('unregisterSubscriber', (err, params, callback) => { 116 | // console.log('unregister subscriber!'); 117 | const resp = [1, 'You did it!', subInfo ? 1 : 0]; 118 | callback(null, resp); 119 | subInfo = null; 120 | }); 121 | 122 | masterStub.on('registerPublisher', (err, params, callback) => { 123 | // console.log('pub reg ' + Date.now()); 124 | pubInfo = params[3]; 125 | const resp = [1, 'You did it!', []]; 126 | if (subInfo) { 127 | resp[2].push(pubInfo); 128 | let subAddrParts = subInfo.replace('http://', '').split(':'); 129 | let client = xmlrpc.createClient({host: subAddrParts[0], port: subAddrParts[1]}); 130 | let data = [1, TOPIC, [pubInfo]]; 131 | client.methodCall('publisherUpdate', data, (err, response) => { }); 132 | } 133 | callback(null, resp); 134 | }); 135 | 136 | masterStub.on('unregisterPublisher', (err, params, callback) => { 137 | // console.log('pub unreg ' + Date.now()); 138 | const resp = [1, 'You did it!', pubInfo ? 1 : 0]; 139 | callback(null, resp); 140 | if (subInfo) { 141 | let subAddrParts = subInfo.replace('http://', '').split(':'); 142 | let client = xmlrpc.createClient({host: subAddrParts[0], port: subAddrParts[1]}); 143 | let data = [1, TOPIC, []]; 144 | client.methodCall('publisherUpdate', data, (err, response) => { }); 145 | } 146 | pubInfo = null; 147 | }); 148 | 149 | masterStub.on('registerService', (err, params, callback) => { 150 | serviceInfo = params[2]; 151 | 152 | const resp = [1, 'You did it!', []]; 153 | callback(null, resp); 154 | }); 155 | 156 | masterStub.on('unregisterService', (err, params, callback) => { 157 | const resp = [1, 'You did it!', subInfo ? 1 : 0]; 158 | callback(null, resp); 159 | serviceInfo = null; 160 | }); 161 | 162 | masterStub.on('lookupService', (err, params, callback) => { 163 | if (serviceInfo) { 164 | const resp = [1, "you did it", serviceInfo]; 165 | callback(null, resp); 166 | } 167 | else { 168 | const resp = [-1, "no provider", ""]; 169 | callback(null, resp); 170 | } 171 | }); 172 | 173 | masterStub.on('NotFound', (method, params) => { 174 | console.error('Got unknown method call %s: %j', method, params); 175 | }); 176 | 177 | return rosnodejs.initNode('/my_node', {rosMasterUri: `http://localhost:${MASTER_PORT}`, logging: {testing: true}, notime:true}); 178 | }); 179 | 180 | afterEach(() => { 181 | sub = null; 182 | pub = null; 183 | service = null; 184 | client = null; 185 | 186 | clearInterval(interval1); 187 | clearInterval(interval2); 188 | clearInterval(interval3); 189 | 190 | const nh = rosnodejs.nh; 191 | 192 | // clear out any service, subs, pubs 193 | nh._node._services = {}; 194 | nh._node._subscribers = {}; 195 | nh._node._publishers = {}; 196 | 197 | // remove any master api handlers we set up 198 | masterStub.removeAllListeners(); 199 | }); 200 | 201 | it('Subscriber Shutdown', (done) => { 202 | const nh = rosnodejs.nh; 203 | const pub = startPub(nh); 204 | 205 | const msg = {data: 'This shouldn\'t crash'}; 206 | 207 | interval1 = setInterval(() => { 208 | pub.publish(msg); 209 | }, 3); 210 | 211 | interval2 = setInterval(() => { 212 | if (sub === null) { 213 | startSub(nh); 214 | } 215 | else { 216 | stopSub(); 217 | } 218 | }, 10); 219 | 220 | setTimeout(done, 8000); 221 | }); 222 | 223 | it('Publisher Shutdown', (done) => { 224 | const nh = rosnodejs.nh; 225 | startSub(nh); 226 | 227 | const msg = {data: 'This shouldn\'t crash'}; 228 | 229 | interval1 = setInterval(() => { 230 | if (pub) { 231 | pub.publish(msg, -1); 232 | } 233 | }, 3); 234 | 235 | interval2 = setInterval(() => { 236 | if (pub === null) { 237 | startPub(nh); 238 | } 239 | else { 240 | stopPub(); 241 | } 242 | }, 10); 243 | 244 | setTimeout(done, 8000); 245 | }); 246 | 247 | it('Pub Sub Shutdown', (done) => { 248 | const nh = rosnodejs.nh; 249 | 250 | const msg = {data: 'This shouldn\'t crash'}; 251 | 252 | interval1 = setInterval(() => { 253 | if (pub) { 254 | pub.publish(msg); 255 | } 256 | }, 3); 257 | 258 | interval2 = setInterval(() => { 259 | if (pub === null) { 260 | startPub(nh); 261 | } 262 | else { 263 | stopPub(); 264 | } 265 | }, 10); 266 | 267 | interval3 = setInterval(() => { 268 | if (sub === null) { 269 | startSub(nh); 270 | } 271 | else { 272 | stopSub(); 273 | } 274 | }, 7); 275 | 276 | setTimeout(done, 8000); 277 | }); 278 | 279 | it('Service Shutdown', (done) => { 280 | const nh = rosnodejs.nh; 281 | const client = startClient(nh); 282 | 283 | const req = {}; 284 | 285 | interval1 = setInterval(() => { 286 | client.call(req); 287 | }, 3); 288 | 289 | interval2 = setInterval(() => { 290 | if (service === null) { 291 | startService(nh); 292 | } 293 | else { 294 | stopService(); 295 | } 296 | }, 10); 297 | 298 | setTimeout(done, 8000); 299 | }); 300 | 301 | it('Client Shutdown', (done) => { 302 | const nh = rosnodejs.nh; 303 | startService(nh); 304 | 305 | const req = {}; 306 | 307 | interval1 = setInterval(() => { 308 | if (client) { 309 | client.call(req); 310 | } 311 | }, 1); 312 | 313 | interval2 = setInterval(() => { 314 | if (client === null) { 315 | startClient(nh); 316 | } 317 | else { 318 | stopClient(); 319 | } 320 | }, 10); 321 | 322 | setTimeout(done, 8000); 323 | }); 324 | 325 | it('Client Service Shutdown', (done) => { 326 | const nh = rosnodejs.nh; 327 | 328 | const req = {}; 329 | 330 | interval1 = setInterval(() => { 331 | if (client) { 332 | client.call(req); 333 | } 334 | }, 1); 335 | 336 | interval2 = setInterval(() => { 337 | if (client === null) { 338 | startClient(nh); 339 | } 340 | else { 341 | stopClient(); 342 | } 343 | }, 10); 344 | 345 | interval3 = setInterval(() => { 346 | if (service === null) { 347 | startService(nh); 348 | } 349 | else { 350 | stopService(); 351 | } 352 | }, 7); 353 | 354 | setTimeout(done, 8000); 355 | }); 356 | 357 | }); 358 | -------------------------------------------------------------------------------- /test/utils/MasterStub.js: -------------------------------------------------------------------------------- 1 | const xmlrpc = require('xmlrpc-rosnodejs'); 2 | const EventEmitter = require('events').EventEmitter; 3 | 4 | class RosMasterStub extends EventEmitter { 5 | constructor(host, port) { 6 | super(); 7 | 8 | this._host = host; 9 | this._port = port; 10 | 11 | this._apiMap = { 12 | getUri: this._onGetUri.bind(this), 13 | getParam: this._onGetParam.bind(this), 14 | registerService: this._onRegisterService.bind(this), 15 | unregisterService: this._onUnregisterService.bind(this), 16 | registerSubscriber: this._onRegisterSubscriber.bind(this), 17 | unregisterSubscriber: this._onUnregisterSubscriber.bind(this), 18 | registerPublisher: this._onRegisterPublisher.bind(this), 19 | unregisterPublisher: this._onUnregisterPublisher.bind(this), 20 | deleteParam: this._deleteParam.bind(this), 21 | setParam: this._setParam.bind(this), 22 | getParam: this._getParam.bind(this), 23 | hasParam: this._hasParam.bind(this), 24 | getParamNames: this._getParamNames.bind(this) 25 | }; 26 | 27 | this._providedApis = new Set(); 28 | 29 | this._clientCache = { 30 | service: null, 31 | sub: null, 32 | pub: null 33 | }; 34 | 35 | this._params = {}; 36 | 37 | this.verbose = true; 38 | 39 | this.listen(); 40 | } 41 | 42 | listen() { 43 | this._server = xmlrpc.createServer({host: this._host, port: this._port}, () => { 44 | this.emit('ready'); 45 | }); 46 | 47 | this._server.on('NotFound', (method, params) => { 48 | if (this.verbose) { 49 | console.error('Method %s does not exist', method); 50 | } 51 | }); 52 | } 53 | 54 | shutdown() { 55 | this._params = {}; 56 | this._providedApis.clear(); 57 | this.removeAllListeners(); 58 | return new Promise((resolve, reject) => { 59 | this._server.close(resolve); 60 | this._server = null; 61 | }); 62 | } 63 | 64 | provide(api) { 65 | const method = this._apiMap[api]; 66 | if (method && !this._providedApis.has(api)) { 67 | this._server.on(api, (err, params, callback) => { 68 | this.emit(api, err, params, callback); 69 | method(err, params, callback); 70 | }); 71 | this._providedApis.add(api); 72 | } 73 | } 74 | 75 | provideAll() { 76 | Object.keys(this._apiMap).forEach((api) => { 77 | this.provide(api); 78 | }); 79 | } 80 | 81 | _onGetUri(err, params, callback) { 82 | const resp = [ 1, '', `${this._host}:${this._port}`]; 83 | callback(null, resp); 84 | } 85 | 86 | _onGetParam(err, params, callback) { 87 | const resp = [0, '', 'Not implemented in stub']; 88 | callback(null, resp); 89 | } 90 | 91 | _onRegisterService(err, params, callback) { 92 | this._clientCache.service = params[2]; 93 | 94 | const resp = [1, 'You did it!', []]; 95 | callback(null, resp); 96 | } 97 | 98 | _onUnregisterService(err, params, callback) { 99 | const resp = [1, 'You did it!', this._clientCache.service ? 1 : 0]; 100 | callback(null, resp); 101 | this._clientCache.service = null; 102 | } 103 | 104 | _onLookupService(err, params, callback) { 105 | const { service } = this._clientCache; 106 | if (service) { 107 | const resp = [1, "you did it", service]; 108 | callback(null, resp); 109 | } 110 | else { 111 | const resp = [-1, "no provider", ""]; 112 | callback(null, resp); 113 | } 114 | } 115 | 116 | _onRegisterSubscriber(err, params, callback) { 117 | this._clientCache.sub = params[3]; 118 | 119 | const resp = [1, 'You did it!', []]; 120 | if (this._clientCache.pub) { 121 | resp[2].push(this._clientCache.pub); 122 | } 123 | callback(null, resp); 124 | } 125 | 126 | _onUnregisterSubscriber(err, params, callback) { 127 | const resp = [1, 'You did it!', this._clientCache.sub ? 1 : 0]; 128 | callback(null, resp); 129 | this._clientCache.sub = null; 130 | } 131 | 132 | _onRegisterPublisher(err, params, callback) { 133 | const pubInfo = params[3]; 134 | this._clientCache.pub = pubInfo; 135 | 136 | const resp = [1, 'You did it!', []]; 137 | if (this._clientCache.sub) { 138 | resp[2].push(pubInfo); 139 | let subAddrParts = this._clientCache.sub.replace('http://', '').split(':'); 140 | let client = xmlrpc.createClient({host: subAddrParts[0], port: subAddrParts[1]}); 141 | let data = [1, topic, [pubInfo]]; 142 | client.methodCall('publisherUpdate', data, (err, response) => { }); 143 | } 144 | callback(null, resp); 145 | } 146 | 147 | _onUnregisterPublisher(err, params, callback) { 148 | const resp = [1, 'You did it!', this._clientCache.pub ? 1 : 0]; 149 | callback(null, resp); 150 | this._clientCache.pub = null; 151 | } 152 | 153 | // Param stubbing 154 | // NOTE: this is NOT a spec ParamServer implementation, 155 | // but it provides simple stubs for calls 156 | 157 | _deleteParam(err, params, callback) { 158 | const key = params[1]; 159 | if (this._params.hasOwnProperty(key)) { 160 | delete this._params[key]; 161 | const resp = [1, 'delete value for ' + key, 1]; 162 | callback(null, resp); 163 | } 164 | else { 165 | const resp = [0, 'no value for ' + key, 1]; 166 | callback(null, resp); 167 | } 168 | } 169 | 170 | _setParam(err, params, callback) { 171 | const key = params[1]; 172 | const val = params[2]; 173 | this._params[key] = val; 174 | const resp = [1, 'set value for ' + key, 1]; 175 | callback(null, resp); 176 | } 177 | 178 | _getParam(err, params, callback) { 179 | const key = params[1]; 180 | const val = this._params[key]; 181 | if (val !== undefined) { 182 | const resp = [1, 'data for ' + key, val]; 183 | callback(null, resp); 184 | } 185 | else { 186 | const resp = [0, 'no data for ' + key, null]; 187 | callback(null, resp); 188 | } 189 | } 190 | 191 | _hasParam(err, params, callback) { 192 | const key = params[1]; 193 | const resp = [1, 'check param ' + key, this._params.hasOwnProperty(key)]; 194 | callback(null, resp); 195 | } 196 | 197 | _getParamNames(err, params, callback) { 198 | const names = Object.keys(this._params); 199 | const resp = [1, 'get param names', names]; 200 | callback(null, resp); 201 | } 202 | } 203 | 204 | module.exports = RosMasterStub; 205 | --------------------------------------------------------------------------------