diff --git a/build/roslib.js b/build/roslib.js index ccd5f1055..f637f2dfe 100644 --- a/build/roslib.js +++ b/build/roslib.js @@ -26,7 +26,7 @@ var ROSLIB = ROSLIB || { * * timeout - the timeout length when connecting to the action server */ ROSLIB.ActionClient = function(options) { - var actionClient = this; + var that = this; options = options || {}; this.ros = options.ros; this.serverName = options.serverName; @@ -43,44 +43,41 @@ ROSLIB.ActionClient = function(options) { name : this.serverName + '/feedback', messageType : this.actionName + 'Feedback' }); + var statusListener = new ROSLIB.Topic({ ros : this.ros, name : this.serverName + '/status', messageType : 'actionlib_msgs/GoalStatusArray' }); + var resultListener = new ROSLIB.Topic({ ros : this.ros, name : this.serverName + '/result', messageType : this.actionName + 'Result' }); + this.goalTopic = new ROSLIB.Topic({ ros : this.ros, name : this.serverName + '/goal', messageType : this.actionName + 'Goal' }); + this.cancelTopic = new ROSLIB.Topic({ ros : this.ros, name : this.serverName + '/cancel', messageType : 'actionlib_msgs/GoalID' }); - /** - * Cancel all goals associated with this ActionClient. - */ - this.cancel = function() { - var cancelMessage = new ROSLIB.Message({}); - this.cancelTopic.publish(cancelMessage); - }; - // advertise the goal and cancel topics this.goalTopic.advertise(); this.cancelTopic.advertise(); // subscribe to the status topic statusListener.subscribe(function(statusMessage) { + var that = this; receivedStatus = true; statusMessage.status_list.forEach(function(status) { - var goal = actionClient.goals[status.goal_id.id]; + var goal = that.goals[status.goal_id.id]; if (goal) { goal.emit('status', status); } @@ -89,7 +86,7 @@ ROSLIB.ActionClient = function(options) { // subscribe the the feedback topic feedbackListener.subscribe(function(feedbackMessage) { - var goal = actionClient.goals[feedbackMessage.status.goal_id.id]; + var goal = that.goals[feedbackMessage.status.goal_id.id]; if (goal) { goal.emit('status', feedbackMessage.status); goal.emit('feedback', feedbackMessage.feedback); @@ -98,7 +95,7 @@ ROSLIB.ActionClient = function(options) { // subscribe to the result topic resultListener.subscribe(function(resultMessage) { - var goal = actionClient.goals[resultMessage.status.goal_id.id]; + var goal = that.goals[resultMessage.status.goal_id.id]; if (goal) { goal.emit('status', resultMessage.status); @@ -110,12 +107,21 @@ ROSLIB.ActionClient = function(options) { if (this.timeout) { setTimeout(function() { if (!receivedStatus) { - actionClient.emit('timeout'); + that.emit('timeout'); } }, this.timeout); } }; ROSLIB.ActionClient.prototype.__proto__ = EventEmitter2.prototype; + +/** + * Cancel all goals associated with this ActionClient. + */ +ROSLIB.ActionClient.prototype.cancel = function() { + var cancelMessage = new ROSLIB.Message({}); + this.cancelTopic.publish(cancelMessage); +}; + /** * @author Russell Toris - rctoris@wpi.edu */ @@ -131,45 +137,18 @@ ROSLIB.ActionClient.prototype.__proto__ = EventEmitter2.prototype; * * actionClient - the ROSLIB.ActionClient to use with this goal * * goalMessage - The JSON object containing the goal for the action server */ - ROSLIB.Goal = function(options) { - var goal = this; + var that = this; this.actionClient = options.actionClient; this.goalMessage = options.goalMessage; this.isFinished = false; - // used to create random IDs + // Used to create random IDs var date = new Date(); - /** - * Send the goal to the action server. - * - * @param timeout (optional) - a timeout length for the goal's result - */ - this.send = function(timeout) { - goal.actionClient.goalTopic.publish(goal.goalMessage); - if (timeout) { - setTimeout(function() { - if (!goal.isFinished) { - goal.emit('timeout'); - } - }, timeout); - } - }; - - /** - * Cancel the current goal. - */ - this.cancel = function() { - var cancelMessage = new ROSLIB.Message({ - id : goal.goalID - }); - goal.actionClient.cancelTopic.publish(cancelMessage); - }; - - // create a random ID - this.goalID = 'goal_' + Math.random() + "_" + date.getTime(); - // fill in the goal message + // Create a random ID + this.goalID = 'goal_' + Math.random() + '_' + date.getTime(); + // Fill in the goal message this.goalMessage = new ROSLIB.Message({ goal_id : { stamp : { @@ -182,30 +161,57 @@ ROSLIB.Goal = function(options) { }); this.on('status', function(status) { - this.status = status; + that.status = status; }); this.on('result', function(result) { - this.isFinished = true; - this.result = result; + that.isFinished = true; + that.result = result; }); this.on('feedback', function(feedback) { - this.feedback = feedback; + that.feedback = feedback; }); - // add the goal + // Add the goal this.actionClient.goals[this.goalID] = this; - }; ROSLIB.Goal.prototype.__proto__ = EventEmitter2.prototype; + /** - * @author Brandon Alexander - balexander@willowgarage.com + * Send the goal to the action server. + * + * @param timeout (optional) - a timeout length for the goal's result + */ +ROSLIB.Goal.prototype.send = function(timeout) { + var that = this; + that.actionClient.goalTopic.publish(that.goalMessage); + if (timeout) { + setTimeout(function() { + if (!that.isFinished) { + that.emit('timeout'); + } + }, timeout); + } +}; + +/** + * Cancel the current goal. + */ +ROSLIB.Goal.prototype.cancel = function() { + var cancelMessage = new ROSLIB.Message({ + id : this.goalID + }); + this.actionClient.cancelTopic.publish(cancelMessage); +}; + +/** + * @author Brandon Alexander - baalexander@gmail.com */ /** * Message objects are used for publishing and subscribing to and from topics. - * + * * @constructor * @param values - object matching the fields defined in the .msg definition file. */ @@ -218,7 +224,7 @@ ROSLIB.Message = function(values) { } }; /** - * @author Brandon Alexander - balexander@willowgarage.com + * @author Brandon Alexander - baalexander@gmail.com */ /** @@ -230,58 +236,58 @@ ROSLIB.Message = function(values) { * * name - the param name, like max_vel_x */ ROSLIB.Param = function(options) { - var param = this; options = options || {}; this.ros = options.ros; this.name = options.name; +}; - /** - * Fetches the value of the param. - * - * @param callback - function with the following params: - * * value - the value of the param from ROS. - */ - this.get = function(callback) { - var paramClient = new ROSLIB.Service({ - ros : param.ros, - name : '/rosapi/get_param', - serviceType : 'rosapi/GetParam' - }); +/** + * Fetches the value of the param. + * + * @param callback - function with the following params: + * * value - the value of the param from ROS. + */ +ROSLIB.Param.prototype.get = function(callback) { + var paramClient = new ROSLIB.Service({ + ros : this.ros, + name : '/rosapi/get_param', + serviceType : 'rosapi/GetParam' + }); - var request = new ROSLIB.ServiceRequest({ - name : param.name, - value : JSON.stringify('') - }); + var request = new ROSLIB.ServiceRequest({ + name : this.name, + value : JSON.stringify('') + }); - paramClient.callService(request, function(result) { - var value = JSON.parse(result.value); - callback(value); - }); - }; + paramClient.callService(request, function(result) { + var value = JSON.parse(result.value); + callback(value); + }); +}; - /** - * Sets the value of the param in ROS. - * - * @param value - value to set param to. - */ - this.set = function(value) { - var paramClient = new ROSLIB.Service({ - ros : param.ros, - name : '/rosapi/set_param', - serviceType : 'rosapi/SetParam' - }); +/** + * Sets the value of the param in ROS. + * + * @param value - value to set param to. + */ +ROSLIB.Param.prototype.set = function(value) { + var paramClient = new ROSLIB.Service({ + ros : this.ros, + name : '/rosapi/set_param', + serviceType : 'rosapi/SetParam' + }); - var request = new ROSLIB.ServiceRequest({ - name : param.name, - value : JSON.stringify(value) - }); + var request = new ROSLIB.ServiceRequest({ + name : this.name, + value : JSON.stringify(value) + }); - paramClient.callService(request, function() { - }); - }; + paramClient.callService(request, function() { + }); }; + /** - * @author Brandon Alexander - balexander@willowgarage.com + * @author Brandon Alexander - baalexander@gmail.com */ /** @@ -298,40 +304,55 @@ ROSLIB.Param = function(options) { * @param url (optional) - The WebSocket URL for rosbridge. Can be specified later with `connect`. */ ROSLIB.Ros = function(url) { - var ros = this; this.socket = null; + + // begin by checking if a URL was given + if (url) { + this.connect(url); + } +}; +ROSLIB.Ros.prototype.__proto__ = EventEmitter2.prototype; + +/** + * Connect to the specified WebSocket. + * + * @param url - WebSocket URL for Rosbridge + */ +ROSLIB.Ros.prototype.connect = function(url) { + var that = this; + /** * Emits a 'connection' event on WebSocket connection. - * + * * @param event - the argument to emit with the event. */ function onOpen(event) { - ros.emit('connection', event); + that.emit('connection', event); }; /** * Emits a 'close' event on WebSocket disconnection. - * + * * @param event - the argument to emit with the event. */ function onClose(event) { - ros.emit('close', event); + that.emit('close', event); }; /** * Emits an 'error' event whenever there was an error. - * + * * @param event - the argument to emit with the event. */ function onError(event) { - ros.emit('error', event); + that.emit('error', event); }; /** - * If a message was compressed as a PNG image (a compression hack since gzipping over WebSockets - * is not supported yet), this function places the "image" in a canvas element then decodes the - * "image" as a Base64 string. + * If a message was compressed as a PNG image (a compression hack since + * gzipping over WebSockets * is not supported yet), this function places the + * "image" in a canvas element then decodes the * "image" as a Base64 string. * * @param data - object containing the PNG data. * @param callback - function with params: @@ -357,7 +378,7 @@ ROSLIB.Ros = function(url) { // Constructs the JSON. var jsonData = ''; - for ( var i = 0; i < imageData.length; i += 4) { + for (var i = 0; i < imageData.length; i += 4) { // RGB jsonData += String.fromCharCode(imageData[i], imageData[i + 1], imageData[i + 2]); } @@ -369,16 +390,17 @@ ROSLIB.Ros = function(url) { } /** - * Parses message responses from rosbridge and sends to the appropriate topic, service, or param. + * Parses message responses from rosbridge and sends to the appropriate + * topic, service, or param. * * @param message - the raw JSON message from rosbridge. */ function onMessage(message) { function handleMessage(message) { if (message.op === 'publish') { - ros.emit(message.topic, message.msg); + that.emit(message.topic, message.msg); } else if (message.op === 'service_response') { - ros.emit(message.id, message.values); + that.emit(message.id, message.values); } }; @@ -392,137 +414,127 @@ ROSLIB.Ros = function(url) { } }; - /** - * Connect to the specified WebSocket. - * - * @param url - WebSocket URL for Rosbridge - */ - this.connect = function(url) { - ros.socket = new WebSocket(url); - ros.socket.onopen = onOpen; - ros.socket.onclose = onClose; - ros.socket.onerror = onError; - ros.socket.onmessage = onMessage; - }; + that.socket = new WebSocket(url); + that.socket.onopen = onOpen; + that.socket.onclose = onClose; + that.socket.onerror = onError; + that.socket.onmessage = onMessage; +}; - /** - * Disconnect from the WebSocket server. - */ - this.close = function() { - if (ros.socket) { - ros.socket.close(); - } - }; +/** + * Disconnect from the WebSocket server. + */ +ROSLIB.Ros.prototype.close = function() { + if (this.socket) { + this.socket.close(); + } +}; - /** - * Sends an authorization request to the server. - * - * @param mac - MAC (hash) string given by the trusted source. - * @param client - IP of the client. - * @param dest - IP of the destination. - * @param rand - Random string given by the trusted source. - * @param t - Time of the authorization request. - * @param level - User level as a string given by the client. - * @param end - End time of the client's session. - */ - this.authenticate = function(mac, client, dest, rand, t, level, end) { - // create the request - var auth = { - op : 'auth', - mac : mac, - client : client, - dest : dest, - rand : rand, - t : t, - level : level, - end : end - }; - // send the request - callOnConnection(auth); +/** + * Sends an authorization request to the server. + * + * @param mac - MAC (hash) string given by the trusted source. + * @param client - IP of the client. + * @param dest - IP of the destination. + * @param rand - Random string given by the trusted source. + * @param t - Time of the authorization request. + * @param level - User level as a string given by the client. + * @param end - End time of the client's session. + */ +ROSLIB.Ros.prototype.authenticate = function(mac, client, dest, rand, t, level, end) { + // create the request + var auth = { + op : 'auth', + mac : mac, + client : client, + dest : dest, + rand : rand, + t : t, + level : level, + end : end }; + // send the request + callOnConnection(auth); +}; - /** - * Sends the message over the WebSocket, but queues the message up if not yet connected. - */ - this.callOnConnection = function(message) { - var messageJson = JSON.stringify(message); - - if (ros.socket.readyState !== WebSocket.OPEN) { - ros.once('connection', function() { - ros.socket.send(messageJson); - }); - } else { - ros.socket.send(messageJson); - } - }; +/** + * Sends the message over the WebSocket, but queues the message up if not yet + * connected. + */ +ROSLIB.Ros.prototype.callOnConnection = function(message) { + var that = this; + var messageJson = JSON.stringify(message); - /** - * Retrieves list of topics in ROS as an array. - * - * @param callback function with params: - * * topics - Array of topic names - */ - this.getTopics = function(callback) { - var topicsClient = new ROSLIB.Service({ - ros : ros, - name : '/rosapi/topics', - serviceType : 'rosapi/Topics' + if (ros.socket.readyState !== WebSocket.OPEN) { + that.once('connection', function() { + that.socket.send(messageJson); }); + } else { + that.socket.send(messageJson); + } +}; - var request = new ROSLIB.ServiceRequest(); +/** + * Retrieves list of topics in ROS as an array. + * + * @param callback function with params: + * * topics - Array of topic names + */ +ROSLIB.Ros.prototype.getTopics = function(callback) { + var topicsClient = new ROSLIB.Service({ + ros : this, + name : '/rosapi/topics', + serviceType : 'rosapi/Topics' + }); - topicsClient.callService(request, function(result) { - callback(result.topics); - }); - }; + var request = new ROSLIB.ServiceRequest(); - /** - * Retrieves list of active service names in ROS. - * - * @param callback - function with the following params: - * * services - array of service names - */ - this.getServices = function(callback) { - var servicesClient = new ROSLIB.Service({ - ros : ros, - name : '/rosapi/services', - serviceType : 'rosapi/Services' - }); + topicsClient.callService(request, function(result) { + callback(result.topics); + }); +}; - var request = new ROSLIB.ServiceRequest(); +/** + * Retrieves list of active service names in ROS. + * + * @param callback - function with the following params: + * * services - array of service names + */ +ROSLIB.Ros.prototype.getServices = function(callback) { + var servicesClient = new ROSLIB.Service({ + ros : this, + name : '/rosapi/services', + serviceType : 'rosapi/Services' + }); - servicesClient.callService(request, function(result) { - callback(result.services); - }); - }; + var request = new ROSLIB.ServiceRequest(); - /** - * Retrieves list of param names from the ROS Parameter Server. - * - * @param callback function with params: - * * params - array of param names. - */ - this.getParams = function(callback) { - var paramsClient = new ROSLIB.Service({ - ros : ros, - name : '/rosapi/get_param_names', - serviceType : 'rosapi/GetParamNames' - }); + servicesClient.callService(request, function(result) { + callback(result.services); + }); +}; - var request = new ROSLIB.ServiceRequest(); - paramsClient.callService(request, function(result) { - callback(result.names); - }); - }; +/** + * Retrieves list of param names from the ROS Parameter Server. + * + * @param callback function with params: + * * params - array of param names. + */ +ROSLIB.Ros.prototype.getParams = function(callback) { + var paramsClient = new ROSLIB.Service({ + ros : this, + name : '/rosapi/get_param_names', + serviceType : 'rosapi/GetParamNames' + }); - // begin by checking if a URL was given - if (url) { - this.connect(url); - } + var request = new ROSLIB.ServiceRequest(); + paramsClient.callService(request, function(result) { + callback(result.names); + }); }; -ROSLIB.Ros.prototype.__proto__ = EventEmitter2.prototype; + /** - * @author Brandon Alexander - balexander@willowgarage.com + * @author Brandon Alexander - baalexander@gmail.com */ /** @@ -535,42 +547,42 @@ ROSLIB.Ros.prototype.__proto__ = EventEmitter2.prototype; * * serviceType - the service type, like 'rospy_tutorials/AddTwoInts' */ ROSLIB.Service = function(options) { - var service = this; options = options || {}; this.ros = options.ros; this.name = options.name; this.serviceType = options.serviceType; +}; - /** - * Calls the service. Returns the service response in the callback. - * - * @param request - the ROSLIB.ServiceRequest to send - * @param callback - function with params: - * * response - the response from the service request - */ - this.callService = function(request, callback) { - service.ros.idCounter++; - serviceCallId = 'call_service:' + service.name + ':' + service.ros.idCounter; +/** + * Calls the service. Returns the service response in the callback. + * + * @param request - the ROSLIB.ServiceRequest to send + * @param callback - function with params: + * * response - the response from the service request + */ +ROSLIB.Service.prototype.callService = function(request, callback) { + this.ros.idCounter++; + serviceCallId = 'call_service:' + this.name + ':' + this.ros.idCounter; - service.ros.once(serviceCallId, function(data) { - var response = new ROSLIB.ServiceResponse(data); - callback(response); - }); + this.ros.once(serviceCallId, function(data) { + var response = new ROSLIB.ServiceResponse(data); + callback(response); + }); - var requestValues = []; - Object.keys(request).forEach(function(name) { - requestValues.push(request[name]); - }); + var requestValues = []; + Object.keys(request).forEach(function(name) { + requestValues.push(request[name]); + }); - var call = { - op : 'call_service', - id : serviceCallId, - service : service.name, - args : requestValues - }; - service.ros.callOnConnection(call); + var call = { + op : 'call_service', + id : serviceCallId, + service : this.name, + args : requestValues }; + this.ros.callOnConnection(call); }; + /** * @author Brandon Alexander - balexander@willowgarage.com */ @@ -582,10 +594,10 @@ ROSLIB.Service = function(options) { * @param values - object matching the values of the request part from the .srv file. */ ROSLIB.ServiceRequest = function(values) { - var serviceRequest = this; + var that = this; if (values) { Object.keys(values).forEach(function(name) { - serviceRequest[name] = values[name]; + that[name] = values[name]; }); } }; @@ -600,24 +612,24 @@ ROSLIB.ServiceRequest = function(values) { * @param values - object matching the values of the response part from the .srv file. */ ROSLIB.ServiceResponse = function(values) { - var serviceResponse = this; + var that = this; if (values) { Object.keys(values).forEach(function(name) { - serviceResponse[name] = values[name]; + that[name] = values[name]; }); } }; /** - * @author Brandon Alexander - balexander@willowgarage.com + * @author Brandon Alexander - baalexander@gmail.com */ /** * Publish and/or subscribe to a topic in ROS. - * + * * Emits the following events: * * 'warning' - if there are any warning during the Topic creation * * 'message' - the message data from rosbridge - * + * * @constructor * @param options - object with following keys: * * ros - the ROSLIB.Ros connection handle @@ -627,7 +639,6 @@ ROSLIB.ServiceResponse = function(values) { * * throttle_rate - the rate at which to throttle the topics */ ROSLIB.Topic = function(options) { - var topic = this; options = options || {}; this.ros = options.ros; this.name = options.name; @@ -647,107 +658,108 @@ ROSLIB.Topic = function(options) { this.emit('warning', this.throttle_rate + ' is not allowed. Set to 0'); this.throttle_rate = 0; } +}; +ROSLIB.Topic.prototype.__proto__ = EventEmitter2.prototype; - /** - * Every time a message is published for the given topic, the callback - * will be called with the message object. - * - * @param callback - function with the following params: - * * message - the published message - */ - this.subscribe = function(callback) { - topic.on('message', function(message) { - callback(message); - }); - - topic.ros.on(topic.name, function(data) { - var message = new ROSLIB.Message(data); - topic.emit('message', message); - }); +/** + * Every time a message is published for the given topic, the callback + * will be called with the message object. + * + * @param callback - function with the following params: + * * message - the published message + */ +ROSLIB.Topic.prototype.subscribe = function(callback) { + this.on('message', function(message) { + callback(message); + }); - topic.ros.idCounter++; - var subscribeId = 'subscribe:' + topic.name + ':' + topic.ros.idCounter; - var call = { - op : 'subscribe', - id : subscribeId, - type : topic.messageType, - topic : topic.name, - compression : topic.compression, - throttle_rate : topic.throttle_rate - }; + this.ros.on(this.name, function(data) { + var message = new ROSLIB.Message(data); + this.emit('message', message); + }); - topic.ros.callOnConnection(call); + this.ros.idCounter++; + var subscribeId = 'subscribe:' + this.name + ':' + this.ros.idCounter; + var call = { + op : 'subscribe', + id : subscribeId, + type : this.messageType, + topic : this.name, + compression : this.compression, + throttle_rate : this.throttle_rate }; - /** - * Unregisters as a subscriber for the topic. Unsubscribing will remove - * all subscribe callbacks. - */ - this.unsubscribe = function() { - topic.ros.removeAllListeners([ topic.name ]); - topic.ros.idCounter++; - var unsubscribeId = 'unsubscribe:' + topic.name + ':' + topic.ros.idCounter; - var call = { - op : 'unsubscribe', - id : unsubscribeId, - topic : topic.name - }; - topic.ros.callOnConnection(call); + this.ros.callOnConnection(call); +}; + +/** + * Unregisters as a subscriber for the topic. Unsubscribing will remove + * all subscribe callbacks. + */ +ROSLIB.Topic.prototype.unsubscribe = function() { + this.ros.removeAllListeners([this.name]); + this.ros.idCounter++; + var unsubscribeId = 'unsubscribe:' + this.name + ':' + this.ros.idCounter; + var call = { + op : 'unsubscribe', + id : unsubscribeId, + topic : this.name }; + this.ros.callOnConnection(call); +}; - /** - * Registers as a publisher for the topic. - */ - this.advertise = function() { - topic.ros.idCounter++; - var advertiseId = 'advertise:' + topic.name + ':' + topic.ros.idCounter; - var call = { - op : 'advertise', - id : advertiseId, - type : topic.messageType, - topic : topic.name - }; - topic.ros.callOnConnection(call); - topic.isAdvertised = true; +/** + * Registers as a publisher for the topic. + */ +ROSLIB.Topic.prototype.advertise = function() { + this.ros.idCounter++; + var advertiseId = 'advertise:' + this.name + ':' + this.ros.idCounter; + var call = { + op : 'advertise', + id : advertiseId, + type : this.messageType, + topic : this.name }; + this.ros.callOnConnection(call); + this.isAdvertised = true; +}; - /** - * Unregisters as a publisher for the topic. - */ - this.unadvertise = function() { - topic.ros.idCounter++; - var unadvertiseId = 'unadvertise:' + topic.name + ':' + topic.ros.idCounter; - var call = { - op : 'unadvertise', - id : unadvertiseId, - topic : topic.name - }; - topic.ros.callOnConnection(call); - topic.isAdvertised = false; +/** + * Unregisters as a publisher for the topic. + */ +ROSLIB.Topic.prototype.unadvertise = function() { + this.ros.idCounter++; + var unadvertiseId = 'unadvertise:' + this.name + ':' + this.ros.idCounter; + var call = { + op : 'unadvertise', + id : unadvertiseId, + topic : this.name }; + this.ros.callOnConnection(call); + this.isAdvertised = false; +}; - /** - * Publish the message. - * - * @param message - A ROSLIB.Message object. - */ - this.publish = function(message) { - if (!topic.isAdvertised) { - topic.advertise(); - } +/** + * Publish the message. + * + * @param message - A ROSLIB.Message object. + */ +ROSLIB.Topic.prototype.publish = function(message) { + if (!this.isAdvertised) { + this.advertise(); + } - topic.ros.idCounter++; - var publishId = 'publish:' + topic.name + ':' + topic.ros.idCounter; - var call = { - op : 'publish', - id : publishId, - topic : topic.name, - msg : message - }; - topic.ros.callOnConnection(call); + this.ros.idCounter++; + var publishId = 'publish:' + this.name + ':' + this.ros.idCounter; + var call = { + op : 'publish', + id : publishId, + topic : this.name, + msg : message }; + this.ros.callOnConnection(call); }; -ROSLIB.Topic.prototype.__proto__ = EventEmitter2.prototype; + /** * @author David Gossow - dgossow@willowgarage.com */ @@ -760,8 +772,7 @@ ROSLIB.Topic.prototype.__proto__ = EventEmitter2.prototype; * @param orientation - the ROSLIB.Quaternion describing the orientation */ ROSLIB.Pose = function(position, orientation) { - var pose = this; - // copy the values into this object if they exist + // Copy the values into this object if they exist this.position = new ROSLIB.Vector3(); this.orientation = new ROSLIB.Quaternion(); if (position !== undefined) { @@ -770,19 +781,20 @@ ROSLIB.Pose = function(position, orientation) { if (orientation !== undefined) { this.orientation.copy(orientation); } +}; - /** - * Copy the values from the given pose into this pose. - * - * @param pose the pose to copy - * @returns a pointer to this pose - */ - this.copy = function(pose) { - pose.position.copy(pose.position); - pose.orientation.copy(pose.orientation); - return pose; - }; +/** + * Copy the values from the given pose into this pose. + * + * @param pose the pose to copy + * @returns a pointer to this pose + */ +ROSLIB.Pose.prototype.copy = function(pose) { + this.position.copy(pose.position); + this.orientation.copy(pose.orientation); + return pose; }; + /** * @author David Gossow - dgossow@willowgarage.com */ @@ -802,111 +814,114 @@ ROSLIB.Quaternion = function(x, y, z, w) { this.y = y || 0; this.z = z || 0; this.w = w || 1; +}; - /** - * Copy the values from the given quaternion into this quaternion. - * - * @param q the quaternion to copy - * @returns a pointer to this quaternion - */ - this.copy = function(q) { - quaternion.x = q.x; - quaternion.y = q.y; - quaternion.z = q.z; - quaternion.w = q.w; - return quaternion; - }; +/** + * Copy the values from the given quaternion into this quaternion. + * + * @param q the quaternion to copy + * @returns a pointer to this quaternion + */ +ROSLIB.Quaternion.prototype.copy = function(q) { + this.x = q.x; + this.y = q.y; + this.z = q.z; + this.w = q.w; + return this; +}; - /** - * Perform a conjugation on this quaternion. - * - * @returns a pointer to this quaternion - */ - this.conjugate = function() { - quaternion.x *= -1; - quaternion.y *= -1; - quaternion.z *= -1; - return quaternion; - }; +/** + * Perform a conjugation on this quaternion. + * + * @returns a pointer to this quaternion + */ +ROSLIB.Quaternion.prototype.conjugate = function() { + this.x *= -1; + this.y *= -1; + this.z *= -1; + return this; +}; - /** - * Perform a normalization on this quaternion. - * - * @returns a pointer to this quaternion - */ - this.normalize = function() { - var l = Math.sqrt(quaternion.x * quaternion.x + quaternion.y * quaternion.y + quaternion.z - * quaternion.z + quaternion.w * quaternion.w); - if (l === 0) { - quaternion.x = 0; - quaternion.y = 0; - quaternion.z = 0; - quaternion.w = 1; - } else { - l = 1 / l; - quaternion.x = quaternion.x * l; - quaternion.y = quaternion.y * l; - quaternion.z = quaternion.z * l; - quaternion.w = quaternion.w * l; - } - return quaternion; - }; +/** + * Perform a normalization on this quaternion. + * + * @returns a pointer to this quaternion + */ +ROSLIB.Quaternion.prototype.normalize = function() { + var l = Math.sqrt( + this.x * this.x + this.y * this.y + + this.z * this.z + this.w * this.w + ); + if (l === 0) { + this.x = 0; + this.y = 0; + this.z = 0; + this.w = 1; + } else { + l = 1 / l; + this.x = this.x * l; + this.y = this.y * l; + this.z = this.z * l; + this.w = this.w * l; + } + return this; +}; - /** - * Convert this quaternion into its inverse. - * - * @returns a pointer to this quaternion - */ - this.inverse = function() { - this.conjugate().normalize(); - return quaternion; - }; +/** + * Convert this quaternion into its inverse. + * + * @returns a pointer to this quaternion + */ +ROSLIB.Quaternion.prototype.inverse = function() { + this.conjugate().normalize(); + return this; +}; - /** - * Set the values of this quaternion to the product of quaternions a and b. - * - * @param a the first quaternion to multiply with - * @param b the second quaternion to multiply with - * @returns a pointer to this quaternion - */ - this.multiply = function(a, b) { - var qax = a.x, qay = a.y, qaz = a.z, qaw = a.w, qbx = b.x, qby = b.y, qbz = b.z, qbw = b.w; - this.x = qax * qbw + qay * qbz - qaz * qby + qaw * qbx; - this.y = -qax * qbz + qay * qbw + qaz * qbx + qaw * qby; - this.z = qax * qby - qay * qbx + qaz * qbw + qaw * qbz; - this.w = -qax * qbx - qay * qby - qaz * qbz + qaw * qbw; - return quaternion; - }; +/** + * Set the values of this quaternion to the product of quaternions a and b. + * + * @param a the first quaternion to multiply with + * @param b the second quaternion to multiply with + * @returns a pointer to this quaternion + */ +ROSLIB.Quaternion.prototype.multiply = function(a, b) { + var qax = a.x, qay = a.y, qaz = a.z, qaw = a.w, qbx = b.x, qby = b.y, qbz = b.z, qbw = b.w; + this.x = qax * qbw + qay * qbz - qaz * qby + qaw * qbx; + this.y = -qax * qbz + qay * qbw + qaz * qbx + qaw * qby; + this.z = qax * qby - qay * qbx + qaz * qbw + qaw * qbz; + this.w = -qax * qbx - qay * qby - qaz * qbz + qaw * qbw; + return this; +}; - /** - * Multiply the given ROSLIB.Vector3 with this quaternion. - * - * @param vector the vector to multiply with - * @param dest (option) - where the computed values will go (defaults to 'vector'). - * @returns a pointer to dest - */ - this.multiplyVec3 = function(vector, dest) { - if (!dest) { - dest = vector; - } - var x = vector.x, y = vector.y, z = vector.z, qx = quaternion.x, qy = quaternion.y, qz = quaternion.z, qw = quaternion.w; - var ix = qw * x + qy * z - qz * y, iy = qw * y + qz * x - qx * z, iz = qw * z + qx * y - qy * x, iw = -qx - * x - qy * y - qz * z; - dest.x = ix * qw + iw * -qx + iy * -qz - iz * -qy; - dest.y = iy * qw + iw * -qy + iz * -qx - ix * -qz; - dest.z = iz * qw + iw * -qz + ix * -qy - iy * -qx; - return dest; - }; +/** + * Multiply the given ROSLIB.Vector3 with this quaternion. + * + * @param vector the vector to multiply with + * @param dest (option) - where the computed values will go (defaults to 'vector'). + * @returns a pointer to dest + */ +ROSLIB.Quaternion.prototype.multiplyVec3 = function(vector, dest) { + if (!dest) { + dest = vector; + } + var x = vector.x, y = vector.y, z = vector.z, qx = this.x, qy = this.y, qz = this.z, qw = this.w; + var ix = qw * x + qy * z - qz * y, iy = qw * y + qz * x - qx * z, iz = qw * z + qx * y - qy * x, iw = -qx + * x - qy * y - qz * z; + dest.x = ix * qw + iw * -qx + iy * -qz - iz * -qy; + dest.y = iy * qw + iw * -qy + iz * -qx - ix * -qz; + dest.z = iz * qw + iw * -qz + ix * -qy - iy * -qx; + return dest; +}; - /** - * Clone a copy of this quaternion. - * - * @returns the cloned quaternion - */ - this.clone = function() { - return new ROSLIB.Quaternion(quaternion.x, quaternion.y, quaternion.z, quaternion.w); - }; +/** + * Clone a copy of this quaternion. + * + * @returns the cloned quaternion + */ +ROSLIB.Quaternion.prototype.clone = function() { + return new ROSLIB.Quaternion(this.x, this.y, this.z, this.w); }; + /** * @author David Gossow - dgossow@willowgarage.com */ @@ -920,61 +935,61 @@ ROSLIB.Quaternion = function(x, y, z, w) { * @param z - the z value */ ROSLIB.Vector3 = function(x, y, z) { - var vector3 = this; this.x = x || 0; this.y = y || 0; this.z = z || 0; +}; - /** - * Copy the values from the given vector into this vector. - * - * @param v the vector to copy - * @returns a pointer to this vector - */ - this.copy = function(v) { - vector3.x = v.x; - vector3.y = v.y; - vector3.z = v.z; - return vector3; - }; +/** + * Copy the values from the given vector into this vector. + * + * @param v the vector to copy + * @returns a pointer to this vector + */ +ROSLIB.Vector3.prototype.copy = function(v) { + this.x = v.x; + this.y = v.y; + this.z = v.z; + return this; +}; - /** - * Set the values of this vector to the sum of vectors a and b. - * - * @param a the first vector to add with - * @param b the second vector to add with - * @returns a pointer to this vector - */ - this.add = function(a, b) { - vector3.x = a.x + b.x; - vector3.y = a.y + b.y; - vector3.z = a.z + b.z; - return vector3; - }; +/** + * Set the values of this vector to the sum of vectors a and b. + * + * @param a the first vector to add with + * @param b the second vector to add with + * @returns a pointer to this vector + */ +ROSLIB.Vector3.prototype.add = function(a, b) { + this.x = a.x + b.x; + this.y = a.y + b.y; + this.z = a.z + b.z; + return this; +}; - /** - * Set the values of this vector to the difference of vectors a and b. - * - * @param a the first vector to add with - * @param b the second vector to add with - * @returns a pointer to this vector - */ - this.sub = function(a, b) { - vector3.x = a.x - b.x; - vector3.y = a.y - b.y; - vector3.z = a.z - b.z; - return vector3; - }; +/** + * Set the values of this vector to the difference of vectors a and b. + * + * @param a the first vector to add with + * @param b the second vector to add with + * @returns a pointer to this vector + */ +ROSLIB.Vector3.prototype.sub = function(a, b) { + this.x = a.x - b.x; + this.y = a.y - b.y; + this.z = a.z - b.z; + return this; +}; - /** - * Clone a copy of this vector. - * - * @returns the cloned vector - */ - this.clone = function() { - return new ROSLIB.Vector3(vector3.x, vector3.y, vector3.z); - }; +/** + * Clone a copy of this vector. + * + * @returns the cloned vector + */ +ROSLIB.Vector3.prototype.clone = function() { + return new ROSLIB.Vector3(this.x, this.y, this.z); }; + /** * @author David Gossow - dgossow@willowgarage.com */ @@ -992,7 +1007,6 @@ ROSLIB.Vector3 = function(x, y, z) { * * goalUpdateDelay - the goal update delay for the TF republisher */ ROSLIB.TFClient = function(options) { - var tfClient = this; var options = options || {}; this.ros = options.ros; this.fixedFrame = options.fixedFrame || '/base_link'; @@ -1005,112 +1019,116 @@ ROSLIB.TFClient = function(options) { this.frameInfos = {}; this.goalUpdateRequested = false; - // create an ActionClient + // Create an ActionClient this.actionClient = new ROSLIB.ActionClient({ ros : this.ros, serverName : '/tf2_web_republisher', actionName : 'tf2_web_republisher/TFSubscriptionAction' }); +}; - /** - * Process the incoming TF message and send them out using the callback functions. - * - * @param tf - the TF message from the server - */ - this.processFeedback = function(tf) { - tf.transforms.forEach(function(transform) { - var frameID = transform.child_frame_id; - var info = tfClient.frameInfos[frameID]; - if (info != undefined) { - info.transform = new ROSLIB.Transform(transform.transform.translation, - transform.transform.rotation); - info.cbs.forEach(function(cb) { - cb(info.transform); - }); - } - }); - }; - - /** - * Create and send a new goal to the tf2_web_republisher based on the current list of TFs. - */ - this.updateGoal = function() { - // Anytime the list of frames changes, we will need to send a new goal. - if (tfClient.currentGoal) { - tfClient.currentGoal.cancel(); +/** + * Process the incoming TF message and send them out using the callback + * functions. + * + * @param tf - the TF message from the server + */ +ROSLIB.TFClient.prototype.processFeedback = function(tf) { + var that = this; + tf.transforms.forEach(function(transform) { + var frameID = transform.child_frame_id; + var info = that.frameInfos[frameID]; + if (info != undefined) { + info.transform = new ROSLIB.Transform(transform.transform.translation, + transform.transform.rotation); + info.cbs.forEach(function(cb) { + cb(info.transform); + }); } + }); +}; - var goalMessage = { - source_frames : [], - target_frame : tfClient.fixedFrame, - angular_thres : tfClient.angularThres, - trans_thres : tfClient.transThres, - rate : tfClient.rate - }; - - for (frame in tfClient.frameInfos) { - goalMessage.source_frames.push(frame); - } +/** + * Create and send a new goal to the tf2_web_republisher based on the current + * list of TFs. + */ +ROSLIB.TFClient.prototype.updateGoal = function() { + // Anytime the list of frames changes, we will need to send a new goal. + if (this.currentGoal) { + this.currentGoal.cancel(); + } - tfClient.currentGoal = new ROSLIB.Goal({ - actionClient : tfClient.actionClient, - goalMessage : goalMessage - }); - tfClient.currentGoal.on('feedback', tfClient.processFeedback.bind(tfClient)); - tfClient.currentGoal.send(); - tfClient.goalUpdateRequested = false; + var goalMessage = { + source_frames : [], + target_frame : this.fixedFrame, + angular_thres : this.angularThres, + trans_thres : this.transThres, + rate : this.rate }; - /** - * Subscribe to the given TF frame. - * - * @param frameID - the TF frame to subscribe to - * @param callback - function with params: - * * transform - the transform data - */ - this.subscribe = function(frameID, callback) { - // make sure the frame id is relative - if (frameID[0] === '/') { - frameID = frameID.substring(1); + for (frame in this.frameInfos) { + goalMessage.source_frames.push(frame); + } + + this.currentGoal = new ROSLIB.Goal({ + actionClient : this.actionClient, + goalMessage : goalMessage + }); + this.currentGoal.on('feedback', this.processFeedback.bind(this)); + this.currentGoal.send(); + this.goalUpdateRequested = false; +}; + +/** + * Subscribe to the given TF frame. + * + * @param frameID - the TF frame to subscribe to + * @param callback - function with params: + * * transform - the transform data + */ +ROSLIB.TFClient.prototype.subscribe = function(frameID, callback) { + // make sure the frame id is relative + if (frameID[0] === '/') { + frameID = frameID.substring(1); + } + // if there is no callback registered for the given frame, create emtpy callback list + if (this.frameInfos[frameID] == undefined) { + this.frameInfos[frameID] = { + cbs : [] + }; + if (!this.goalUpdateRequested) { + setTimeout(this.updateGoal.bind(tfClient), this.goalUpdateDelay); + this.goalUpdateRequested = true; } - // if there is no callback registered for the given frame, create emtpy callback list - if (tfClient.frameInfos[frameID] == undefined) { - tfClient.frameInfos[frameID] = { - cbs : [] - }; - if (!tfClient.goalUpdateRequested) { - setTimeout(tfClient.updateGoal.bind(tfClient), tfClient.goalUpdateDelay); - tfClient.goalUpdateRequested = true; - } - } else { - // if we already have a transform, call back immediately - if (tfClient.frameInfos[frameID].transform != undefined) { - callback(tfClient.frameInfos[frameID].transform); - } + } else { + // if we already have a transform, call back immediately + if (this.frameInfos[frameID].transform != undefined) { + callback(this.frameInfos[frameID].transform); } - tfClient.frameInfos[frameID].cbs.push(callback); - }; + } + this.frameInfos[frameID].cbs.push(callback); +}; - /** - * Unsubscribe from the given TF frame. - * - * @param frameID - the TF frame to unsubscribe from - * @param callback - the callback function to remove - */ - this.unsubscribe = function(frameID, callback) { - var info = tfClient.frameInfos[frameID]; - if (info != undefined) { - var cbIndex = info.cbs.indexOf(callback); - if (cbIndex >= 0) { - info.cbs.splice(cbIndex, 1); - if (info.cbs.length == 0) { - delete tfClient.frameInfos[frameID]; - } - tfClient.needUpdate = true; +/** + * Unsubscribe from the given TF frame. + * + * @param frameID - the TF frame to unsubscribe from + * @param callback - the callback function to remove + */ +ROSLIB.TFClient.prototype.unsubscribe = function(frameID, callback) { + var info = this.frameInfos[frameID]; + if (info != undefined) { + var cbIndex = info.cbs.indexOf(callback); + if (cbIndex >= 0) { + info.cbs.splice(cbIndex, 1); + if (info.cbs.length == 0) { + delete this.frameInfos[frameID]; } + this.needUpdate = true; } - }; + } }; + /** * @author David Gossow - dgossow@willowgarage.com */ @@ -1123,8 +1141,6 @@ ROSLIB.TFClient = function(options) { * @param rotation - the ROSLIB.Quaternion describing the rotation */ ROSLIB.Transform = function(translation, rotation) { - var transform = this; - // copy the values into this object if they exist this.translation = new ROSLIB.Vector3(); this.rotation = new ROSLIB.Quaternion(); if (translation !== undefined) { @@ -1133,43 +1149,44 @@ ROSLIB.Transform = function(translation, rotation) { if (rotation !== undefined) { this.rotation.copy(rotation); } +}; - /** - * Apply a transform against the given ROSLIB.Pose. - * - * @param pose the pose to transform with - * @returns a pointer to the pose - */ - this.apply = function(pose) { - transform.rotation.multiplyVec3(pose.position); - pose.position.add(pose.position, transform.translation); - pose.orientation.multiply(transform.rotation, pose.orientation); - return pose; - }; +/** + * Apply a transform against the given ROSLIB.Pose. + * + * @param pose the pose to transform with + * @returns a pointer to the pose + */ +ROSLIB.Transform.prototype.apply = function(pose) { + this.rotation.multiplyVec3(pose.position); + pose.position.add(pose.position, this.translation); + pose.orientation.multiply(this.rotation, pose.orientation); + return pose; +}; - /** - * Apply an inverse transform against the given ROSLIB.Pose. - * - * @param pose the pose to transform with - * @returns a pointer to the pose - */ - this.applyInverse = function(pose) { - var rotInv = transform.rotation.clone().inverse(); - rotInv.multiplyVec3(pose.position); - pose.position.sub(pose.position, transform.translation); - pose.orientation.multiply(rotInv, pose.orientation); - return pose; - }; +/** + * Apply an inverse transform against the given ROSLIB.Pose. + * + * @param pose the pose to transform with + * @returns a pointer to the pose + */ +ROSLIB.Transform.prototype.applyInverse = function(pose) { + var rotInv = this.rotation.clone().inverse(); + rotInv.multiplyVec3(pose.position); + pose.position.sub(pose.position, this.translation); + pose.orientation.multiply(rotInv, pose.orientation); + return pose; +}; - /** - * Copy the values from the given transform into this transform. - * - * @param transform the transform to copy - * @returns a pointer to this transform - */ - this.copy = function(transform) { - transform.translation.copy(transform.translation); - transform.rotation.copy(transform.rotation); - return transform; - }; +/** + * Copy the values from the given transform into this transform. + * + * @param transform the transform to copy + * @returns a pointer to this transform + */ +ROSLIB.Transform.prototype.copy = function(transform) { + transform.translation.copy(transform.translation); + transform.rotation.copy(transform.rotation); + return transform; }; + diff --git a/build/roslib.min.js b/build/roslib.min.js index 4fe68f964..cc6b6ace8 100644 --- a/build/roslib.min.js +++ b/build/roslib.min.js @@ -1 +1 @@ -var ROSLIB=ROSLIB||{REVISION:"1"};ROSLIB.ActionClient=function(b){var d=this;b=b||{};this.ros=b.ros;this.serverName=b.serverName;this.actionName=b.actionName;this.timeout=b.timeout;this.goals={};var a=false;var c=new ROSLIB.Topic({ros:this.ros,name:this.serverName+"/feedback",messageType:this.actionName+"Feedback"});var e=new ROSLIB.Topic({ros:this.ros,name:this.serverName+"/status",messageType:"actionlib_msgs/GoalStatusArray"});var f=new ROSLIB.Topic({ros:this.ros,name:this.serverName+"/result",messageType:this.actionName+"Result"});this.goalTopic=new ROSLIB.Topic({ros:this.ros,name:this.serverName+"/goal",messageType:this.actionName+"Goal"});this.cancelTopic=new ROSLIB.Topic({ros:this.ros,name:this.serverName+"/cancel",messageType:"actionlib_msgs/GoalID"});this.cancel=function(){var g=new ROSLIB.Message({});this.cancelTopic.publish(g)};this.goalTopic.advertise();this.cancelTopic.advertise();e.subscribe(function(g){a=true;g.status_list.forEach(function(h){var i=d.goals[h.goal_id.id];if(i){i.emit("status",h)}})});c.subscribe(function(h){var g=d.goals[h.status.goal_id.id];if(g){g.emit("status",h.status);g.emit("feedback",h.feedback)}});f.subscribe(function(h){var g=d.goals[h.status.goal_id.id];if(g){g.emit("status",h.status);g.emit("result",h.result)}});if(this.timeout){setTimeout(function(){if(!a){d.emit("timeout")}},this.timeout)}};ROSLIB.ActionClient.prototype.__proto__=EventEmitter2.prototype;ROSLIB.Goal=function(c){var b=this;this.actionClient=c.actionClient;this.goalMessage=c.goalMessage;this.isFinished=false;var a=new Date();this.send=function(d){b.actionClient.goalTopic.publish(b.goalMessage);if(d){setTimeout(function(){if(!b.isFinished){b.emit("timeout")}},d)}};this.cancel=function(){var d=new ROSLIB.Message({id:b.goalID});b.actionClient.cancelTopic.publish(d)};this.goalID="goal_"+Math.random()+"_"+a.getTime();this.goalMessage=new ROSLIB.Message({goal_id:{stamp:{secs:0,nsecs:0},id:this.goalID},goal:this.goalMessage});this.on("status",function(d){this.status=d});this.on("result",function(d){this.isFinished=true;this.result=d});this.on("feedback",function(d){this.feedback=d});this.actionClient.goals[this.goalID]=this};ROSLIB.Goal.prototype.__proto__=EventEmitter2.prototype;ROSLIB.Message=function(a){var b=this;if(a){Object.keys(a).forEach(function(c){b[c]=a[c]})}};ROSLIB.Param=function(a){var b=this;a=a||{};this.ros=a.ros;this.name=a.name;this.get=function(e){var d=new ROSLIB.Service({ros:b.ros,name:"/rosapi/get_param",serviceType:"rosapi/GetParam"});var c=new ROSLIB.ServiceRequest({name:b.name,value:JSON.stringify("")});d.callService(c,function(f){var g=JSON.parse(f.value);e(g)})};this.set=function(e){var d=new ROSLIB.Service({ros:b.ros,name:"/rosapi/set_param",serviceType:"rosapi/SetParam"});var c=new ROSLIB.ServiceRequest({name:b.name,value:JSON.stringify(e)});d.callService(c,function(){})}};ROSLIB.Ros=function(d){var c=this;this.socket=null;function b(h){c.emit("connection",h)}function a(h){c.emit("close",h)}function e(h){c.emit("error",h)}function g(h,j){var i=new Image();i.onload=function(){var k=document.createElement("canvas");var m=k.getContext("2d");k.width=i.width;k.height=i.height;m.drawImage(i,0,0);var p=m.getImageData(0,0,i.width,i.height).data;var n="";for(var l=0;l