Full Service model integration.

Author: neocoretechs

src/main/java/org/ros/internal/node/service/DefaultServiceClient.java

@@ -1,9 +1,8 @@
 package org.ros.internal.node.service;
 
-
-import org.ros.exception.RosRuntimeException;
+import org.apache.commons.logging.Log;
+import org.apache.commons.logging.LogFactory;
 import org.ros.internal.message.MessageBufferPool;
-import org.ros.internal.system.Utility;
 import org.ros.internal.transport.ClientHandshakeListener;
 import org.ros.internal.transport.ConnectionHeader;
 import org.ros.internal.transport.ConnectionHeaderFields;
@@ -14,9 +13,10 @@
 import org.ros.node.service.ServiceClient;
 import org.ros.node.service.ServiceResponseListener;
 
+//import rosgraph_msgs.Log;
+
 import java.io.IOException;
 import java.net.InetSocketAddress;
-import java.nio.ByteBuffer;
 import java.util.LinkedList;
 import java.util.Queue;
 import java.util.concurrent.CountDownLatch;
@@ -29,7 +29,8 @@
  * @author jg
  */
 public class DefaultServiceClient<T, S> implements ServiceClient<T, S> {
-
+  private static final boolean DEBUG = true;
+  private final Log log = LogFactory.getLog(this.getClass());
   private final class HandshakeLatch implements ClientHandshakeListener {
 
     private CountDownLatch latch;
@@ -112,14 +113,20 @@ public void connect(InetSocketAddress inetSocketAddress) throws Exception {
     try {
 		tcpClient = tcpClientManager.connect(toString(), inetSocketAddress);
 	} catch (IOException e1) {
-		throw new RosRuntimeException("TcpClient failed to connect to remote "+toString()+" "+inetSocketAddress, e1);
+		log.error(this.toString()+" failed to connect to server at:"+inetSocketAddress+
+				" using context:"+tcpClient.getContext()+" due to IOException:"+e1.getMessage());
+		throw new Exception(this.toString()+" failed to connect to server at address:"+inetSocketAddress, e1);
 	}
     try {
       if (!handshakeLatch.await(1, TimeUnit.SECONDS)) {
-        throw new RosRuntimeException(handshakeLatch.getErrorMessage());
+  		log.error(this.toString()+" failed to connect to server at:"+inetSocketAddress+
+				" using context:"+tcpClient.getContext()+" due to IOException:"+handshakeLatch.getErrorMessage());
+        throw new Exception("Handshake Latch:"+handshakeLatch.toString()+" TIMED OUT with error message:"+handshakeLatch.getErrorMessage());
       }
     } catch (InterruptedException e) {
-      throw new RosRuntimeException("Handshake timed out.");
+ 		log.error(this.toString()+" failed to connect to server at:"+inetSocketAddress+
+				" using context:"+tcpClient.getContext()+" due to IOException:"+handshakeLatch.getErrorMessage());
+      throw new Exception("Handshake Latch:"+handshakeLatch.toString()+" INTERRUPTED with error message:"+handshakeLatch.getErrorMessage());
     }
   }
 
@@ -138,7 +145,8 @@ public void call(T request, ServiceResponseListener<S> listener) {
     try {
 		tcpClient.getContext().write(request);
 	} catch (IOException e) {
-		
+		log.error(this.toString()+" failed on call to server with request:"+request+
+				" using context:"+tcpClient.getContext()+" due to IOException:"+e.getMessage());
 		e.printStackTrace();
 	}
     //messageBufferPool.release(buffer);
@@ -151,7 +159,7 @@ public GraphName getName() {
 
   @Override
   public String toString() {
-    return "ServiceClient<" + serviceDeclaration + ">";
+    return "DefaultServiceClient<" + serviceDeclaration + ">";
   }
 
   @Override

src/main/java/org/ros/internal/node/service/DefaultServiceServer.java

@@ -80,7 +80,7 @@ public void onMasterUnregistrationFailure(ServiceServer<T, S> registrant) {
     });
   }
 
-  public ByteBuffer finishHandshake(ConnectionHeader incomingConnectionHeader) {
+  public ConnectionHeader finishHandshake(ConnectionHeader incomingConnectionHeader) {
     if (DEBUG) {
       log.info("Client handshake header: " + incomingConnectionHeader);
     }
@@ -93,9 +93,9 @@ public ByteBuffer finishHandshake(ConnectionHeader incomingConnectionHeader) {
     if (DEBUG) {
       log.info("Server handshake header: " + connectionHeader);
     }
-    ByteBuffer headbuf = MessageBuffers.dynamicBuffer();
-    Utility.serialize(connectionHeader, headbuf);
-    return headbuf;
+    //ByteBuffer headbuf = MessageBuffers.dynamicBuffer();
+    //Utility.serialize(connectionHeader, headbuf);
+    return connectionHeader;
   }
 
   @Override

src/main/java/org/ros/internal/transport/tcp/TcpServerHandshakeHandler.java

@@ -76,6 +76,7 @@ private void handleServiceHandshake(ChannelHandlerContext ctx, ConnectionHeader
     GraphName serviceName = GraphName.of(incomingHeader.getField(ConnectionHeaderFields.SERVICE));
     assert(serviceManager.hasServer(serviceName));
     DefaultServiceServer<?, ?> serviceServer = serviceManager.getServer(serviceName);
+    // finishHandshake previously returned ByteBuffer, now returns ConnectionHeader
     ctx.write(serviceServer.finishHandshake(incomingHeader));
     String probe = incomingHeader.getField(ConnectionHeaderFields.PROBE);
     if (probe != null && probe.equals("1")) {

src/main/java/org/ros/master/client/MasterStateClient.java

@@ -59,15 +59,15 @@ public MasterStateClient(Node caller, InetSocketAddress masterUri) throws IOExce
   /**
    * @param nodeName
    *          the name of the {@link Node} to lookup
-   * @return the {@link URI} of the {@link Node} with the given name
+   * @return the {@link InetSocketAddress} of the {@link Node} with the given name
    */
-  public URI lookupNode(String nodeName) {
+  public InetSocketAddress lookupNode(String nodeName) {
     Response<InetSocketAddress> response = masterClient.lookupNode(caller.getName(), nodeName);
     return response.getResult();
   }
 
   /**
-   * @return the {@link URI} of the {@link MasterServer}
+   * @return the {@link InetSocketAddress0} of the {@link MasterServer}
    */
   public InetSocketAddress getUri() {
     Response<InetSocketAddress> response = masterClient.getUri(caller.getName());
@@ -77,7 +77,7 @@ public InetSocketAddress getUri() {
   /**
    * @param serviceName
    *          the name of the {@link ServiceServer} to look up
-   * @return the {@link URI} of the {@link ServiceServer} with the given name
+   * @return the {@link InetSocketAddress} of the {@link ServiceServer} with the given name
    */
   public InetSocketAddress lookupService(String serviceName) {
     Response<InetSocketAddress> result = masterClient.lookupService(caller.getName(), serviceName);