only allow const callbacks for loaned messages

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -180,6 +180,20 @@ public:
     TRACEPOINT(callback_end, (const void *)this);
   }
 
+  void dispatch(
+    std::shared_ptr<const MessageT> message, const rmw_message_info_t & message_info)
+  {
+    TRACEPOINT(callback_start, (const void*)this, false);
+    if (const_shared_ptr_callback_) {
+      const_shared_ptr_callback_(message);
+    } else if (const_shared_ptr_with_info_callback_) {
+      const_shared_ptr_with_info_callback_(message, message_info);
+    } else {
+      throw std::runtime_error("can't dispatch const message to non-cost callback");
+    }
+    TRACEPOINT(callback_end, (const void *)this);
+  }
+
   void dispatch_intra_process(
     ConstMessageSharedPtr message, const rmw_message_info_t & message_info)
   {

rclcpp/include/rclcpp/subscription.hpp

@@ -221,12 +221,12 @@ public:
 
   void
   handle_loaned_message(
-    void * loaned_message, const rmw_message_info_t & message_info) override
+    const void * loaned_message, const rmw_message_info_t & message_info) override
   {
-    auto typed_message = static_cast<CallbackMessageT *>(loaned_message);
+    auto typed_message = static_cast<const CallbackMessageT *>(loaned_message);
     // message is loaned, so we have to make sure that the deleter does not deallocate the message
-    auto sptr = std::shared_ptr<CallbackMessageT>(
-      typed_message, [](CallbackMessageT * msg) {(void) msg;});
+    auto sptr = std::shared_ptr<const CallbackMessageT>(
+      typed_message, [](const CallbackMessageT * msg) {(void) msg;});
     any_callback_.dispatch(sptr, message_info);
   }
 

rclcpp/include/rclcpp/subscription_base.hpp

@@ -137,7 +137,7 @@ public:
   RCLCPP_PUBLIC
   virtual
   void
-  handle_loaned_message(void * loaned_message, const rmw_message_info_t & message_info) = 0;
+  handle_loaned_message(const void * loaned_message, const rmw_message_info_t & message_info) = 0;
 
   /// Return the message borrowed in create_message.
   /** \param[in] message Shared pointer to the returned message. */

rclcpp/src/rclcpp/executor.cpp

@@ -329,7 +329,7 @@ Executor::execute_subscription(
     }
     subscription->return_serialized_message(serialized_msg);
   } else if (subscription->can_loan_messages()) {
-    void * loaned_msg = nullptr;
+    const void * loaned_msg = nullptr;
     auto ret = rcl_take_loaned_message(
       subscription->get_subscription_handle().get(),
       &loaned_msg,