major refactor of rclcpp to support executors

rclcpp/include/rclcpp/Node.h

@@ -1,66 +0,0 @@
-#ifndef __rclcpp__Node__h__
-#define __rclcpp__Node__h__
-
-#include <memory>
-#include <string>
-#include <vector>
-
-#include "Publisher.h"
-#include "Subscriber.h"
-
-#include "ros_middleware_interface/functions.h"
-#include "ros_middleware_interface/handles.h"
-
-#include "ros_middleware_interface/get_type_support_handle.h"
-
-
-namespace rclcpp
-{
-
-namespace executor
-{
-  class SingleThreadExecutor;
-}
-
-class Node
-{
-  friend class rclcpp::executor::SingleThreadExecutor;
-public:
-  Node()
-  {
-    node_handle_ = ::ros_middleware_interface::create_node();
-  }
-
-  template<typename ROSMessage>
-  Publisher<ROSMessage>* create_publisher(const char * topic_name)
-  {
-    const rosidl_generator_cpp::MessageTypeSupportHandle & type_support_handle = ::ros_middleware_interface::get_type_support_handle<ROSMessage>();
-    ros_middleware_interface::PublisherHandle publisher_handle = ::ros_middleware_interface::create_publisher(node_handle_, type_support_handle, topic_name);
-    return new Publisher<ROSMessage>(publisher_handle);
-  }
-
-  template<typename ROSMessage>
-  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), callback);
-    this->subscribers_.push_back(sub);
-    return static_cast<Subscriber<ROSMessage> *>(sub);
-  }
-
-private:
-  ros_middleware_interface::NodeHandle node_handle_;
-  std::vector<SubscriberInterface *> subscribers_;
-
-};
-
-rclcpp::Node* create_node()
-{
-  return new rclcpp::Node();
-}
-
-} /* namespace rclcpp */
-
-
-#endif  // __rclcpp__Node__h__

rclcpp/include/rclcpp/Publisher.h

@@ -1,30 +0,0 @@
-#ifndef __rclcpp__Publisher__h__
-#define __rclcpp__Publisher__h__
-
-#include "ros_middleware_interface/functions.h"
-#include "ros_middleware_interface/handles.h"
-
-
-namespace rclcpp
-{
-
-template<typename ROSMessage>
-class Publisher
-{
-public:
-  Publisher(const ros_middleware_interface::PublisherHandle& publisher_handle)
-    : publisher_handle_(publisher_handle)
-  {}
-
-  void publish(const ROSMessage& ros_message)
-  {
-    ::ros_middleware_interface::publish(publisher_handle_, &ros_message);
-  }
-
-private:
-  ros_middleware_interface::PublisherHandle publisher_handle_;
-};
-
-}
-
-#endif  // __rclcpp__Publisher__h__

rclcpp/include/rclcpp/Subscriber.h

@@ -1,75 +0,0 @@
-#ifndef __rclcpp__Subscriber__h__
-#define __rclcpp__Subscriber__h__
-
-#include <functional>
-#include <string>
-
-#include "ros_middleware_interface/functions.h"
-#include "ros_middleware_interface/handles.h"
-
-
-namespace rclcpp
-{
-
-class Node;
-
-namespace executor
-{
-  class SingleThreadExecutor;
-}
-
-class SubscriberInterface
-{
-  friend class rclcpp::executor::SingleThreadExecutor;
-public:
-  SubscriberInterface(const ros_middleware_interface::SubscriberHandle &subscriber_handle, std::string topic_name)
-    : subscriber_handle_(subscriber_handle), topic_name_(topic_name)
-  {}
-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_;
-
-};
-
-template<typename ROSMessage>
-class Subscriber : public SubscriberInterface
-{
-  friend class rclcpp::Node;
-public:
-  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_;
-
-};
-
-}
-
-#endif  // __rclcpp__Subscriber__h__

rclcpp/include/rclcpp/callback_group.hpp

@@ -0,0 +1,64 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_
+#define RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_
+
+#include <string>
+#include <vector>
+
+#include <rclcpp/subscription.hpp>
+
+namespace rclcpp
+{
+
+// Forward declarations for friend statement in class CallbackGroup
+namespace node {class Node;}
+namespace executor {class Executor;}
+
+namespace callback_group
+{
+
+enum class CallbackGroupType {NonThreadSafe, ThreadSafe};
+
+class CallbackGroup
+{
+  friend class rclcpp::node::Node;
+  friend class rclcpp::executor::Executor;
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup);
+
+private:
+  CallbackGroup(std::string group_name, CallbackGroupType group_type)
+    : name_(group_name), type_(group_type)
+  {}
+  RCLCPP_DISABLE_COPY(CallbackGroup);
+
+  void
+  add_subscription(subscription::SubscriptionBase::SharedPtr &subscription_ptr)
+  {
+    subscription_ptrs_.push_back(subscription_ptr);
+  }
+
+  std::string name_;
+  CallbackGroupType type_;
+  std::vector<subscription::SubscriptionBase::SharedPtr> subscription_ptrs_;
+
+};
+
+} /* namespace callback_group */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ */

rclcpp/include/rclcpp/context.hpp

@@ -0,0 +1,44 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_CONTEXT_HPP_
+#define RCLCPP_RCLCPP_CONTEXT_HPP_
+
+#include <memory>
+
+#include <rclcpp/macros.hpp>
+
+namespace rclcpp
+{
+namespace context
+{
+
+/* ROS Context */
+class Context
+{
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(Context);
+
+  Context() {}
+
+private:
+  RCLCPP_DISABLE_COPY(Context);
+
+};
+
+} /* namespace context */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_CONTEXT_HPP_ */

rclcpp/include/rclcpp/contexts/default_context.hpp

@@ -0,0 +1,41 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_
+#define RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_
+
+#include <rclcpp/context.hpp>
+
+namespace rclcpp
+{
+namespace contexts
+{
+namespace default_context
+{
+
+class DefaultContext : public rclcpp::context::Context
+{
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(DefaultContext);
+
+  DefaultContext() {}
+
+};
+
+}
+}
+}
+
+#endif /* RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ */

rclcpp/include/rclcpp/executor.hpp

@@ -0,0 +1,293 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_EXECUTOR_HPP_
+#define RCLCPP_RCLCPP_EXECUTOR_HPP_
+
+#include <iostream>
+
+#include <algorithm>
+#include <cassert>
+#include <cstdlib>
+#include <list>
+#include <memory>
+#include <vector>
+
+#include <rclcpp/macros.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/utilities.hpp>
+
+namespace rclcpp
+{
+namespace executor
+{
+
+class Executor
+{
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(Executor);
+
+  Executor()
+  {
+    subscriber_handles_.subscriber_count_ = 0;
+    subscriber_handles_.subscribers_ = 0;
+  }
+  ~Executor() {}
+
+  void add_node(rclcpp::node::Node::SharedPtr &node_ptr)
+  {
+    this->weak_nodes_.push_back(node_ptr);
+  }
+
+protected:
+  static void execute_subscription(
+    rclcpp::subscription::SubscriptionBase::SharedPtr &subscription)
+  {
+    std::shared_ptr<void> message = subscription->create_message();
+    bool taken = ros_middleware_interface::take(
+      subscription->subscription_handle_,
+      message.get());
+    if (taken)
+    {
+      subscription->handle_message(message);
+    }
+    else
+    {
+      std::cout << "[rclcpp::error] take failed for subscription on topic: "
+                << subscription->get_topic_name()
+                << std::endl;
+    }
+  }
+
+  void reset_subscriber_handles()
+  {
+    if (subscriber_handles_.subscriber_count_ != 0)
+    {
+      subscriber_handles_.subscriber_count_ = 0;
+      std::free(subscriber_handles_.subscribers_);
+    }
+  }
+
+  void populate_subscriber_handles_with_node(rclcpp::node::Node &node)
+  {
+    // Use the number of subscriptions to pre allocate subscriber_handles
+    subscriber_handles_.subscriber_count_ = node.number_of_subscriptions_;
+    subscriber_handles_.subscribers_ = static_cast<void **>(
+      std::malloc(sizeof(void *) * node.number_of_subscriptions_));
+    // Add subscriptions from groups
+    size_t handles_index = 0;
+    for (auto group : node.callback_groups_)
+    {
+      for (auto subscription : group->subscription_ptrs_)
+      {
+        assert(handles_index < node.number_of_subscriptions_);
+        subscriber_handles_.subscribers_[handles_index] = \
+          subscription->subscription_handle_.data_;
+        handles_index += 1;
+      }
+    }
+    assert(handles_index == node.number_of_subscriptions_);
+  }
+
+  void populate_subscriber_handles()
+  {
+    // Calculate the number of subscriptions and create shared_ptrs
+    size_t number_of_subscriptions = 0;
+    std::vector<rclcpp::node::Node::SharedPtr> nodes;
+    bool has_invalid_weak_nodes = false;
+    for (auto &weak_node : weak_nodes_)
+    {
+      auto node = weak_node.lock();
+      if (!node)
+      {
+        has_invalid_weak_nodes = false;
+        continue;
+      }
+      nodes.push_back(node);
+      number_of_subscriptions += node->number_of_subscriptions_;
+    }
+    // Clean up any invalid nodes, if they were detected
+    if (has_invalid_weak_nodes)
+    {
+      weak_nodes_.erase(remove_if(weak_nodes_.begin(), weak_nodes_.end(),
+                        [](std::weak_ptr<rclcpp::node::Node> i)
+                        {
+                          return i.expired();
+                        }));
+    }
+    // Use the number of subscriptions to pre allocate subscriber_handles
+    subscriber_handles_.subscriber_count_ = number_of_subscriptions;
+    subscriber_handles_.subscribers_ = static_cast<void **>(
+      std::malloc(sizeof(void *) * number_of_subscriptions));
+    if (subscriber_handles_.subscribers_ == NULL)
+    {
+      // TODO: Use a different error here? maybe std::bad_alloc?
+      throw std::runtime_error("Could not malloc for subscriber pointers.");
+    }
+    // Now fill the subscriber_handles with subscribers from the nodes
+    size_t handles_index = 0;
+    for (auto &node : nodes)
+    {
+      for (auto group : node->callback_groups_)
+      {
+        for (auto subscription : group->subscription_ptrs_)
+        {
+          assert(handles_index < number_of_subscriptions);
+          subscriber_handles_.subscribers_[handles_index] = \
+            subscription->subscription_handle_.data_;
+          handles_index += 1;
+        }
+      }
+    }
+    assert(handles_index == number_of_subscriptions);
+  }
+
+  struct AnyExecutable
+  {
+    AnyExecutable() : subscription(0), guard_condition_handle(0) {}
+    rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
+    ros_middleware_interface::GuardConditionHandle *guard_condition_handle;
+  };
+
+  std::list<void *> get_ready_subscriber_handles()
+  {
+    std::list<void *> ready_subscriber_handles;
+    for (size_t i = 0; i < subscriber_handles_.subscriber_count_; ++i)
+    {
+      void *handle = subscriber_handles_.subscribers_[i];
+      if (!handle)
+      {
+        continue;
+      }
+      ready_subscriber_handles.push_back(handle);
+    }
+    return ready_subscriber_handles;
+  }
+
+  rclcpp::subscription::SubscriptionBase::SharedPtr get_subscription_by_handle(
+    void * subscriber_handle)
+  {
+    for (auto weak_node : weak_nodes_)
+    {
+      auto node = weak_node.lock();
+      if (!node)
+      {
+        continue;
+      }
+      for (auto group : node->callback_groups_)
+      {
+        for (auto subscription : group->subscription_ptrs_)
+        {
+          if (subscription->subscription_handle_.data_ == subscriber_handle)
+          {
+            return subscription;
+          }
+        }
+      }
+    }
+    return rclcpp::subscription::SubscriptionBase::SharedPtr();
+  }
+
+  void remove_subscriber_handle_from_subscriber_handles_(void *handle)
+  {
+    for (size_t i = 0; i < subscriber_handles_.subscriber_count_; ++i)
+    {
+      if (handle == subscriber_handles_.subscribers_[i])
+      {
+        // TODO: use nullptr here?
+        subscriber_handles_.subscribers_[i] = NULL;
+      }
+    }
+  }
+
+  rclcpp::subscription::SubscriptionBase::SharedPtr get_next_subscription(
+    std::list<void *> &ready_subscriber_handles)
+  {
+    if (ready_subscriber_handles.size() == 0)
+    {
+      return rclcpp::subscription::SubscriptionBase::SharedPtr();
+    }
+    while (ready_subscriber_handles.size() != 0)
+    {
+      void *handle = ready_subscriber_handles.front();
+      ready_subscriber_handles.pop_front();
+      remove_subscriber_handle_from_subscriber_handles_(handle);
+      auto subscription = get_subscription_by_handle(handle);
+      if (subscription) return subscription;
+    }
+    return rclcpp::subscription::SubscriptionBase::SharedPtr();
+  }
+
+  std::shared_ptr<AnyExecutable> get_next_executable(bool nonblocking=false)
+  {
+    namespace rmi = ros_middleware_interface;
+    // Get any ready subscriber handles out of subscriber_handles_.
+    // TODO: operate directly on subscriber_handles_ or
+    // store ready_subscriber_handles to prevent rebuilding it every time
+    auto ready_subscriber_handles = get_ready_subscriber_handles();
+    // If there are none
+    if (ready_subscriber_handles.size() == 0)
+    {
+      // Repopulate the subscriber handles and wait on them
+      reset_subscriber_handles();
+      populate_subscriber_handles();
+      // TODO: populate the guard handles correctly
+      rmi::GuardConditionHandles guard_condition_handles;
+      guard_condition_handles.guard_condition_count_ = 1;
+      guard_condition_handles.guard_conditions_ = static_cast<void **>(
+        std::malloc(sizeof(void *) * 1));
+      guard_condition_handles.guard_conditions_[0] = \
+        rclcpp::utilities::get_global_sigint_guard_cond().data_;
+      // Wait on the handles.
+      rmi::wait(subscriber_handles_, guard_condition_handles, nonblocking);
+      std::free(guard_condition_handles.guard_conditions_);
+      ready_subscriber_handles = get_ready_subscriber_handles();
+    }
+    // At this point there should be something in either the subscriber handles
+    // Or in the guard handles.
+    // TODO: also check the timers here
+    std::shared_ptr<AnyExecutable> any_exec(new AnyExecutable());
+    // TODO: actually check the guard handles
+    any_exec->guard_condition_handle = 0;
+    // If any_exec.guard_condition_handle is valid, return now before checking
+    // for valid subscriptions.
+    if (any_exec->guard_condition_handle)
+    {
+      return any_exec;
+    }
+    // Check for subscriptions which are ready to be handled
+    any_exec->subscription = get_next_subscription(ready_subscriber_handles);
+    // If any_exec.subscription is valid, return now
+    if (any_exec->subscription)
+    {
+      return any_exec;
+    }
+    // If there is neither a ready guard condition nor subscription, return an
+    // empty any_exec anyways
+    return any_exec;
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(Executor);
+
+  std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
+  ros_middleware_interface::SubscriberHandles subscriber_handles_;
+
+};
+
+} /* executor */
+} /* rclcpp */
+
+#endif /* RCLCPP_RCLCPP_EXECUTOR_HPP_ */

rclcpp/include/rclcpp/executors.hpp

@@ -0,0 +1,33 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_EXECUTORS_HPP_
+#define RCLCPP_RCLCPP_EXECUTORS_HPP_
+
+#include <rclcpp/executors/multi_threaded_executor.hpp>
+#include <rclcpp/executors/single_threaded_executor.hpp>
+
+namespace rclcpp
+{
+namespace executors
+{
+
+using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
+using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
+
+}
+}
+
+#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -0,0 +1,68 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_
+#define RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_
+
+#include <cassert>
+#include <cstdlib>
+#include <memory>
+#include <vector>
+
+#include <ros_middleware_interface/functions.h>
+#include <ros_middleware_interface/handles.h>
+
+#include <rclcpp/executor.hpp>
+#include <rclcpp/macros.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/utilities.hpp>
+
+namespace rclcpp
+{
+namespace executors
+{
+namespace multi_threaded_executor
+{
+
+class MultiThreadedExecutor : public executor::Executor
+{
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(MultiThreadedExecutor);
+
+  void add_node(rclcpp::node::Node::SharedPtr &node_ptr)
+  {
+    this->weak_nodes_.push_back(node_ptr);
+  }
+
+  void spin()
+  {
+  }
+
+  void spin_some()
+  {
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(MultiThreadedExecutor);
+
+  std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
+
+};
+
+} /* namespace multi_threaded_executor */
+} /* namespace executors */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_ */

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -0,0 +1,88 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_
+#define RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_
+
+#include <cassert>
+#include <cstdlib>
+#include <memory>
+#include <vector>
+
+#include <ros_middleware_interface/functions.h>
+#include <ros_middleware_interface/handles.h>
+
+#include <rclcpp/executor.hpp>
+#include <rclcpp/macros.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/utilities.hpp>
+#include <rclcpp/rate.hpp>
+
+namespace rclcpp
+{
+namespace executors
+{
+namespace single_threaded_executor
+{
+
+class SingleThreadedExecutor : public executor::Executor
+{
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(SingleThreadedExecutor);
+
+  SingleThreadedExecutor() {}
+
+  ~SingleThreadedExecutor() {}
+
+  void spin()
+  {
+    while (rclcpp::utilities::ok())
+    {
+      auto any_exec = get_next_executable();
+      if (any_exec->subscription)
+      {
+        // Do callback
+        execute_subscription(any_exec->subscription);
+      }
+    }
+  }
+
+  void spin_node_some(rclcpp::node::Node &node)
+  {
+    reset_subscriber_handles();
+    populate_subscriber_handles_with_node(node);
+    // non-blocking = true
+    auto any_exec = get_next_executable(true);
+    while (any_exec->subscription)
+    {
+      execute_subscription(any_exec->subscription);
+      // non-blocking = true
+      any_exec = get_next_executable(true);
+    }
+    reset_subscriber_handles();
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(SingleThreadedExecutor);
+
+  std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
+
+};
+
+} /* namespace single_threaded_executor */
+} /* namespace executors */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_ */

rclcpp/include/rclcpp/macros.hpp

@@ -0,0 +1,42 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_MACROS_HPP_
+#define RCLCPP_RCLCPP_MACROS_HPP_
+
+/* Disables the copy constructor and operator= for the given class.
+ *
+ * Use in the private section of the class.
+ */
+#define RCLCPP_DISABLE_COPY(Class) \
+  Class(const Class&) = delete; \
+  Class& operator=(const Class&) = delete;
+
+/* Defines a make_shared static function on the class using perfect forwarding.
+ *
+ * Use in the public section of the class.
+ * Make sure to include <memory> in the header when using this.
+ */
+#define RCLCPP_MAKE_SHARED_DEFINITIONS(Class) \
+  typedef std::shared_ptr<Class> SharedPtr; \
+  \
+  template<typename...Args> \
+  static std::shared_ptr<Class> \
+  make_shared(Args &&...args) \
+  { \
+    return std::make_shared<Class>(std::forward<Args>(args)...); \
+  }
+
+#endif /* RCLCPP_RCLCPP_MACROS_HPP_ */

rclcpp/include/rclcpp/node.hpp

@@ -0,0 +1,102 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_NODE_HPP_
+#define RCLCPP_RCLCPP_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include <rclcpp/callback_group.hpp>
+#include <rclcpp/context.hpp>
+#include <rclcpp/macros.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/subscription.hpp>
+
+namespace rclcpp
+{
+
+// Forward declaration for friend statement
+namespace executor {class Executor;}
+
+namespace node
+{
+
+/* ROS Node Interface.
+ *
+ * This is the single point of entry for creating publishers and subscribers.
+ */
+class Node
+{
+  friend class rclcpp::executor::Executor;
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(Node);
+
+  /* Create a node based on the node name. */
+  Node(std::string node_name);
+  /* Create a node based on the node name and a rclcpp::context::Context. */
+  Node(std::string node_name, rclcpp::context::Context::SharedPtr context);
+
+  /* Get the name of the node. */
+  std::string
+  get_name();
+
+  /* Creates a named callback group. */
+  void
+  create_callback_group(
+    std::string group_name,
+    rclcpp::callback_group::CallbackGroupType callback_group_type);
+
+  /* Return callback group if it exists, otherwise evaluates to false. */
+  rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_callback_group(std::string group_name);
+
+  /* Create and return a Publisher. */
+  template<typename MessageT> rclcpp::publisher::Publisher::SharedPtr
+  create_publisher(std::string topic_name, size_t queue_size);
+
+  /* Create and return a Subscription. */
+  template<typename MessageT>
+  typename rclcpp::subscription::Subscription<MessageT>::SharedPtr
+  create_subscription(
+    std::string topic_name,
+    size_t queue_size,
+    std::function<void(const std::shared_ptr<MessageT> &)> callback);
+
+private:
+  RCLCPP_DISABLE_COPY(Node);
+
+  std::string name_;
+
+  ros_middleware_interface::NodeHandle node_handle_;
+
+  rclcpp::context::Context::SharedPtr context_;
+
+  rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_;
+  std::vector<rclcpp::callback_group::CallbackGroup::SharedPtr> callback_groups_;
+
+  size_t number_of_subscriptions_;
+
+};
+
+} /* namespace node */
+} /* namespace rclcpp */
+
+#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_
+// Template implementations
+#include "node_impl.hpp"
+#endif
+
+#endif /* RCLCPP_RCLCPP_NODE_HPP_ */

rclcpp/include/rclcpp/node_impl.hpp

@@ -0,0 +1,91 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_
+#define RCLCPP_RCLCPP_NODE_IMPL_HPP_
+
+#include <memory>
+#include <string>
+
+#include <ros_middleware_interface/functions.h>
+#include <ros_middleware_interface/handles.h>
+
+#include <ros_middleware_interface/get_type_support_handle.h>
+
+#include <rclcpp/contexts/default_context.hpp>
+
+#ifndef RCLCPP_RCLCPP_NODE_HPP_
+#include "node.hpp"
+#endif
+
+using namespace rclcpp;
+using namespace rclcpp::node;
+
+using rclcpp::contexts::default_context::DefaultContext;
+
+Node::Node(std::string node_name)
+  : Node(node_name, DefaultContext::make_shared())
+{}
+
+Node::Node(std::string node_name, context::Context::SharedPtr context)
+  : name_(node_name), context_(context), number_of_subscriptions_(0)
+{
+  node_handle_ = ::ros_middleware_interface::create_node();
+  using rclcpp::callback_group::CallbackGroup;
+  using rclcpp::callback_group::CallbackGroupType;
+  default_callback_group_.reset(new CallbackGroup(
+    "default_group", CallbackGroupType::NonThreadSafe));
+  callback_groups_.push_back(default_callback_group_);
+}
+
+template<typename MessageT> publisher::Publisher::SharedPtr
+Node::create_publisher(std::string topic_name, size_t queue_size)
+{
+  namespace rmi = ::ros_middleware_interface;
+
+  auto type_support_handle = rmi::get_type_support_handle<MessageT>();
+  auto publisher_handle = rmi::create_publisher(this->node_handle_,
+                                                type_support_handle,
+                                                topic_name.c_str());
+
+  return publisher::Publisher::make_shared(publisher_handle);
+}
+
+template<typename MessageT>
+typename subscription::Subscription<MessageT>::SharedPtr
+Node::create_subscription(
+  std::string topic_name,
+  size_t queue_size,
+  std::function<void(const std::shared_ptr<MessageT> &)> callback)
+{
+  namespace rmi = ::ros_middleware_interface;
+
+  auto &type_support_handle = rmi::get_type_support_handle<MessageT>();
+  auto subscriber_handle = rmi::create_subscriber(this->node_handle_,
+                                                  type_support_handle,
+                                                  topic_name.c_str());
+
+  using namespace rclcpp::subscription;
+
+  auto sub = Subscription<MessageT>::make_shared(subscriber_handle,
+                                                 topic_name,
+                                                 callback);
+  auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
+  this->default_callback_group_->add_subscription(sub_base_ptr);
+  number_of_subscriptions_++;
+  return sub;
+}
+
+#endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */

rclcpp/include/rclcpp/publisher.hpp

@@ -0,0 +1,58 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_PUBLISHER_HPP_
+#define RCLCPP_RCLCPP_PUBLISHER_HPP_
+
+#include <memory>
+
+#include <ros_middleware_interface/functions.h>
+#include <ros_middleware_interface/handles.h>
+
+#include <rclcpp/macros.hpp>
+
+namespace rclcpp
+{
+
+// Forward declaration for friend statement
+namespace node {class Node;}
+
+namespace publisher
+{
+
+class Publisher
+{
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher);
+
+  Publisher(ros_middleware_interface::PublisherHandle publisher_handle)
+    : publisher_handle_(publisher_handle)
+  {}
+
+  template<typename MessageT> void
+  publish(std::shared_ptr<MessageT> &msg)
+  {
+    ::ros_middleware_interface::publish(publisher_handle_, msg.get());
+  }
+
+private:
+  ros_middleware_interface::PublisherHandle publisher_handle_;
+
+};
+
+} /* namespace publisher */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_PUBLISHER_HPP_ */

rclcpp/include/rclcpp/rate.hpp

@@ -0,0 +1,115 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_RATE_HPP_
+#define RCLCPP_RCLCPP_RATE_HPP_
+
+#include <chrono>
+#include <memory>
+#include <thread>
+
+#include <rclcpp/macros.hpp>
+
+namespace rclcpp
+{
+namespace rate
+{
+
+class RateBase
+{
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(RateBase);
+
+  virtual bool sleep() = 0;
+  virtual bool is_steady() = 0;
+  virtual void reset() = 0;
+};
+
+template<class Clock = std::chrono::high_resolution_clock>
+class GenericRate : public RateBase
+{
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(GenericRate);
+
+  GenericRate(double rate) : rate_(rate), last_interval_(Clock::now())
+  {
+    typedef std::chrono::nanoseconds nanoseconds;
+    auto tmp = std::chrono::duration<double>(1.0f/rate_);
+    period_ = std::chrono::duration_cast<nanoseconds>(tmp);
+  }
+
+  virtual bool
+  sleep()
+  {
+    // Time coming into sleep
+    auto now = Clock::now();
+    // Time of next interval
+    auto next_interval = last_interval_ + period_;
+    // Detect backwards time flow
+    if (now < last_interval_)
+    {
+      // Best thing to do is to set the next_interval to now + period
+      next_interval = now + period_;
+    }
+    // Calculate the time to sleep
+    auto time_to_sleep = next_interval - now;
+    // Update the interval
+    last_interval_ += period_;
+    // If the time_to_sleep is negative or zero, don't sleep
+    if (time_to_sleep <= std::chrono::seconds(0))
+    {
+      // If an entire cycle was missed then reset next interval.
+      // This might happen if the loop took more than a cycle.
+      // Or if time jumps forward.
+      if (now > next_interval + period_)
+      {
+        last_interval_ = now + period_;
+      }
+      // Either way do not sleep and return false
+      return false;
+    }
+    // Sleep
+    std::this_thread::sleep_for(time_to_sleep);
+    return true;
+  }
+
+  virtual bool
+  is_steady()
+  {
+    return Clock::is_steady;
+  }
+
+  virtual void
+  reset()
+  {
+    last_interval_ = Clock::now();
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(GenericRate);
+
+  double rate_;
+  std::chrono::nanoseconds period_;
+  std::chrono::time_point<Clock> last_interval_;
+
+};
+
+typedef GenericRate<std::chrono::system_clock> Rate;
+typedef GenericRate<std::chrono::steady_clock> WallRate;
+
+}
+}
+
+#endif /* RCLCPP_RCLCPP_RATE_HPP_ */

rclcpp/include/rclcpp/rclcpp.hpp

@@ -0,0 +1,68 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_RCLCPP_HPP_
+#define RCLCPP_RCLCPP_RCLCPP_HPP_
+
+#include <csignal>
+#include <memory>
+#include <mutex>
+
+#include <rclcpp/node.hpp>
+#include <rclcpp/executors.hpp>
+#include <rclcpp/rate.hpp>
+#include <rclcpp/utilities.hpp>
+
+namespace
+{
+  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> g_executor(0);
+  std::mutex g_executor_mutex;
+}
+
+namespace rclcpp
+{
+
+using rclcpp::node::Node;
+using rclcpp::rate::GenericRate;
+using rclcpp::rate::WallRate;
+using rclcpp::utilities::ok;
+using rclcpp::utilities::init;
+
+void spin_some(Node &node)
+{
+  std::lock_guard<std::mutex> lock(g_executor_mutex);
+  if (!g_executor)
+  {
+    // Create an executor
+    g_executor.reset(new rclcpp::executors::SingleThreadedExecutor());
+  }
+  g_executor->spin_node_some(node);
+}
+
+void spin_some(Node::SharedPtr &node_ptr)
+{
+  spin_some(*node_ptr);
+}
+
+void spin(Node::SharedPtr &node_ptr)
+{
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node_ptr);
+  executor.spin();
+}
+
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_RCLCPP_HPP_ */

rclcpp/include/rclcpp/subscription.hpp

@@ -0,0 +1,99 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
+#define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <ros_middleware_interface/functions.h>
+#include <ros_middleware_interface/handles.h>
+
+#include <rclcpp/macros.hpp>
+
+namespace rclcpp
+{
+
+// Forward declaration is for friend statement in SubscriptionBase
+namespace executor {class Executor;}
+
+namespace subscription
+{
+
+class SubscriptionBase
+{
+  friend class rclcpp::executor::Executor;
+public:
+  RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase);
+
+  SubscriptionBase(
+    ros_middleware_interface::SubscriberHandle subscription_handle,
+    std::string &topic_name)
+    : subscription_handle_(subscription_handle), topic_name_(topic_name)
+  {}
+
+  std::string get_topic_name()
+  {
+    return this->topic_name_;
+  }
+
+  virtual std::shared_ptr<void> create_message() = 0;
+  virtual void handle_message(std::shared_ptr<void> &message) = 0;
+
+private:
+  RCLCPP_DISABLE_COPY(SubscriptionBase);
+
+  ros_middleware_interface::SubscriberHandle subscription_handle_;
+  std::string topic_name_;
+
+};
+
+template<typename MessageT>
+class Subscription : public SubscriptionBase
+{
+public:
+  typedef std::function<void(const std::shared_ptr<MessageT> &)> CallbackType;
+  RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription);
+
+  Subscription(ros_middleware_interface::SubscriberHandle subscription_handle,
+               std::string &topic_name,
+               CallbackType callback)
+    : SubscriptionBase(subscription_handle, topic_name), callback_(callback)
+  {}
+
+  std::shared_ptr<void> create_message()
+  {
+    return std::shared_ptr<void>(new MessageT());
+  }
+
+  void handle_message(std::shared_ptr<void> &message)
+  {
+    auto typed_message = std::static_pointer_cast<MessageT>(message);
+    callback_(typed_message);
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(Subscription);
+
+  CallbackType callback_;
+
+};
+
+} /* namespace subscription */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_SUBSCRIPTION_HPP_ */

rclcpp/include/rclcpp/utilities.hpp

@@ -0,0 +1,64 @@
+/* Copyright 2014 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.
+ */
+
+#ifndef RCLCPP_RCLCPP_UTILITIES_HPP_
+#define RCLCPP_RCLCPP_UTILITIES_HPP_
+
+#include <iostream>
+
+#include <csignal>
+
+#include <ros_middleware_interface/functions.h>
+#include <ros_middleware_interface/handles.h>
+
+namespace
+{
+  volatile sig_atomic_t g_signal_status = 0;
+  ros_middleware_interface::GuardConditionHandle g_sigint_guard_cond_handle_;
+
+  void signal_handler(int signal_value)
+  {
+    std::cout << "signal_handler(" << signal_value << ")" << std::endl;
+    g_signal_status = signal_value;
+    ros_middleware_interface::trigger_guard_condition(g_sigint_guard_cond_handle_);
+  }
+}
+
+namespace rclcpp
+{
+namespace utilities
+{
+
+void init(int argc, char *argv[])
+{
+  ::g_sigint_guard_cond_handle_ = \
+    ros_middleware_interface::create_guard_condition();
+  std::signal(SIGINT, ::signal_handler);
+}
+
+bool ok()
+{
+  return ::g_signal_status == 0;
+}
+
+ros_middleware_interface::GuardConditionHandle get_global_sigint_guard_cond()
+{
+  return ::g_sigint_guard_cond_handle_;
+}
+
+} /* namespace utilities */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_UTILITIES_HPP_ */