From 0559fafcd063599d167030547e236fdbb84201d4 Mon Sep 17 00:00:00 2001
From: William Woodall <william@osrfoundation.org>
Date: Wed, 13 Aug 2014 14:44:15 -0700
Subject: [PATCH] added callback to subscriber signature

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

diff --git a/rclcpp/include/rclcpp/Node.h b/rclcpp/include/rclcpp/Node.h
index 7c2f0bb..0580680 100644
--- a/rclcpp/include/rclcpp/Node.h
+++ b/rclcpp/include/rclcpp/Node.h
@@ -40,11 +40,11 @@ public:
   }
 
   template<typename ROSMessage>
-  Subscriber<ROSMessage>* create_subscriber(const char * topic_name)
+  Subscriber<ROSMessage>* create_subscriber(const char * topic_name, typename Subscriber<ROSMessage>::CallbackType callback)
   {
     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, std::string(topic_name));
+    SubscriberInterface *sub = new Subscriber<ROSMessage>(subscriber_handle, std::string(topic_name), callback);
     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 e5c628b..8d73559 100644
--- a/rclcpp/include/rclcpp/Subscriber.h
+++ b/rclcpp/include/rclcpp/Subscriber.h
@@ -25,15 +25,13 @@ public:
   SubscriberInterface(const ros_middleware_interface::SubscriberHandle &subscriber_handle, std::string topic_name)
     : subscriber_handle_(subscriber_handle), topic_name_(topic_name)
   {}
-  virtual void * create_message()
-  {
-    throw std::runtime_error("not implemented - should always use override from subclass");
-  }
-  virtual void delete_message(void * ros_message)
-  {
-    throw std::runtime_error("not implemented - should always use override from subclass");
-  }
 private:
+  virtual void * create_message() = 0;
+
+  virtual void delete_message(void * ros_message) = 0;
+
+  virtual void handle_message(void * ros_message) = 0;
+
   ros_middleware_interface::SubscriberHandle subscriber_handle_;
   std::string topic_name_;
 
@@ -44,20 +42,32 @@ class Subscriber : public SubscriberInterface
 {
   friend class rclcpp::Node;
 public:
-  typedef std::function<void(ROSMessage *)> CallbackType;
-  Subscriber(const ros_middleware_interface::SubscriberHandle &subscriber_handle, std::string topic_name)
-    : SubscriberInterface(subscriber_handle, topic_name)
+  typedef std::function<void(const ROSMessage *)> CallbackType;
+  Subscriber(const ros_middleware_interface::SubscriberHandle &subscriber_handle, std::string topic_name, CallbackType callback)
+    : SubscriberInterface(subscriber_handle, topic_name), callback_(callback)
   {}
+
+private:
   void * create_message()
   {
     return new ROSMessage();
   }
+
   void delete_message(void * ros_message)
   {
     ROSMessage* msg = (ROSMessage*)ros_message;
     delete msg;
     ros_message = 0;
   }
+
+  void handle_message(void * ros_message)
+  {
+    ROSMessage* msg = (ROSMessage*)ros_message;
+    callback_(msg);
+  }
+
+  CallbackType callback_;
+
 };
 
 }
diff --git a/rclcpp/include/rclcpp/executor/SingleThreadExecutor.h b/rclcpp/include/rclcpp/executor/SingleThreadExecutor.h
index 1857ecf..c83ddcc 100644
--- a/rclcpp/include/rclcpp/executor/SingleThreadExecutor.h
+++ b/rclcpp/include/rclcpp/executor/SingleThreadExecutor.h
@@ -85,6 +85,7 @@ public:
               if (taken)
               {
                 std::cout << "- received message on topic: " << subscriber->topic_name_ << std::endl;
+                subscriber->handle_message(ros_msg);
               }
               subscriber->delete_message(ros_msg);
             }