Refactor to use rcl (#207)

rclcpp/CMakeLists.txt

@@ -3,6 +3,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(rclcpp)
 
 find_package(ament_cmake REQUIRED)
+find_package(rcl REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rmw_implementation REQUIRED)
@@ -42,13 +43,14 @@ set(${PROJECT_NAME}_SRCS
 )
 
 macro(target)
+  if(NOT "${target_suffix} " STREQUAL " ")
+    get_rcl_information("${rmw_implementation}" "rcl${target_suffix}")
+  endif()
   add_library(${PROJECT_NAME}${target_suffix} SHARED
     ${${PROJECT_NAME}_SRCS})
   ament_target_dependencies(${PROJECT_NAME}${target_suffix}
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_generator_cpp"
-    "${rmw_implementation}")
+    "rcl${target_suffix}"
+    "rosidl_generator_cpp")
 
   # Causes the visibility macros to use dllexport rather than dllimport,
   # which is appropriate when building the dll but not consuming it.
@@ -66,9 +68,7 @@ endmacro()
 call_for_each_rmw_implementation(target GENERATE_DEFAULT)
 
 ament_export_dependencies(ament_cmake)
-ament_export_dependencies(rcl_interfaces)
-ament_export_dependencies(rmw)
-ament_export_dependencies(rmw_implementation)
+ament_export_dependencies(rcl)
 ament_export_dependencies(rosidl_generator_cpp)
 
 ament_export_include_directories(include)
@@ -90,6 +90,7 @@ if(AMENT_ENABLE_TESTING)
   ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp)
   if(TARGET test_mapped_ring_buffer)
     target_include_directories(test_mapped_ring_buffer PUBLIC
+      ${rcl_INCLUDE_DIRS}
       ${rcl_interfaces_INCLUDE_DIRS}
       ${rmw_INCLUDE_DIRS}
       ${rosidl_generator_cpp_INCLUDE_DIRS}
@@ -98,6 +99,7 @@ if(AMENT_ENABLE_TESTING)
   ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp)
   if(TARGET test_intra_process_manager)
     target_include_directories(test_intra_process_manager PUBLIC
+      ${rcl_INCLUDE_DIRS}
       ${rcl_interfaces_INCLUDE_DIRS}
       ${rmw_INCLUDE_DIRS}
       ${rosidl_generator_cpp_INCLUDE_DIRS}

rclcpp/cmake/get_rclcpp_information.cmake

@@ -28,6 +28,11 @@ macro(get_rclcpp_information rmw_implementation var_prefix)
   # so that the variables can be used by various functions / macros
   set(${var_prefix}_FOUND TRUE)
 
+  # Get rcl using the existing macro
+  if(NOT "${target_suffix} " STREQUAL " ")
+    get_rcl_information("${rmw_implementation}" "rcl${target_suffix}")
+  endif()
+
   # include directories
   normalize_path(${var_prefix}_INCLUDE_DIRS
     "${rclcpp_DIR}/../../../include")
@@ -63,8 +68,7 @@ macro(get_rclcpp_information rmw_implementation var_prefix)
   # dependencies
   set(_exported_dependencies
     "rcl_interfaces"
-    "rmw"
-    "${rmw_implementation}"
+    "rcl${target_suffix}"
     "rosidl_generator_cpp")
   set(${var_prefix}_DEFINITIONS)
   foreach(_dep ${_exported_dependencies})

rclcpp/include/rclcpp/allocator/allocator_common.hpp

@@ -17,6 +17,8 @@
 
 #include <memory>
 
+#include "rcl/allocator.h"
+
 #include "rclcpp/allocator/allocator_deleter.hpp"
 
 namespace rclcpp
@@ -27,6 +29,64 @@ namespace allocator
 template<typename T, typename Alloc>
 using AllocRebind = typename std::allocator_traits<Alloc>::template rebind_traits<T>;
 
+template<typename Alloc>
+void * retyped_allocate(size_t size, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
+}
+
+template<typename T, typename Alloc>
+void retyped_deallocate(void * untyped_pointer, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  auto typed_ptr = static_cast<T *>(untyped_pointer);
+  std::allocator_traits<Alloc>::deallocate(*typed_allocator, typed_ptr, 1);
+}
+
+template<typename T, typename Alloc>
+void * retyped_reallocate(void * untyped_pointer, size_t size, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  auto typed_ptr = static_cast<T *>(untyped_pointer);
+  std::allocator_traits<Alloc>::deallocate(*typed_allocator, typed_ptr, 1);
+  return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
+}
+
+
+// Convert a std::allocator_traits-formatted Allocator into an rcl allocator
+template<typename T, typename Alloc,
+typename std::enable_if<!std::is_same<Alloc, std::allocator<void>>::value>::type * = nullptr>
+rcl_allocator_t get_rcl_allocator(Alloc & allocator)
+{
+  rcl_allocator_t rcl_allocator = rcl_get_default_allocator();
+#ifndef _WIN32
+  rcl_allocator.allocate = &retyped_allocate<Alloc>;
+  rcl_allocator.deallocate = &retyped_deallocate<T, Alloc>;
+  rcl_allocator.reallocate = &retyped_reallocate<T, Alloc>;
+  rcl_allocator.state = &allocator;
+#endif
+  return rcl_allocator;
+}
+
+// TODO(jacquelinekay) Workaround for an incomplete implementation of std::allocator<void>
+template<typename T, typename Alloc,
+typename std::enable_if<std::is_same<Alloc, std::allocator<void>>::value>::type * = nullptr>
+rcl_allocator_t get_rcl_allocator(Alloc & allocator)
+{
+  (void)allocator;
+  return rcl_get_default_allocator();
+}
+
 }  // namespace allocator
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/client.hpp

@@ -23,10 +23,15 @@
 #include <tuple>
 #include <utility>
 
+#include "rcl/client.h"
+#include "rcl/error_handling.h"
+
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
@@ -42,32 +47,31 @@ class ClientBase
 
   RCLCPP_PUBLIC
   ClientBase(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_client_t * client_handle,
+    std::shared_ptr<rcl_node_t> node_handle,
     const std::string & service_name);
 
   RCLCPP_PUBLIC
-  virtual ~ClientBase();
+  ~ClientBase();
 
   RCLCPP_PUBLIC
   const std::string &
   get_service_name() const;
 
   RCLCPP_PUBLIC
-  const rmw_client_t *
+  const rcl_client_t *
   get_client_handle() const;
 
   virtual std::shared_ptr<void> create_response() = 0;
   virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
   virtual void handle_response(
     std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
 
-private:
+protected:
   RCLCPP_DISABLE_COPY(ClientBase);
 
-  std::shared_ptr<rmw_node_t> node_handle_;
+  std::shared_ptr<rcl_node_t> node_handle_;
 
-  rmw_client_t * client_handle_;
+  rcl_client_t client_handle_ = rcl_get_zero_initialized_client();
   std::string service_name_;
 };
 
@@ -93,11 +97,32 @@ class Client : public ClientBase
   RCLCPP_SMART_PTR_DEFINITIONS(Client);
 
   Client(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_client_t * client_handle,
-    const std::string & service_name)
-  : ClientBase(node_handle, client_handle, service_name)
-  {}
+    std::shared_ptr<rcl_node_t> node_handle,
+    const std::string & service_name,
+    rcl_client_options_t & client_options)
+  : ClientBase(node_handle, service_name)
+  {
+    using rosidl_generator_cpp::get_service_type_support_handle;
+    auto service_type_support_handle =
+      get_service_type_support_handle<ServiceT>();
+    if (rcl_client_init(&client_handle_, this->node_handle_.get(),
+      service_type_support_handle, service_name.c_str(), &client_options) != RCL_RET_OK)
+    {
+      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      throw std::runtime_error(
+        std::string("could not create client: ") +
+        rcl_get_error_string_safe());
+      // *INDENT-ON*
+    }
+  }
+
+  virtual ~Client()
+  {
+    if (rcl_client_fini(&client_handle_, node_handle_.get()) != RCL_RET_OK) {
+      fprintf(stderr,
+        "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
+    }
+  }
 
   std::shared_ptr<void> create_response()
   {
@@ -149,10 +174,10 @@ class Client : public ClientBase
   {
     std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     int64_t sequence_number;
-    if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) {
+    if (RCL_RET_OK != rcl_send_request(get_client_handle(), request.get(), &sequence_number)) {
       // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
       throw std::runtime_error(
-        std::string("failed to send request: ") + rmw_get_error_string_safe());
+        std::string("failed to send request: ") + rcl_get_error_string_safe());
       // *INDENT-ON*
     }
 

rclcpp/include/rclcpp/executor.hpp

@@ -25,6 +25,9 @@
 #include <string>
 #include <vector>
 
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+
 #include "rclcpp/any_executable.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/memory_strategies.hpp"
@@ -193,10 +196,12 @@ class Executor
     }
 
     auto end_time = std::chrono::steady_clock::now();
-    if (timeout > std::chrono::nanoseconds::zero()) {
-      end_time += timeout;
+    std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+      timeout);
+    if (timeout_ns > std::chrono::nanoseconds::zero()) {
+      end_time += timeout_ns;
     }
-    auto timeout_left = timeout;
+    std::chrono::nanoseconds timeout_left = timeout_ns;
 
     while (rclcpp::utilities::ok()) {
       // Do one item of work.
@@ -207,7 +212,7 @@ class Executor
         return FutureReturnCode::SUCCESS;
       }
       // If the original timeout is < 0, then this is blocking, never TIMEOUT.
-      if (timeout < std::chrono::nanoseconds::zero()) {
+      if (timeout_ns < std::chrono::nanoseconds::zero()) {
         continue;
       }
       // Otherwise check if we still have time to wait, return TIMEOUT if not.
@@ -216,8 +221,7 @@ class Executor
         return FutureReturnCode::TIMEOUT;
       }
       // Subtract the elapsed time from the original timeout.
-      using duration_type = std::chrono::duration<int64_t, TimeT>;
-      timeout_left = std::chrono::duration_cast<duration_type>(end_time - now);
+      timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
     }
 
     // The future did not complete before ok() returned false, return INTERRUPTED.
@@ -291,10 +295,6 @@ class Executor
   void
   get_next_timer(AnyExecutable::SharedPtr any_exec);
 
-  RCLCPP_PUBLIC
-  std::chrono::nanoseconds
-  get_earliest_timer();
-
   RCLCPP_PUBLIC
   AnyExecutable::SharedPtr
   get_next_ready_executable();
@@ -307,10 +307,10 @@ class Executor
   std::atomic_bool spinning;
 
   /// Guard condition for signaling the rmw layer to wake up for special events.
-  rmw_guard_condition_t * interrupt_guard_condition_;
+  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
 
   /// Waitset for managing entities that the rmw layer waits on.
-  rmw_waitset_t * waitset_;
+  rcl_wait_set_t waitset_ = rcl_get_zero_initialized_wait_set();
 
   /// The memory strategy: an interface for handling user-defined memory allocation strategies.
   memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -18,6 +18,9 @@
 #include <memory>
 #include <vector>
 
+#include "rcl/allocator.h"
+#include "rcl/wait.h"
+
 #include "rclcpp/any_executable.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node.hpp"
@@ -40,31 +43,25 @@ class RCLCPP_PUBLIC MemoryStrategy
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy);
   using WeakNodeVector = std::vector<std::weak_ptr<rclcpp::node::Node>>;
 
-  // return the new number of subscribers
-  virtual size_t fill_subscriber_handles(void ** & ptr) = 0;
-
-  // return the new number of services
-  virtual size_t fill_service_handles(void ** & ptr) = 0;
-
-  // return the new number of clients
-  virtual size_t fill_client_handles(void ** & ptr) = 0;
-
-  // return the new number of guard_conditions
-  virtual size_t fill_guard_condition_handles(void ** & ptr) = 0;
+  virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
 
-  virtual void clear_active_entities() = 0;
+  virtual size_t number_of_ready_subscriptions() const = 0;
+  virtual size_t number_of_ready_services() const = 0;
+  virtual size_t number_of_ready_clients() const = 0;
+  virtual size_t number_of_ready_timers() const = 0;
+  virtual size_t number_of_guard_conditions() const = 0;
 
+  virtual bool add_handles_to_waitset(rcl_wait_set_t * wait_set) = 0;
   virtual void clear_handles() = 0;
-  virtual void remove_null_handles() = 0;
-  virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
+  virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;
 
   /// Provide a newly initialized AnyExecutable object.
   // \return Shared pointer to the fresh executable.
   virtual rclcpp::executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;
 
-  virtual void add_guard_condition(const rmw_guard_condition_t * guard_condition) = 0;
+  virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
 
-  virtual void remove_guard_condition(const rmw_guard_condition_t * guard_condition) = 0;
+  virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
 
   virtual void
   get_next_subscription(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
@@ -78,15 +75,18 @@ class RCLCPP_PUBLIC MemoryStrategy
   get_next_client(rclcpp::executor::AnyExecutable::SharedPtr any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
+  virtual rcl_allocator_t
+  get_allocator() = 0;
+
   static rclcpp::subscription::SubscriptionBase::SharedPtr
-  get_subscription_by_handle(void * subscriber_handle,
+  get_subscription_by_handle(const rcl_subscription_t * subscriber_handle,
     const WeakNodeVector & weak_nodes);
 
   static rclcpp::service::ServiceBase::SharedPtr
-  get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes);
+  get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes);
 
   static rclcpp::client::ClientBase::SharedPtr
-  get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes);
+  get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes);
 
   static rclcpp::node::Node::SharedPtr
   get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,