Robot Operating System——深度解析“借用内存型消息”

在《Robot Operating System——借用内存型消息》一文中，我们介绍了“借用内存型消息”的特点和使用方法。本文我们将深度分析下“借用内存型消息”为什么可以节省内存分配。

我们将通过两个时期相关消息的处理流程来做对比，看看普通消息、序列化消息
内存借用型消息在过程中分配做了哪些内存分配相关的操作。

准备期

普通消息

作为对比，我们先看下普通的消息的内存分配情况。

// /opt/ros/jazzy/include/rclcpp/rclcpp/publisher.hpp

  /// Publish a message on the topic.
  /**
   * This signature is enabled if the object being published is
   * a ROS message type, as opposed to the custom_type of a TypeAdapter, and
   * that type matches the type given when creating the publisher.
   *
   * This signature allows the user to give a reference to a message, which is
   * copied onto the heap without modification so that a copy can be owned by
   * rclcpp and ownership of the copy can be moved later if needed.
   *
   * \param[in] msg A const reference to the message to send.
   */
  template<typename T>
  typename std::enable_if_t<
    rosidl_generator_traits::is_message<T>::value &&
    std::is_same<T, ROSMessageType>::value
  >
  publish(const T & msg)
  {
    // Avoid allocating when not using intra process.
    if (!intra_process_is_enabled_) {
      this->do_inter_process_publish(msg);
      return;
    }
    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
    // As the message is not const, a copy should be made.
    // A shared_ptr<const MessageT> could also be constructed here.
    auto unique_msg = this->duplicate_ros_message_as_unique_ptr(msg);
    this->publish(std::move(unique_msg));
  }
  
  /// Publish a message on the topic.
  /**
   * This signature is enabled if this class was created with a TypeAdapter and
   * the given type matches the custom_type of the TypeAdapter.
   *
   * This signature allows the user to give a reference to a message, which is
   * copied onto the heap without modification so that a copy can be owned by
   * rclcpp and ownership of the copy can be moved later if needed.
   *
   * \param[in] msg A const reference to the message to send.
   */
  template<typename T>
  typename std::enable_if_t<
    rclcpp::TypeAdapter<MessageT>::is_specialized::value &&
    std::is_same<T, PublishedType>::value
  >
  publish(const T & msg)
  {
    if (!intra_process_is_enabled_) {
      // Convert to the ROS message equivalent and publish it.
      auto ros_msg_ptr = std::make_unique<ROSMessageType>();
      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(msg, *ros_msg_ptr);
      this->do_inter_process_publish(*ros_msg_ptr);
      return;
    }

    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
    // As the message is not const, a copy should be made.
    // A shared_ptr<const MessageT> could also be constructed here.
    auto unique_msg = this->duplicate_type_adapt_message_as_unique_ptr(msg);
    this->publish(std::move(unique_msg));
  }

上面代码显示，这个函数至少会做一次内存分配：

• std::make_unique<ROSMessageType>()
• duplicate_type_adapt_message_as_unique_ptr：它会先使用ROSMessageTypeAllocatorTraits::allocate分配一块内存，然后调用ROSMessageTypeAllocatorTraits::construct构建对应的结构。

  std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>
  duplicate_ros_message_as_unique_ptr(const ROSMessageType & msg)
  {
    auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1);
    ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr, msg);
    return std::unique_ptr<ROSMessageType, ROSMessageTypeDeleter>(ptr, ros_message_type_deleter_);
  }

经过这次内存分配后，消息会进入下面的函数

/// Publish a message on the topic.
  /**
   * This signature is enabled if this class was created with a TypeAdapter and
   * the element_type of the std::unique_ptr matches the custom_type for the
   * TypeAdapter used with this class.
   *
   * This signature allows the user to give ownership of the message to rclcpp,
   * allowing for more efficient intra-process communication optimizations.
   *
   * \param[in] msg A unique pointer to the message to send.
   */
  template<typename T>
  typename std::enable_if_t<
    rclcpp::TypeAdapter<MessageT>::is_specialized::value &&
    std::is_same<T, PublishedType>::value
  >
  publish(std::unique_ptr<T, PublishedTypeDeleter> msg)
  {
    if (!intra_process_is_enabled_) {
      // In this case we're not using intra process.
      auto ros_msg_ptr = std::make_unique<ROSMessageType>();
      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *ros_msg_ptr);
      this->do_inter_process_publish(*ros_msg_ptr);
      return;
    }

    bool inter_process_publish_needed =
      get_subscription_count() > get_intra_process_subscription_count();

    if (inter_process_publish_needed) {
      auto ros_msg_ptr = std::make_shared<ROSMessageType>();
      rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *ros_msg_ptr);
      this->do_intra_process_publish(std::move(msg));
      this->do_inter_process_publish(*ros_msg_ptr);
      if (buffer_) {
        buffer_->add_shared(ros_msg_ptr);
      }
    } else {
      if (buffer_) {
        auto ros_msg_ptr = std::make_shared<ROSMessageType>();
        rclcpp::TypeAdapter<MessageT>::convert_to_ros_message(*msg, *ros_msg_ptr);
        buffer_->add_shared(ros_msg_ptr);
      }
      this->do_intra_process_publish(std::move(msg));
    }
  }

可以看到中间还会有智能指针的内存分配。

最终转换后的消息会进入下面两个函数

• do_intra_process_publish
• do_inter_process_publish

后续我们主要关注do_inter_process_publish的实现。因为它是用于跨进程通信的，更具有通用性。而do_intra_process_publish用于进程内通信的。

  void
  do_inter_process_publish(const ROSMessageType & msg)
  {
    TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(&msg));
    auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr);

    if (RCL_RET_PUBLISHER_INVALID == status) {
      rcl_reset_error();  // next call will reset error message if not context
      if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) {
        rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get());
        if (nullptr != context && !rcl_context_is_valid(context)) {
          // publisher is invalid due to context being shutdown
          return;
        }
      }
    }
    if (RCL_RET_OK != status) {
      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message");
    }
  }

在do_inter_process_publish中没有什么内存分配的情况。最终消息进入rcl_publish方法。

序列化消息

下面代码可以看到，序列化消息的处理也很干净利落，没什么智能指针分配等现象。

// /opt/ros/jazzy/include/rclcpp/rclcpp/publisher.hpp

  void
  publish(const rcl_serialized_message_t & serialized_msg)
  {
    return this->do_serialized_publish(&serialized_msg);
  }

  void
  publish(const SerializedMessage & serialized_msg)
  {
    return this->do_serialized_publish(&serialized_msg.get_rcl_serialized_message());
  }

  void
  do_serialized_publish(const rcl_serialized_message_t * serialized_msg)
  {
    if (intra_process_is_enabled_) {
      // TODO(Karsten1987): support serialized message passed by intraprocess
      throw std::runtime_error("storing serialized messages in intra process is not supported yet");
    }
    auto status = rcl_publish_serialized_message(publisher_handle_.get(), serialized_msg, nullptr);
    if (RCL_RET_OK != status) {
      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message");
    }
  }

最终消息进入rcl_publish_serialized_message。

借用内存型消息

和序列化消息类似，借用内存型消息也没经过什么内存分配，而直接进入rcl_publish_loaned_message方法。

// /opt/ros/jazzy/include/rclcpp/rclcpp/publisher.hpp

  void
  do_loaned_message_publish(
    std::unique_ptr<ROSMessageType, std::function<void(ROSMessageType *)>> msg)
  {
    TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast<const void *>(msg.get()));
    auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg.get(), nullptr);

    if (RCL_RET_PUBLISHER_INVALID == status) {
      rcl_reset_error();  // next call will reset error message if not context
      if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) {
        rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get());
        if (nullptr != context && !rcl_context_is_valid(context)) {
          // publisher is invalid due to context being shutdown
          return;
        }
      }
    }
    if (RCL_RET_OK != status) {
      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message");
    }
  }

处理期

rcl

我们分别看下普通消息对应的rcl_publish、序列化消息对应的rcl_publish_serialized_message和借用内存型消息对应的rcl_publish_loaned_message的实现。

// https://github.com/ros2/rcl/blob/master/rcl/src/rcl/publisher.c

rcl_ret_t
rcl_publish(
  const rcl_publisher_t * publisher,
  const void * ros_message,
  rmw_publisher_allocation_t * allocation)
{
  RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_PUBLISHER_INVALID);
  RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR);

  if (!rcl_publisher_is_valid(publisher)) {
    return RCL_RET_PUBLISHER_INVALID;  // error already set
  }
  RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT);
  TRACETOOLS_TRACEPOINT(rcl_publish, (const void *)publisher, (const void *)ros_message);
  if (rmw_publish(publisher->impl->rmw_handle, ros_message, allocation) != RMW_RET_OK) {
    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
    return RCL_RET_ERROR;
  }
  return RCL_RET_OK;
}

rcl_ret_t
rcl_publish_serialized_message(
  const rcl_publisher_t * publisher,
  const rcl_serialized_message_t * serialized_message,
  rmw_publisher_allocation_t * allocation)
{
  if (!rcl_publisher_is_valid(publisher)) {
    return RCL_RET_PUBLISHER_INVALID;  // error already set
  }
  RCL_CHECK_ARGUMENT_FOR_NULL(serialized_message, RCL_RET_INVALID_ARGUMENT);
  TRACETOOLS_TRACEPOINT(rcl_publish, (const void *)publisher, (const void *)serialized_message);
  rmw_ret_t ret = rmw_publish_serialized_message(
    publisher->impl->rmw_handle, serialized_message, allocation);
  if (ret != RMW_RET_OK) {
    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
    if (ret == RMW_RET_BAD_ALLOC) {
      return RCL_RET_BAD_ALLOC;
    }
    return RCL_RET_ERROR;
  }
  return RCL_RET_OK;
}

rcl_ret_t
rcl_publish_loaned_message(
  const rcl_publisher_t * publisher,
  void * ros_message,
  rmw_publisher_allocation_t * allocation)
{
  if (!rcl_publisher_is_valid(publisher)) {
    return RCL_RET_PUBLISHER_INVALID;  // error already set
  }
  RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT);
  TRACETOOLS_TRACEPOINT(rcl_publish, (const void *)publisher, (const void *)ros_message);
  rmw_ret_t ret = rmw_publish_loaned_message(publisher->impl->rmw_handle, ros_message, allocation);
  if (ret != RMW_RET_OK) {
    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
    return RCL_RET_ERROR;
  }
  return RCL_RET_OK;
}

可以看到它们过程相似度极高，只是最终走向了不同的函数：

• rcl_publish->rmw_publish
• rcl_publish_serialized_message->rmw_publish_serialized_message
• rcl_publish_loaned_message->rmw_publish_loaned_message

rmw

可以看到上述几个函数在rmw层也相似性极高。

// https://github.com/ros2/rmw_fastrtps/blob/master/rmw_fastrtps_cpp/src/rmw_publish.cpp

rmw_ret_t
rmw_publish(
  const rmw_publisher_t * publisher,
  const void * ros_message,
  rmw_publisher_allocation_t * allocation)
{
  return rmw_fastrtps_shared_cpp::__rmw_publish(
    eprosima_fastrtps_identifier, publisher, ros_message, allocation);
}

rmw_ret_t
rmw_publish_serialized_message(
  const rmw_publisher_t * publisher,
  const rmw_serialized_message_t * serialized_message,
  rmw_publisher_allocation_t * allocation)
{
  return rmw_fastrtps_shared_cpp::__rmw_publish_serialized_message(
    eprosima_fastrtps_identifier, publisher, serialized_message, allocation);
}

rmw_ret_t
rmw_publish_loaned_message(
  const rmw_publisher_t * publisher,
  void * ros_message,
  rmw_publisher_allocation_t * allocation)
{
  return rmw_fastrtps_shared_cpp::__rmw_publish_loaned_message(
    eprosima_fastrtps_identifier, publisher, ros_message, allocation);
}

最终它们又走向不同函数：

• rmw_publish->__rmw_publish
• rmw_publish_serialized_message->__rmw_publish_serialized_message
• rmw_publish_loaned_message->__rmw_publish_loaned_message

// https://github.com/ros2/rmw_fastrtps/blob/master/rmw_fastrtps_shared_cpp/src/rmw_publish.cpp

rmw_ret_t
__rmw_publish(
  const char * identifier,
  const rmw_publisher_t * publisher,
  const void * ros_message,
  rmw_publisher_allocation_t * allocation)
{
  ……

  auto info = static_cast<CustomPublisherInfo *>(publisher->data);
  RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "publisher info pointer is null", return RMW_RET_ERROR);

  rmw_fastrtps_shared_cpp::SerializedData data;
  data.type = FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE;
  data.data = const_cast<void *>(ros_message);
  data.impl = info->type_support_impl_;
  eprosima::fastrtps::Time_t stamp;
  eprosima::fastrtps::Time_t::now(stamp);
  TRACETOOLS_TRACEPOINT(rmw_publish, publisher, ros_message, stamp.to_ns());
  if (!info->data_writer_->write_w_timestamp(&data, eprosima::fastdds::dds::HANDLE_NIL, stamp)) {
    RMW_SET_ERROR_MSG("cannot publish data");
    return RMW_RET_ERROR;
  }

  return RMW_RET_OK;
}

rmw_ret_t
__rmw_publish_serialized_message(
  const char * identifier,
  const rmw_publisher_t * publisher,
  const rmw_serialized_message_t * serialized_message,
  rmw_publisher_allocation_t * allocation)
{
  ……

  auto info = static_cast<CustomPublisherInfo *>(publisher->data);
  RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "publisher info pointer is null", return RMW_RET_ERROR);

  eprosima::fastcdr::FastBuffer buffer(
    reinterpret_cast<char *>(serialized_message->buffer), serialized_message->buffer_length);
  eprosima::fastcdr::Cdr ser(
    buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::CdrVersion::XCDRv1);
  ser.set_encoding_flag(eprosima::fastcdr::EncodingAlgorithmFlag::PLAIN_CDR);
  if (!ser.jump(serialized_message->buffer_length)) {
    RMW_SET_ERROR_MSG("cannot correctly set serialized buffer");
    return RMW_RET_ERROR;
  }

  rmw_fastrtps_shared_cpp::SerializedData data;
  data.type = FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER;
  data.data = &ser;
  data.impl = nullptr;  // not used when type is FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER
  eprosima::fastrtps::Time_t stamp;
  eprosima::fastrtps::Time_t::now(stamp);
  TRACETOOLS_TRACEPOINT(rmw_publish, publisher, serialized_message, stamp.to_ns());
  if (!info->data_writer_->write_w_timestamp(&data, eprosima::fastdds::dds::HANDLE_NIL, stamp)) {
    RMW_SET_ERROR_MSG("cannot publish data");
    return RMW_RET_ERROR;
  }

  return RMW_RET_OK;
}
rmw_ret_t
__rmw_publish_loaned_message(
  const char * identifier,
  const rmw_publisher_t * publisher,
  const void * ros_message,
  rmw_publisher_allocation_t * allocation)
{
  ……

  auto info = static_cast<CustomPublisherInfo *>(publisher->data);
  eprosima::fastrtps::Time_t stamp;
  eprosima::fastrtps::Time_t::now(stamp);
  TRACETOOLS_TRACEPOINT(rmw_publish, publisher, ros_message, stamp.to_ns());
  if (!info->data_writer_->write_w_timestamp(
      const_cast<void *>(ros_message),
      eprosima::fastdds::dds::HANDLE_NIL, stamp))
  {
    RMW_SET_ERROR_MSG("cannot publish data");
    return RMW_RET_ERROR;
  }

  return RMW_RET_OK;
}

我们看到除了序列化消息的流程稍微复杂点，普通消息和借用内存型消息的处理流程差不多。而且序列化消息处理流程中的FastBuffer的生成也不用分配额外的空间来承载消息数据，它底层只是简单的赋值操作。

// https://github.com/eProsima/Fast-CDR/blob/master/src/cpp/FastBuffer.cpp
FastBuffer::FastBuffer(
        char* const buffer,
        const size_t bufferSize)
    : buffer_(buffer)
    , size_(bufferSize)
    , m_internalBuffer(false)
{
}

我们继续跟踪下去，去看下它们底层都要走向的write_w_timestamp的实现。

write_w_timestamp是CustomPublisherInfo::data_writer_的方法，CustomPublisherInfo::data_writer_的类型是eprosima::fastdds::dds::DataWriter。

// https://github.com/eProsima/Fast-DDS/blob/master/src/cpp/fastdds/publisher/DataWriter.cpp
ReturnCode_t DataWriter::write_w_timestamp(
        const void* const data,
        const InstanceHandle_t& handle,
        const fastdds::Time_t& timestamp)
{
    return impl_->write_w_timestamp(data, handle, timestamp);
}

impl_是DataWriterImpl对象

ReturnCode_t DataWriterImpl::write_w_timestamp(
        const void* const data,
        const InstanceHandle_t& handle,
        const fastdds::Time_t& timestamp)
{
    InstanceHandle_t instance_handle;
    ReturnCode_t ret = RETCODE_OK;
    if (timestamp.is_infinite() || timestamp.seconds < 0)
    {
        ret = RETCODE_BAD_PARAMETER;
    }

    if (RETCODE_OK == ret)
    {
        ret = check_write_preconditions(data, handle, instance_handle);
    }

    if (RETCODE_OK == ret)
    {
        EPROSIMA_LOG_INFO(DATA_WRITER, "Writing new data with Handle and timestamp");
        WriteParams wparams;
        wparams.source_timestamp(timestamp);
        ret = create_new_change_with_params(ALIVE, data, wparams, instance_handle);
    }

    return ret;
}

ReturnCode_t DataWriterImpl::create_new_change_with_params(
        ChangeKind_t changeKind,
        const void* const data,
        WriteParams& wparams)
{
    ReturnCode_t ret_code = check_new_change_preconditions(changeKind, data);
    if (RETCODE_OK != ret_code)
    {
        return ret_code;
    }

    InstanceHandle_t handle;
    if (type_->is_compute_key_provided)
    {
        bool is_key_protected = false;
#if HAVE_SECURITY
        is_key_protected = writer_->getAttributes().security_attributes().is_key_protected;
#endif // if HAVE_SECURITY
        type_->compute_key(data, handle, is_key_protected);
    }

    return perform_create_new_change(changeKind, data, wparams, handle);
}

其底层最终走向了perform_create_new_change方法。

perform_create_new_change也真是我们解开迷局的函数。

perform_create_new_change函数略长，就不贴出来全部，我们看下和本主题相关的部分。

// https://github.com/eProsima/Fast-DDS/blob/master/src/cpp/fastdds/publisher/DataWriterImpl.cpp

    SerializedPayload_t payload;
    bool was_loaned = check_and_remove_loan(data, payload);
    if (!was_loaned)
    {
        uint32_t payload_size = fixed_payload_size_ ? fixed_payload_size_ : type_->calculate_serialized_size(
            data, data_representation_);
        if (!get_free_payload_from_pool(payload_size, payload))
        {
            return RETCODE_OUT_OF_RESOURCES;
        }

        if ((ALIVE == change_kind) && !type_->serialize(data, payload, data_representation_))
        {
            EPROSIMA_LOG_WARNING(DATA_WRITER, "Data serialization returned false");
            payload_pool_->release_payload(payload);
            return RETCODE_ERROR;
        }
    }

这段逻辑会判断消息是否是“借用内存型消息”。如果不是，就会进入if的逻辑中。这样普通消息和序列化消息都会进入这段逻辑。

我们看到在上述代码的下面两行进行了数据序列化。

SerializedPayload_t payload;
……
type_->serialize(data, payload, data_representation_)
……

这意味着在这层，普通消息和序列化消息比借用内存型消息多了一次内存序列化。

总结

在前期，普通消息处理会有很多智能指针以及内存分配操作；而序列化消息和借用内存型消息没有太多复杂操作。
在后期，普通消息和序列化消息比借用内存型消息多了一次序列化。
所以整体看，借用内存型消息的效率是最高的。