WIP

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

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,10 +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();
-    using AllocatorTraits = std::allocator_traits<Allocator>;
-    using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
-    auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
-    result.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(*message_alloc);
+    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;
@@ -102,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/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,10 +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();
-    using AllocatorTraits = std::allocator_traits<Allocator>;
-    using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
-    auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
-    result.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc);
+    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 =
@@ -129,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_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();
+}