From 36e0426b7ada49d1a9fff8438b4636bd96366a29 Mon Sep 17 00:00:00 2001
From: Mickael Gaillard <mickael.gaillard@tactfactory.com>
Date: Sun, 28 Aug 2016 19:59:23 +0200
Subject: [PATCH 1/6] Add Javadoc

---
 rcljava/CMakeLists.txt                        |   6 +
 .../main/java/org/ros2/rcljava/Consumer.java  |  19 ++
 .../src/main/java/org/ros2/rcljava/Log.java   |  36 +++
 .../src/main/java/org/ros2/rcljava/Node.java  |  90 +++++--
 .../main/java/org/ros2/rcljava/Publisher.java |  90 ++++++-
 .../java/org/ros2/rcljava/QoSProfile.java     |  30 +++
 .../main/java/org/ros2/rcljava/RCLJava.java   | 233 ++++++++++++------
 .../java/org/ros2/rcljava/Subscription.java   |  74 +++++-
 ...mplementationAlreadyImportedException.java |  34 +++
 .../InvalidRCLJAVAImplementation.java         |  34 +++
 .../NoImplementationAvailableException.java   |  36 +++
 .../exception/NotInitializedException.java    |  33 +++
 12 files changed, 606 insertions(+), 109 deletions(-)
 create mode 100644 rcljava/src/main/java/org/ros2/rcljava/Log.java
 create mode 100644 rcljava/src/main/java/org/ros2/rcljava/QoSProfile.java
 create mode 100644 rcljava/src/main/java/org/ros2/rcljava/exception/ImplementationAlreadyImportedException.java
 create mode 100644 rcljava/src/main/java/org/ros2/rcljava/exception/InvalidRCLJAVAImplementation.java
 create mode 100644 rcljava/src/main/java/org/ros2/rcljava/exception/NoImplementationAvailableException.java
 create mode 100644 rcljava/src/main/java/org/ros2/rcljava/exception/NotInitializedException.java

diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt
index 991af41a..a9920e1d 100644
--- a/rcljava/CMakeLists.txt
+++ b/rcljava/CMakeLists.txt
@@ -87,6 +87,12 @@ set(${PROJECT_NAME}_sources
   "src/main/java/org/ros2/rcljava/Publisher.java"
   "src/main/java/org/ros2/rcljava/Subscription.java"
   "src/main/java/org/ros2/rcljava/Consumer.java"
+  "src/main/java/org/ros2/rcljava/Log.java"
+  "src/main/java/org/ros2/rcljava/QoSProfile.java"
+  "src/main/java/org/ros2/rcljava/exception/ImplementationAlreadyImportedException.java"
+  "src/main/java/org/ros2/rcljava/exception/InvalidRCLJAVAImplementation.java"
+  "src/main/java/org/ros2/rcljava/exception/NoImplementationAvailableException.java"
+  "src/main/java/org/ros2/rcljava/exception/NotInitializedException.java"
 )
 
 add_jar("${PROJECT_NAME}_jar"
diff --git a/rcljava/src/main/java/org/ros2/rcljava/Consumer.java b/rcljava/src/main/java/org/ros2/rcljava/Consumer.java
index 74abbca4..3b2dd690 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/Consumer.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/Consumer.java
@@ -1,5 +1,24 @@
+/* Copyright 2016 Open Source Robotics Foundation, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
 package org.ros2.rcljava;
 
+/**
+ *
+ *
+ * @author Esteve Fernandez <esteve@apache.org>
+ */
 public interface Consumer<T> {
     void accept(T t);
 }
diff --git a/rcljava/src/main/java/org/ros2/rcljava/Log.java b/rcljava/src/main/java/org/ros2/rcljava/Log.java
new file mode 100644
index 00000000..42c3f079
--- /dev/null
+++ b/rcljava/src/main/java/org/ros2/rcljava/Log.java
@@ -0,0 +1,36 @@
+/* Copyright 2016 Open Source Robotics Foundation, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.ros2.rcljava;
+
+import org.ros2.rcljava.exception.NoImplementationAvailableException;
+
+/**
+ * Not define in ROS2.
+ *
+ * @author Mickael Gaillard
+ */
+public class Log {
+
+    public Log() { }
+
+    public void fatal(Exception e) {
+        throw new NoImplementationAvailableException(e);
+    }
+
+    public void info(String string) {
+        throw new NoImplementationAvailableException(new Exception(string));
+    }
+
+}
diff --git a/rcljava/src/main/java/org/ros2/rcljava/Node.java b/rcljava/src/main/java/org/ros2/rcljava/Node.java
index 56e1bac4..e9560c38 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/Node.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/Node.java
@@ -1,12 +1,32 @@
+/* Copyright 2016 Open Source Robotics Foundation, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
 package org.ros2.rcljava;
 
 import java.lang.ref.WeakReference;
-
 import java.util.ArrayList;
 import java.util.List;
 
+/**
+ * <h1>Node ROS2.</h1>
+ * <p></p>
+ * @author Esteve Fernandez <esteve@apache.org>
+ * @author Mickael Gaillard <mick.gaillard@gmail.com>
+ */
 public class Node {
-   static {
+
+    static {
         try {
             System.loadLibrary("rcljavaNode__" + RCLJava.getRMWIdentifier());
         } catch (UnsatisfiedLinkError e) {
@@ -15,35 +35,73 @@ public class Node {
         }
     }
 
-    private long nodeHandle;
-    private List<Subscription> subscriptions = new ArrayList<Subscription>();
+    /** Node handler */
+    private final long nodeHandle;
 
-    public Node(long nodeHandle) {
-        this.nodeHandle = nodeHandle;
-    }
+    /** List of subscriptions */
+    private final List<Subscription<?>> subscriptions;
 
+    // Native call.
     private static native <T> long nativeCreatePublisherHandle(
-        long nodeHandle, Class<T> cls, String topic);
+            long nodeHandle, Class<T> cls, String topic);
 
     private static native <T> long nativeCreateSubscriptionHandle(
         long nodeHandle, Class<T> cls, String topic);
 
-    public <T> Publisher<T> createPublisher(Class<T> cls, String topic) {
-        long publisherHandle = nativeCreatePublisherHandle(this.nodeHandle, cls, topic);
-        Publisher<T> publisher = new Publisher<T>(this.nodeHandle, publisherHandle);
-        RCLJava.publisherReferences.add(new WeakReference<Publisher>(publisher));
+    /**
+     * Constructor of Node.
+     * @param nodeHandle Handler to the node.
+     */
+    public Node(long nodeHandle) {
+        this.nodeHandle = nodeHandle;
+        this.subscriptions = new ArrayList<Subscription<?>>();
+    }
+
+    /**
+     * <h1>Create a new publisher.</h1>
+     *
+     * @param <T> Message definition.
+     * @param message Message class.
+     * @param topic Topic to publish.
+     * @param qos QOS profile.
+     * @return Publisher instance.
+     */
+    public <T> Publisher<T> createPublisher(Class<T> message, String topic,  QoSProfile qos) {
+        long publisherHandle = Node.nativeCreatePublisherHandle(this.nodeHandle, message, topic);
+
+        Publisher<T> publisher = new Publisher<T>(this.nodeHandle, publisherHandle, message, topic, qos);
+        RCLJava.publisherReferences.add(new WeakReference<Publisher<?>>(publisher));
+
         return publisher;
     }
 
-    public <T> Subscription<T> createSubscription(Class<T> cls, String topic, Consumer<T> callback) {
-        long subscriptionHandle = nativeCreateSubscriptionHandle(this.nodeHandle, cls, topic);
+    /**
+     * <h1>Create a new Subscriber with callback.</h1>
+     *
+     * @param <T> Message definition.
+     * @param message Message Class
+     * @param topic Topic to subscribe.
+     * @param callback Function to call on recieve.
+     * @param qos QOS profile.
+     * @return
+     */
+    public <T> Subscription<T> createSubscription(Class<T> message, String topic, Consumer<T> callback, QoSProfile qos) {
+        long subscriptionHandle = Node.nativeCreateSubscriptionHandle(this.nodeHandle, message, topic);
 
-        Subscription<T> subscription = new Subscription<T>(this.nodeHandle, subscriptionHandle, cls, topic, callback);
+        Subscription<T> subscription = new Subscription<T>(this.nodeHandle, subscriptionHandle, message, topic, callback, qos);
         this.subscriptions.add(subscription);
         return subscription;
     }
 
-    public List<Subscription> getSubscriptions() {
+    /**
+     * Get list of Subscriptions.
+     * @return ArrayList of Subscriptions
+     */
+    public List<Subscription<?>> getSubscriptions() {
         return this.subscriptions;
     }
+
+    public Log getLog() {
+        return new Log();
+    }
 }
diff --git a/rcljava/src/main/java/org/ros2/rcljava/Publisher.java b/rcljava/src/main/java/org/ros2/rcljava/Publisher.java
index 12696128..ac642ec0 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/Publisher.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/Publisher.java
@@ -1,7 +1,48 @@
+/* Copyright 2016 Open Source Robotics Foundation, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
 package org.ros2.rcljava;
 
+/**
+ * <h1>Publisher of node.</h1>
+ * <p></p>
+ * @param <T> Message Type.
+ * @author Esteve Fernandez <esteve@apache.org>
+ * @author Mickael Gaillard <mick.gaillard@gmail.com>
+ */
 public class Publisher<T> {
-   static {
+
+    /** Node Handler. */
+    private final long nodeHandle;
+
+    /** Publisher Hander. */
+    private final long publisherHandle;
+
+    /** Message Type. */
+    private final Class<T> messageType;
+
+    /** Topic to publish. */
+    private final String topic;
+
+    /** Quality of Service profil. */
+    private final QoSProfile qosProfile;
+
+    // Native call.
+    private static native <T> void nativePublish(long publisherHandle, T msg);
+    private static native void nativeDispose(long nodeHandle, long publisherHandle);
+
+    static {
         try {
             System.loadLibrary("rcljavaPublisher__" + RCLJava.getRMWIdentifier());
         } catch (UnsatisfiedLinkError e) {
@@ -10,24 +51,51 @@ public class Publisher<T> {
         }
     }
 
-    private long nodeHandle;
-    private long publisherHandle;
-
-    public Publisher(long nodeHandle, long publisherHandle) {
+    /**
+     * Constructor of Publisher.
+     *
+     * @param nodeHandle Node handler initialize.
+     * @param publisherHandle Publisher handler.
+     * @param messageType Message type.
+     * @param topic Topic to publish.
+     * @param qos Quality of Service profile.
+     */
+    public Publisher(long nodeHandle, long publisherHandle, Class<T> messageType, String topic, QoSProfile qosProfile) {
         this.nodeHandle = nodeHandle;
         this.publisherHandle = publisherHandle;
+        this.messageType = messageType;
+        this.topic = topic;
+        this.qosProfile = qosProfile;
     }
 
-    private static native <T> void nativePublish(long publisherHandle, T msg);
-
+    /**
+     * Publish a message.
+     * @param msg Message to publish.
+     */
     public void publish(T msg) {
-        nativePublish(this.publisherHandle, msg);
+        Publisher.nativePublish(this.publisherHandle, msg); //TODO(theos) add qosProfile
     }
 
-    private static native void nativeDispose(
-        long nodeHandle, long publisherHandle);
+    /**
+     * Get message type.
+     * @return
+     */
+    public Class<T> getMsgType() {
+        return this.messageType;
+    }
+
+    /**
+     * Get topic name.
+     * @return
+     */
+    public String getTopic() {
+        return this.topic;
+    }
 
+    /**
+     * Release all Publisher ressource.
+     */
     public void dispose() {
-        nativeDispose(this.nodeHandle, this.publisherHandle);
+        Publisher.nativeDispose(this.nodeHandle, this.publisherHandle);
     }
 }
diff --git a/rcljava/src/main/java/org/ros2/rcljava/QoSProfile.java b/rcljava/src/main/java/org/ros2/rcljava/QoSProfile.java
new file mode 100644
index 00000000..e894ad75
--- /dev/null
+++ b/rcljava/src/main/java/org/ros2/rcljava/QoSProfile.java
@@ -0,0 +1,30 @@
+/* Copyright 2016 Open Source Robotics Foundation, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.ros2.rcljava;
+
+/**
+ * <h1>Quality of Service</h1>
+ * <p><i>TODO Not full implemented !!</i></p>
+ * @author Mickael Gaillard <mickael.gaillard@gmail.com>
+ */
+public class QoSProfile {
+
+    public static final QoSProfile DEFAULT = new QoSProfile();
+
+    public QoSProfile() {
+
+    }
+
+}
diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
index cd8fd36c..28d01fd4 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
@@ -1,21 +1,76 @@
+/* Copyright 2016 Open Source Robotics Foundation, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
 package org.ros2.rcljava;
 
 import java.lang.ref.WeakReference;
-
 import java.util.Queue;
-import java.util.concurrent.BlockingQueue;
 import java.util.concurrent.LinkedBlockingQueue;
 
+import org.ros2.rcljava.exception.NoImplementationAvailableException;
+
 import java.util.Map;
 import java.util.concurrent.ConcurrentSkipListMap;
 
+/**
+ * <h1>ROS2 java client wrapper.</h1>
+ * <p>JNI call of ROS2 c client.</p>
+ *
+ * @author Esteve Fernandez <esteve@apache.org>
+ */
 public class RCLJava {
+
+    /** Global List/Queue of publishers. */
     public static Queue<WeakReference> publisherReferences = new LinkedBlockingQueue();
 
+    /** Current ROS2 middleware implementation use. */
+    private static String rmwImplementation = null;
+
+    /** ROS2 client is initialized. */
+    private static boolean initialized = false;
+
+    /** List of ROS2 middleware supported. */
+    private static final Map<String, String> rmwToTypesupport = new ConcurrentSkipListMap<String, String>() {
+        /** Serial Id */
+        private static final long serialVersionUID = 1L;
+
+        {
+            put("rmw_fastrtps_cpp",         "rosidl_typesupport_introspection_c");
+            put("rmw_opensplice_cpp",       "rosidl_typesupport_opensplice_c");
+            put("rmw_connext_cpp",          "rosidl_typesupport_connext_c");
+            put("rmw_connext_dynamic_cpp",  "rosidl_typesupport_introspection_c");
+        }
+    };
+
+    // Natives definitions
+    private static native void nativeRCLJavaInit();
+    private static native long nativeCreateNodeHandle(String nodeName);
+    private static native boolean nativeOk();
+    private static native String nativeGetRMWIdentifier();
+    private static native void nativeShutdown();
+    private static native long nativeGetZeroInitializedWaitSet();
+    private static native void nativeWaitSetInit(long waitSetHandle, int numberOfSubscriptions, int numberOfGuardConditions, int numberOfTimers);
+    private static native void nativeWaitSetClearSubscriptions(long waitSetHandle);
+    private static native void nativeWaitSetAddSubscription(long waitSetHandle, long subscriptionHandle);
+    private static native void nativeWait(long waitSetHandle);
+    private static native Object nativeTake(long SubscriptionHandle, Class<?> msgType);
+
+    /** Release all ressources at shutdown. */
     static {
         Runtime.getRuntime().addShutdownHook(new Thread() {
             public void run() {
-                for(WeakReference<Publisher> publisherReference : publisherReferences) {
+                for(WeakReference<Publisher<?>> publisherReference : RCLJava.publisherReferences) {
                     if(publisherReference.get() != null) {
                         publisherReference.get().dispose();
                     }
@@ -24,112 +79,140 @@ public void run() {
         });
     }
 
-    private static String rmwImplementation = null;
-    private static boolean initialized = false;
-
-    private static final Map<String, String> rmwToTypesupport = new ConcurrentSkipListMap<String, String>() {{
-        put("rmw_fastrtps_cpp", "rosidl_typesupport_introspection_c");
-        put("rmw_opensplice_cpp", "rosidl_typesupport_opensplice_c");
-        put("rmw_connext_cpp", "rosidl_typesupport_connext_c");
-        put("rmw_connext_dynamic_cpp", "rosidl_typesupport_introspection_c");
-    }};
-
+    /**
+     * <h1>Global initialization of rcl.</h1>
+     * <p>
+     * Unless otherwise noted, this must be called
+     * before using any rcl functions.
+     * </p><p>
+     * This function can only be run once after starting the program, and once
+     * after each call to rcl_shutdown. Repeated calls will fail with
+     * RCL_RET_ALREADY_INIT. This function is not thread safe.
+     * </p>
+     */
     public static void rclJavaInit() {
-        synchronized(RCLJava.class) {
-            if (!initialized) {
+        synchronized (RCLJava.class) {
+            if (!RCLJava.initialized) {
                 if (RCLJava.rmwImplementation == null) {
-                    for(Map.Entry<String, String> entry : rmwToTypesupport.entrySet()) {
+                    for (Map.Entry<String, String> entry : rmwToTypesupport.entrySet()) {
                         try {
-                            setRMWImplementation(entry.getKey());
+                            RCLJava.setRMWImplementation(entry.getKey());
                             break;
-                        } catch(UnsatisfiedLinkError ule) {
-                            // TODO(esteve): handle exception
-                        } catch(Exception e) {
+                        } catch (NoImplementationAvailableException e) {
                             // TODO(esteve): handle exception
                         }
                     }
                 }
+
                 if (RCLJava.rmwImplementation == null) {
                     System.err.println("No RMW implementation found");
                     System.exit(1);
                 } else {
-                    nativeRCLJavaInit();
-                    initialized = true;
+                    RCLJava.nativeRCLJavaInit();
+                    RCLJava.initialized = true;
                 }
             }
         }
     }
 
-    private static native void nativeRCLJavaInit();
-
-    private static native long nativeCreateNodeHandle(String nodeName);
+    /**
+     * Node Reference of native node.
+     *
+     * @param nodeName Name of the node.
+     * @return Instance of Node Reference.
+     */
+    public static Node createNode(String nodeName) {
+        long nodeHandle = RCLJava.nativeCreateNodeHandle(nodeName);
 
-    public static String getTypesupportIdentifier() {
-        String typesupportIdentifier = rmwToTypesupport.get(nativeGetRMWIdentifier());
-        return typesupportIdentifier;
+        return new Node(nodeHandle);
     }
 
-    public static void setRMWImplementation(String rmwImplementation) throws Exception {
-        synchronized(RCLJava.class) {
-            System.loadLibrary("rcljavaRCLJava__" + rmwImplementation);
-            RCLJava.rmwImplementation = rmwImplementation;
+    /**
+     * <h1>Wait for once loop.</h1>
+     * @param node
+     */
+    public static void spinOnce(Node node) {
+        long waitSetHandle = RCLJava.nativeGetZeroInitializedWaitSet();
+
+        RCLJava.nativeWaitSetInit(waitSetHandle, node.getSubscriptions().size(), 0, 0);
+        RCLJava.nativeWaitSetClearSubscriptions(waitSetHandle);
+
+        for(Subscription<?> subscription : node.getSubscriptions()) {
+            RCLJava.nativeWaitSetAddSubscription(waitSetHandle, subscription.getSubscriptionHandle());
         }
-    }
 
-    private static native String nativeGetRMWIdentifier();
+        RCLJava.nativeWait(waitSetHandle);
 
-    public static String getRMWIdentifier() {
-        return nativeGetRMWIdentifier();
+        for(Subscription subscription : node.getSubscriptions()) {
+            Object msg = RCLJava.nativeTake(subscription.getSubscriptionHandle(), subscription.getMsgType());
+            if (msg != null) {
+                subscription.getCallback().accept(msg);
+            }
+        }
     }
 
-    private static native boolean nativeOk();
-
+    /**
+     * Check if native is ready.
+     *
+     * @see rcl_node_is_valid(const rcl_node_t * node);
+     * @return true if the node is valid, else false.
+     */
     public static boolean ok() {
-        return nativeOk();
+        return RCLJava.nativeOk();
     }
 
-    public static Node createNode(String nodeName) {
-        long nodeHandle = nativeCreateNodeHandle(nodeName);
-        Node node = new Node(nodeHandle);
-        return node;
+    /**
+     * <h1>Signal global shutdown of RCLJava.</h1>
+     * <p>
+     * This function does not have to be
+     * called on exit, but does have to be called making a repeat call to
+     * RCLJava.rclJavaInit.
+     * </p><p>
+     * This function can only be called once after each call to RCLJava.rclJavaInit.
+     * </p>
+     */
+    public static void shutdown() {
+        RCLJava.nativeShutdown();
     }
 
-    public static void spinOnce(Node node) {
-        long waitSetHandle = nativeGetZeroInitializedWaitSet();
-
-         nativeWaitSetInit(waitSetHandle, node.getSubscriptions().size(), 0, 0);
-
-         nativeWaitSetClearSubscriptions(waitSetHandle);
-
-         for(Subscription subscription : node.getSubscriptions()) {
-             nativeWaitSetAddSubscription(waitSetHandle, subscription.getSubscriptionHandle());
-         }
-
-         nativeWait(waitSetHandle);
-
-         for(Subscription subscription : node.getSubscriptions()) {
-             Object msg = nativeTake(subscription.getSubscriptionHandle(), subscription.getMsgType());
-             if (msg != null) {
-                 subscription.getCallback().accept(msg);
-             }
-         }
+    /**
+     * Get ROS Midleware indentifier.
+     *
+     * @return indentifier.
+     */
+    public static String getRMWIdentifier() {
+        return RCLJava.nativeGetRMWIdentifier();
     }
 
-    private static native void nativeShutdown();
+    /**
+     * <h1>Get identifier of the ROS2 middleware use.</h1>
+     * <p>TODO rename to list of RMW available.</p>
+     * @return Identifier string of ROS2 middleware.
+     */
+    public static String getTypesupportIdentifier() {
+        String typesupportIdentifier = rmwToTypesupport.get(RCLJava.getRMWIdentifier());
 
-    public static void shutdown() {
-        nativeShutdown();
+        return typesupportIdentifier;
     }
 
-    private static native long nativeGetZeroInitializedWaitSet();
-
-    private static native void nativeWaitSetInit(long waitSetHandle, int numberOfSubscriptions, int numberOfGuardConditions, int numberOfTimers);
-
-    private static native void nativeWaitSetClearSubscriptions(long waitSetHandle);
+    /**
+     * <h1>Switch of ROS2 middleware implementation</h1>
+     * <p>TODO need to check implementation available.</p>
+     * @param rmwImplementation
+     * @throws NoImplementationAvailableException
+     */
+    public static void setRMWImplementation(String rmwImplementation)
+            throws NoImplementationAvailableException {
 
-    private static native void nativeWaitSetAddSubscription(long waitSetHandle, long subscriptionHandle);
-
-    private static native void nativeWait(long waitSetHandle);
-
-    private static native Object nativeTake(long SubscriptionHandle, Class msgType);
+        synchronized(RCLJava.class) {
+            try {
+                System.loadLibrary("rcljavaRCLJava__" + rmwImplementation);
+                RCLJava.rmwImplementation = rmwImplementation;
+            } catch (UnsatisfiedLinkError ule) {
+                throw new NoImplementationAvailableException(ule);
+            } catch (Exception e) {
+                throw new NoImplementationAvailableException(e);
+            }
+        }
+    }
 }
diff --git a/rcljava/src/main/java/org/ros2/rcljava/Subscription.java b/rcljava/src/main/java/org/ros2/rcljava/Subscription.java
index 035e14c8..5843c24d 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/Subscription.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/Subscription.java
@@ -1,30 +1,90 @@
+/* Copyright 2016 Open Source Robotics Foundation, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
 package org.ros2.rcljava;
 
+/**
+ * <h1>Subscrition of node.</h1>
+ * <p></p>
+ * @param <T> Message Type.
+ * @author Esteve Fernandez <esteve@apache.org>
+ * @author Mickael Gaillard <mick.gaillard@gmail.com>
+ */
 public class Subscription<T> {
 
+    /** Node Handler. */
     private long nodeHandle;
+
+    /** Subsciption Hander. */
     private long subscriptionHandle;
-    private Class<T> msgType;
+
+    /** Message Type. */
+    private Class<T> messageType;
+
+    /** Topic subscribed. */
     private String topic;
+
+    /** Callback. */
     private Consumer<T> callback;
 
-    public Subscription(long nodeHandle, long subscriptionHandle, Class<T> msgType, String topic, Consumer<T> callback) {
+    /** Quality of Service profil. */
+    private QoSProfile qosProfile;
+
+    // Native call.
+    private static native void nativeDispose(long nodeHandle, long publisherHandle);
+
+    public Subscription(long nodeHandle, long subscriptionHandle, Class<T> messageType, String topic, Consumer<T> callback, QoSProfile qosProfile) {
         this.nodeHandle = nodeHandle;
         this.subscriptionHandle = subscriptionHandle;
-        this.msgType = msgType;
+        this.messageType = messageType;
         this.topic = topic;
         this.callback = callback;
+        this.qosProfile = qosProfile;
     }
 
+    /**
+     * Get Callback.
+     * @return
+     */
     public Consumer<T> getCallback() {
-        return callback;
+        return this.callback;
     }
 
+    public long getSubscriptionHandle() {
+        return this.subscriptionHandle;
+    }
+
+    /**
+     * Get message type.
+     * @return
+     */
     public Class<T> getMsgType() {
-        return msgType;
+        return this.messageType;
     }
 
-    public long getSubscriptionHandle() {
-        return subscriptionHandle;
+    /**
+     * Get topic name.
+     * @return
+     */
+    public String getTopic() {
+        return this.topic;
+    }
+
+    /**
+     * Release all Publisher ressource.
+     */
+    public void dispose() {
+        Subscription.nativeDispose(this.nodeHandle, this.subscriptionHandle);
     }
 }
diff --git a/rcljava/src/main/java/org/ros2/rcljava/exception/ImplementationAlreadyImportedException.java b/rcljava/src/main/java/org/ros2/rcljava/exception/ImplementationAlreadyImportedException.java
new file mode 100644
index 00000000..d225a55f
--- /dev/null
+++ b/rcljava/src/main/java/org/ros2/rcljava/exception/ImplementationAlreadyImportedException.java
@@ -0,0 +1,34 @@
+/* Copyright 2016 Open Source Robotics Foundation, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.ros2.rcljava.exception;
+
+/**
+ * Raised on select_rmw_implemenation() after import_rmw_implementation() has
+ * been called.
+ * 
+ * @author Mickael Gaillard
+ */
+public class ImplementationAlreadyImportedException extends Exception {
+
+    /**
+     * Constructor.
+     * 
+     * @param cause
+     */
+    public ImplementationAlreadyImportedException(Throwable cause) {
+        super("rmw implementation already imported", cause);
+    }
+
+}
diff --git a/rcljava/src/main/java/org/ros2/rcljava/exception/InvalidRCLJAVAImplementation.java b/rcljava/src/main/java/org/ros2/rcljava/exception/InvalidRCLJAVAImplementation.java
new file mode 100644
index 00000000..e97e8ee8
--- /dev/null
+++ b/rcljava/src/main/java/org/ros2/rcljava/exception/InvalidRCLJAVAImplementation.java
@@ -0,0 +1,34 @@
+/* Copyright 2016 Open Source Robotics Foundation, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.ros2.rcljava.exception;
+
+/**
+ * Raised when an invalid RCLPYImplementation is requested.
+ * 
+ * @author Mickael Gaillard
+ *
+ */
+public class InvalidRCLJAVAImplementation extends Exception {
+
+    /**
+     * Constructor.
+     * 
+     * @param cause
+     */
+    public InvalidRCLJAVAImplementation(Throwable cause) {
+        super("requested invalid rmw implementation", cause);
+    }
+
+}
diff --git a/rcljava/src/main/java/org/ros2/rcljava/exception/NoImplementationAvailableException.java b/rcljava/src/main/java/org/ros2/rcljava/exception/NoImplementationAvailableException.java
new file mode 100644
index 00000000..49305510
--- /dev/null
+++ b/rcljava/src/main/java/org/ros2/rcljava/exception/NoImplementationAvailableException.java
@@ -0,0 +1,36 @@
+/* Copyright 2016 Open Source Robotics Foundation, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.ros2.rcljava.exception;
+
+/**
+ * Raised when there is no rmw implementation with a Java extension available.
+ *
+ * @author Mickael Gaillard <mickael.gaillard@gmail.com>
+ *
+ */
+public class NoImplementationAvailableException extends RuntimeException {
+
+    /** Serial ID */
+    private static final long serialVersionUID = -2351440132432398102L;
+
+    /**
+     * Constructor.
+     *
+     * @param cause
+     */
+    public NoImplementationAvailableException(Throwable cause) {
+        super("no rmw implementation with a Java extension available", cause);
+    }
+}
diff --git a/rcljava/src/main/java/org/ros2/rcljava/exception/NotInitializedException.java b/rcljava/src/main/java/org/ros2/rcljava/exception/NotInitializedException.java
new file mode 100644
index 00000000..d6c1ffa9
--- /dev/null
+++ b/rcljava/src/main/java/org/ros2/rcljava/exception/NotInitializedException.java
@@ -0,0 +1,33 @@
+/* Copyright 2016 Open Source Robotics Foundation, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package org.ros2.rcljava.exception;
+
+/**
+ * Raised when the rcljava implementation is accessed before RclJava().
+ * 
+ * @author Mickael Gaillard
+ *
+ */
+public class NotInitializedException extends Exception {
+
+    /**
+     * Constructor.
+     * 
+     * @param cause
+     */
+    public NotInitializedException(Throwable cause) {
+        super("RclJava() has not been called", cause);
+    }
+}

From bbe8d3a4a7930888f0fe199da1370c92f0e49cfa Mon Sep 17 00:00:00 2001
From: Mickael Gaillard <mickael.gaillard@tactfactory.com>
Date: Mon, 29 Aug 2016 02:28:14 +0200
Subject: [PATCH 2/6] Add QOS profile

---
 .../java/org/ros2/rcljava/QoSProfile.java     | 103 +++++++++++++++++-
 1 file changed, 100 insertions(+), 3 deletions(-)

diff --git a/rcljava/src/main/java/org/ros2/rcljava/QoSProfile.java b/rcljava/src/main/java/org/ros2/rcljava/QoSProfile.java
index e894ad75..c10b0693 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/QoSProfile.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/QoSProfile.java
@@ -15,16 +15,113 @@
 package org.ros2.rcljava;
 
 /**
- * <h1>Quality of Service</h1>
+ * <h1>Quality of Service Profile</h1>
  * <p><i>TODO Not full implemented !!</i></p>
  * @author Mickael Gaillard <mickael.gaillard@gmail.com>
  */
 public class QoSProfile {
 
-    public static final QoSProfile DEFAULT = new QoSProfile();
+    /** Default Depth level */
+    public static final int DEPTH_SYSTEM_DEFAULT = 0;
 
-    public QoSProfile() {
+    /** Reliability Policy enum. */
+    public enum ReliabilityPolicy {
+        SYSTEM_DEFAULT,
+        RELIABLE,
+        BEST_EFFORT
+    }
+
+    /** History Policy enum. */
+    public enum HistoryPolicy {
+        SYSTEM_DEFAULT,
+        KEEP_LAST,
+        KEEP_ALL
+    }
 
+    /** Durability Policy enum. */
+    public enum DurabilityPolicy {
+        SYSTEM_DEFAULT,
+        TRANSIENT_LOCAL,
+        VOLATILE
     }
 
+    /** Sensor QOS Profile */
+    public static final QoSProfile PROFILE_SENSOR_DATA = new QoSProfile(
+            HistoryPolicy.KEEP_LAST,
+            5,
+            ReliabilityPolicy.BEST_EFFORT,
+            DurabilityPolicy.SYSTEM_DEFAULT
+            );
+
+    /** Parameter QOS Profile */
+    public static final QoSProfile PROFILE_PARAMETER = new QoSProfile(
+            HistoryPolicy.KEEP_LAST,
+            1000,
+            ReliabilityPolicy.RELIABLE,
+            DurabilityPolicy.SYSTEM_DEFAULT
+            );
+
+    /** Default QOS Profile */
+    public static final QoSProfile PROFILE_DEFAULT = new QoSProfile(
+            HistoryPolicy.KEEP_ALL,
+            10,
+            ReliabilityPolicy.RELIABLE,
+            DurabilityPolicy.SYSTEM_DEFAULT
+            );
+
+    /** Services default QOS Profile */
+    public static final QoSProfile PROFILE_SERVICES_DEFAULT = new QoSProfile(
+            HistoryPolicy.KEEP_LAST,
+            10,
+            ReliabilityPolicy.RELIABLE,
+            DurabilityPolicy.TRANSIENT_LOCAL
+            );
+
+    /** Parameter events QOS Profile */
+    public static final QoSProfile PROFILE_PARAMETER_EVENTS = new QoSProfile(
+            HistoryPolicy.KEEP_ALL,
+            1000,
+            ReliabilityPolicy.RELIABLE,
+            DurabilityPolicy.SYSTEM_DEFAULT
+            );
+
+    /** System default QOS Profile */
+    public static final QoSProfile PROFILE_SYSTEM_DEFAULT = new QoSProfile(
+            HistoryPolicy.SYSTEM_DEFAULT,
+            DEPTH_SYSTEM_DEFAULT,
+            ReliabilityPolicy.SYSTEM_DEFAULT,
+            DurabilityPolicy.SYSTEM_DEFAULT
+            );
+
+    /** History Policy */
+    protected final HistoryPolicy history;
+
+    /** Depth */
+    protected final int depth;
+
+    /** Reliability Policy */
+    protected final ReliabilityPolicy reliability;
+
+    /** Durability Policy */
+    protected final DurabilityPolicy durability;
+
+    /**
+     * Constuctor.
+     *
+     * @param history
+     * @param depth
+     * @param reliability
+     * @param durability
+     */
+    public QoSProfile(
+            HistoryPolicy history,
+            int depth,
+            ReliabilityPolicy reliability,
+            DurabilityPolicy durability) {
+
+        this.history = history;
+        this.depth = depth;
+        this.reliability = reliability;
+        this.durability = durability;
+    }
 }

From 3d92ae0e0efce04954bf994c7295616df4773407 Mon Sep 17 00:00:00 2001
From: Mickael Gaillard <mickael.gaillard@tactfactory.com>
Date: Mon, 29 Aug 2016 02:28:47 +0200
Subject: [PATCH 3/6] Refactor code

---
 rcljava/src/main/java/org/ros2/rcljava/Node.java     |  9 ++++++---
 .../src/main/java/org/ros2/rcljava/Subscription.java | 12 ++++++------
 2 files changed, 12 insertions(+), 9 deletions(-)

diff --git a/rcljava/src/main/java/org/ros2/rcljava/Node.java b/rcljava/src/main/java/org/ros2/rcljava/Node.java
index e9560c38..274f1c9c 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/Node.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/Node.java
@@ -46,7 +46,7 @@ private static native <T> long nativeCreatePublisherHandle(
             long nodeHandle, Class<T> cls, String topic);
 
     private static native <T> long nativeCreateSubscriptionHandle(
-        long nodeHandle, Class<T> cls, String topic);
+            long nodeHandle, Class<T> cls, String topic);
 
     /**
      * Constructor of Node.
@@ -101,7 +101,10 @@ public List<Subscription<?>> getSubscriptions() {
         return this.subscriptions;
     }
 
-    public Log getLog() {
-        return new Log();
+    /**
+     * Release all Publisher ressource.
+     */
+    public void dispose() {
+        //TODO
     }
 }
diff --git a/rcljava/src/main/java/org/ros2/rcljava/Subscription.java b/rcljava/src/main/java/org/ros2/rcljava/Subscription.java
index 5843c24d..5aa32cff 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/Subscription.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/Subscription.java
@@ -24,22 +24,22 @@
 public class Subscription<T> {
 
     /** Node Handler. */
-    private long nodeHandle;
+    private final long nodeHandle;
 
     /** Subsciption Hander. */
-    private long subscriptionHandle;
+    private final long subscriptionHandle;
 
     /** Message Type. */
-    private Class<T> messageType;
+    private final Class<T> messageType;
 
     /** Topic subscribed. */
-    private String topic;
+    private final String topic;
 
     /** Callback. */
-    private Consumer<T> callback;
+    private final Consumer<T> callback;
 
     /** Quality of Service profil. */
-    private QoSProfile qosProfile;
+    private final QoSProfile qosProfile;
 
     // Native call.
     private static native void nativeDispose(long nodeHandle, long publisherHandle);

From 4ce5c2c76b22033ea831d6c7cb9ccdc280b31ecd Mon Sep 17 00:00:00 2001
From: Mickael Gaillard <mickael.gaillard@tactfactory.com>
Date: Sun, 4 Sep 2016 16:25:37 +0200
Subject: [PATCH 4/6] Move/refactor loadLibrary and added logger.

---
 rcljava/CMakeLists.txt                        |  1 +
 .../java/org/ros2/rcljava/NativeUtils.java    | 34 +++++++++++++
 .../src/main/java/org/ros2/rcljava/Node.java  | 27 ++++++----
 .../main/java/org/ros2/rcljava/Publisher.java | 10 ++--
 .../main/java/org/ros2/rcljava/RCLJava.java   | 51 +++++++++++++++++--
 .../resource/msg.java.template                |  7 +--
 .../rosidl_generator_java/RCLJavaProxy.java   |  9 ++++
 7 files changed, 113 insertions(+), 26 deletions(-)
 create mode 100644 rcljava/src/main/java/org/ros2/rcljava/NativeUtils.java

diff --git a/rcljava/CMakeLists.txt b/rcljava/CMakeLists.txt
index a9920e1d..6015398c 100644
--- a/rcljava/CMakeLists.txt
+++ b/rcljava/CMakeLists.txt
@@ -89,6 +89,7 @@ set(${PROJECT_NAME}_sources
   "src/main/java/org/ros2/rcljava/Consumer.java"
   "src/main/java/org/ros2/rcljava/Log.java"
   "src/main/java/org/ros2/rcljava/QoSProfile.java"
+  "src/main/java/org/ros2/rcljava/NativeUtils.java"
   "src/main/java/org/ros2/rcljava/exception/ImplementationAlreadyImportedException.java"
   "src/main/java/org/ros2/rcljava/exception/InvalidRCLJAVAImplementation.java"
   "src/main/java/org/ros2/rcljava/exception/NoImplementationAvailableException.java"
diff --git a/rcljava/src/main/java/org/ros2/rcljava/NativeUtils.java b/rcljava/src/main/java/org/ros2/rcljava/NativeUtils.java
new file mode 100644
index 00000000..8f84ee19
--- /dev/null
+++ b/rcljava/src/main/java/org/ros2/rcljava/NativeUtils.java
@@ -0,0 +1,34 @@
+package org.ros2.rcljava;
+
+import java.util.Vector;
+
+public abstract class NativeUtils {
+
+    private static java.lang.reflect.Field LIBRARIES;
+
+    static {
+        try {
+            LIBRARIES = ClassLoader.class.getDeclaredField("loadedLibraryNames");
+            LIBRARIES.setAccessible(true);
+        } catch (NoSuchFieldException e) {
+            e.printStackTrace();
+        } catch (SecurityException e) {
+            e.printStackTrace();
+        }
+    }
+
+    @SuppressWarnings("unchecked")
+    public static String[] getLoadedLibraries(final ClassLoader loader) {
+        Vector<String> libraries = new Vector<String>();
+
+        try {
+            libraries = (Vector<String>) LIBRARIES.get(loader);
+        } catch (IllegalArgumentException e) {
+            e.printStackTrace();
+        } catch (IllegalAccessException e) {
+            e.printStackTrace();
+        }
+
+        return libraries.toArray(new String[] {});
+    }
+}
diff --git a/rcljava/src/main/java/org/ros2/rcljava/Node.java b/rcljava/src/main/java/org/ros2/rcljava/Node.java
index 274f1c9c..71bcf1a9 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/Node.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/Node.java
@@ -17,6 +17,7 @@
 import java.lang.ref.WeakReference;
 import java.util.ArrayList;
 import java.util.List;
+import java.util.logging.Logger;
 
 /**
  * <h1>Node ROS2.</h1>
@@ -25,14 +26,10 @@
  * @author Mickael Gaillard <mick.gaillard@gmail.com>
  */
 public class Node {
+    private static Logger logger = Logger.getLogger(RCLJava.LOG_NAME);
 
     static {
-        try {
-            System.loadLibrary("rcljavaNode__" + RCLJava.getRMWIdentifier());
-        } catch (UnsatisfiedLinkError e) {
-            System.err.println("Native code library failed to load.\n" + e);
-            System.exit(1);
-        }
+        RCLJava.loadLibrary("rcljavaNode__" + RCLJava.getRMWIdentifier());
     }
 
     /** Node handler */
@@ -67,6 +64,7 @@ public Node(long nodeHandle) {
      * @return Publisher instance.
      */
     public <T> Publisher<T> createPublisher(Class<T> message, String topic,  QoSProfile qos) {
+        logger.fine("Create Publisher : " + topic);
         long publisherHandle = Node.nativeCreatePublisherHandle(this.nodeHandle, message, topic);
 
         Publisher<T> publisher = new Publisher<T>(this.nodeHandle, publisherHandle, message, topic, qos);
@@ -85,10 +83,21 @@ public <T> Publisher<T> createPublisher(Class<T> message, String topic,  QoSProf
      * @param qos QOS profile.
      * @return
      */
-    public <T> Subscription<T> createSubscription(Class<T> message, String topic, Consumer<T> callback, QoSProfile qos) {
+    public <T> Subscription<T> createSubscription(
+            Class<T> message,
+            String topic,
+            Consumer<T> callback,
+            QoSProfile qos) {
+        logger.fine("Create Subscription : " + topic);
         long subscriptionHandle = Node.nativeCreateSubscriptionHandle(this.nodeHandle, message, topic);
 
-        Subscription<T> subscription = new Subscription<T>(this.nodeHandle, subscriptionHandle, message, topic, callback, qos);
+        Subscription<T> subscription = new Subscription<T>(
+                this.nodeHandle,
+                subscriptionHandle,
+                message,
+                topic,
+                callback,
+                qos);
         this.subscriptions.add(subscription);
         return subscription;
     }
@@ -105,6 +114,6 @@ public List<Subscription<?>> getSubscriptions() {
      * Release all Publisher ressource.
      */
     public void dispose() {
-        //TODO
+        //TODO Implement on JNI
     }
 }
diff --git a/rcljava/src/main/java/org/ros2/rcljava/Publisher.java b/rcljava/src/main/java/org/ros2/rcljava/Publisher.java
index ac642ec0..c8f8b27d 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/Publisher.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/Publisher.java
@@ -43,12 +43,7 @@ public class Publisher<T> {
     private static native void nativeDispose(long nodeHandle, long publisherHandle);
 
     static {
-        try {
-            System.loadLibrary("rcljavaPublisher__" + RCLJava.getRMWIdentifier());
-        } catch (UnsatisfiedLinkError e) {
-            System.err.println("Native code library failed to load.\n" + e);
-            System.exit(1);
-        }
+        RCLJava.loadLibrary("rcljavaPublisher__" + RCLJava.getRMWIdentifier());
     }
 
     /**
@@ -96,6 +91,7 @@ public String getTopic() {
      * Release all Publisher ressource.
      */
     public void dispose() {
-        Publisher.nativeDispose(this.nodeHandle, this.publisherHandle);
+        //TODO implement to JNI
+        //Publisher.nativeDispose(this.nodeHandle, this.publisherHandle);
     }
 }
diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
index 28d01fd4..13341844 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
@@ -17,6 +17,10 @@
 import java.lang.ref.WeakReference;
 import java.util.Queue;
 import java.util.concurrent.LinkedBlockingQueue;
+import java.util.logging.ConsoleHandler;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+import java.util.logging.SimpleFormatter;
 
 import org.ros2.rcljava.exception.NoImplementationAvailableException;
 
@@ -31,6 +35,10 @@
  */
 public class RCLJava {
 
+    public static final String LOG_NAME = RCLJava.class.getName();
+
+    private static Logger logger = Logger.getLogger(LOG_NAME);
+
     /** Global List/Queue of publishers. */
     public static Queue<WeakReference> publisherReferences = new LinkedBlockingQueue();
 
@@ -68,8 +76,16 @@ public class RCLJava {
 
     /** Release all ressources at shutdown. */
     static {
+        logger.setLevel(Level.ALL);
+        ConsoleHandler handler = new ConsoleHandler();
+        handler.setFormatter(new SimpleFormatter());
+        logger.addHandler(handler);
+        handler.setLevel(Level.INFO);
+
+
         Runtime.getRuntime().addShutdownHook(new Thread() {
             public void run() {
+                logger.fine("Shutdown...");
                 for(WeakReference<Publisher<?>> publisherReference : RCLJava.publisherReferences) {
                     if(publisherReference.get() != null) {
                         publisherReference.get().dispose();
@@ -93,19 +109,24 @@ public void run() {
     public static void rclJavaInit() {
         synchronized (RCLJava.class) {
             if (!RCLJava.initialized) {
+                String libpath = System.getProperty("java.library.path");
+                logger.fine("Native Library path : \n" + libpath.replace(':', '\n'));
+
                 if (RCLJava.rmwImplementation == null) {
                     for (Map.Entry<String, String> entry : rmwToTypesupport.entrySet()) {
                         try {
+                            logger.config("Try to load native " + entry.getKey() + "...");
                             RCLJava.setRMWImplementation(entry.getKey());
+                            logger.config(entry.getKey() + " loaded !");
                             break;
                         } catch (NoImplementationAvailableException e) {
-                            // TODO(esteve): handle exception
+                            logger.config(entry.getKey() + " not available ! (" + e.getMessage() + ")");
                         }
                     }
                 }
 
                 if (RCLJava.rmwImplementation == null) {
-                    System.err.println("No RMW implementation found");
+                    logger.severe("No RMW implementation found...");
                     System.exit(1);
                 } else {
                     RCLJava.nativeRCLJavaInit();
@@ -122,6 +143,7 @@ public static void rclJavaInit() {
      * @return Instance of Node Reference.
      */
     public static Node createNode(String nodeName) {
+        logger.fine("Create Node stack : " + nodeName);
         long nodeHandle = RCLJava.nativeCreateNodeHandle(nodeName);
 
         return new Node(nodeHandle);
@@ -172,6 +194,7 @@ public static boolean ok() {
      * </p>
      */
     public static void shutdown() {
+        logger.fine("Shutdown...");
         RCLJava.nativeShutdown();
     }
 
@@ -205,13 +228,33 @@ public static void setRMWImplementation(String rmwImplementation)
             throws NoImplementationAvailableException {
 
         synchronized(RCLJava.class) {
+            String file = "rcljavaRCLJava__" + rmwImplementation;
+            logger.fine("Load native file : lib" + file + ".so");
+
             try {
-                System.loadLibrary("rcljavaRCLJava__" + rmwImplementation);
+                System.loadLibrary(file);
                 RCLJava.rmwImplementation = rmwImplementation;
             } catch (UnsatisfiedLinkError ule) {
                 throw new NoImplementationAvailableException(ule);
             } catch (Exception e) {
-                throw new NoImplementationAvailableException(e);
+                throw new RuntimeException(e);
+            }
+        }
+    }
+
+    /**
+     * <h1>Load Native ROS library</h1>
+     * <p>load from java.library.path .</p>
+     * @param name Name of the library.
+     */
+    public static void loadLibrary(String name) {
+        synchronized(RCLJava.class) {
+            logger.fine("Load native file : lib" + name + ".so");
+            try {
+                System.loadLibrary(name);
+            } catch (UnsatisfiedLinkError e) {
+                System.err.println("Native code library failed to load.\n" + e);
+                System.exit(1);
             }
         }
     }
diff --git a/rosidl_generator_java/resource/msg.java.template b/rosidl_generator_java/resource/msg.java.template
index 57b72dfd..4c18f309 100644
--- a/rosidl_generator_java/resource/msg.java.template
+++ b/rosidl_generator_java/resource/msg.java.template
@@ -12,12 +12,7 @@ import @(field.type.pkg_name).msg.@(field.type.type);
 
 public class @(type_name) {
    static {
-        try {
-            System.loadLibrary("@(spec.base_type.pkg_name)_@(type_name)_s__" + RCLJavaProxy.getTypesupportIdentifier());
-        } catch (UnsatisfiedLinkError e) {
-            System.err.println("Native code library failed to load.\n" + e);
-            System.exit(1);
-        }
+        RCLJavaProxy.loadLibrary("@(spec.base_type.pkg_name)_@(type_name)_s__" + RCLJavaProxy.getTypesupportIdentifier());
     }
 
     public static native long getFromJavaConverter();
diff --git a/rosidl_generator_java/src/main/java/org/ros2/rosidl_generator_java/RCLJavaProxy.java b/rosidl_generator_java/src/main/java/org/ros2/rosidl_generator_java/RCLJavaProxy.java
index 9f7d7bf3..f5d46731 100644
--- a/rosidl_generator_java/src/main/java/org/ros2/rosidl_generator_java/RCLJavaProxy.java
+++ b/rosidl_generator_java/src/main/java/org/ros2/rosidl_generator_java/RCLJavaProxy.java
@@ -27,4 +27,13 @@ public static synchronized String getRMWIdentifier() {
         }
     }
 
+    public static synchronized void loadLibrary(String name) {
+        try {
+            Class c = Class.forName("org.ros2.rcljava.RCLJava");
+            Method m = c.getDeclaredMethod("loadLibrary", String.class);
+            Object o = m.invoke(null, name);
+        } catch(Exception e) {
+            // TODO(esteve): handle exception
+        }
+    }
 }

From 8039e0abc617a51aeb31cbfe558db26f50bde89b Mon Sep 17 00:00:00 2001
From: Mickael Gaillard <mickael.gaillard@tactfactory.com>
Date: Sun, 4 Sep 2016 16:37:04 +0200
Subject: [PATCH 5/6] Added log native library and Fix

---
 rcljava/src/main/java/org/ros2/rcljava/RCLJava.java      | 9 +++++++++
 rcljava/src/main/java/org/ros2/rcljava/Subscription.java | 3 ++-
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
index 13341844..ee8ce5d1 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
@@ -86,6 +86,15 @@ public class RCLJava {
         Runtime.getRuntime().addShutdownHook(new Thread() {
             public void run() {
                 logger.fine("Shutdown...");
+
+                String[] list = NativeUtils.getLoadedLibraries(RCLJava.class.getClassLoader());
+                StringBuilder msgLog = new StringBuilder();
+                for (String key : list) {
+                    msgLog.append(key);
+                    msgLog.append("\n");
+                }
+                logger.fine("Native libraries Loaded: \n" + msgLog.toString());
+
                 for(WeakReference<Publisher<?>> publisherReference : RCLJava.publisherReferences) {
                     if(publisherReference.get() != null) {
                         publisherReference.get().dispose();
diff --git a/rcljava/src/main/java/org/ros2/rcljava/Subscription.java b/rcljava/src/main/java/org/ros2/rcljava/Subscription.java
index 5aa32cff..4f2a7c5b 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/Subscription.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/Subscription.java
@@ -85,6 +85,7 @@ public String getTopic() {
      * Release all Publisher ressource.
      */
     public void dispose() {
-        Subscription.nativeDispose(this.nodeHandle, this.subscriptionHandle);
+        //TODO implement to JNI
+        // Subscription.nativeDispose(this.nodeHandle, this.subscriptionHandle);
     }
 }

From 87456710eec6783f992e59dc38ea6ccacc6d7887 Mon Sep 17 00:00:00 2001
From: Mickael Gaillard <mickael.gaillard@tactfactory.com>
Date: Mon, 12 Sep 2016 17:37:56 +0200
Subject: [PATCH 6/6] Fixed bad type object

---
 rosidl_generator_java/rosidl_generator_java/__init__.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rosidl_generator_java/rosidl_generator_java/__init__.py b/rosidl_generator_java/rosidl_generator_java/__init__.py
index 8b114e22..4e0ebd4f 100644
--- a/rosidl_generator_java/rosidl_generator_java/__init__.py
+++ b/rosidl_generator_java/rosidl_generator_java/__init__.py
@@ -135,7 +135,7 @@ def get_builtin_java_type(type_, use_primitives=True):
         return 'byte' if use_primitives else 'Byte'
 
     if type_ == 'char':
-        return 'char' if use_primitives else 'Char'
+        return 'char' if use_primitives else 'Character'
 
     if type_ == 'float32':
         return 'float' if use_primitives else 'Float'