From a1b0a3309825be177f037cff71b704ce47f258eb Mon Sep 17 00:00:00 2001
From: William Woodall <william@osrfoundation.org>
Date: Tue, 12 Aug 2014 21:46:41 -0700
Subject: [PATCH] updated subscriber interface

---
 rclcpp/include/rclcpp/Node.h                          |  3 ++-
 rclcpp/include/rclcpp/Subscriber.h                    | 11 +++++++----
 rclcpp/include/rclcpp/executor/SingleThreadExecutor.h |  4 ++++
 3 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/rclcpp/include/rclcpp/Node.h b/rclcpp/include/rclcpp/Node.h
index b0504a6..7c2f0bb 100644
--- a/rclcpp/include/rclcpp/Node.h
+++ b/rclcpp/include/rclcpp/Node.h
@@ -2,6 +2,7 @@
 #define __rclcpp__Node__h__
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "Publisher.h"
@@ -43,7 +44,7 @@ public:
   {
     const rosidl_generator_cpp::MessageTypeSupportHandle & type_support_handle = ::ros_middleware_interface::get_type_support_handle<ROSMessage>();
     ros_middleware_interface::SubscriberHandle subscriber_handle = ::ros_middleware_interface::create_subscriber(node_handle_, type_support_handle, topic_name);
-    SubscriberInterface *sub = new Subscriber<ROSMessage>(subscriber_handle);
+    SubscriberInterface *sub = new Subscriber<ROSMessage>(subscriber_handle, std::string(topic_name));
     this->subscribers_.push_back(sub);
     return static_cast<Subscriber<ROSMessage> *>(sub);
   }
diff --git a/rclcpp/include/rclcpp/Subscriber.h b/rclcpp/include/rclcpp/Subscriber.h
index fb9dbe6..4f05d5a 100644
--- a/rclcpp/include/rclcpp/Subscriber.h
+++ b/rclcpp/include/rclcpp/Subscriber.h
@@ -2,6 +2,7 @@
 #define __rclcpp__Subscriber__h__
 
 #include <functional>
+#include <string>
 
 #include "ros_middleware_interface/functions.h"
 #include "ros_middleware_interface/handles.h"
@@ -21,11 +22,13 @@ class SubscriberInterface
 {
   friend class rclcpp::executor::SingleThreadExecutor;
 public:
-  SubscriberInterface(const ros_middleware_interface::SubscriberHandle &subscriber_handle)
-    : subscriber_handle_(subscriber_handle)
+  SubscriberInterface(const ros_middleware_interface::SubscriberHandle &subscriber_handle, std::string topic_name)
+    : subscriber_handle_(subscriber_handle), topic_name_(topic_name)
   {}
 private:
   ros_middleware_interface::SubscriberHandle subscriber_handle_;
+  std::string topic_name_;
+
 };
 
 template<typename ROSMessage>
@@ -34,8 +37,8 @@ class Subscriber : public SubscriberInterface
   friend class rclcpp::Node;
 public:
   typedef std::function<void(ROSMessage *)> CallbackType;
-  Subscriber(const ros_middleware_interface::SubscriberHandle &subscriber_handle)
-    : SubscriberInterface(subscriber_handle)
+  Subscriber(const ros_middleware_interface::SubscriberHandle &subscriber_handle, std::string topic_name)
+    : SubscriberInterface(subscriber_handle, topic_name)
   {}
 };
 
diff --git a/rclcpp/include/rclcpp/executor/SingleThreadExecutor.h b/rclcpp/include/rclcpp/executor/SingleThreadExecutor.h
index 5f377dd..f4accc1 100644
--- a/rclcpp/include/rclcpp/executor/SingleThreadExecutor.h
+++ b/rclcpp/include/rclcpp/executor/SingleThreadExecutor.h
@@ -3,6 +3,7 @@
 
 #include <algorithm>
 #include <cassert>
+#include <iostream>
 #include <vector>
 
 #include "rclcpp/Node.h"
@@ -78,6 +79,9 @@ public:
             if (subscriber->subscriber_handle_.data_ == handle)
             {
               // Do callback
+              std::cout << "Callback for subscriber of topic: " << subscriber->topic_name_ << std::endl;
+              const void *ros_msg = 0;
+              ros_middleware_interface::take(subscriber->subscriber_handle_, ros_msg);
             }
           }
         }