Support publishing client message as loaned message (#314)

### Changelog
Support publishing client message as loaned message

### Docs
None

### Description
Support publishing client message as loaned message when the middleware
supports it.

[`publish_as_loaned_msg`](https://docs.ros.org/en/humble/p/rclcpp/generated/classrclcpp_1_1GenericPublisher.html#_CPPv4N6rclcpp16GenericPublisher21publish_as_loaned_msgERKN6rclcpp17SerializedMessageE)
is available as of ROS humble.

Fixes #280

README.md

@@ -84,6 +84,7 @@ Parameters are provided to configure the behavior of the bridge. These parameter
  * (ROS 2) __min_qos_depth__: Minimum depth used for the QoS profile of subscriptions. Defaults to `1`. This is to set a lower limit for a subscriber's QoS depth which is computed by summing up depths of all publishers. See also [#208](https://github.com/foxglove/ros-foxglove-bridge/issues/208).
  * (ROS 2) __max_qos_depth__: Maximum depth used for the QoS profile of subscriptions. Defaults to `25`.
  * (ROS 2) __include_hidden__: Include hidden topics and services. Defaults to `false`.
+ * (ROS 2) __disable_load_message__: Do not publish as loaned message when publishing a client message. Defaults to `true`.
 
 ## Building from source
 

ros2_foxglove_bridge/include/foxglove_bridge/param_utils.hpp

@@ -23,6 +23,7 @@ constexpr char PARAM_USE_COMPRESSION[] = "use_compression";
 constexpr char PARAM_CAPABILITIES[] = "capabilities";
 constexpr char PARAM_CLIENT_TOPIC_WHITELIST[] = "client_topic_whitelist";
 constexpr char PARAM_INCLUDE_HIDDEN[] = "include_hidden";
+constexpr char PARAM_DISABLE_LOAN_MESSAGE[] = "disable_load_message";
 constexpr char PARAM_ASSET_URI_ALLOWLIST[] = "asset_uri_allowlist";
 
 constexpr int64_t DEFAULT_PORT = 8765;

ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp

@@ -81,6 +81,7 @@ class FoxgloveBridge : public rclcpp::Node {
   std::vector<std::string> _capabilities;
   std::atomic<bool> _subscribeGraphUpdates = false;
   bool _includeHidden = false;
+  bool _disableLoanMessage = true;
   std::unique_ptr<foxglove::CallbackQueue> _fetchAssetQueue;
   std::atomic<bool> _shuttingDown = false;
 

ros2_foxglove_bridge/src/param_utils.cpp

@@ -147,6 +147,14 @@ void declareParameters(rclcpp::Node* node) {
   includeHiddenDescription.read_only = true;
   node->declare_parameter(PARAM_INCLUDE_HIDDEN, false, includeHiddenDescription);
 
+  auto disableLoanMessageDescription = rcl_interfaces::msg::ParameterDescriptor{};
+  disableLoanMessageDescription.name = PARAM_DISABLE_LOAN_MESSAGE;
+  disableLoanMessageDescription.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  disableLoanMessageDescription.description =
+    "Do not publish as loaned message when publishing a client message";
+  disableLoanMessageDescription.read_only = true;
+  node->declare_parameter(PARAM_DISABLE_LOAN_MESSAGE, true, disableLoanMessageDescription);
+
   auto assetUriAllowlistDescription = rcl_interfaces::msg::ParameterDescriptor{};
   assetUriAllowlistDescription.name = PARAM_ASSET_URI_ALLOWLIST;
   assetUriAllowlistDescription.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY;

ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp

@@ -51,6 +51,7 @@ FoxgloveBridge::FoxgloveBridge(const rclcpp::NodeOptions& options)
   _includeHidden = this->get_parameter(PARAM_INCLUDE_HIDDEN).as_bool();
   const auto assetUriAllowlist = this->get_parameter(PARAM_ASSET_URI_ALLOWLIST).as_string_array();
   _assetUriAllowlistPatterns = parseRegexStrings(this, assetUriAllowlist);
+  _disableLoanMessage = this->get_parameter(PARAM_DISABLE_LOAN_MESSAGE).as_bool();
 
   const auto logHandler = std::bind(&FoxgloveBridge::logHandler, this, _1, _2);
   // Fetching of assets may be blocking, hence we fetch them in a separate thread.
@@ -710,7 +711,11 @@ void FoxgloveBridge::clientMessage(const foxglove::ClientMessage& message, Conne
   rclSerializedMsg.buffer_length = message.getLength();
 
   // Publish the message
-  publisher->publish(serializedMessage);
+  if (_disableLoanMessage || !publisher->can_loan_messages()) {
+    publisher->publish(serializedMessage);
+  } else {
+    publisher->publish_as_loaned_msg(serializedMessage);
+  }
 }
 
 void FoxgloveBridge::setParameters(const std::vector<foxglove::Parameter>& parameters,