WIP

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

rclcpp/include/rclcpp/allocator/allocator_common.hpp

@@ -29,23 +29,71 @@ namespace allocator
 template<typename T, typename Alloc>
 using AllocRebind = typename std::allocator_traits<Alloc>::template rebind_traits<T>;
 
-/// Return the equivalent rcl_allocator_t for the C++ standard allocator.
-/**
- * This assumes that the user intent behind both allocators is the
- * same: Using system malloc for allocation.
- *
- * If you're using a custom allocator in ROS, you'll need to provide
- * your own overload for this function.
- */
-template<typename T>
-rcl_allocator_t get_rcl_allocator(std::allocator<T> allocator)
+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;
+#else
   (void)allocator;  // Remove warning
+#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
 
 #endif  // RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -61,7 +61,7 @@ public:
     message_allocator_ = std::make_shared<MessageAlloc>();
     serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>();
     buffer_allocator_ = std::make_shared<BufferAlloc>();
-    rcutils_allocator_ = rclcpp::allocator::get_rcl_allocator(*buffer_allocator_.get());
+    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
   }
 
   explicit MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
@@ -69,7 +69,7 @@ public:
     message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
     serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>(*allocator.get());
     buffer_allocator_ = std::make_shared<BufferAlloc>(*allocator.get());
-    rcutils_allocator_ = rclcpp::allocator::get_rcl_allocator(*buffer_allocator_.get());
+    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
   }
 
   virtual ~MessageMemoryStrategy() = default;

rclcpp/include/rclcpp/publisher_options.hpp

@@ -64,8 +64,12 @@ struct PublisherOptionsBase
 template<typename Allocator>
 struct PublisherOptionsWithAllocator : public PublisherOptionsBase
 {
+  static_assert(
+    std::is_void<typename std::allocator_traits<Allocator>::value_type>::value,
+    "Publisher allocator value type must be void");
+
   /// Optional custom allocator.
-  std::shared_ptr<Allocator> allocator = nullptr;
+  mutable std::shared_ptr<Allocator> allocator = nullptr;
 
   PublisherOptionsWithAllocator<Allocator>() {}
 
@@ -80,7 +84,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   to_rcl_publisher_options(const rclcpp::QoS & qos) const
   {
     rcl_publisher_options_t result = rcl_publisher_get_default_options();
-    result.allocator = rclcpp::allocator::get_rcl_allocator(*this->get_allocator());
+    result.allocator = rclcpp::allocator::get_rcl_allocator<void>(*this->get_allocator());
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_publisher_options.require_unique_network_flow_endpoints =
       this->require_unique_network_flow_endpoints;
@@ -99,7 +103,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   get_allocator() const
   {
     if (!this->allocator) {
-      return std::make_shared<Allocator>();
+      this->allocator = std::make_shared<Allocator>();
     }
     return this->allocator;
   }

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -437,7 +437,7 @@ public:
 
   rcl_allocator_t get_allocator() override
   {
-    return rclcpp::allocator::get_rcl_allocator(*allocator_.get());
+    return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
   }
 
   size_t number_of_ready_subscriptions() const override

rclcpp/include/rclcpp/subscription_options.hpp

@@ -86,8 +86,12 @@ struct SubscriptionOptionsBase
 template<typename Allocator>
 struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
 {
+  static_assert(
+    std::is_void<typename std::allocator_traits<Allocator>::value_type>::value,
+    "Subscription allocator value type must be void");
+
   /// Optional custom allocator.
-  std::shared_ptr<Allocator> allocator = nullptr;
+  mutable std::shared_ptr<Allocator> allocator = nullptr;
 
   SubscriptionOptionsWithAllocator<Allocator>() {}
 
@@ -107,7 +111,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
   to_rcl_subscription_options(const rclcpp::QoS & qos) const
   {
     rcl_subscription_options_t result = rcl_subscription_get_default_options();
-    result.allocator = rclcpp::allocator::get_rcl_allocator(*this->get_allocator());
+    result.allocator = allocator::get_rcl_allocator<void>(*this->get_allocator());
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;
     result.rmw_subscription_options.require_unique_network_flow_endpoints =
@@ -126,7 +130,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
   get_allocator() const
   {
     if (!this->allocator) {
-      return std::make_shared<Allocator>();
+      this->allocator = std::make_shared<Allocator>();
     }
     return this->allocator;
   }

rclcpp/test/rclcpp/allocator/test_allocator_common.cpp

@@ -18,9 +18,42 @@
 
 #include "rclcpp/allocator/allocator_common.hpp"
 
+TEST(TestAllocatorCommon, retyped_allocate) {
+  std::allocator<int> allocator;
+  void * untyped_allocator = &allocator;
+  void * allocated_mem =
+    rclcpp::allocator::retyped_allocate<std::allocator<char>>(1u, untyped_allocator);
+  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
+  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
+  ASSERT_TRUE(nullptr != allocated_mem);
+
+  auto code = [&untyped_allocator, allocated_mem]() {
+      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
+        allocated_mem, untyped_allocator);
+    };
+  EXPECT_NO_THROW(code());
+
+  allocated_mem = allocator.allocate(1);
+  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
+  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
+  ASSERT_TRUE(nullptr != allocated_mem);
+  void * reallocated_mem =
+    rclcpp::allocator::retyped_reallocate<int, std::allocator<int>>(
+    allocated_mem, 2u, untyped_allocator);
+  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
+  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
+  ASSERT_TRUE(nullptr != reallocated_mem);
+
+  auto code2 = [&untyped_allocator, reallocated_mem]() {
+      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
+        reallocated_mem, untyped_allocator);
+    };
+  EXPECT_NO_THROW(code2());
+}
+
 TEST(TestAllocatorCommon, get_rcl_allocator) {
   std::allocator<int> allocator;
-  auto rcl_allocator = rclcpp::allocator::get_rcl_allocator(allocator);
+  auto rcl_allocator = rclcpp::allocator::get_rcl_allocator<int>(allocator);
   EXPECT_NE(nullptr, rcl_allocator.allocate);
   EXPECT_NE(nullptr, rcl_allocator.deallocate);
   EXPECT_NE(nullptr, rcl_allocator.reallocate);
@@ -30,7 +63,8 @@ TEST(TestAllocatorCommon, get_rcl_allocator) {
 
 TEST(TestAllocatorCommon, get_void_rcl_allocator) {
   std::allocator<void> allocator;
-  auto rcl_allocator = rclcpp::allocator::get_rcl_allocator(allocator);
+  auto rcl_allocator =
+    rclcpp::allocator::get_rcl_allocator<void, std::allocator<void>>(allocator);
   EXPECT_NE(nullptr, rcl_allocator.allocate);
   EXPECT_NE(nullptr, rcl_allocator.deallocate);
   EXPECT_NE(nullptr, rcl_allocator.reallocate);

rclcpp_action/include/rclcpp_action/server.hpp

@@ -356,16 +356,20 @@ protected:
   CancelResponse
   call_handle_cancel_callback(const GoalUUID & uuid) override
   {
-    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
+    {
+      std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+      auto element = goal_handles_.find(uuid);
+      if (element != goal_handles_.end()) {
+        goal_handle = element->second.lock();
+      }
+    }
+
     CancelResponse resp = CancelResponse::REJECT;
-    auto element = goal_handles_.find(uuid);
-    if (element != goal_handles_.end()) {
-      std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle = element->second.lock();
-      if (goal_handle) {
-        resp = handle_cancel_(goal_handle);
-        if (CancelResponse::ACCEPT == resp) {
-          goal_handle->_cancel_goal();
-        }
+    if (goal_handle) {
+      resp = handle_cancel_(goal_handle);
+      if (CancelResponse::ACCEPT == resp) {
+        goal_handle->_cancel_goal();
       }
     }
     return resp;

rclcpp_action/test/test_server.cpp

@@ -1234,10 +1234,14 @@ public:
         this->TryLockFor(lock, std::chrono::milliseconds(1000));
         return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
       },
-      [this](std::shared_ptr<GoalHandle>) {
+      [this](std::shared_ptr<GoalHandle> handle) {
         // instead of making a deadlock, check if it can acquire the lock in a second
         std::unique_lock<std::recursive_timed_mutex> lock(server_mutex_, std::defer_lock);
         this->TryLockFor(lock, std::chrono::milliseconds(1000));
+        // TODO(KavenYau): this check may become obsolete with https://github.com/ros2/rclcpp/issues/1599
+        if (!handle->is_active()) {
+          return rclcpp_action::CancelResponse::REJECT;
+        }
         return rclcpp_action::CancelResponse::ACCEPT;
       },
       [this](std::shared_ptr<GoalHandle> handle) {
@@ -1306,3 +1310,12 @@ TEST_F(TestDeadlockServer, deadlock_while_canceled)
   send_goal_request(node_, uuid2_);  // deadlock here
   t.join();
 }
+
+TEST_F(TestDeadlockServer, deadlock_while_succeed_and_canceled)
+{
+  send_goal_request(node_, uuid1_);
+  std::thread t(&TestDeadlockServer::GoalSucceeded, this);
+  rclcpp::sleep_for(std::chrono::milliseconds(50));
+  send_cancel_request(node_, uuid1_);
+  t.join();
+}