use dynamic_pointer_cast to detect allocator mismatch in intra process manager (backport #1643) (#1645)

* use dynamic_pointer_cast to detect allocator mismatch in intra process manager (#1643)

* use dynamic_pointer_cast to detect allocator mismatch in intra process manager

Signed-off-by: William Woodall <william@osrfoundation.org>

* add test case for mismatched allocators

Signed-off-by: William Woodall <william@osrfoundation.org>

* forward template arguments to avoid mismatched types in intra process manager

Signed-off-by: William Woodall <william@osrfoundation.org>

* style fixes

Signed-off-by: William Woodall <william@osrfoundation.org>

* refactor to test message address and count, more DRY

Signed-off-by: William Woodall <william@osrfoundation.org>

* update copyright

Signed-off-by: William Woodall <william@osrfoundation.org>

* fix typo

Signed-off-by: William Woodall <william@osrfoundation.org>

Co-authored-by: Michel Hidalgo <michel@ekumenlabs.com>

Co-authored-by: Michel Hidalgo <michel@ekumenlabs.com>
(cherry picked from commit 79c2dd8e8b3e73366c59474b91271a2fd57954bc)

# Conflicts:
#	rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

* fix conflicts

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

* cpp14 compatibility

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

* More cpp14 compat

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

* fix

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

* Fix bug

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

Co-authored-by: William Woodall <william@osrfoundation.org>
Co-authored-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -202,7 +202,8 @@ public:
       // None of the buffers require ownership, so we promote the pointer
       std::shared_ptr<MessageT> msg = std::move(message);
 
-      this->template add_shared_msg_to_buffers<MessageT>(msg, sub_ids.take_shared_subscriptions);
+      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
+        msg, sub_ids.take_shared_subscriptions);
     } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
       sub_ids.take_shared_subscriptions.size() <= 1)
     {
@@ -227,7 +228,7 @@ public:
       // for the buffers that do not require ownership
       auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(*allocator, *message);
 
-      this->template add_shared_msg_to_buffers<MessageT>(
+      this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
         shared_msg, sub_ids.take_shared_subscriptions);
       this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
         std::move(message), sub_ids.take_ownership_subscriptions, allocator);
@@ -263,7 +264,7 @@ public:
       // If there are no owning, just convert to shared.
       std::shared_ptr<MessageT> shared_msg = std::move(message);
       if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT>(
+        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
           shared_msg, sub_ids.take_shared_subscriptions);
       }
       return shared_msg;
@@ -273,7 +274,7 @@ public:
       auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(*allocator, *message);
 
       if (!sub_ids.take_shared_subscriptions.empty()) {
-        this->template add_shared_msg_to_buffers<MessageT>(
+        this->template add_shared_msg_to_buffers<MessageT, Alloc, Deleter>(
           shared_msg,
           sub_ids.take_shared_subscriptions);
       }
@@ -350,7 +351,10 @@ private:
   bool
   can_communicate(PublisherInfo pub_info, SubscriptionInfo sub_info) const;
 
-  template<typename MessageT>
+  template<
+    typename MessageT,
+    typename Alloc,
+    typename Deleter>
   void
   add_shared_msg_to_buffers(
     std::shared_ptr<const MessageT> message,
@@ -363,10 +367,16 @@ private:
       }
       auto subscription_base = subscription_it->second.subscription;
 
-      auto subscription = std::static_pointer_cast<
+      auto subscription = std::dynamic_pointer_cast<
-        rclcpp::experimental::SubscriptionIntraProcess<MessageT>
+        rclcpp::experimental::SubscriptionIntraProcess<MessageT, Alloc, Deleter>
         >(subscription_base);
-
+      if (nullptr == subscription) {
+        throw std::runtime_error(
+                "failed to dynamic cast SubscriptionIntraProcessBase to "
+                "SubscriptionIntraProcess<MessageT, Alloc, Deleter>, which "
+                "can happen when the publisher and subscription use different "
+                "allocator types, which is not supported");
+      }
       subscription->provide_intra_process_message(message);
     }
   }
@@ -391,9 +401,16 @@ private:
       }
       auto subscription_base = subscription_it->second.subscription;
 
-      auto subscription = std::static_pointer_cast<
+      auto subscription = std::dynamic_pointer_cast<
-        rclcpp::experimental::SubscriptionIntraProcess<MessageT>
+        rclcpp::experimental::SubscriptionIntraProcess<MessageT, Alloc, Deleter>
         >(subscription_base);
+      if (nullptr == subscription) {
+        throw std::runtime_error(
+                "failed to dynamic cast SubscriptionIntraProcessBase to "
+                "SubscriptionIntraProcess<MessageT, Alloc, Deleter>, which "
+                "can happen when the publisher and subscription use different "
+                "allocator types, which is not supported");
+      }
 
       if (std::next(it) == subscription_ids.end()) {
         // If this is the last subscription, give up ownership

rclcpp/test/rclcpp/CMakeLists.txt

@@ -134,6 +134,13 @@ if(TARGET test_intra_process_manager)
   )
   target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
 endif()
+ament_add_gmock(test_intra_process_manager_with_allocators test_intra_process_manager_with_allocators.cpp)
+if(TARGET test_intra_process_manager_with_allocators)
+  ament_target_dependencies(test_intra_process_manager_with_allocators
+    "test_msgs"
+  )
+  target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME})
+endif()
 ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp)
 if(TARGET test_ring_buffer_implementation)
   ament_target_dependencies(test_ring_buffer_implementation

rclcpp/test/rclcpp/test_intra_process_manager.cpp

@@ -216,7 +216,10 @@ public:
   const char * topic_name;
 };
 
-template<typename MessageT>
+template<
+  typename MessageT,
+  typename Alloc = std::allocator<void>,
+  typename Deleter = std::default_delete<MessageT>>
 class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
 {
 public:

rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp

@@ -0,0 +1,303 @@
+// Copyright 2021 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.
+
+#include <gmock/gmock.h>
+
+#include <chrono>
+#include <list>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "test_msgs/msg/empty.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/strategies/allocator_memory_strategy.hpp"
+
+// For demonstration purposes only, not necessary for allocator_traits
+static uint32_t num_allocs = 0;
+static uint32_t num_deallocs = 0;
+// A very simple custom allocator. Counts calls to allocate and deallocate.
+template<typename T = void>
+struct MyAllocator
+{
+public:
+  using value_type = T;
+  using size_type = std::size_t;
+  using pointer = T *;
+  using const_pointer = const T *;
+  using difference_type = typename std::pointer_traits<pointer>::difference_type;
+
+  MyAllocator() noexcept
+  {
+  }
+
+  ~MyAllocator() noexcept {}
+
+  template<typename U>
+  MyAllocator(const MyAllocator<U> &) noexcept
+  {
+  }
+
+  T * allocate(size_t size, const void * = 0)
+  {
+    if (size == 0) {
+      return nullptr;
+    }
+    num_allocs++;
+    return static_cast<T *>(std::malloc(size * sizeof(T)));
+  }
+
+  void deallocate(T * ptr, size_t size)
+  {
+    (void)size;
+    if (!ptr) {
+      return;
+    }
+    num_deallocs++;
+    std::free(ptr);
+  }
+
+  template<typename U>
+  struct rebind
+  {
+    typedef MyAllocator<U> other;
+  };
+};
+
+template<typename T, typename U>
+constexpr bool operator==(
+  const MyAllocator<T> &,
+  const MyAllocator<U> &) noexcept
+{
+  return true;
+}
+
+template<typename T, typename U>
+constexpr bool operator!=(
+  const MyAllocator<T> &,
+  const MyAllocator<U> &) noexcept
+{
+  return false;
+}
+
+template<
+  typename ExpectedExceptionT,
+  typename PublisherT,
+  typename FutureT,
+  typename MessageT,
+  typename ExpectedMessagePtr,
+  typename std::enable_if<std::is_same<ExpectedExceptionT, void>::value, int>::type = 0>
+void check_exception(
+  PublisherT & publisher, rclcpp::Executor & executor, FutureT received_message_future,
+  uint32_t & counter, MessageT msg, ExpectedMessagePtr expected_ptr)
+{
+  // no exception expected
+  EXPECT_NO_THROW(
+  {
+    publisher->publish(std::move(msg));
+    executor.spin_until_future_complete(received_message_future, std::chrono::seconds(10));
+  });
+  EXPECT_EQ(expected_ptr, received_message_future.get().get());
+  EXPECT_EQ(1u, counter);
+}
+
+template<
+  typename ExpectedExceptionT,
+  typename PublisherT,
+  typename FutureT,
+  typename MessageT,
+  typename ExpectedMessagePtr,
+  typename std::enable_if<!std::is_same<ExpectedExceptionT, void>::value, int>::type = 0>
+void check_exception(
+  PublisherT & publisher, rclcpp::Executor & executor, FutureT received_message_future,
+  uint32_t counter, MessageT msg, ExpectedMessagePtr expected_ptr)
+{
+  (void)counter;
+  (void)expected_ptr;
+  // exception expected
+  EXPECT_THROW(
+  {
+    publisher->publish(std::move(msg));
+    executor.spin_until_future_complete(received_message_future, std::chrono::seconds(10));
+  }, ExpectedExceptionT);
+}
+
+template<
+  typename PublishedMessageAllocatorT,
+  typename PublisherAllocatorT,
+  typename SubscribedMessageAllocatorT,
+  typename SubscriptionAllocatorT,
+  typename MessageMemoryStrategyAllocatorT,
+  typename MemoryStrategyAllocatorT,
+  typename ExpectedExceptionT
+>
+void
+do_custom_allocator_test(
+  PublishedMessageAllocatorT published_message_allocator,
+  PublisherAllocatorT publisher_allocator,
+  SubscribedMessageAllocatorT /* subscribed_message_allocator */,  // intentionally unused
+  SubscriptionAllocatorT subscription_allocator,
+  MessageMemoryStrategyAllocatorT message_memory_strategy,
+  MemoryStrategyAllocatorT memory_strategy_allocator)
+{
+  using PublishedMessageAllocTraits =
+    rclcpp::allocator::AllocRebind<test_msgs::msg::Empty, PublishedMessageAllocatorT>;
+  using PublishedMessageAlloc = typename PublishedMessageAllocTraits::allocator_type;
+  using PublishedMessageDeleter =
+    rclcpp::allocator::Deleter<PublishedMessageAlloc, test_msgs::msg::Empty>;
+
+  using SubscribedMessageAllocTraits =
+    rclcpp::allocator::AllocRebind<test_msgs::msg::Empty, SubscribedMessageAllocatorT>;
+  using SubscribedMessageAlloc = typename SubscribedMessageAllocTraits::allocator_type;
+  using SubscribedMessageDeleter =
+    rclcpp::allocator::Deleter<SubscribedMessageAlloc, test_msgs::msg::Empty>;
+
+  // init and node
+  auto context = std::make_shared<rclcpp::Context>();
+  context->init(0, nullptr);
+  auto node = std::make_shared<rclcpp::Node>(
+    "custom_allocator_test",
+    rclcpp::NodeOptions().context(context).use_intra_process_comms(true));
+
+  // publisher
+  auto shared_publisher_allocator = std::make_shared<PublisherAllocatorT>(publisher_allocator);
+  rclcpp::PublisherOptionsWithAllocator<PublisherAllocatorT> publisher_options;
+  publisher_options.allocator = shared_publisher_allocator;
+  auto publisher =
+    node->create_publisher<test_msgs::msg::Empty>("custom_allocator_test", 10, publisher_options);
+
+  // callback for subscription
+  uint32_t counter = 0;
+  std::promise<std::unique_ptr<test_msgs::msg::Empty, SubscribedMessageDeleter>> received_message;
+  auto received_message_future = received_message.get_future().share();
+  auto callback =
+    [&counter, &received_message](
+    std::unique_ptr<test_msgs::msg::Empty, SubscribedMessageDeleter> msg)
+    {
+      ++counter;
+      received_message.set_value(std::move(msg));
+    };
+
+  // subscription
+  auto shared_subscription_allocator =
+    std::make_shared<SubscriptionAllocatorT>(subscription_allocator);
+  rclcpp::SubscriptionOptionsWithAllocator<SubscriptionAllocatorT> subscription_options;
+  subscription_options.allocator = shared_subscription_allocator;
+  auto shared_message_strategy_allocator =
+    std::make_shared<MessageMemoryStrategyAllocatorT>(message_memory_strategy);
+  auto msg_mem_strat = std::make_shared<
+    rclcpp::message_memory_strategy::MessageMemoryStrategy<
+      test_msgs::msg::Empty,
+      MessageMemoryStrategyAllocatorT
+    >
+    >(shared_message_strategy_allocator);
+  using CallbackMessageT =
+    typename rclcpp::subscription_traits::has_message_type<decltype(callback)>::type;
+  auto subscriber = node->create_subscription<
+    test_msgs::msg::Empty,
+    decltype(callback),
+    SubscriptionAllocatorT,
+    CallbackMessageT,
+    rclcpp::Subscription<CallbackMessageT, SubscriptionAllocatorT>,
+    rclcpp::message_memory_strategy::MessageMemoryStrategy<
+      CallbackMessageT,
+      MessageMemoryStrategyAllocatorT
+    >
+    >(
+    "custom_allocator_test",
+    10,
+    std::forward<decltype(callback)>(callback),
+    subscription_options,
+    msg_mem_strat);
+
+  // executor memory strategy
+  using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
+  auto shared_memory_strategy_allocator = std::make_shared<MemoryStrategyAllocatorT>(
+    memory_strategy_allocator);
+  std::shared_ptr<rclcpp::memory_strategy::MemoryStrategy> memory_strategy =
+    std::make_shared<AllocatorMemoryStrategy<MemoryStrategyAllocatorT>>(
+    shared_memory_strategy_allocator);
+
+  // executor
+  rclcpp::ExecutorOptions options;
+  options.memory_strategy = memory_strategy;
+  options.context = context;
+  rclcpp::executors::SingleThreadedExecutor executor(options);
+
+  executor.add_node(node);
+
+  // rebind message_allocator to ensure correct type
+  PublishedMessageDeleter message_deleter;
+  PublishedMessageAlloc rebound_message_allocator = published_message_allocator;
+  rclcpp::allocator::set_allocator_for_deleter(&message_deleter, &rebound_message_allocator);
+
+  // allocate a message
+  auto ptr = PublishedMessageAllocTraits::allocate(rebound_message_allocator, 1);
+  PublishedMessageAllocTraits::construct(rebound_message_allocator, ptr);
+  std::unique_ptr<test_msgs::msg::Empty, PublishedMessageDeleter> msg(ptr, message_deleter);
+
+  // publisher and receive
+  check_exception<ExpectedExceptionT>(
+    publisher, executor, received_message_future, counter, std::move(msg), ptr);
+}
+
+/*
+   This tests the case where a custom allocator is used correctly, i.e. the same
+   custom allocator on both sides.
+ */
+TEST(TestIntraProcessManagerWithAllocators, custom_allocator) {
+  using PublishedMessageAllocatorT = std::allocator<void>;
+  using PublisherAllocatorT = std::allocator<void>;
+  using SubscribedMessageAllocatorT = std::allocator<void>;
+  using SubscriptionAllocatorT = std::allocator<void>;
+  using MessageMemoryStrategyAllocatorT = std::allocator<void>;
+  using MemoryStrategyAllocatorT = std::allocator<void>;
+  auto allocator = std::allocator<void>();
+  do_custom_allocator_test<
+    PublishedMessageAllocatorT,
+    PublisherAllocatorT,
+    SubscribedMessageAllocatorT,
+    SubscriptionAllocatorT,
+    MessageMemoryStrategyAllocatorT,
+    MemoryStrategyAllocatorT,
+    void  // no exception expected
+  >(allocator, allocator, allocator, allocator, allocator, allocator);
+}
+
+/*
+   This tests the case where a custom allocator is used incorrectly, i.e. different
+   custom allocators on both sides.
+ */
+TEST(TestIntraProcessManagerWithAllocators, custom_allocator_wrong) {
+  // explicitly use a different allocator here to provoke a failure
+  using PublishedMessageAllocatorT = std::allocator<void>;
+  using PublisherAllocatorT = std::allocator<void>;
+  using SubscribedMessageAllocatorT = MyAllocator<void>;
+  using SubscriptionAllocatorT = MyAllocator<void>;
+  using MessageMemoryStrategyAllocatorT = MyAllocator<void>;
+  using MemoryStrategyAllocatorT = std::allocator<void>;
+  auto allocator = std::allocator<void>();
+  auto my_allocator = MyAllocator<void>();
+  do_custom_allocator_test<
+    PublishedMessageAllocatorT,
+    PublisherAllocatorT,
+    SubscribedMessageAllocatorT,
+    SubscriptionAllocatorT,
+    MessageMemoryStrategyAllocatorT,
+    MemoryStrategyAllocatorT,
+    std::runtime_error  // expected exception
+  >(allocator, allocator, my_allocator, my_allocator, my_allocator, allocator);
+}