From 7421ab8f6fe92080acf8b2ed64e15c941f29b7a8 Mon Sep 17 00:00:00 2001
From: Chris Smith <csmith@rethinkrobotics.com>
Date: Thu, 4 Feb 2016 15:13:55 -0500
Subject: [PATCH] Update roslibjs to advertise Services

---
 src/core/Service.js       | 62 ++++++++++++++++++++++++++++++++++++++-
 src/core/SocketAdapter.js |  2 ++
 2 files changed, 63 insertions(+), 1 deletion(-)

diff --git a/src/core/Service.js b/src/core/Service.js
index 17467b11d..89cc1baa5 100644
--- a/src/core/Service.js
+++ b/src/core/Service.js
@@ -4,6 +4,8 @@
  */
 
 var ServiceResponse = require('./ServiceResponse');
+var ServiceRequest = require('./ServiceRequest');
+var EventEmitter2 = require('eventemitter2').EventEmitter2;
 
 /**
  * A ROS service client.
@@ -19,8 +21,11 @@ function Service(options) {
   this.ros = options.ros;
   this.name = options.name;
   this.serviceType = options.serviceType;
-}
+  this.isAdvertised = false;
 
+  this._serviceCallback = null;
+}
+Service.prototype.__proto__ = EventEmitter2.prototype;
 /**
  * Calls the service. Returns the service response in the callback.
  *
@@ -31,6 +36,10 @@ function Service(options) {
  *   * error - the error message reported by ROS
  */
 Service.prototype.callService = function(request, callback, failedCallback) {
+  if (this.isAdvertised) {
+    return;
+  }
+
   var serviceCallId = 'call_service:' + this.name + ':' + (++this.ros.idCounter);
 
   if (callback || failedCallback) {
@@ -54,4 +63,55 @@ Service.prototype.callService = function(request, callback, failedCallback) {
   this.ros.callOnConnection(call);
 };
 
+/**
+ * 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
+ */
+Service.prototype.advertise = function(callback) {
+  if (this.isAdvertised || typeof callback !== 'function') {
+    return;
+  }
+
+  this._serviceCallback = callback;
+  this.ros.on(this.name, this._serviceResponse.bind(this));
+  this.ros.callOnConnection({
+    op: 'advertise_service',
+    type: this.serviceType,
+    service: this.name
+  });
+  this.isAdvertised = true;
+};
+
+Service.prototype.unadvertise = function() {
+  if (!this.isAdvertised) {
+    return;
+  }
+  this.ros.callOnConnection({
+    op: 'unadvertise_service',
+    service: this.name
+  });
+  this.isAdvertised = false;
+};
+
+Service.prototype._serviceResponse = function(rosbridgeRequest) {
+  var response = {};
+  var success = this._serviceCallback(rosbridgeRequest.args, response);
+
+  var call = {
+    op: 'service_response',
+    service: this.name,
+    values: new ServiceResponse(response),
+    result: success
+  };
+
+  if (rosbridgeRequest.id) {
+    call.id = rosbridgeRequest.id;
+  }
+
+  this.ros.callOnConnection(call);
+};
+
 module.exports = Service;
\ No newline at end of file
diff --git a/src/core/SocketAdapter.js b/src/core/SocketAdapter.js
index 728fd8917..f9425b1be 100644
--- a/src/core/SocketAdapter.js
+++ b/src/core/SocketAdapter.js
@@ -29,6 +29,8 @@ function SocketAdapter(client) {
       client.emit(message.topic, message.msg);
     } else if (message.op === 'service_response') {
       client.emit(message.id, message);
+    } else if (message.op === 'call_service') {
+      client.emit(message.service, message);
     }
   }