From 9270074d5fcac24bd4312dc6bfd60cfa616834b7 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Tue, 12 Jul 2022 16:31:07 +0200 Subject: [PATCH 1/2] Make loaned messages const on the subscription side Signed-off-by: Nikolai Morin --- .../rclcpp/any_subscription_callback.hpp | 18 ++++++++++++++++-- rclcpp/include/rclcpp/generic_subscription.hpp | 2 +- rclcpp/include/rclcpp/subscription.hpp | 8 ++++---- rclcpp/include/rclcpp/subscription_base.hpp | 2 +- rclcpp/src/rclcpp/executor.cpp | 2 +- rclcpp/src/rclcpp/generic_subscription.cpp | 2 +- .../node_interfaces/test_node_topics.cpp | 2 +- 7 files changed, 25 insertions(+), 11 deletions(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index d4f5fc309b..fb2179e16c 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -487,9 +487,13 @@ class AnySubscriptionCallback } // Dispatch when input is a ros message and the output could be anything. - void + template + typename std::enable_if::type + >::value + >::type dispatch( - std::shared_ptr message, + std::shared_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), false); @@ -505,6 +509,7 @@ class AnySubscriptionCallback [&message, &message_info, this](auto && callback) { using T = std::decay_t; static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; + static constexpr bool is_const = std::is_const::value; // conditions for output is custom message if constexpr (is_ta && std::is_same_v) { @@ -548,6 +553,15 @@ class AnySubscriptionCallback callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); } else if constexpr (std::is_same_v) { callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + } else if constexpr ( // NOLINT[readability/braces] + is_const && ( + std::is_same_v|| + std::is_same_v + )) + { + throw std::runtime_error( + "Cannot dispatch std::shared_ptr message " + "to callback taking a non-const message"); } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v|| diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 673712eedb..341984788f 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -146,7 +146,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase /// This function is currently not implemented. RCLCPP_PUBLIC void handle_loaned_message( - void * loaned_message, const rclcpp::MessageInfo & message_info) override; + const void * loaned_message, const rclcpp::MessageInfo & message_info) override; // Same as return_serialized_message() as the subscription is to serialized_messages only RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 11bf9c6e43..2317ec309b 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -360,7 +360,7 @@ class Subscription : public SubscriptionBase void handle_loaned_message( - void * loaned_message, + const void * loaned_message, const rclcpp::MessageInfo & message_info) override { if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) { @@ -369,10 +369,10 @@ class Subscription : public SubscriptionBase return; } - auto typed_message = static_cast(loaned_message); + const auto typed_message = static_cast(loaned_message); // message is loaned, so we have to make sure that the deleter does not deallocate the message - auto sptr = std::shared_ptr( - typed_message, [](ROSMessageType * msg) {(void) msg;}); + auto sptr = std::shared_ptr( + typed_message, [](const ROSMessageType * msg) {(void) msg;}); std::chrono::time_point now; if (subscription_topic_statistics_) { diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index f21f27e864..7313c10610 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -199,7 +199,7 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_PUBLIC virtual void - handle_loaned_message(void * loaned_message, const rclcpp::MessageInfo & message_info) = 0; + handle_loaned_message(const void * loaned_message, const rclcpp::MessageInfo & message_info) = 0; /// Return the message borrowed in create_message. /** \param[in] message Shared pointer to the returned message. */ diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 73b7b8d80d..ae48b0afb1 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -598,7 +598,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) // This is the case where a loaned message is taken from the middleware via // inter-process communication, given to the user for their callback, // and then returned. - void * loaned_msg = nullptr; + const void * loaned_msg = nullptr; // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage // is extened to support subscriptions as well. take_and_do_error_handling( diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index cc50955773..426dbad1a8 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -52,7 +52,7 @@ GenericSubscription::handle_serialized_message( } void GenericSubscription::handle_loaned_message( - void * message, const rclcpp::MessageInfo & message_info) + const void * message, const rclcpp::MessageInfo & message_info) { (void) message; (void) message_info; diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index a99633bec6..a8f0de7d5f 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -70,7 +70,7 @@ class TestSubscription : public rclcpp::SubscriptionBase create_serialized_message() override {return nullptr;} void handle_message(std::shared_ptr &, const rclcpp::MessageInfo &) override {} - void handle_loaned_message(void *, const rclcpp::MessageInfo &) override {} + void handle_loaned_message(const void *, const rclcpp::MessageInfo &) override {} void handle_serialized_message( const std::shared_ptr &, const rclcpp::MessageInfo &) override {} void return_message(std::shared_ptr &) override {} From b7023805bf6894f2aa410ffc71547bd2ec6f6225 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Tue, 12 Jul 2022 18:38:17 +0200 Subject: [PATCH 2/2] Make a copy of the const message when callback requires non-const message Signed-off-by: Nikolai Morin --- rclcpp/include/rclcpp/any_subscription_callback.hpp | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index fb2179e16c..1852603bec 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -553,15 +553,10 @@ class AnySubscriptionCallback callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); } else if constexpr (std::is_same_v) { callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); - } else if constexpr ( // NOLINT[readability/braces] - is_const && ( - std::is_same_v|| - std::is_same_v - )) - { - throw std::runtime_error( - "Cannot dispatch std::shared_ptr message " - "to callback taking a non-const message"); + } else if constexpr (is_const && std::is_same_v) { + callback(std::make_shared(*message)); + } else if constexpr (is_const && std::is_same_v) { + callback(std::make_shared(*message), message_info); } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v||