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=0){e.cbs.splice(d,1);if(e.cbs.length==0){delete a.frameInfos[c]}a.needUpdate=true}}}};ROSLIB.Transform=function(c,b){var a=this;this.translation=new ROSLIB.Vector3();this.rotation=new ROSLIB.Quaternion();if(c!==undefined){this.translation.copy(c)}if(b!==undefined){this.rotation.copy(b)}this.apply=function(d){a.rotation.multiplyVec3(d.position);d.position.add(d.position,a.translation);d.orientation.multiply(a.rotation,d.orientation);return d};this.applyInverse=function(e){var d=a.rotation.clone().inverse();d.multiplyVec3(e.position);e.position.sub(e.position,a.translation);e.orientation.multiply(d,e.orientation);return e};this.copy=function(d){d.translation.copy(d.translation);d.rotation.copy(d.rotation);return d}}; \ No newline at end of file +var ROSLIB=ROSLIB||{REVISION:"1"};ROSLIB.ActionClient=function(b){var c=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 d=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.goalTopic.advertise();this.cancelTopic.advertise();e.subscribe(function(h){var g=this;a=true;h.status_list.forEach(function(i){var j=g.goals[i.goal_id.id];if(j){j.emit("status",i)}})});d.subscribe(function(h){var g=c.goals[h.status.goal_id.id];if(g){g.emit("status",h.status);g.emit("feedback",h.feedback)}});f.subscribe(function(h){var g=c.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){c.emit("timeout")}},this.timeout)}};ROSLIB.ActionClient.prototype.__proto__=EventEmitter2.prototype;ROSLIB.ActionClient.prototype.cancel=function(){var a=new ROSLIB.Message({});this.cancelTopic.publish(a)};ROSLIB.Goal=function(b){var c=this;this.actionClient=b.actionClient;this.goalMessage=b.goalMessage;this.isFinished=false;var a=new Date();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){c.status=d});this.on("result",function(d){c.isFinished=true;c.result=d});this.on("feedback",function(d){c.feedback=d});this.actionClient.goals[this.goalID]=this};ROSLIB.Goal.prototype.__proto__=EventEmitter2.prototype;ROSLIB.Goal.prototype.send=function(b){var a=this;a.actionClient.goalTopic.publish(a.goalMessage);if(b){setTimeout(function(){if(!a.isFinished){a.emit("timeout")}},b)}};ROSLIB.Goal.prototype.cancel=function(){var a=new ROSLIB.Message({id:this.goalID});this.actionClient.cancelTopic.publish(a)};ROSLIB.Message=function(a){var b=this;if(a){Object.keys(a).forEach(function(c){b[c]=a[c]})}};ROSLIB.Param=function(a){a=a||{};this.ros=a.ros;this.name=a.name};ROSLIB.Param.prototype.get=function(c){var b=new ROSLIB.Service({ros:this.ros,name:"/rosapi/get_param",serviceType:"rosapi/GetParam"});var a=new ROSLIB.ServiceRequest({name:this.name,value:JSON.stringify("")});b.callService(a,function(d){var e=JSON.parse(d.value);c(e)})};ROSLIB.Param.prototype.set=function(c){var b=new ROSLIB.Service({ros:this.ros,name:"/rosapi/set_param",serviceType:"rosapi/SetParam"});var a=new ROSLIB.ServiceRequest({name:this.name,value:JSON.stringify(c)});b.callService(a,function(){})};ROSLIB.Ros=function(a){this.socket=null;if(a){this.connect(a)}};ROSLIB.Ros.prototype.__proto__=EventEmitter2.prototype;ROSLIB.Ros.prototype.connect=function(c){var e=this;function b(h){e.emit("connection",h)}function a(h){e.emit("close",h)}function d(h){e.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=0){c.cbs.splice(b,1);if(c.cbs.length==0){delete this.frameInfos[a]}this.needUpdate=true}}};ROSLIB.Transform=function(b,a){this.translation=new ROSLIB.Vector3();this.rotation=new ROSLIB.Quaternion();if(b!==undefined){this.translation.copy(b)}if(a!==undefined){this.rotation.copy(a)}};ROSLIB.Transform.prototype.apply=function(a){this.rotation.multiplyVec3(a.position);a.position.add(a.position,this.translation);a.orientation.multiply(this.rotation,a.orientation);return a};ROSLIB.Transform.prototype.applyInverse=function(b){var a=this.rotation.clone().inverse();a.multiplyVec3(b.position);b.position.sub(b.position,this.translation);b.orientation.multiply(a,b.orientation);return b};ROSLIB.Transform.prototype.copy=function(a){a.translation.copy(a.translation);a.rotation.copy(a.rotation);return a}; \ No newline at end of file diff --git a/doc/files.html b/doc/files.html index 6d1ae6119..eb6c74d29 100644 --- a/doc/files.html +++ b/doc/files.html @@ -223,7 +223,7 @@

File Index

-

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/actionlib/ActionClient.js

+

/home/vagrant/ws/roslibjs/src/actionlib/ActionClient.js

@@ -235,7 +235,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/actionlib/Goal.js

+

/home/vagrant/ws/roslibjs/src/actionlib/Goal.js

@@ -247,7 +247,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/core/Message.js

+

/home/vagrant/ws/roslibjs/src/core/Message.js

@@ -259,7 +259,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/core/Param.js

+

/home/vagrant/ws/roslibjs/src/core/Param.js

@@ -271,7 +271,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/core/Ros.js

+

/home/vagrant/ws/roslibjs/src/core/Ros.js

@@ -283,7 +283,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/core/Service.js

+

/home/vagrant/ws/roslibjs/src/core/Service.js

@@ -295,7 +295,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/core/ServiceRequest.js

+

/home/vagrant/ws/roslibjs/src/core/ServiceRequest.js

@@ -307,7 +307,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/core/ServiceResponse.js

+

/home/vagrant/ws/roslibjs/src/core/ServiceResponse.js

@@ -319,7 +319,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/core/Topic.js

+

/home/vagrant/ws/roslibjs/src/core/Topic.js

@@ -331,7 +331,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/math/Pose.js

+

/home/vagrant/ws/roslibjs/src/math/Pose.js

@@ -343,7 +343,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/math/Quaternion.js

+

/home/vagrant/ws/roslibjs/src/math/Quaternion.js

@@ -355,7 +355,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/math/Vector3.js

+

/home/vagrant/ws/roslibjs/src/math/Vector3.js

@@ -367,7 +367,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/RosLib.js

+

/home/vagrant/ws/roslibjs/src/RosLib.js

@@ -379,7 +379,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/tf/TFClient.js

+

/home/vagrant/ws/roslibjs/src/tf/TFClient.js

@@ -391,7 +391,7 @@

/wg/stor5/rtoris/Documents/WPI/RAIL/ROS/groovy/src/roslibjs/src/tf/Transform.js

+

/home/vagrant/ws/roslibjs/src/tf/Transform.js

@@ -406,7 +406,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)

\ No newline at end of file diff --git a/doc/index.html b/doc/index.html index 928c8f653..39fe481e4 100644 --- a/doc/index.html +++ b/doc/index.html @@ -316,7 +316,7 @@

ROSLIB.Vector3

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
\ No newline at end of file diff --git a/doc/symbols/ROSLIB.ActionClient.html b/doc/symbols/ROSLIB.ActionClient.html index 3468b3d12..6d8a2662e 100644 --- a/doc/symbols/ROSLIB.ActionClient.html +++ b/doc/symbols/ROSLIB.ActionClient.html @@ -239,7 +239,7 @@

-
Defined in: ActionClient.js. +
Defined in: ActionClient.js.

@@ -399,7 +399,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.Goal.html b/doc/symbols/ROSLIB.Goal.html index ac1b95ff6..2f13731a0 100644 --- a/doc/symbols/ROSLIB.Goal.html +++ b/doc/symbols/ROSLIB.Goal.html @@ -239,7 +239,7 @@

-
Defined in: Goal.js. +
Defined in: Goal.js.

@@ -439,7 +439,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.Message.html b/doc/symbols/ROSLIB.Message.html index 88078ae90..5a3f4c9f3 100644 --- a/doc/symbols/ROSLIB.Message.html +++ b/doc/symbols/ROSLIB.Message.html @@ -239,7 +239,7 @@

-
Defined in: Message.js. +
Defined in: Message.js.

@@ -332,7 +332,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.Param.html b/doc/symbols/ROSLIB.Param.html index e124e8c18..8c2bf3424 100644 --- a/doc/symbols/ROSLIB.Param.html +++ b/doc/symbols/ROSLIB.Param.html @@ -239,7 +239,7 @@

-
Defined in: Param.js. +
Defined in: Param.js.

@@ -448,7 +448,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.Pose.html b/doc/symbols/ROSLIB.Pose.html index 6df8f1548..9649a3e01 100644 --- a/doc/symbols/ROSLIB.Pose.html +++ b/doc/symbols/ROSLIB.Pose.html @@ -239,7 +239,7 @@

-
Defined in: Pose.js. +
Defined in: Pose.js.

@@ -413,7 +413,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.Quaternion.html b/doc/symbols/ROSLIB.Quaternion.html index 8657e5913..a8486c7eb 100644 --- a/doc/symbols/ROSLIB.Quaternion.html +++ b/doc/symbols/ROSLIB.Quaternion.html @@ -239,7 +239,7 @@

-
Defined in: Quaternion.js. +
Defined in: Quaternion.js.

@@ -705,7 +705,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.Ros.html b/doc/symbols/ROSLIB.Ros.html index 0b6fd19a1..eda360fe6 100644 --- a/doc/symbols/ROSLIB.Ros.html +++ b/doc/symbols/ROSLIB.Ros.html @@ -239,7 +239,7 @@

-
Defined in: Ros.js. +
Defined in: Ros.js.

@@ -298,7 +298,8 @@

-
Sends the message over the WebSocket, but queues the message up if not yet connected.
+
Sends the message over the WebSocket, but queues the message up if not yet +connected.
@@ -320,17 +321,6 @@

- - <inner>   - -
decompressPng(data, callback) -
-
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.
- - -   @@ -358,42 +348,6 @@

- - <inner>   - -
onClose(event) -
-
Emits a 'close' event on WebSocket disconnection.
- - - - - <inner>   - -
onError(event) -
-
Emits an 'error' event whenever there was an error.
- - - - - <inner>   - -
onMessage(message) -
-
Parses message responses from rosbridge and sends to the appropriate topic, service, or param.
- - - - - <inner>   - -
onOpen(event) -
-
Emits a 'connection' event on WebSocket connection.
- - - @@ -540,7 +494,8 @@

- Sends the message over the WebSocket, but queues the message up if not yet connected. + Sends the message over the WebSocket, but queues the message up if not yet +connected.
@@ -627,51 +582,6 @@

-
- - -
<inner> - - - decompressPng(data, callback) - -
-
- 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. - - -
- - - - -
-
Parameters:
- -
- data - -
-
- object containing the PNG data.
- -
- callback - -
-
- function with params: - * data - the uncompressed data
- -
- - - - - - - -
@@ -783,150 +693,6 @@

-
- - -
<inner> - - - onClose(event) - -
-
- Emits a 'close' event on WebSocket disconnection. - - -
- - - - -
-
Parameters:
- -
- event - -
-
- the argument to emit with the event.
- -
- - - - - - - - -
- - -
<inner> - - - onError(event) - -
-
- Emits an 'error' event whenever there was an error. - - -
- - - - -
-
Parameters:
- -
- event - -
-
- the argument to emit with the event.
- -
- - - - - - - - -
- - -
<inner> - - - onMessage(message) - -
-
- Parses message responses from rosbridge and sends to the appropriate topic, service, or param. - - -
- - - - -
-
Parameters:
- -
- message - -
-
- the raw JSON message from rosbridge.
- -
- - - - - - - - -
- - -
<inner> - - - onOpen(event) - -
-
- Emits a 'connection' event on WebSocket connection. - - -
- - - - -
-
Parameters:
- -
- event - -
-
- the argument to emit with the event.
- -
- - - - - - - - @@ -941,7 +707,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.Service.html b/doc/symbols/ROSLIB.Service.html index db2e4946b..f1896c3b1 100644 --- a/doc/symbols/ROSLIB.Service.html +++ b/doc/symbols/ROSLIB.Service.html @@ -239,7 +239,7 @@

-
Defined in: Service.js. +
Defined in: Service.js.

@@ -407,7 +407,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.ServiceRequest.html b/doc/symbols/ROSLIB.ServiceRequest.html index 81e77bb52..c4775dccd 100644 --- a/doc/symbols/ROSLIB.ServiceRequest.html +++ b/doc/symbols/ROSLIB.ServiceRequest.html @@ -239,7 +239,7 @@

-
Defined in: ServiceRequest.js. +
Defined in: ServiceRequest.js.

@@ -332,7 +332,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.ServiceResponse.html b/doc/symbols/ROSLIB.ServiceResponse.html index 4a0783ce5..dc7118cda 100644 --- a/doc/symbols/ROSLIB.ServiceResponse.html +++ b/doc/symbols/ROSLIB.ServiceResponse.html @@ -239,7 +239,7 @@

-
Defined in: ServiceResponse.js. +
Defined in: ServiceResponse.js.

@@ -332,7 +332,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.TFClient.html b/doc/symbols/ROSLIB.TFClient.html index 81716c940..b1c44396f 100644 --- a/doc/symbols/ROSLIB.TFClient.html +++ b/doc/symbols/ROSLIB.TFClient.html @@ -239,7 +239,7 @@

-
Defined in: TFClient.js. +
Defined in: TFClient.js.

@@ -289,7 +289,8 @@

-
Process the incoming TF message and send them out using the callback functions.
+
Process the incoming TF message and send them out using the callback +functions.
@@ -316,7 +317,8 @@

-
Create and send a new goal to the tf2_web_republisher based on the current list of TFs.
+
Create and send a new goal to the tf2_web_republisher based on the current +list of TFs.
@@ -393,7 +395,8 @@

- Process the incoming TF message and send them out using the callback functions. + Process the incoming TF message and send them out using the callback +functions.
@@ -514,7 +517,8 @@

- Create and send a new goal to the tf2_web_republisher based on the current list of TFs. + Create and send a new goal to the tf2_web_republisher based on the current +list of TFs.
@@ -543,7 +547,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.Topic.html b/doc/symbols/ROSLIB.Topic.html index dd199faf1..2698d7212 100644 --- a/doc/symbols/ROSLIB.Topic.html +++ b/doc/symbols/ROSLIB.Topic.html @@ -239,7 +239,7 @@

-
Defined in: Topic.js. +
Defined in: Topic.js.

@@ -560,7 +560,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.Transform.html b/doc/symbols/ROSLIB.Transform.html index 589949a7e..4735cfa4c 100644 --- a/doc/symbols/ROSLIB.Transform.html +++ b/doc/symbols/ROSLIB.Transform.html @@ -239,7 +239,7 @@

-
Defined in: Transform.js. +
Defined in: Transform.js.

@@ -517,7 +517,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/ROSLIB.Vector3.html b/doc/symbols/ROSLIB.Vector3.html index 91ef1a28a..6c3d2c4a6 100644 --- a/doc/symbols/ROSLIB.Vector3.html +++ b/doc/symbols/ROSLIB.Vector3.html @@ -239,7 +239,7 @@

-
Defined in: Vector3.js. +
Defined in: Vector3.js.

@@ -576,7 +576,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/_global_.html b/doc/symbols/_global_.html index d8f9a26cb..9798bf64f 100644 --- a/doc/symbols/_global_.html +++ b/doc/symbols/_global_.html @@ -300,7 +300,7 @@


- Defined in: RosLib.js. + Defined in: RosLib.js.
Author: Russell Toris - rctoris@wpi.edu. @@ -329,7 +329,7 @@

- Documentation generated by JsDoc Toolkit 2.4.0 on Sun Mar 17 2013 15:42:05 GMT-0700 (PDT) + Documentation generated by JsDoc Toolkit 2.4.0 on Tue Mar 19 2013 22:13:00 GMT-0000 (UTC)
diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_RosLib.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_RosLib.js.html similarity index 100% rename from doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_RosLib.js.html rename to doc/symbols/src/_home_vagrant_ws_roslibjs_src_RosLib.js.html diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_actionlib_ActionClient.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_actionlib_ActionClient.js.html similarity index 75% rename from doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_actionlib_ActionClient.js.html rename to doc/symbols/src/_home_vagrant_ws_roslibjs_src_actionlib_ActionClient.js.html index b187db792..b1ca92764 100644 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_actionlib_ActionClient.js.html +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_actionlib_ActionClient.js.html @@ -26,7 +26,7 @@ 19 * * timeout - the timeout length when connecting to the action server 20 */ 21 ROSLIB.ActionClient = function(options) { - 22 var actionClient = this; + 22 var that = this; 23 options = options || {}; 24 this.ros = options.ros; 25 this.serverName = options.serverName; @@ -43,77 +43,83 @@ 36 name : this.serverName + '/feedback', 37 messageType : this.actionName + 'Feedback' 38 }); - 39 var statusListener = new ROSLIB.Topic({ - 40 ros : this.ros, - 41 name : this.serverName + '/status', - 42 messageType : 'actionlib_msgs/GoalStatusArray' - 43 }); - 44 var resultListener = new ROSLIB.Topic({ - 45 ros : this.ros, - 46 name : this.serverName + '/result', - 47 messageType : this.actionName + 'Result' - 48 }); - 49 this.goalTopic = new ROSLIB.Topic({ - 50 ros : this.ros, - 51 name : this.serverName + '/goal', - 52 messageType : this.actionName + 'Goal' - 53 }); - 54 this.cancelTopic = new ROSLIB.Topic({ - 55 ros : this.ros, - 56 name : this.serverName + '/cancel', - 57 messageType : 'actionlib_msgs/GoalID' - 58 }); - 59 - 60 /** - 61 * Cancel all goals associated with this ActionClient. - 62 */ - 63 this.cancel = function() { - 64 var cancelMessage = new ROSLIB.Message({}); - 65 this.cancelTopic.publish(cancelMessage); - 66 }; + 39 + 40 var statusListener = new ROSLIB.Topic({ + 41 ros : this.ros, + 42 name : this.serverName + '/status', + 43 messageType : 'actionlib_msgs/GoalStatusArray' + 44 }); + 45 + 46 var resultListener = new ROSLIB.Topic({ + 47 ros : this.ros, + 48 name : this.serverName + '/result', + 49 messageType : this.actionName + 'Result' + 50 }); + 51 + 52 this.goalTopic = new ROSLIB.Topic({ + 53 ros : this.ros, + 54 name : this.serverName + '/goal', + 55 messageType : this.actionName + 'Goal' + 56 }); + 57 + 58 this.cancelTopic = new ROSLIB.Topic({ + 59 ros : this.ros, + 60 name : this.serverName + '/cancel', + 61 messageType : 'actionlib_msgs/GoalID' + 62 }); + 63 + 64 // advertise the goal and cancel topics + 65 this.goalTopic.advertise(); + 66 this.cancelTopic.advertise(); 67 - 68 // advertise the goal and cancel topics - 69 this.goalTopic.advertise(); - 70 this.cancelTopic.advertise(); - 71 - 72 // subscribe to the status topic - 73 statusListener.subscribe(function(statusMessage) { - 74 receivedStatus = true; - 75 statusMessage.status_list.forEach(function(status) { - 76 var goal = actionClient.goals[status.goal_id.id]; - 77 if (goal) { - 78 goal.emit('status', status); - 79 } - 80 }); - 81 }); - 82 - 83 // subscribe the the feedback topic - 84 feedbackListener.subscribe(function(feedbackMessage) { - 85 var goal = actionClient.goals[feedbackMessage.status.goal_id.id]; - 86 if (goal) { - 87 goal.emit('status', feedbackMessage.status); - 88 goal.emit('feedback', feedbackMessage.feedback); - 89 } - 90 }); - 91 - 92 // subscribe to the result topic - 93 resultListener.subscribe(function(resultMessage) { - 94 var goal = actionClient.goals[resultMessage.status.goal_id.id]; - 95 - 96 if (goal) { - 97 goal.emit('status', resultMessage.status); - 98 goal.emit('result', resultMessage.result); - 99 } -100 }); -101 -102 // If timeout specified, emit a 'timeout' event if the action server does not respond -103 if (this.timeout) { -104 setTimeout(function() { -105 if (!receivedStatus) { -106 actionClient.emit('timeout'); -107 } -108 }, this.timeout); -109 } -110 }; -111 ROSLIB.ActionClient.prototype.__proto__ = EventEmitter2.prototype; -112 \ No newline at end of file + 68 // subscribe to the status topic + 69 statusListener.subscribe(function(statusMessage) { + 70 var that = this; + 71 receivedStatus = true; + 72 statusMessage.status_list.forEach(function(status) { + 73 var goal = that.goals[status.goal_id.id]; + 74 if (goal) { + 75 goal.emit('status', status); + 76 } + 77 }); + 78 }); + 79 + 80 // subscribe the the feedback topic + 81 feedbackListener.subscribe(function(feedbackMessage) { + 82 var goal = that.goals[feedbackMessage.status.goal_id.id]; + 83 if (goal) { + 84 goal.emit('status', feedbackMessage.status); + 85 goal.emit('feedback', feedbackMessage.feedback); + 86 } + 87 }); + 88 + 89 // subscribe to the result topic + 90 resultListener.subscribe(function(resultMessage) { + 91 var goal = that.goals[resultMessage.status.goal_id.id]; + 92 + 93 if (goal) { + 94 goal.emit('status', resultMessage.status); + 95 goal.emit('result', resultMessage.result); + 96 } + 97 }); + 98 + 99 // If timeout specified, emit a 'timeout' event if the action server does not respond +100 if (this.timeout) { +101 setTimeout(function() { +102 if (!receivedStatus) { +103 that.emit('timeout'); +104 } +105 }, this.timeout); +106 } +107 }; +108 ROSLIB.ActionClient.prototype.__proto__ = EventEmitter2.prototype; +109 +110 /** +111 * Cancel all goals associated with this ActionClient. +112 */ +113 ROSLIB.ActionClient.prototype.cancel = function() { +114 var cancelMessage = new ROSLIB.Message({}); +115 this.cancelTopic.publish(cancelMessage); +116 }; +117 +118 \ No newline at end of file diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_actionlib_Goal.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_actionlib_Goal.js.html similarity index 55% rename from doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_actionlib_Goal.js.html rename to doc/symbols/src/_home_vagrant_ws_roslibjs_src_actionlib_Goal.js.html index 9e27a3c5f..743e8aa12 100644 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_actionlib_Goal.js.html +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_actionlib_Goal.js.html @@ -20,72 +20,72 @@ 13 * * actionClient - the ROSLIB.ActionClient to use with this goal 14 * * goalMessage - The JSON object containing the goal for the action server 15 */ - 16 - 17 ROSLIB.Goal = function(options) { - 18 var goal = this; - 19 this.actionClient = options.actionClient; - 20 this.goalMessage = options.goalMessage; - 21 this.isFinished = false; - 22 - 23 // used to create random IDs - 24 var date = new Date(); - 25 - 26 /** - 27 * Send the goal to the action server. - 28 * - 29 * @param timeout (optional) - a timeout length for the goal's result - 30 */ - 31 this.send = function(timeout) { - 32 goal.actionClient.goalTopic.publish(goal.goalMessage); - 33 if (timeout) { - 34 setTimeout(function() { - 35 if (!goal.isFinished) { - 36 goal.emit('timeout'); - 37 } - 38 }, timeout); - 39 } - 40 }; - 41 - 42 /** - 43 * Cancel the current goal. - 44 */ - 45 this.cancel = function() { - 46 var cancelMessage = new ROSLIB.Message({ - 47 id : goal.goalID - 48 }); - 49 goal.actionClient.cancelTopic.publish(cancelMessage); - 50 }; + 16 ROSLIB.Goal = function(options) { + 17 var that = this; + 18 this.actionClient = options.actionClient; + 19 this.goalMessage = options.goalMessage; + 20 this.isFinished = false; + 21 + 22 // Used to create random IDs + 23 var date = new Date(); + 24 + 25 // Create a random ID + 26 this.goalID = 'goal_' + Math.random() + '_' + date.getTime(); + 27 // Fill in the goal message + 28 this.goalMessage = new ROSLIB.Message({ + 29 goal_id : { + 30 stamp : { + 31 secs : 0, + 32 nsecs : 0 + 33 }, + 34 id : this.goalID + 35 }, + 36 goal : this.goalMessage + 37 }); + 38 + 39 this.on('status', function(status) { + 40 that.status = status; + 41 }); + 42 + 43 this.on('result', function(result) { + 44 that.isFinished = true; + 45 that.result = result; + 46 }); + 47 + 48 this.on('feedback', function(feedback) { + 49 that.feedback = feedback; + 50 }); 51 - 52 // create a random ID - 53 this.goalID = 'goal_' + Math.random() + "_" + date.getTime(); - 54 // fill in the goal message - 55 this.goalMessage = new ROSLIB.Message({ - 56 goal_id : { - 57 stamp : { - 58 secs : 0, - 59 nsecs : 0 - 60 }, - 61 id : this.goalID - 62 }, - 63 goal : this.goalMessage - 64 }); - 65 - 66 this.on('status', function(status) { - 67 this.status = status; - 68 }); - 69 - 70 this.on('result', function(result) { - 71 this.isFinished = true; - 72 this.result = result; - 73 }); - 74 - 75 this.on('feedback', function(feedback) { - 76 this.feedback = feedback; - 77 }); - 78 - 79 // add the goal - 80 this.actionClient.goals[this.goalID] = this; - 81 + 52 // Add the goal + 53 this.actionClient.goals[this.goalID] = this; + 54 }; + 55 ROSLIB.Goal.prototype.__proto__ = EventEmitter2.prototype; + 56 + 57 /** + 58 * Send the goal to the action server. + 59 * + 60 * @param timeout (optional) - a timeout length for the goal's result + 61 */ + 62 ROSLIB.Goal.prototype.send = function(timeout) { + 63 var that = this; + 64 that.actionClient.goalTopic.publish(that.goalMessage); + 65 if (timeout) { + 66 setTimeout(function() { + 67 if (!that.isFinished) { + 68 that.emit('timeout'); + 69 } + 70 }, timeout); + 71 } + 72 }; + 73 + 74 /** + 75 * Cancel the current goal. + 76 */ + 77 ROSLIB.Goal.prototype.cancel = function() { + 78 var cancelMessage = new ROSLIB.Message({ + 79 id : this.goalID + 80 }); + 81 this.actionClient.cancelTopic.publish(cancelMessage); 82 }; - 83 ROSLIB.Goal.prototype.__proto__ = EventEmitter2.prototype; + 83 84 \ No newline at end of file diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Message.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Message.js.html similarity index 96% rename from doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Message.js.html rename to doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Message.js.html index b238319ea..ed39e7542 100644 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Message.js.html +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Message.js.html @@ -6,12 +6,12 @@ .REGX {color: #339;} .line {border-right: 1px dotted #666; color: #666; font-style: normal;}
  1 /**
-  2  * @author Brandon Alexander - balexander@willowgarage.com
+  2  * @author Brandon Alexander - baalexander@gmail.com
   3  */
   4 
   5 /**
   6  * Message objects are used for publishing and subscribing to and from topics.
-  7  * 
+  7  *
   8  * @constructor
   9  * @param values - object matching the fields defined in the .msg definition file.
  10  */
diff --git a/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Param.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Param.js.html
new file mode 100644
index 000000000..fdaa06380
--- /dev/null
+++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Param.js.html
@@ -0,0 +1,71 @@
+ 
  1 /**
+  2  * @author Brandon Alexander - baalexander@gmail.com
+  3  */
+  4 
+  5 /**
+  6  * A ROS parameter.
+  7  *
+  8  * @constructor
+  9  * @param options - possible keys include:
+ 10  *   * ros - the ROSLIB.Ros connection handle
+ 11  *   * name - the param name, like max_vel_x
+ 12  */
+ 13 ROSLIB.Param = function(options) {
+ 14   options = options || {};
+ 15   this.ros = options.ros;
+ 16   this.name = options.name;
+ 17 };
+ 18 
+ 19 /**
+ 20  * Fetches the value of the param.
+ 21  *
+ 22  * @param callback - function with the following params:
+ 23  *  * value - the value of the param from ROS.
+ 24  */
+ 25 ROSLIB.Param.prototype.get = function(callback) {
+ 26   var paramClient = new ROSLIB.Service({
+ 27     ros : this.ros,
+ 28     name : '/rosapi/get_param',
+ 29     serviceType : 'rosapi/GetParam'
+ 30   });
+ 31 
+ 32   var request = new ROSLIB.ServiceRequest({
+ 33     name : this.name,
+ 34     value : JSON.stringify('')
+ 35   });
+ 36 
+ 37   paramClient.callService(request, function(result) {
+ 38     var value = JSON.parse(result.value);
+ 39     callback(value);
+ 40   });
+ 41 };
+ 42 
+ 43 /**
+ 44  * Sets the value of the param in ROS.
+ 45  *
+ 46  * @param value - value to set param to.
+ 47  */
+ 48 ROSLIB.Param.prototype.set = function(value) {
+ 49   var paramClient = new ROSLIB.Service({
+ 50     ros : this.ros,
+ 51     name : '/rosapi/set_param',
+ 52     serviceType : 'rosapi/SetParam'
+ 53   });
+ 54 
+ 55   var request = new ROSLIB.ServiceRequest({
+ 56     name : this.name,
+ 57     value : JSON.stringify(value)
+ 58   });
+ 59 
+ 60   paramClient.callService(request, function() {
+ 61   });
+ 62 };
+ 63 
+ 64 
\ No newline at end of file diff --git a/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Ros.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Ros.js.html new file mode 100644 index 000000000..595bc7dc7 --- /dev/null +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Ros.js.html @@ -0,0 +1,255 @@ +
  1 /**
+  2  * @author Brandon Alexander - baalexander@gmail.com
+  3  */
+  4 
+  5 /**
+  6  * Manages connection to the server and all interactions with ROS.
+  7  *
+  8  * Emits the following events:
+  9  *  * 'error' - there was an error with ROS
+ 10  *  * 'connection' - connected to the WebSocket server
+ 11  *  * 'close' - disconnected to the WebSocket server
+ 12  *  * <topicName> - a message came from rosbridge with the given topic name
+ 13  *  * <serviceID> - a service response came from rosbridge with the given ID
+ 14  *
+ 15  *  @constructor
+ 16  *  @param url (optional) - The WebSocket URL for rosbridge. Can be specified later with `connect`.
+ 17  */
+ 18 ROSLIB.Ros = function(url) {
+ 19   this.socket = null;
+ 20 
+ 21 
+ 22   // begin by checking if a URL was given
+ 23   if (url) {
+ 24     this.connect(url);
+ 25   }
+ 26 };
+ 27 ROSLIB.Ros.prototype.__proto__ = EventEmitter2.prototype;
+ 28 
+ 29 /**
+ 30  * Connect to the specified WebSocket.
+ 31  *
+ 32  * @param url - WebSocket URL for Rosbridge
+ 33  */
+ 34 ROSLIB.Ros.prototype.connect = function(url) {
+ 35   var that = this;
+ 36 
+ 37   /**
+ 38    * Emits a 'connection' event on WebSocket connection.
+ 39    *
+ 40    * @param event - the argument to emit with the event.
+ 41    */
+ 42   function onOpen(event) {
+ 43     that.emit('connection', event);
+ 44   };
+ 45 
+ 46   /**
+ 47    * Emits a 'close' event on WebSocket disconnection.
+ 48    *
+ 49    * @param event - the argument to emit with the event.
+ 50    */
+ 51   function onClose(event) {
+ 52     that.emit('close', event);
+ 53   };
+ 54 
+ 55   /**
+ 56    * Emits an 'error' event whenever there was an error.
+ 57    *
+ 58    * @param event - the argument to emit with the event.
+ 59    */
+ 60   function onError(event) {
+ 61     that.emit('error', event);
+ 62   };
+ 63 
+ 64   /**
+ 65    * If a message was compressed as a PNG image (a compression hack since
+ 66    * gzipping over WebSockets * is not supported yet), this function places the
+ 67    * "image" in a canvas element then decodes the * "image" as a Base64 string.
+ 68    *
+ 69    * @param data - object containing the PNG data.
+ 70    * @param callback - function with params:
+ 71    *   * data - the uncompressed data
+ 72    */
+ 73   function decompressPng(data, callback) {
+ 74     // Uncompresses the data before sending it through (use image/canvas to do so).
+ 75     var image = new Image();
+ 76     // When the image loads, extracts the raw data (JSON message).
+ 77     image.onload = function() {
+ 78       // Creates a local canvas to draw on.
+ 79       var canvas = document.createElement('canvas');
+ 80       var context = canvas.getContext('2d');
+ 81 
+ 82       // Sets width and height.
+ 83       canvas.width = image.width;
+ 84       canvas.height = image.height;
+ 85 
+ 86       // Puts the data into the image.
+ 87       context.drawImage(image, 0, 0);
+ 88       // Grabs the raw, uncompressed data.
+ 89       var imageData = context.getImageData(0, 0, image.width, image.height).data;
+ 90 
+ 91       // Constructs the JSON.
+ 92       var jsonData = '';
+ 93       for (var i = 0; i < imageData.length; i += 4) {
+ 94         // RGB
+ 95         jsonData += String.fromCharCode(imageData[i], imageData[i + 1], imageData[i + 2]);
+ 96       }
+ 97       var decompressedData = JSON.parse(jsonData);
+ 98       callback(decompressedData);
+ 99     };
+100     // Sends the image data to load.
+101     image.src = 'data:image/png;base64,' + data.data;
+102   }
+103 
+104   /**
+105    * Parses message responses from rosbridge and sends to the appropriate
+106    * topic, service, or param.
+107    *
+108    * @param message - the raw JSON message from rosbridge.
+109    */
+110   function onMessage(message) {
+111     function handleMessage(message) {
+112       if (message.op === 'publish') {
+113         that.emit(message.topic, message.msg);
+114       } else if (message.op === 'service_response') {
+115         that.emit(message.id, message.values);
+116       }
+117     };
+118 
+119     var data = JSON.parse(message.data);
+120     if (data.op === 'png') {
+121       decompressPng(data, function(decompressedData) {
+122         handleMessage(decompressedData);
+123       });
+124     } else {
+125       handleMessage(data);
+126     }
+127   };
+128 
+129   that.socket = new WebSocket(url);
+130   that.socket.onopen = onOpen;
+131   that.socket.onclose = onClose;
+132   that.socket.onerror = onError;
+133   that.socket.onmessage = onMessage;
+134 };
+135 
+136 /**
+137  * Disconnect from the WebSocket server.
+138  */
+139 ROSLIB.Ros.prototype.close = function() {
+140   if (this.socket) {
+141     this.socket.close();
+142   }
+143 };
+144 
+145 /**
+146  * Sends an authorization request to the server.
+147  *
+148  * @param mac - MAC (hash) string given by the trusted source.
+149  * @param client - IP of the client.
+150  * @param dest - IP of the destination.
+151  * @param rand - Random string given by the trusted source.
+152  * @param t - Time of the authorization request.
+153  * @param level - User level as a string given by the client.
+154  * @param end - End time of the client's session.
+155  */
+156 ROSLIB.Ros.prototype.authenticate = function(mac, client, dest, rand, t, level, end) {
+157   // create the request
+158   var auth = {
+159     op : 'auth',
+160     mac : mac,
+161     client : client,
+162     dest : dest,
+163     rand : rand,
+164     t : t,
+165     level : level,
+166     end : end
+167   };
+168   // send the request
+169   callOnConnection(auth);
+170 };
+171 
+172 /**
+173  * Sends the message over the WebSocket, but queues the message up if not yet
+174  * connected.
+175  */
+176 ROSLIB.Ros.prototype.callOnConnection = function(message) {
+177   var that = this;
+178   var messageJson = JSON.stringify(message);
+179 
+180   if (ros.socket.readyState !== WebSocket.OPEN) {
+181     that.once('connection', function() {
+182       that.socket.send(messageJson);
+183     });
+184   } else {
+185     that.socket.send(messageJson);
+186   }
+187 };
+188 
+189 /**
+190  * Retrieves list of topics in ROS as an array.
+191  *
+192  * @param callback function with params:
+193  *   * topics - Array of topic names
+194  */
+195 ROSLIB.Ros.prototype.getTopics = function(callback) {
+196   var topicsClient = new ROSLIB.Service({
+197     ros : this,
+198     name : '/rosapi/topics',
+199     serviceType : 'rosapi/Topics'
+200   });
+201 
+202   var request = new ROSLIB.ServiceRequest();
+203 
+204   topicsClient.callService(request, function(result) {
+205     callback(result.topics);
+206   });
+207 };
+208 
+209 /**
+210  * Retrieves list of active service names in ROS.
+211  *
+212  * @param callback - function with the following params:
+213  *   * services - array of service names
+214  */
+215 ROSLIB.Ros.prototype.getServices = function(callback) {
+216   var servicesClient = new ROSLIB.Service({
+217     ros : this,
+218     name : '/rosapi/services',
+219     serviceType : 'rosapi/Services'
+220   });
+221 
+222   var request = new ROSLIB.ServiceRequest();
+223 
+224   servicesClient.callService(request, function(result) {
+225     callback(result.services);
+226   });
+227 };
+228 
+229 /**
+230  * Retrieves list of param names from the ROS Parameter Server.
+231  *
+232  * @param callback function with params:
+233  *  * params - array of param names.
+234  */
+235 ROSLIB.Ros.prototype.getParams = function(callback) {
+236   var paramsClient = new ROSLIB.Service({
+237     ros : this,
+238     name : '/rosapi/get_param_names',
+239     serviceType : 'rosapi/GetParamNames'
+240   });
+241 
+242   var request = new ROSLIB.ServiceRequest();
+243   paramsClient.callService(request, function(result) {
+244     callback(result.names);
+245   });
+246 };
+247 
+248 
\ No newline at end of file diff --git a/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Service.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Service.js.html new file mode 100644 index 000000000..784461ec1 --- /dev/null +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Service.js.html @@ -0,0 +1,58 @@ +
  1 /**
+  2  * @author Brandon Alexander - baalexander@gmail.com
+  3  */
+  4 
+  5 /**
+  6  * A ROS service client.
+  7  *
+  8  * @constructor
+  9  * @params options - possible keys include:
+ 10  *   * ros - the ROSLIB.Ros connection handle
+ 11  *   * name - the service name, like /add_two_ints
+ 12  *   * serviceType - the service type, like 'rospy_tutorials/AddTwoInts'
+ 13  */
+ 14 ROSLIB.Service = function(options) {
+ 15   options = options || {};
+ 16   this.ros = options.ros;
+ 17   this.name = options.name;
+ 18   this.serviceType = options.serviceType;
+ 19 };
+ 20 
+ 21 /**
+ 22  * Calls the service. Returns the service response in the callback.
+ 23  *
+ 24  * @param request - the ROSLIB.ServiceRequest to send
+ 25  * @param callback - function with params:
+ 26  *   * response - the response from the service request
+ 27  */
+ 28 ROSLIB.Service.prototype.callService = function(request, callback) {
+ 29   this.ros.idCounter++;
+ 30   serviceCallId = 'call_service:' + this.name + ':' + this.ros.idCounter;
+ 31 
+ 32   this.ros.once(serviceCallId, function(data) {
+ 33     var response = new ROSLIB.ServiceResponse(data);
+ 34     callback(response);
+ 35   });
+ 36 
+ 37   var requestValues = [];
+ 38   Object.keys(request).forEach(function(name) {
+ 39     requestValues.push(request[name]);
+ 40   });
+ 41 
+ 42   var call = {
+ 43     op : 'call_service',
+ 44     id : serviceCallId,
+ 45     service : this.name,
+ 46     args : requestValues
+ 47   };
+ 48   this.ros.callOnConnection(call);
+ 49 };
+ 50 
+ 51 
\ No newline at end of file diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_ServiceRequest.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_ServiceRequest.js.html similarity index 84% rename from doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_ServiceRequest.js.html rename to doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_ServiceRequest.js.html index 47caa9742..f21169b20 100644 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_ServiceRequest.js.html +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_ServiceRequest.js.html @@ -16,10 +16,10 @@ 9 * @param values - object matching the values of the request part from the .srv file. 10 */
11 ROSLIB.ServiceRequest = function(values) { - 12 var serviceRequest = this; + 12 var that = this; 13 if (values) { 14 Object.keys(values).forEach(function(name) { - 15 serviceRequest[name] = values[name]; + 15 that[name] = values[name]; 16 }); 17 } 18 }; diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_ServiceResponse.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_ServiceResponse.js.html similarity index 82% rename from doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_ServiceResponse.js.html rename to doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_ServiceResponse.js.html index 9db966a3f..0dff1bed1 100644 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_ServiceResponse.js.html +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_ServiceResponse.js.html @@ -16,10 +16,10 @@ 9 * @param values - object matching the values of the response part from the .srv file. 10 */ 11 ROSLIB.ServiceResponse = function(values) { - 12 var serviceResponse = this; + 12 var that = this; 13 if (values) { 14 Object.keys(values).forEach(function(name) { - 15 serviceResponse[name] = values[name]; + 15 that[name] = values[name]; 16 }); 17 } 18 }; diff --git a/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Topic.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Topic.js.html new file mode 100644 index 000000000..40dbd284d --- /dev/null +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_core_Topic.js.html @@ -0,0 +1,149 @@ +
  1 /**
+  2  * @author Brandon Alexander - baalexander@gmail.com
+  3  */
+  4 
+  5 /**
+  6  * Publish and/or subscribe to a topic in ROS.
+  7  *
+  8  * Emits the following events:
+  9  *  * 'warning' - if there are any warning during the Topic creation
+ 10  *  * 'message' - the message data from rosbridge
+ 11  *
+ 12  * @constructor
+ 13  * @param options - object with following keys:
+ 14  *   * ros - the ROSLIB.Ros connection handle
+ 15  *   * name - the topic name, like /cmd_vel
+ 16  *   * messageType - the message type, like 'std_msgs/String'
+ 17  *   * compression - the type of compression to use, like 'png'
+ 18  *   * throttle_rate - the rate at which to throttle the topics
+ 19  */
+ 20 ROSLIB.Topic = function(options) {
+ 21   options = options || {};
+ 22   this.ros = options.ros;
+ 23   this.name = options.name;
+ 24   this.messageType = options.messageType;
+ 25   this.isAdvertised = false;
+ 26   this.compression = options.compression || 'none';
+ 27   this.throttle_rate = options.throttle_rate || 0;
+ 28 
+ 29   // Check for valid compression types
+ 30   if (this.compression && this.compression !== 'png' && this.compression !== 'none') {
+ 31     this.emit('warning', this.compression
+ 32         + ' compression is not supported. No comression will be used.');
+ 33   }
+ 34 
+ 35   // Check if throttle rate is negative
+ 36   if (this.throttle_rate < 0) {
+ 37     this.emit('warning', this.throttle_rate + ' is not allowed. Set to 0');
+ 38     this.throttle_rate = 0;
+ 39   }
+ 40 };
+ 41 ROSLIB.Topic.prototype.__proto__ = EventEmitter2.prototype;
+ 42 
+ 43 /**
+ 44  * Every time a message is published for the given topic, the callback
+ 45  * will be called with the message object.
+ 46  *
+ 47  * @param callback - function with the following params:
+ 48  *   * message - the published message
+ 49  */
+ 50 ROSLIB.Topic.prototype.subscribe = function(callback) {
+ 51   this.on('message', function(message) {
+ 52     callback(message);
+ 53   });
+ 54 
+ 55   this.ros.on(this.name, function(data) {
+ 56     var message = new ROSLIB.Message(data);
+ 57     this.emit('message', message);
+ 58   });
+ 59 
+ 60   this.ros.idCounter++;
+ 61   var subscribeId = 'subscribe:' + this.name + ':' + this.ros.idCounter;
+ 62   var call = {
+ 63     op : 'subscribe',
+ 64     id : subscribeId,
+ 65     type : this.messageType,
+ 66     topic : this.name,
+ 67     compression : this.compression,
+ 68     throttle_rate : this.throttle_rate
+ 69   };
+ 70 
+ 71   this.ros.callOnConnection(call);
+ 72 };
+ 73 
+ 74 /**
+ 75  * Unregisters as a subscriber for the topic. Unsubscribing will remove
+ 76  * all subscribe callbacks.
+ 77  */
+ 78 ROSLIB.Topic.prototype.unsubscribe = function() {
+ 79   this.ros.removeAllListeners([this.name]);
+ 80   this.ros.idCounter++;
+ 81   var unsubscribeId = 'unsubscribe:' + this.name + ':' + this.ros.idCounter;
+ 82   var call = {
+ 83     op : 'unsubscribe',
+ 84     id : unsubscribeId,
+ 85     topic : this.name
+ 86   };
+ 87   this.ros.callOnConnection(call);
+ 88 };
+ 89 
+ 90 /**
+ 91  * Registers as a publisher for the topic.
+ 92  */
+ 93 ROSLIB.Topic.prototype.advertise = function() {
+ 94   this.ros.idCounter++;
+ 95   var advertiseId = 'advertise:' + this.name + ':' + this.ros.idCounter;
+ 96   var call = {
+ 97     op : 'advertise',
+ 98     id : advertiseId,
+ 99     type : this.messageType,
+100     topic : this.name
+101   };
+102   this.ros.callOnConnection(call);
+103   this.isAdvertised = true;
+104 };
+105 
+106 /**
+107  * Unregisters as a publisher for the topic.
+108  */
+109 ROSLIB.Topic.prototype.unadvertise = function() {
+110   this.ros.idCounter++;
+111   var unadvertiseId = 'unadvertise:' + this.name + ':' + this.ros.idCounter;
+112   var call = {
+113     op : 'unadvertise',
+114     id : unadvertiseId,
+115     topic : this.name
+116   };
+117   this.ros.callOnConnection(call);
+118   this.isAdvertised = false;
+119 };
+120 
+121 /**
+122  * Publish the message.
+123  *
+124  * @param message - A ROSLIB.Message object.
+125  */
+126 ROSLIB.Topic.prototype.publish = function(message) {
+127   if (!this.isAdvertised) {
+128     this.advertise();
+129   }
+130 
+131   this.ros.idCounter++;
+132   var publishId = 'publish:' + this.name + ':' + this.ros.idCounter;
+133   var call = {
+134     op : 'publish',
+135     id : publishId,
+136     topic : this.name,
+137     msg : message
+138   };
+139   this.ros.callOnConnection(call);
+140 };
+141 
+142 
\ No newline at end of file diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_math_Pose.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_math_Pose.js.html similarity index 61% rename from doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_math_Pose.js.html rename to doc/symbols/src/_home_vagrant_ws_roslibjs_src_math_Pose.js.html index ccb28663d..312594eba 100644 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_math_Pose.js.html +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_math_Pose.js.html @@ -17,27 +17,27 @@ 10 * @param orientation - the ROSLIB.Quaternion describing the orientation 11 */
12 ROSLIB.Pose = function(position, orientation) { - 13 var pose = this; - 14 // copy the values into this object if they exist - 15 this.position = new ROSLIB.Vector3(); - 16 this.orientation = new ROSLIB.Quaternion(); - 17 if (position !== undefined) { - 18 this.position.copy(position); - 19 } - 20 if (orientation !== undefined) { - 21 this.orientation.copy(orientation); - 22 } + 13 // Copy the values into this object if they exist + 14 this.position = new ROSLIB.Vector3(); + 15 this.orientation = new ROSLIB.Quaternion(); + 16 if (position !== undefined) { + 17 this.position.copy(position); + 18 } + 19 if (orientation !== undefined) { + 20 this.orientation.copy(orientation); + 21 } + 22 }; 23 - 24 /** - 25 * Copy the values from the given pose into this pose. - 26 * - 27 * @param pose the pose to copy - 28 * @returns a pointer to this pose - 29 */ - 30 this.copy = function(pose) { - 31 pose.position.copy(pose.position); - 32 pose.orientation.copy(pose.orientation); - 33 return pose; - 34 }; - 35 }; + 24 /** + 25 * Copy the values from the given pose into this pose. + 26 * + 27 * @param pose the pose to copy + 28 * @returns a pointer to this pose + 29 */ + 30 ROSLIB.Pose.prototype.copy = function(pose) { + 31 this.position.copy(pose.position); + 32 this.orientation.copy(pose.orientation); + 33 return pose; + 34 }; + 35 36
\ No newline at end of file diff --git a/doc/symbols/src/_home_vagrant_ws_roslibjs_src_math_Quaternion.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_math_Quaternion.js.html new file mode 100644 index 000000000..3c2042202 --- /dev/null +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_math_Quaternion.js.html @@ -0,0 +1,135 @@ +
  1 /**
+  2  * @author David Gossow - dgossow@willowgarage.com
+  3  */
+  4 
+  5 /**
+  6  * A Quaternion.
+  7  *
+  8  *  @constructor
+  9  *  @param x - the x value 
+ 10  *  @param y - the y value
+ 11  *  @param z - the z value
+ 12  *  @param w - the w value
+ 13  */
+ 14 ROSLIB.Quaternion = function(x, y, z, w) {
+ 15   var quaternion = this;
+ 16   this.x = x || 0;
+ 17   this.y = y || 0;
+ 18   this.z = z || 0;
+ 19   this.w = w || 1;
+ 20 };
+ 21 
+ 22 /**
+ 23  * Copy the values from the given quaternion into this quaternion.
+ 24  *
+ 25  * @param q the quaternion to copy
+ 26  * @returns a pointer to this quaternion
+ 27  */
+ 28 ROSLIB.Quaternion.prototype.copy = function(q) {
+ 29   this.x = q.x;
+ 30   this.y = q.y;
+ 31   this.z = q.z;
+ 32   this.w = q.w;
+ 33   return this;
+ 34 };
+ 35 
+ 36 /**
+ 37  * Perform a conjugation on this quaternion.
+ 38  *
+ 39  * @returns a pointer to this quaternion
+ 40  */
+ 41 ROSLIB.Quaternion.prototype.conjugate = function() {
+ 42   this.x *= -1;
+ 43   this.y *= -1;
+ 44   this.z *= -1;
+ 45   return this;
+ 46 };
+ 47 
+ 48 /**
+ 49  * Perform a normalization on this quaternion.
+ 50  *
+ 51  * @returns a pointer to this quaternion
+ 52  */
+ 53 ROSLIB.Quaternion.prototype.normalize = function() {
+ 54   var l = Math.sqrt(
+ 55     this.x * this.x + this.y * this.y
+ 56   + this.z * this.z + this.w * this.w
+ 57   );
+ 58   if (l === 0) {
+ 59     this.x = 0;
+ 60     this.y = 0;
+ 61     this.z = 0;
+ 62     this.w = 1;
+ 63   } else {
+ 64     l = 1 / l;
+ 65     this.x = this.x * l;
+ 66     this.y = this.y * l;
+ 67     this.z = this.z * l;
+ 68     this.w = this.w * l;
+ 69   }
+ 70   return this;
+ 71 };
+ 72 
+ 73 /**
+ 74  * Convert this quaternion into its inverse.
+ 75  *
+ 76  * @returns a pointer to this quaternion
+ 77  */
+ 78 ROSLIB.Quaternion.prototype.inverse = function() {
+ 79   this.conjugate().normalize();
+ 80   return this;
+ 81 };
+ 82 
+ 83 /**
+ 84  * Set the values of this quaternion to the product of quaternions a and b.
+ 85  *
+ 86  * @param a the first quaternion to multiply with
+ 87  * @param b the second quaternion to multiply with
+ 88  * @returns a pointer to this quaternion
+ 89  */
+ 90 ROSLIB.Quaternion.prototype.multiply = function(a, b) {
+ 91   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;
+ 92   this.x = qax * qbw + qay * qbz - qaz * qby + qaw * qbx;
+ 93   this.y = -qax * qbz + qay * qbw + qaz * qbx + qaw * qby;
+ 94   this.z = qax * qby - qay * qbx + qaz * qbw + qaw * qbz;
+ 95   this.w = -qax * qbx - qay * qby - qaz * qbz + qaw * qbw;
+ 96   return this;
+ 97 };
+ 98 
+ 99 /**
+100  * Multiply the given ROSLIB.Vector3 with this quaternion.
+101  *
+102  * @param vector the vector to multiply with
+103  * @param dest (option) - where the computed values will go (defaults to 'vector').
+104  * @returns a pointer to dest
+105  */
+106 ROSLIB.Quaternion.prototype.multiplyVec3 = function(vector, dest) {
+107   if (!dest) {
+108     dest = vector;
+109   }
+110   var x = vector.x, y = vector.y, z = vector.z, qx = this.x, qy = this.y, qz = this.z, qw = this.w;
+111   var ix = qw * x + qy * z - qz * y, iy = qw * y + qz * x - qx * z, iz = qw * z + qx * y - qy * x, iw = -qx
+112       * x - qy * y - qz * z;
+113   dest.x = ix * qw + iw * -qx + iy * -qz - iz * -qy;
+114   dest.y = iy * qw + iw * -qy + iz * -qx - ix * -qz;
+115   dest.z = iz * qw + iw * -qz + ix * -qy - iy * -qx;
+116   return dest;
+117 };
+118 
+119 /**
+120  * Clone a copy of this quaternion.
+121  *
+122  * @returns the cloned quaternion
+123  */
+124 ROSLIB.Quaternion.prototype.clone = function() {
+125   return new ROSLIB.Quaternion(this.x, this.y, this.z, this.w);
+126 };
+127 
+128 
\ No newline at end of file diff --git a/doc/symbols/src/_home_vagrant_ws_roslibjs_src_math_Vector3.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_math_Vector3.js.html new file mode 100644 index 000000000..41493534f --- /dev/null +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_math_Vector3.js.html @@ -0,0 +1,76 @@ +
  1 /**
+  2  * @author David Gossow - dgossow@willowgarage.com
+  3  */
+  4 
+  5 /**
+  6  * A 3D vector.
+  7  *
+  8  *  @constructor
+  9  *  @param x - the x value 
+ 10  *  @param y - the y value
+ 11  *  @param z - the z value
+ 12  */
+ 13 ROSLIB.Vector3 = function(x, y, z) {
+ 14   this.x = x || 0;
+ 15   this.y = y || 0;
+ 16   this.z = z || 0;
+ 17 };
+ 18 
+ 19 /**
+ 20  * Copy the values from the given vector into this vector.
+ 21  *
+ 22  * @param v the vector to copy
+ 23  * @returns a pointer to this vector
+ 24  */
+ 25 ROSLIB.Vector3.prototype.copy = function(v) {
+ 26   this.x = v.x;
+ 27   this.y = v.y;
+ 28   this.z = v.z;
+ 29   return this;
+ 30 };
+ 31 
+ 32 /**
+ 33  * Set the values of this vector to the sum of vectors a and b.
+ 34  *
+ 35  * @param a the first vector to add with
+ 36  * @param b the second vector to add with
+ 37  * @returns a pointer to this vector
+ 38  */
+ 39 ROSLIB.Vector3.prototype.add = function(a, b) {
+ 40   this.x = a.x + b.x;
+ 41   this.y = a.y + b.y;
+ 42   this.z = a.z + b.z;
+ 43   return this;
+ 44 };
+ 45 
+ 46 /**
+ 47  * Set the values of this vector to the difference of vectors a and b.
+ 48  *
+ 49  * @param a the first vector to add with
+ 50  * @param b the second vector to add with
+ 51  * @returns a pointer to this vector
+ 52  */
+ 53 ROSLIB.Vector3.prototype.sub = function(a, b) {
+ 54   this.x = a.x - b.x;
+ 55   this.y = a.y - b.y;
+ 56   this.z = a.z - b.z;
+ 57   return this;
+ 58 };
+ 59 
+ 60 /**
+ 61  * Clone a copy of this vector.
+ 62  *
+ 63  * @returns the cloned vector
+ 64  */
+ 65 ROSLIB.Vector3.prototype.clone = function() {
+ 66   return new ROSLIB.Vector3(this.x, this.y, this.z);
+ 67 };
+ 68 
+ 69 
\ No newline at end of file diff --git a/doc/symbols/src/_home_vagrant_ws_roslibjs_src_tf_TFClient.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_tf_TFClient.js.html new file mode 100644 index 000000000..f414a50ec --- /dev/null +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_tf_TFClient.js.html @@ -0,0 +1,147 @@ +
  1 /**
+  2  * @author David Gossow - dgossow@willowgarage.com
+  3  */
+  4 
+  5 /**
+  6  * A TF Client that listens to TFs from tf2_web_republisher.
+  7  *
+  8  *  @constructor
+  9  *  @param options - object with following keys:
+ 10  *   * ros - the ROSLIB.Ros connection handle
+ 11  *   * fixedFrame - the fixed frame, like /base_link
+ 12  *   * angularThres - the angular threshold for the TF republisher
+ 13  *   * transThres - the translation threshold for the TF republisher
+ 14  *   * rate - the rate for the TF republisher
+ 15  *   * goalUpdateDelay - the goal update delay for the TF republisher
+ 16  */
+ 17 ROSLIB.TFClient = function(options) {
+ 18   var options = options || {};
+ 19   this.ros = options.ros;
+ 20   this.fixedFrame = options.fixedFrame || '/base_link';
+ 21   this.angularThres = options.angularThres || 2.0;
+ 22   this.transThres = options.transThres || 0.01;
+ 23   this.rate = options.rate || 10.0;
+ 24   this.goalUpdateDelay = options.goalUpdateDelay || 50;
+ 25 
+ 26   this.currentGoal = false;
+ 27   this.frameInfos = {};
+ 28   this.goalUpdateRequested = false;
+ 29 
+ 30   // Create an ActionClient
+ 31   this.actionClient = new ROSLIB.ActionClient({
+ 32     ros : this.ros,
+ 33     serverName : '/tf2_web_republisher',
+ 34     actionName : 'tf2_web_republisher/TFSubscriptionAction'
+ 35   });
+ 36 };
+ 37 
+ 38 /**
+ 39  * Process the incoming TF message and send them out using the callback
+ 40  * functions.
+ 41  *
+ 42  * @param tf - the TF message from the server
+ 43  */
+ 44 ROSLIB.TFClient.prototype.processFeedback = function(tf) {
+ 45   var that = this;
+ 46   tf.transforms.forEach(function(transform) {
+ 47     var frameID = transform.child_frame_id;
+ 48     var info = that.frameInfos[frameID];
+ 49     if (info != undefined) {
+ 50       info.transform = new ROSLIB.Transform(transform.transform.translation,
+ 51         transform.transform.rotation);
+ 52       info.cbs.forEach(function(cb) {
+ 53         cb(info.transform);
+ 54       });
+ 55     }
+ 56   });
+ 57 };
+ 58 
+ 59 /**
+ 60  * Create and send a new goal to the tf2_web_republisher based on the current
+ 61  * list of TFs.
+ 62  */
+ 63 ROSLIB.TFClient.prototype.updateGoal = function() {
+ 64   // Anytime the list of frames changes, we will need to send a new goal.
+ 65   if (this.currentGoal) {
+ 66     this.currentGoal.cancel();
+ 67   }
+ 68 
+ 69   var goalMessage = {
+ 70     source_frames : [],
+ 71     target_frame : this.fixedFrame,
+ 72     angular_thres : this.angularThres,
+ 73     trans_thres : this.transThres,
+ 74     rate : this.rate
+ 75   };
+ 76 
+ 77   for (frame in this.frameInfos) {
+ 78     goalMessage.source_frames.push(frame);
+ 79   }
+ 80 
+ 81   this.currentGoal = new ROSLIB.Goal({
+ 82     actionClient : this.actionClient,
+ 83     goalMessage : goalMessage
+ 84   });
+ 85   this.currentGoal.on('feedback', this.processFeedback.bind(this));
+ 86   this.currentGoal.send();
+ 87   this.goalUpdateRequested = false;
+ 88 };
+ 89 
+ 90 /**
+ 91  * Subscribe to the given TF frame.
+ 92  *
+ 93  * @param frameID - the TF frame to subscribe to
+ 94  * @param callback - function with params:
+ 95  *   * transform - the transform data
+ 96  */
+ 97 ROSLIB.TFClient.prototype.subscribe = function(frameID, callback) {
+ 98   // make sure the frame id is relative
+ 99   if (frameID[0] === '/') {
+100     frameID = frameID.substring(1);
+101   }
+102   // if there is no callback registered for the given frame, create emtpy callback list
+103   if (this.frameInfos[frameID] == undefined) {
+104     this.frameInfos[frameID] = {
+105       cbs : []
+106     };
+107     if (!this.goalUpdateRequested) {
+108       setTimeout(this.updateGoal.bind(tfClient), this.goalUpdateDelay);
+109       this.goalUpdateRequested = true;
+110     }
+111   } else {
+112     // if we already have a transform, call back immediately
+113     if (this.frameInfos[frameID].transform != undefined) {
+114       callback(this.frameInfos[frameID].transform);
+115     }
+116   }
+117   this.frameInfos[frameID].cbs.push(callback);
+118 };
+119 
+120 /**
+121  * Unsubscribe from the given TF frame.
+122  *
+123  * @param frameID - the TF frame to unsubscribe from
+124  * @param callback - the callback function to remove
+125  */
+126 ROSLIB.TFClient.prototype.unsubscribe = function(frameID, callback) {
+127   var info = this.frameInfos[frameID];
+128   if (info != undefined) {
+129     var cbIndex = info.cbs.indexOf(callback);
+130     if (cbIndex >= 0) {
+131       info.cbs.splice(cbIndex, 1);
+132       if (info.cbs.length == 0) {
+133         delete this.frameInfos[frameID];
+134       }
+135       this.needUpdate = true;
+136     }
+137   }
+138 };
+139 
+140 
\ No newline at end of file diff --git a/doc/symbols/src/_home_vagrant_ws_roslibjs_src_tf_Transform.js.html b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_tf_Transform.js.html new file mode 100644 index 000000000..72d441f5c --- /dev/null +++ b/doc/symbols/src/_home_vagrant_ws_roslibjs_src_tf_Transform.js.html @@ -0,0 +1,69 @@ +
  1 /**
+  2  * @author David Gossow - dgossow@willowgarage.com
+  3  */
+  4 
+  5 /**
+  6  * A Transform in 3-space. Values are copied into this object.
+  7  *
+  8  *  @constructor
+  9  *  @param translation - the ROSLIB.Vector3 describing the translation
+ 10  *  @param rotation - the ROSLIB.Quaternion describing the rotation
+ 11  */
+ 12 ROSLIB.Transform = function(translation, rotation) {
+ 13   this.translation = new ROSLIB.Vector3();
+ 14   this.rotation = new ROSLIB.Quaternion();
+ 15   if (translation !== undefined) {
+ 16     this.translation.copy(translation);
+ 17   }
+ 18   if (rotation !== undefined) {
+ 19     this.rotation.copy(rotation);
+ 20   }
+ 21 };
+ 22 
+ 23 /**
+ 24  * Apply a transform against the given ROSLIB.Pose.
+ 25  *
+ 26  * @param pose the pose to transform with
+ 27  * @returns a pointer to the pose
+ 28  */
+ 29 ROSLIB.Transform.prototype.apply = function(pose) {
+ 30   this.rotation.multiplyVec3(pose.position);
+ 31   pose.position.add(pose.position, this.translation);
+ 32   pose.orientation.multiply(this.rotation, pose.orientation);
+ 33   return pose;
+ 34 };
+ 35 
+ 36 /**
+ 37  * Apply an inverse transform against the given ROSLIB.Pose.
+ 38  *
+ 39  * @param pose the pose to transform with
+ 40  * @returns a pointer to the pose
+ 41  */
+ 42 ROSLIB.Transform.prototype.applyInverse = function(pose) {
+ 43   var rotInv = this.rotation.clone().inverse();
+ 44   rotInv.multiplyVec3(pose.position);
+ 45   pose.position.sub(pose.position, this.translation);
+ 46   pose.orientation.multiply(rotInv, pose.orientation);
+ 47   return pose;
+ 48 };
+ 49 
+ 50 /**
+ 51  * Copy the values from the given transform into this transform.
+ 52  *
+ 53  * @param transform the transform to copy
+ 54  * @returns a pointer to this transform
+ 55  */
+ 56 ROSLIB.Transform.prototype.copy = function(transform) {
+ 57   transform.translation.copy(transform.translation);
+ 58   transform.rotation.copy(transform.rotation);
+ 59   return transform;
+ 60 };
+ 61 
+ 62 
\ No newline at end of file diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Param.js.html b/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Param.js.html deleted file mode 100644 index 8a7bb1dc9..000000000 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Param.js.html +++ /dev/null @@ -1,71 +0,0 @@ -
  1 /**
-  2  * @author Brandon Alexander - balexander@willowgarage.com
-  3  */
-  4 
-  5 /**
-  6  * A ROS parameter.
-  7  *
-  8  * @constructor
-  9  * @param options - possible keys include:
- 10  *   * ros - the ROSLIB.Ros connection handle
- 11  *   * name - the param name, like max_vel_x
- 12  */
- 13 ROSLIB.Param = function(options) {
- 14   var param = this;
- 15   options = options || {};
- 16   this.ros = options.ros;
- 17   this.name = options.name;
- 18 
- 19   /**
- 20    * Fetches the value of the param.
- 21    *
- 22    * @param callback - function with the following params:
- 23    *  * value - the value of the param from ROS.
- 24    */
- 25   this.get = function(callback) {
- 26     var paramClient = new ROSLIB.Service({
- 27       ros : param.ros,
- 28       name : '/rosapi/get_param',
- 29       serviceType : 'rosapi/GetParam'
- 30     });
- 31 
- 32     var request = new ROSLIB.ServiceRequest({
- 33       name : param.name,
- 34       value : JSON.stringify('')
- 35     });
- 36 
- 37     paramClient.callService(request, function(result) {
- 38       var value = JSON.parse(result.value);
- 39       callback(value);
- 40     });
- 41   };
- 42 
- 43   /**
- 44    * Sets the value of the param in ROS.
- 45    *
- 46    * @param value - value to set param to.
- 47    */
- 48   this.set = function(value) {
- 49     var paramClient = new ROSLIB.Service({
- 50       ros : param.ros,
- 51       name : '/rosapi/set_param',
- 52       serviceType : 'rosapi/SetParam'
- 53     });
- 54 
- 55     var request = new ROSLIB.ServiceRequest({
- 56       name : param.name,
- 57       value : JSON.stringify(value)
- 58     });
- 59 
- 60     paramClient.callService(request, function() {
- 61     });
- 62   };
- 63 };
- 64 
\ No newline at end of file diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Ros.js.html b/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Ros.js.html deleted file mode 100644 index 2fc7e766d..000000000 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Ros.js.html +++ /dev/null @@ -1,249 +0,0 @@ -
  1 /**
-  2  * @author Brandon Alexander - balexander@willowgarage.com
-  3  */
-  4 
-  5 /**
-  6  * Manages connection to the server and all interactions with ROS.
-  7  *
-  8  * Emits the following events:
-  9  *  * 'error' - there was an error with ROS
- 10  *  * 'connection' - connected to the WebSocket server
- 11  *  * 'close' - disconnected to the WebSocket server
- 12  *  * <topicName> - a message came from rosbridge with the given topic name
- 13  *  * <serviceID> - a service response came from rosbridge with the given ID
- 14  *
- 15  *  @constructor
- 16  *  @param url (optional) - The WebSocket URL for rosbridge. Can be specified later with `connect`.
- 17  */
- 18 ROSLIB.Ros = function(url) {
- 19   var ros = this;
- 20   this.socket = null;
- 21 
- 22   /**
- 23    * Emits a 'connection' event on WebSocket connection.
- 24    * 
- 25    * @param event - the argument to emit with the event.
- 26    */
- 27   function onOpen(event) {
- 28     ros.emit('connection', event);
- 29   };
- 30 
- 31   /**
- 32    * Emits a 'close' event on WebSocket disconnection.
- 33    * 
- 34    * @param event - the argument to emit with the event.
- 35    */
- 36   function onClose(event) {
- 37     ros.emit('close', event);
- 38   };
- 39 
- 40   /**
- 41    * Emits an 'error' event whenever there was an error.
- 42    * 
- 43    * @param event - the argument to emit with the event.
- 44    */
- 45   function onError(event) {
- 46     ros.emit('error', event);
- 47   };
- 48 
- 49   /**
- 50    * If a message was compressed as a PNG image (a compression hack since gzipping over WebSockets
- 51    * is not supported yet), this function places the "image" in a canvas element then decodes the
- 52    * "image" as a Base64 string.
- 53    *
- 54    * @param data - object containing the PNG data.
- 55    * @param callback - function with params:
- 56    *   * data - the uncompressed data
- 57    */
- 58   function decompressPng(data, callback) {
- 59     // Uncompresses the data before sending it through (use image/canvas to do so).
- 60     var image = new Image();
- 61     // When the image loads, extracts the raw data (JSON message).
- 62     image.onload = function() {
- 63       // Creates a local canvas to draw on.
- 64       var canvas = document.createElement('canvas');
- 65       var context = canvas.getContext('2d');
- 66 
- 67       // Sets width and height.
- 68       canvas.width = image.width;
- 69       canvas.height = image.height;
- 70 
- 71       // Puts the data into the image.
- 72       context.drawImage(image, 0, 0);
- 73       // Grabs the raw, uncompressed data.
- 74       var imageData = context.getImageData(0, 0, image.width, image.height).data;
- 75 
- 76       // Constructs the JSON.
- 77       var jsonData = '';
- 78       for ( var i = 0; i < imageData.length; i += 4) {
- 79         // RGB
- 80         jsonData += String.fromCharCode(imageData[i], imageData[i + 1], imageData[i + 2]);
- 81       }
- 82       var decompressedData = JSON.parse(jsonData);
- 83       callback(decompressedData);
- 84     };
- 85     // Sends the image data to load.
- 86     image.src = 'data:image/png;base64,' + data.data;
- 87   }
- 88 
- 89   /**
- 90    * Parses message responses from rosbridge and sends to the appropriate topic, service, or param.
- 91    *
- 92    * @param message - the raw JSON message from rosbridge.
- 93    */
- 94   function onMessage(message) {
- 95     function handleMessage(message) {
- 96       if (message.op === 'publish') {
- 97         ros.emit(message.topic, message.msg);
- 98       } else if (message.op === 'service_response') {
- 99         ros.emit(message.id, message.values);
-100       }
-101     };
-102 
-103     var data = JSON.parse(message.data);
-104     if (data.op === 'png') {
-105       decompressPng(data, function(decompressedData) {
-106         handleMessage(decompressedData);
-107       });
-108     } else {
-109       handleMessage(data);
-110     }
-111   };
-112 
-113   /**
-114    * Connect to the specified WebSocket.
-115    *
-116    * @param url - WebSocket URL for Rosbridge
-117    */
-118   this.connect = function(url) {
-119     ros.socket = new WebSocket(url);
-120     ros.socket.onopen = onOpen;
-121     ros.socket.onclose = onClose;
-122     ros.socket.onerror = onError;
-123     ros.socket.onmessage = onMessage;
-124   };
-125 
-126   /**
-127    * Disconnect from the WebSocket server.
-128    */
-129   this.close = function() {
-130     if (ros.socket) {
-131       ros.socket.close();
-132     }
-133   };
-134 
-135   /**
-136    * Sends an authorization request to the server.
-137    *
-138    * @param mac - MAC (hash) string given by the trusted source.
-139    * @param client - IP of the client.
-140    * @param dest - IP of the destination.
-141    * @param rand - Random string given by the trusted source.
-142    * @param t - Time of the authorization request.
-143    * @param level - User level as a string given by the client.
-144    * @param end - End time of the client's session.
-145    */
-146   this.authenticate = function(mac, client, dest, rand, t, level, end) {
-147     // create the request
-148     var auth = {
-149       op : 'auth',
-150       mac : mac,
-151       client : client,
-152       dest : dest,
-153       rand : rand,
-154       t : t,
-155       level : level,
-156       end : end
-157     };
-158     // send the request
-159     callOnConnection(auth);
-160   };
-161 
-162   /**
-163    * Sends the message over the WebSocket, but queues the message up if not yet connected.
-164    */
-165   this.callOnConnection = function(message) {
-166     var messageJson = JSON.stringify(message);
-167 
-168     if (ros.socket.readyState !== WebSocket.OPEN) {
-169       ros.once('connection', function() {
-170         ros.socket.send(messageJson);
-171       });
-172     } else {
-173       ros.socket.send(messageJson);
-174     }
-175   };
-176 
-177   /**
-178    * Retrieves list of topics in ROS as an array.
-179    *
-180    * @param callback function with params:
-181    *   * topics - Array of topic names
-182    */
-183   this.getTopics = function(callback) {
-184     var topicsClient = new ROSLIB.Service({
-185       ros : ros,
-186       name : '/rosapi/topics',
-187       serviceType : 'rosapi/Topics'
-188     });
-189 
-190     var request = new ROSLIB.ServiceRequest();
-191 
-192     topicsClient.callService(request, function(result) {
-193       callback(result.topics);
-194     });
-195   };
-196 
-197   /**
-198    * Retrieves list of active service names in ROS.
-199    *
-200    * @param callback - function with the following params:
-201    *   * services - array of service names
-202    */
-203   this.getServices = function(callback) {
-204     var servicesClient = new ROSLIB.Service({
-205       ros : ros,
-206       name : '/rosapi/services',
-207       serviceType : 'rosapi/Services'
-208     });
-209 
-210     var request = new ROSLIB.ServiceRequest();
-211 
-212     servicesClient.callService(request, function(result) {
-213       callback(result.services);
-214     });
-215   };
-216 
-217   /**
-218    * Retrieves list of param names from the ROS Parameter Server.
-219    *
-220    * @param callback function with params:
-221    *  * params - array of param names.
-222    */
-223   this.getParams = function(callback) {
-224     var paramsClient = new ROSLIB.Service({
-225       ros : ros,
-226       name : '/rosapi/get_param_names',
-227       serviceType : 'rosapi/GetParamNames'
-228     });
-229 
-230     var request = new ROSLIB.ServiceRequest();
-231     paramsClient.callService(request, function(result) {
-232       callback(result.names);
-233     });
-234   };
-235 
-236   // begin by checking if a URL was given
-237   if (url) {
-238     this.connect(url);
-239   }
-240 };
-241 ROSLIB.Ros.prototype.__proto__ = EventEmitter2.prototype;
-242 
\ No newline at end of file diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Service.js.html b/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Service.js.html deleted file mode 100644 index db03f4fa6..000000000 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Service.js.html +++ /dev/null @@ -1,58 +0,0 @@ -
  1 /**
-  2  * @author Brandon Alexander - balexander@willowgarage.com
-  3  */
-  4 
-  5 /**
-  6  * A ROS service client.
-  7  *
-  8  * @constructor
-  9  * @params options - possible keys include:
- 10  *   * ros - the ROSLIB.Ros connection handle
- 11  *   * name - the service name, like /add_two_ints
- 12  *   * serviceType - the service type, like 'rospy_tutorials/AddTwoInts'
- 13  */
- 14 ROSLIB.Service = function(options) {
- 15   var service = this;
- 16   options = options || {};
- 17   this.ros = options.ros;
- 18   this.name = options.name;
- 19   this.serviceType = options.serviceType;
- 20 
- 21   /**
- 22    * Calls the service. Returns the service response in the callback.
- 23    * 
- 24    * @param request - the ROSLIB.ServiceRequest to send
- 25    * @param callback - function with params:
- 26    *   * response - the response from the service request
- 27    */
- 28   this.callService = function(request, callback) {
- 29     service.ros.idCounter++;
- 30     serviceCallId = 'call_service:' + service.name + ':' + service.ros.idCounter;
- 31 
- 32     service.ros.once(serviceCallId, function(data) {
- 33       var response = new ROSLIB.ServiceResponse(data);
- 34       callback(response);
- 35     });
- 36 
- 37     var requestValues = [];
- 38     Object.keys(request).forEach(function(name) {
- 39       requestValues.push(request[name]);
- 40     });
- 41 
- 42     var call = {
- 43       op : 'call_service',
- 44       id : serviceCallId,
- 45       service : service.name,
- 46       args : requestValues
- 47     };
- 48     service.ros.callOnConnection(call);
- 49   };
- 50 };
- 51 
\ No newline at end of file diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Topic.js.html b/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Topic.js.html deleted file mode 100644 index ca2a028a8..000000000 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_core_Topic.js.html +++ /dev/null @@ -1,149 +0,0 @@ -
  1 /**
-  2  * @author Brandon Alexander - balexander@willowgarage.com
-  3  */
-  4 
-  5 /**
-  6  * Publish and/or subscribe to a topic in ROS.
-  7  * 
-  8  * Emits the following events:
-  9  *  * 'warning' - if there are any warning during the Topic creation
- 10  *  * 'message' - the message data from rosbridge
- 11  *  
- 12  * @constructor
- 13  * @param options - object with following keys:
- 14  *   * ros - the ROSLIB.Ros connection handle
- 15  *   * name - the topic name, like /cmd_vel
- 16  *   * messageType - the message type, like 'std_msgs/String'
- 17  *   * compression - the type of compression to use, like 'png'
- 18  *   * throttle_rate - the rate at which to throttle the topics
- 19  */
- 20 ROSLIB.Topic = function(options) {
- 21   var topic = this;
- 22   options = options || {};
- 23   this.ros = options.ros;
- 24   this.name = options.name;
- 25   this.messageType = options.messageType;
- 26   this.isAdvertised = false;
- 27   this.compression = options.compression || 'none';
- 28   this.throttle_rate = options.throttle_rate || 0;
- 29 
- 30   // Check for valid compression types
- 31   if (this.compression && this.compression !== 'png' && this.compression !== 'none') {
- 32     this.emit('warning', this.compression
- 33         + ' compression is not supported. No comression will be used.');
- 34   }
- 35 
- 36   // Check if throttle rate is negative
- 37   if (this.throttle_rate < 0) {
- 38     this.emit('warning', this.throttle_rate + ' is not allowed. Set to 0');
- 39     this.throttle_rate = 0;
- 40   }
- 41 
- 42   /**
- 43    * Every time a message is published for the given topic, the callback
- 44    * will be called with the message object.
- 45    *
- 46    * @param callback - function with the following params:
- 47    *   * message - the published message
- 48    */
- 49   this.subscribe = function(callback) {
- 50     topic.on('message', function(message) {
- 51       callback(message);
- 52     });
- 53 
- 54     topic.ros.on(topic.name, function(data) {
- 55       var message = new ROSLIB.Message(data);
- 56       topic.emit('message', message);
- 57     });
- 58 
- 59     topic.ros.idCounter++;
- 60     var subscribeId = 'subscribe:' + topic.name + ':' + topic.ros.idCounter;
- 61     var call = {
- 62       op : 'subscribe',
- 63       id : subscribeId,
- 64       type : topic.messageType,
- 65       topic : topic.name,
- 66       compression : topic.compression,
- 67       throttle_rate : topic.throttle_rate
- 68     };
- 69 
- 70     topic.ros.callOnConnection(call);
- 71   };
- 72 
- 73   /**
- 74    * Unregisters as a subscriber for the topic. Unsubscribing will remove
- 75    * all subscribe callbacks.
- 76    */
- 77   this.unsubscribe = function() {
- 78     topic.ros.removeAllListeners([ topic.name ]);
- 79     topic.ros.idCounter++;
- 80     var unsubscribeId = 'unsubscribe:' + topic.name + ':' + topic.ros.idCounter;
- 81     var call = {
- 82       op : 'unsubscribe',
- 83       id : unsubscribeId,
- 84       topic : topic.name
- 85     };
- 86     topic.ros.callOnConnection(call);
- 87   };
- 88 
- 89   /**
- 90    * Registers as a publisher for the topic.
- 91    */
- 92   this.advertise = function() {
- 93     topic.ros.idCounter++;
- 94     var advertiseId = 'advertise:' + topic.name + ':' + topic.ros.idCounter;
- 95     var call = {
- 96       op : 'advertise',
- 97       id : advertiseId,
- 98       type : topic.messageType,
- 99       topic : topic.name
-100     };
-101     topic.ros.callOnConnection(call);
-102     topic.isAdvertised = true;
-103   };
-104 
-105   /**
-106    * Unregisters as a publisher for the topic.
-107    */
-108   this.unadvertise = function() {
-109     topic.ros.idCounter++;
-110     var unadvertiseId = 'unadvertise:' + topic.name + ':' + topic.ros.idCounter;
-111     var call = {
-112       op : 'unadvertise',
-113       id : unadvertiseId,
-114       topic : topic.name
-115     };
-116     topic.ros.callOnConnection(call);
-117     topic.isAdvertised = false;
-118   };
-119 
-120   /**
-121    * Publish the message.
-122    *
-123    * @param message - A ROSLIB.Message object.
-124    */
-125   this.publish = function(message) {
-126     if (!topic.isAdvertised) {
-127       topic.advertise();
-128     }
-129 
-130     topic.ros.idCounter++;
-131     var publishId = 'publish:' + topic.name + ':' + topic.ros.idCounter;
-132     var call = {
-133       op : 'publish',
-134       id : publishId,
-135       topic : topic.name,
-136       msg : message
-137     };
-138     topic.ros.callOnConnection(call);
-139   };
-140 };
-141 ROSLIB.Topic.prototype.__proto__ = EventEmitter2.prototype;
-142 
\ No newline at end of file diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_math_Quaternion.js.html b/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_math_Quaternion.js.html deleted file mode 100644 index 77a304050..000000000 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_math_Quaternion.js.html +++ /dev/null @@ -1,132 +0,0 @@ -
  1 /**
-  2  * @author David Gossow - dgossow@willowgarage.com
-  3  */
-  4 
-  5 /**
-  6  * A Quaternion.
-  7  *
-  8  *  @constructor
-  9  *  @param x - the x value 
- 10  *  @param y - the y value
- 11  *  @param z - the z value
- 12  *  @param w - the w value
- 13  */
- 14 ROSLIB.Quaternion = function(x, y, z, w) {
- 15   var quaternion = this;
- 16   this.x = x || 0;
- 17   this.y = y || 0;
- 18   this.z = z || 0;
- 19   this.w = w || 1;
- 20 
- 21   /**
- 22    * Copy the values from the given quaternion into this quaternion.
- 23    * 
- 24    * @param q the quaternion to copy
- 25    * @returns a pointer to this quaternion
- 26    */
- 27   this.copy = function(q) {
- 28     quaternion.x = q.x;
- 29     quaternion.y = q.y;
- 30     quaternion.z = q.z;
- 31     quaternion.w = q.w;
- 32     return quaternion;
- 33   };
- 34 
- 35   /**
- 36    * Perform a conjugation on this quaternion.
- 37    * 
- 38    * @returns a pointer to this quaternion
- 39    */
- 40   this.conjugate = function() {
- 41     quaternion.x *= -1;
- 42     quaternion.y *= -1;
- 43     quaternion.z *= -1;
- 44     return quaternion;
- 45   };
- 46 
- 47   /**
- 48    * Perform a normalization on this quaternion.
- 49    * 
- 50    * @returns a pointer to this quaternion
- 51    */
- 52   this.normalize = function() {
- 53     var l = Math.sqrt(quaternion.x * quaternion.x + quaternion.y * quaternion.y + quaternion.z
- 54         * quaternion.z + quaternion.w * quaternion.w);
- 55     if (l === 0) {
- 56       quaternion.x = 0;
- 57       quaternion.y = 0;
- 58       quaternion.z = 0;
- 59       quaternion.w = 1;
- 60     } else {
- 61       l = 1 / l;
- 62       quaternion.x = quaternion.x * l;
- 63       quaternion.y = quaternion.y * l;
- 64       quaternion.z = quaternion.z * l;
- 65       quaternion.w = quaternion.w * l;
- 66     }
- 67     return quaternion;
- 68   };
- 69 
- 70   /**
- 71    * Convert this quaternion into its inverse.
- 72    * 
- 73    * @returns a pointer to this quaternion
- 74    */
- 75   this.inverse = function() {
- 76     this.conjugate().normalize();
- 77     return quaternion;
- 78   };
- 79 
- 80   /**
- 81    * Set the values of this quaternion to the product of quaternions a and b.
- 82    * 
- 83    * @param a the first quaternion to multiply with
- 84    * @param b the second quaternion to multiply with
- 85    * @returns a pointer to this quaternion
- 86    */
- 87   this.multiply = function(a, b) {
- 88     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;
- 89     this.x = qax * qbw + qay * qbz - qaz * qby + qaw * qbx;
- 90     this.y = -qax * qbz + qay * qbw + qaz * qbx + qaw * qby;
- 91     this.z = qax * qby - qay * qbx + qaz * qbw + qaw * qbz;
- 92     this.w = -qax * qbx - qay * qby - qaz * qbz + qaw * qbw;
- 93     return quaternion;
- 94   };
- 95 
- 96   /**
- 97    * Multiply the given ROSLIB.Vector3 with this quaternion.
- 98    * 
- 99    * @param vector the vector to multiply with
-100    * @param dest (option) - where the computed values will go (defaults to 'vector').
-101    * @returns a pointer to dest
-102    */
-103   this.multiplyVec3 = function(vector, dest) {
-104     if (!dest) {
-105       dest = vector;
-106     }
-107     var x = vector.x, y = vector.y, z = vector.z, qx = quaternion.x, qy = quaternion.y, qz = quaternion.z, qw = quaternion.w;
-108     var ix = qw * x + qy * z - qz * y, iy = qw * y + qz * x - qx * z, iz = qw * z + qx * y - qy * x, iw = -qx
-109         * x - qy * y - qz * z;
-110     dest.x = ix * qw + iw * -qx + iy * -qz - iz * -qy;
-111     dest.y = iy * qw + iw * -qy + iz * -qx - ix * -qz;
-112     dest.z = iz * qw + iw * -qz + ix * -qy - iy * -qx;
-113     return dest;
-114   };
-115 
-116   /**
-117    * Clone a copy of this quaternion.
-118    * 
-119    * @returns the cloned quaternion
-120    */
-121   this.clone = function() {
-122     return new ROSLIB.Quaternion(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
-123   };
-124 };
-125 
\ No newline at end of file diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_math_Vector3.js.html b/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_math_Vector3.js.html deleted file mode 100644 index 7d2862505..000000000 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_math_Vector3.js.html +++ /dev/null @@ -1,76 +0,0 @@ -
  1 /**
-  2  * @author David Gossow - dgossow@willowgarage.com
-  3  */
-  4 
-  5 /**
-  6  * A 3D vector.
-  7  *
-  8  *  @constructor
-  9  *  @param x - the x value 
- 10  *  @param y - the y value
- 11  *  @param z - the z value
- 12  */
- 13 ROSLIB.Vector3 = function(x, y, z) {
- 14   var vector3 = this;
- 15   this.x = x || 0;
- 16   this.y = y || 0;
- 17   this.z = z || 0;
- 18 
- 19   /**
- 20    * Copy the values from the given vector into this vector.
- 21    * 
- 22    * @param v the vector to copy
- 23    * @returns a pointer to this vector
- 24    */
- 25   this.copy = function(v) {
- 26     vector3.x = v.x;
- 27     vector3.y = v.y;
- 28     vector3.z = v.z;
- 29     return vector3;
- 30   };
- 31 
- 32   /**
- 33    * Set the values of this vector to the sum of vectors a and b.
- 34    * 
- 35    * @param a the first vector to add with
- 36    * @param b the second vector to add with
- 37    * @returns a pointer to this vector
- 38    */
- 39   this.add = function(a, b) {
- 40     vector3.x = a.x + b.x;
- 41     vector3.y = a.y + b.y;
- 42     vector3.z = a.z + b.z;
- 43     return vector3;
- 44   };
- 45 
- 46   /**
- 47    * Set the values of this vector to the difference of vectors a and b.
- 48    * 
- 49    * @param a the first vector to add with
- 50    * @param b the second vector to add with
- 51    * @returns a pointer to this vector
- 52    */
- 53   this.sub = function(a, b) {
- 54     vector3.x = a.x - b.x;
- 55     vector3.y = a.y - b.y;
- 56     vector3.z = a.z - b.z;
- 57     return vector3;
- 58   };
- 59 
- 60   /**
- 61    * Clone a copy of this vector.
- 62    * 
- 63    * @returns the cloned vector
- 64    */
- 65   this.clone = function() {
- 66     return new ROSLIB.Vector3(vector3.x, vector3.y, vector3.z);
- 67   };
- 68 };
- 69 
\ No newline at end of file diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_tf_TFClient.js.html b/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_tf_TFClient.js.html deleted file mode 100644 index 2e2db8748..000000000 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_tf_TFClient.js.html +++ /dev/null @@ -1,144 +0,0 @@ -
  1 /**
-  2  * @author David Gossow - dgossow@willowgarage.com
-  3  */
-  4 
-  5 /**
-  6  * A TF Client that listens to TFs from tf2_web_republisher.
-  7  *
-  8  *  @constructor
-  9  *  @param options - object with following keys:
- 10  *   * ros - the ROSLIB.Ros connection handle
- 11  *   * fixedFrame - the fixed frame, like /base_link
- 12  *   * angularThres - the angular threshold for the TF republisher
- 13  *   * transThres - the translation threshold for the TF republisher
- 14  *   * rate - the rate for the TF republisher
- 15  *   * goalUpdateDelay - the goal update delay for the TF republisher
- 16  */
- 17 ROSLIB.TFClient = function(options) {
- 18   var tfClient = this;
- 19   var options = options || {};
- 20   this.ros = options.ros;
- 21   this.fixedFrame = options.fixedFrame || '/base_link';
- 22   this.angularThres = options.angularThres || 2.0;
- 23   this.transThres = options.transThres || 0.01;
- 24   this.rate = options.rate || 10.0;
- 25   this.goalUpdateDelay = options.goalUpdateDelay || 50;
- 26 
- 27   this.currentGoal = false;
- 28   this.frameInfos = {};
- 29   this.goalUpdateRequested = false;
- 30 
- 31   // create an ActionClient
- 32   this.actionClient = new ROSLIB.ActionClient({
- 33     ros : this.ros,
- 34     serverName : '/tf2_web_republisher',
- 35     actionName : 'tf2_web_republisher/TFSubscriptionAction'
- 36   });
- 37 
- 38   /**
- 39    * Process the incoming TF message and send them out using the callback functions.
- 40    * 
- 41    * @param tf - the TF message from the server
- 42    */
- 43   this.processFeedback = function(tf) {
- 44     tf.transforms.forEach(function(transform) {
- 45       var frameID = transform.child_frame_id;
- 46       var info = tfClient.frameInfos[frameID];
- 47       if (info != undefined) {
- 48         info.transform = new ROSLIB.Transform(transform.transform.translation,
- 49             transform.transform.rotation);
- 50         info.cbs.forEach(function(cb) {
- 51           cb(info.transform);
- 52         });
- 53       }
- 54     });
- 55   };
- 56 
- 57   /**
- 58    * Create and send a new goal to the tf2_web_republisher based on the current list of TFs.
- 59    */
- 60   this.updateGoal = function() {
- 61     // Anytime the list of frames changes, we will need to send a new goal.
- 62     if (tfClient.currentGoal) {
- 63       tfClient.currentGoal.cancel();
- 64     }
- 65 
- 66     var goalMessage = {
- 67       source_frames : [],
- 68       target_frame : tfClient.fixedFrame,
- 69       angular_thres : tfClient.angularThres,
- 70       trans_thres : tfClient.transThres,
- 71       rate : tfClient.rate
- 72     };
- 73 
- 74     for (frame in tfClient.frameInfos) {
- 75       goalMessage.source_frames.push(frame);
- 76     }
- 77 
- 78     tfClient.currentGoal = new ROSLIB.Goal({
- 79       actionClient : tfClient.actionClient,
- 80       goalMessage : goalMessage
- 81     });
- 82     tfClient.currentGoal.on('feedback', tfClient.processFeedback.bind(tfClient));
- 83     tfClient.currentGoal.send();
- 84     tfClient.goalUpdateRequested = false;
- 85   };
- 86 
- 87   /**
- 88    * Subscribe to the given TF frame.
- 89    * 
- 90    * @param frameID - the TF frame to subscribe to
- 91    * @param callback - function with params:
- 92    *   * transform - the transform data
- 93    */
- 94   this.subscribe = function(frameID, callback) {
- 95     // make sure the frame id is relative
- 96     if (frameID[0] === '/') {
- 97       frameID = frameID.substring(1);
- 98     }
- 99     // if there is no callback registered for the given frame, create emtpy callback list
-100     if (tfClient.frameInfos[frameID] == undefined) {
-101       tfClient.frameInfos[frameID] = {
-102         cbs : []
-103       };
-104       if (!tfClient.goalUpdateRequested) {
-105         setTimeout(tfClient.updateGoal.bind(tfClient), tfClient.goalUpdateDelay);
-106         tfClient.goalUpdateRequested = true;
-107       }
-108     } else {
-109       // if we already have a transform, call back immediately
-110       if (tfClient.frameInfos[frameID].transform != undefined) {
-111         callback(tfClient.frameInfos[frameID].transform);
-112       }
-113     }
-114     tfClient.frameInfos[frameID].cbs.push(callback);
-115   };
-116 
-117   /**
-118    * Unsubscribe from the given TF frame.
-119    * 
-120    * @param frameID - the TF frame to unsubscribe from
-121    * @param callback - the callback function to remove
-122    */
-123   this.unsubscribe = function(frameID, callback) {
-124     var info = tfClient.frameInfos[frameID];
-125     if (info != undefined) {
-126       var cbIndex = info.cbs.indexOf(callback);
-127       if (cbIndex >= 0) {
-128         info.cbs.splice(cbIndex, 1);
-129         if (info.cbs.length == 0) {
-130           delete tfClient.frameInfos[frameID];
-131         }
-132         tfClient.needUpdate = true;
-133       }
-134     }
-135   };
-136 };
-137 
\ No newline at end of file diff --git a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_tf_Transform.js.html b/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_tf_Transform.js.html deleted file mode 100644 index 749358e79..000000000 --- a/doc/symbols/src/_wg_stor5_rtoris_Documents_WPI_RAIL_ROS_groovy_src_roslibjs_src_tf_Transform.js.html +++ /dev/null @@ -1,70 +0,0 @@ -
  1 /**
-  2  * @author David Gossow - dgossow@willowgarage.com
-  3  */
-  4 
-  5 /**
-  6  * A Transform in 3-space. Values are copied into this object.
-  7  *
-  8  *  @constructor
-  9  *  @param translation - the ROSLIB.Vector3 describing the translation
- 10  *  @param rotation - the ROSLIB.Quaternion describing the rotation
- 11  */
- 12 ROSLIB.Transform = function(translation, rotation) {
- 13   var transform = this;
- 14   // copy the values into this object if they exist
- 15   this.translation = new ROSLIB.Vector3();
- 16   this.rotation = new ROSLIB.Quaternion();
- 17   if (translation !== undefined) {
- 18     this.translation.copy(translation);
- 19   }
- 20   if (rotation !== undefined) {
- 21     this.rotation.copy(rotation);
- 22   }
- 23 
- 24   /**
- 25    * Apply a transform against the given ROSLIB.Pose.
- 26    * 
- 27    * @param pose the pose to transform with
- 28    * @returns a pointer to the pose
- 29    */
- 30   this.apply = function(pose) {
- 31     transform.rotation.multiplyVec3(pose.position);
- 32     pose.position.add(pose.position, transform.translation);
- 33     pose.orientation.multiply(transform.rotation, pose.orientation);
- 34     return pose;
- 35   };
- 36 
- 37   /**
- 38    * Apply an inverse transform against the given ROSLIB.Pose.
- 39    * 
- 40    * @param pose the pose to transform with
- 41    * @returns a pointer to the pose
- 42    */
- 43   this.applyInverse = function(pose) {
- 44     var rotInv = transform.rotation.clone().inverse();
- 45     rotInv.multiplyVec3(pose.position);
- 46     pose.position.sub(pose.position, transform.translation);
- 47     pose.orientation.multiply(rotInv, pose.orientation);
- 48     return pose;
- 49   };
- 50 
- 51   /**
- 52    * Copy the values from the given transform into this transform.
- 53    * 
- 54    * @param transform the transform to copy
- 55    * @returns a pointer to this transform
- 56    */
- 57   this.copy = function(transform) {
- 58     transform.translation.copy(transform.translation);
- 59     transform.rotation.copy(transform.rotation);
- 60     return transform;
- 61   };
- 62 };
- 63 
\ No newline at end of file diff --git a/src/actionlib/ActionClient.js b/src/actionlib/ActionClient.js index 4d51ec3ff..df1a59817 100644 --- a/src/actionlib/ActionClient.js +++ b/src/actionlib/ActionClient.js @@ -19,7 +19,7 @@ * * 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; @@ -36,44 +36,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); } @@ -82,7 +79,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); @@ -91,7 +88,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); @@ -103,9 +100,18 @@ 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); +}; + diff --git a/src/actionlib/Goal.js b/src/actionlib/Goal.js index e58310689..4d8eb4c8f 100644 --- a/src/actionlib/Goal.js +++ b/src/actionlib/Goal.js @@ -13,45 +13,18 @@ * * 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 : { @@ -64,20 +37,47 @@ 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; + +/** + * 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); +}; + diff --git a/src/core/Message.js b/src/core/Message.js index bad20908a..775526ab6 100644 --- a/src/core/Message.js +++ b/src/core/Message.js @@ -1,10 +1,10 @@ /** - * @author Brandon Alexander - balexander@willowgarage.com + * @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. */ diff --git a/src/core/Param.js b/src/core/Param.js index a1d7bb810..b6c720011 100644 --- a/src/core/Param.js +++ b/src/core/Param.js @@ -1,5 +1,5 @@ /** - * @author Brandon Alexander - balexander@willowgarage.com + * @author Brandon Alexander - baalexander@gmail.com */ /** @@ -11,53 +11,53 @@ * * 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. + */ +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 : this.name, + value : JSON.stringify('') + }); + + paramClient.callService(request, function(result) { + var value = JSON.parse(result.value); + callback(value); + }); +}; - /** - * 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' - }); - - var request = new ROSLIB.ServiceRequest({ - name : param.name, - value : JSON.stringify('') - }); - - 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' - }); - - var request = new ROSLIB.ServiceRequest({ - name : param.name, - value : JSON.stringify(value) - }); - - paramClient.callService(request, function() { - }); - }; +/** + * 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 : this.name, + value : JSON.stringify(value) + }); + + paramClient.callService(request, function() { + }); }; + diff --git a/src/core/Ros.js b/src/core/Ros.js index 2dc20e148..ecc933c1a 100644 --- a/src/core/Ros.js +++ b/src/core/Ros.js @@ -1,5 +1,5 @@ /** - * @author Brandon Alexander - balexander@willowgarage.com + * @author Brandon Alexander - baalexander@gmail.com */ /** @@ -16,40 +16,55 @@ * @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: @@ -75,7 +90,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]); } @@ -87,16 +102,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); } }; @@ -110,132 +126,122 @@ 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; - }; - - /** - * Disconnect from the WebSocket server. - */ - this.close = function() { - if (ros.socket) { - ros.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 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); - } - }; - - /** - * 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' - }); + that.socket = new WebSocket(url); + that.socket.onopen = onOpen; + that.socket.onclose = onClose; + that.socket.onerror = onError; + that.socket.onmessage = onMessage; +}; - var request = new ROSLIB.ServiceRequest(); +/** + * Disconnect from the WebSocket server. + */ +ROSLIB.Ros.prototype.close = function() { + if (this.socket) { + this.socket.close(); + } +}; - topicsClient.callService(request, function(result) { - callback(result.topics); - }); +/** + * 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); +}; - /** - * 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' - }); - - var request = new ROSLIB.ServiceRequest(); +/** + * 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); - servicesClient.callService(request, function(result) { - callback(result.services); + if (ros.socket.readyState !== WebSocket.OPEN) { + that.once('connection', function() { + that.socket.send(messageJson); }); - }; + } else { + that.socket.send(messageJson); + } +}; - /** - * 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' - }); +/** + * 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' + }); + + var request = new ROSLIB.ServiceRequest(); + + topicsClient.callService(request, function(result) { + callback(result.topics); + }); +}; - var request = new ROSLIB.ServiceRequest(); - paramsClient.callService(request, function(result) { - callback(result.names); - }); - }; +/** + * 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' + }); + + var request = new ROSLIB.ServiceRequest(); + + servicesClient.callService(request, function(result) { + callback(result.services); + }); +}; - // begin by checking if a URL was given - if (url) { - this.connect(url); - } +/** + * 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' + }); + + var request = new ROSLIB.ServiceRequest(); + paramsClient.callService(request, function(result) { + callback(result.names); + }); }; -ROSLIB.Ros.prototype.__proto__ = EventEmitter2.prototype; + diff --git a/src/core/Service.js b/src/core/Service.js index 9eaee213c..b0409f4bd 100644 --- a/src/core/Service.js +++ b/src/core/Service.js @@ -1,5 +1,5 @@ /** - * @author Brandon Alexander - balexander@willowgarage.com + * @author Brandon Alexander - baalexander@gmail.com */ /** @@ -12,39 +12,39 @@ * * 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); }; + diff --git a/src/core/ServiceRequest.js b/src/core/ServiceRequest.js index 505200c44..7a6fe52fb 100644 --- a/src/core/ServiceRequest.js +++ b/src/core/ServiceRequest.js @@ -9,10 +9,10 @@ * @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]; }); } }; diff --git a/src/core/ServiceResponse.js b/src/core/ServiceResponse.js index 8e612f474..c577ed4cd 100644 --- a/src/core/ServiceResponse.js +++ b/src/core/ServiceResponse.js @@ -9,10 +9,10 @@ * @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]; }); } }; diff --git a/src/core/Topic.js b/src/core/Topic.js index 2d0532628..7c157eb65 100644 --- a/src/core/Topic.js +++ b/src/core/Topic.js @@ -1,14 +1,14 @@ /** - * @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 @@ -18,7 +18,6 @@ * * 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; @@ -38,104 +37,105 @@ 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; + diff --git a/src/math/Pose.js b/src/math/Pose.js index 30cfc7436..6c48f0a90 100644 --- a/src/math/Pose.js +++ b/src/math/Pose.js @@ -10,8 +10,7 @@ * @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) { @@ -20,16 +19,17 @@ 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; }; + diff --git a/src/math/Quaternion.js b/src/math/Quaternion.js index 70a35bedb..4a7cf0ed5 100644 --- a/src/math/Quaternion.js +++ b/src/math/Quaternion.js @@ -17,108 +17,111 @@ 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); }; + diff --git a/src/math/Vector3.js b/src/math/Vector3.js index 3d94fbd56..11b01ce66 100644 --- a/src/math/Vector3.js +++ b/src/math/Vector3.js @@ -11,58 +11,58 @@ * @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); }; + diff --git a/src/tf/TFClient.js b/src/tf/TFClient.js index 0e1992fb2..5f04ef9e0 100644 --- a/src/tf/TFClient.js +++ b/src/tf/TFClient.js @@ -15,7 +15,6 @@ * * 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'; @@ -28,109 +27,113 @@ 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; } - }; + } }; + diff --git a/src/tf/Transform.js b/src/tf/Transform.js index 7f3eeb4dc..5aa63fb96 100644 --- a/src/tf/Transform.js +++ b/src/tf/Transform.js @@ -10,8 +10,6 @@ * @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) { @@ -20,43 +18,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; }; +