diff --git a/src/ros_tf_listener.cpp b/src/ros_tf_listener.cpp
index 7dd625ad7657d47f31d17074200e3f53313e7c26..e852c22e28d85d35fb75ab89808a33118baf2856 100644
--- a/src/ros_tf_listener.cpp
+++ b/src/ros_tf_listener.cpp
@@ -5,5 +5,28 @@
 
 namespace dynamicgraph
 {
+  namespace internal
+  {
+    sot::MatrixHomogeneous& TransformListenerData::getTransform (sot::MatrixHomogeneous& res, int time)
+    {
+      static const ros::Time rosTime(0);
+      try {
+        listener.lookupTransform (toFrame, fromFrame, rosTime, transform);
+      } catch (const tf::TransformException& ex) {
+        res.setIdentity();
+        std::ostringstream oss;
+        oss << "Enable to get transform at time " << time << ": " << ex.what();
+        entity->SEND_WARNING_STREAM_MSG(oss.str());
+        return res;
+      }
+      for (int r = 0; r < 3; ++r) {
+        for (int c = 0; c < 3; ++c)
+          res.linear ()(r,c) = transform.getBasis().getRow(r)[c];
+        res.translation()[r] = transform.getOrigin()[r];
+      }
+      return res;
+    }
+  }
+
   DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosTfListener, "RosTfListener");
 }
diff --git a/src/ros_tf_listener.hh b/src/ros_tf_listener.hh
index 3c072513e9cc8f1ca894d5db59629249bf10f939..acf1d76f8d93cd735a9e462619953d0cd0bd7aac 100644
--- a/src/ros_tf_listener.hh
+++ b/src/ros_tf_listener.hh
@@ -19,15 +19,18 @@ namespace dynamicgraph {
     struct TransformListenerData {
       typedef Signal<sot::MatrixHomogeneous, int> signal_t;
 
+      RosTfListener* entity;
       tf::TransformListener& listener;
       const std::string toFrame, fromFrame;
       tf::StampedTransform transform;
       signal_t signal;
 
-      TransformListenerData (tf::TransformListener& l,
+      TransformListenerData (RosTfListener* e,
+          tf::TransformListener& l,
           const std::string& to, const std::string& from,
           const std::string& signame)
-        : listener (l)
+        : entity (e)
+        , listener (l)
         , toFrame (to)
         , fromFrame (from)
         , signal (signame)
@@ -35,23 +38,7 @@ namespace dynamicgraph {
         signal.setFunction (boost::bind(&TransformListenerData::getTransform, this, _1, _2));
       }
 
-      sot::MatrixHomogeneous& getTransform (sot::MatrixHomogeneous& res, int time)
-      {
-        static const ros::Time rosTime(0);
-        try {
-          listener.lookupTransform (toFrame, fromFrame, rosTime, transform);
-        } catch (const tf::TransformException& ex) {
-          res.setIdentity();
-          ROS_ERROR("Enable to get transform at time %i: %s",time,ex.what());
-          return res;
-        }
-        for (sot::MatrixHomogeneous::Index r = 0; r < 3; ++r) {
-          for (sot::MatrixHomogeneous::Index c = 0; c < 3; ++c)
-            res.linear ()(r,c) = transform.getBasis().getRow(r)[c];
-          res.translation()[r] = transform.getOrigin()[r];
-        }
-        return res;
-      }
+      sot::MatrixHomogeneous& getTransform (sot::MatrixHomogeneous& res, int time);
     };
   } // end of internal namespace.
 
@@ -93,7 +80,7 @@ namespace dynamicgraph {
         signalName % getName () % signame;
 
         TransformListenerData* tld = new TransformListenerData (
-            listener, to, from, signalName.str());
+            this, listener, to, from, signalName.str());
         signalRegistration (tld->signal);
         listenerDatas[signame] = tld;
       }