〖ROS2 源码解析〗rclcpp/include/rclcpp/create_subscription.hpp

深入解析ROS 2的订阅者创建:create_subscription 函数逐行解析

概述

在 ROS 2 中,订阅者是节点接收消息的重要机制。为了简化订阅者的创建和配置,ROS 2 提供了 create_subscription 函数,它封装了订阅者的创建过程,并提供了多种选项来定制订阅行为。本文将详细解析 create_subscription 函数的实现,逐行解释其工作原理,并展示如何在 ROS 2 中创建一个订阅者。

源码

首先,我们来看一下 create_subscription 函数的完整源代码:

// Copyright 2016 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.

#ifndef RCLCPP__CREATE_SUBSCRIPTION_HPP_
#define RCLCPP__CREATE_SUBSCRIPTION_HPP_

#include <chrono>
#include <functional>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>

#include "rclcpp/detail/resolve_enable_topic_statistics.hpp"

#include "rclcpp/node_interfaces/get_node_timers_interface.hpp"
#include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
#include "rclcpp/node_interfaces/node_timers_interface.hpp"
#include "rclcpp/node_interfaces/node_topics_interface.hpp"

#include "rclcpp/create_publisher.hpp"
#include "rclcpp/create_timer.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/subscription_factory.hpp"
#include "rclcpp/subscription_options.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
#include "rmw/qos_profiles.h"

namespace rclcpp
{

namespace detail
{
template<
  typename MessageT,
  typename CallbackT,
  typename AllocatorT,
  typename SubscriptionT,
  typename MessageMemoryStrategyT,
  typename NodeParametersT,
  typename NodeTopicsT
>
typename std::shared_ptr<SubscriptionT>
create_subscription(
  NodeParametersT & node_parameters,
  NodeTopicsT & node_topics,
  const std::string & topic_name,
  const rclcpp::QoS & qos,
  CallbackT && callback,
  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
  ),
  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
    MessageMemoryStrategyT::create_default()
  )
)
{
  using rclcpp::node_interfaces::get_node_topics_interface;
  auto node_topics_interface = get_node_topics_interface(node_topics);

  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>
  subscription_topic_stats = nullptr;

  if (rclcpp::detail::resolve_enable_topic_statistics(
      options,
      *node_topics_interface->get_node_base_interface()))
  {
    if (options.topic_stats_options.publish_period <= std::chrono::milliseconds(0)) {
      throw std::invalid_argument(
              "topic_stats_options.publish_period must be greater than 0, specified value of " +
              std::to_string(options.topic_stats_options.publish_period.count()) + " ms");
    }

    std::shared_ptr<Publisher<statistics_msgs::msg::MetricsMessage>>
    publisher = rclcpp::detail::create_publisher<statistics_msgs::msg::MetricsMessage>(
      node_parameters,
      node_topics_interface,
      options.topic_stats_options.publish_topic,
      options.topic_stats_options.qos);

    subscription_topic_stats =
      std::make_shared<rclcpp::topic_statistics::SubscriptionTopicStatistics>(
      node_topics_interface->get_node_base_interface()->get_name(), publisher);

    std::weak_ptr<
      rclcpp::topic_statistics::SubscriptionTopicStatistics
    > weak_subscription_topic_stats(subscription_topic_stats);
    auto sub_call_back = [weak_subscription_topic_stats]() {
        auto subscription_topic_stats = weak_subscription_topic_stats.lock();
        if (subscription_topic_stats) {
          subscription_topic_stats->publish_message_and_reset_measurements();
        }
      };

    auto node_timer_interface = node_topics_interface->get_node_timers_interface();

    auto timer = create_wall_timer(
      std::chrono::duration_cast<std::chrono::nanoseconds>(
        options.topic_stats_options.publish_period),
      sub_call_back,
      options.callback_group,
      node_topics_interface->get_node_base_interface(),
      node_timer_interface
    );

    subscription_topic_stats->set_publisher_timer(timer);
  }

  auto factory = rclcpp::create_subscription_factory<MessageT>(
    std::forward<CallbackT>(callback),
    options,
    msg_mem_strat,
    subscription_topic_stats
  );

  const rclcpp::QoS & actual_qos = options.qos_overriding_options.get_policy_kinds().size() ?
    rclcpp::detail::declare_qos_parameters(
    options.qos_overriding_options, node_parameters,
    node_topics_interface->resolve_topic_name(topic_name),
    qos, rclcpp::detail::SubscriptionQosParametersTraits{}) :
    qos;

  auto sub = node_topics_interface->create_subscription(topic_name, factory, actual_qos);
  node_topics_interface->add_subscription(sub, options.callback_group);

  return std::dynamic_pointer_cast<SubscriptionT>(sub);
}
}  // namespace detail

/// Create and return a subscription of the given MessageT type.
/**
 * The NodeT type only needs to have a method called get_node_topics_interface()
 * which returns a shared_ptr to a NodeTopicsInterface, or be a
 * NodeTopicsInterface pointer itself.
 *
 * In case `options.qos_overriding_options` is enabling qos parameter overrides,
 * NodeT must also have a method called get_node_parameters_interface()
 * which returns a shared_ptr to a NodeParametersInterface.
 *
 * \tparam MessageT
 * \tparam CallbackT
 * \tparam AllocatorT
 * \tparam SubscriptionT
 * \tparam MessageMemoryStrategyT
 * \tparam NodeT
 * \param node
 * \param topic_name
 * \param qos
 * \param callback
 * \param options
 * \param msg_mem_strat
 * \return the created subscription
 * \throws std::invalid_argument if topic statistics is enabled and the publish period is
 * less than or equal to zero.
 */
template<
  typename MessageT,
  typename CallbackT,
  typename AllocatorT = std::allocator<void>,
  typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType,
  typename NodeT>
typename std::shared_ptr<SubscriptionT>
create_subscription(
  NodeT & node,
  const std::string & topic_name,
  const rclcpp::QoS & qos,
  CallbackT && callback,
  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
  ),
  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
    MessageMemoryStrategyT::create_default()
  )
)
{
  return rclcpp::detail::create_subscription<
    MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>(
    node, node, topic_name, qos, std::forward<CallbackT>(callback), options, msg_mem_strat);
}

/// Create and return a subscription of the given MessageT type.
/**
 * See \ref create_subscription().
 */
template<
  typename MessageT,
  typename CallbackT,
  typename AllocatorT = std::allocator<void>,
  typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType>
typename std::shared_ptr<SubscriptionT>
create_subscription(
  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters,
  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics,
  const std::string & topic_name,
  const rclcpp::QoS & qos,
  CallbackT && callback,
  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
  ),
  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
    MessageMemoryStrategyT::create_default()
  )
)
{
  return rclcpp::detail::create_subscription<
    MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>(
    node_parameters, node_topics, topic_name, qos,
    std::forward<CallbackT>(callback), options, msg_mem_strat);
}

}  // namespace rclcpp

#endif  // RCLCPP__CREATE_SUBSCRIPTION_HPP_

逐行解析

文件头部

// Copyright 2016 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.
  • 解释
    • 这部分是版权声明和许可证信息,说明该代码是由 Open Source Robotics Foundation, Inc. 开发的,并且受 Apache License 2.0 约束。用户可以在遵循许可证的前提下自由使用、修改和分发该代码。

头文件保护

#ifndef RCLCPP__CREATE_SUBSCRIPTION_HPP_
#define RCLCPP__CREATE_SUBSCRIPTION_HPP_
  • 解释
    • 这是头文件保护机制,防止头文件被多次包含。#ifndef 宏检查 RCLCPP__CREATE_SUBSCRIPTION_HPP_ 是否已经定义,如果没有定义,则继续定义它并包含文件内容。

包含的头文件

#include <chrono>
#include <functional>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>

#include "rclcpp/detail/resolve_enable_topic_statistics.hpp"

#include "rclcpp/node_interfaces/get_node_timers_interface.hpp"
#include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
#include "rclcpp/node_interfaces/node_timers_interface.hpp"
#include "rclcpp/node_interfaces/node_topics_interface.hpp"

#include "rclcpp/create_publisher.hpp"
#include "rclcpp/create_timer.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/subscription_factory.hpp"
#include "rclcpp/subscription_options.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
#include "rmw/qos_profiles.h"
  • 解释
    • 这些头文件提供了创建订阅者所需的基础设施:
      • <chrono>: 用于处理时间相关的操作。
      • <functional>: 用于处理回调函数和函数对象。
      • <memory>: 用于智能指针管理。
      • <stdexcept>: 用于处理异常。
      • <string>: 用于字符串处理。
      • <utility>: 提供了如 std::move 之类的实用函数。
      • rclcpp 相关头文件:这些头文件定义了 ROS 2 中的节点接口、计时器接口、订阅者工厂、QoS 设置、订阅者选项以及话题统计的支持。

命名空间声明

namespace rclcpp
{
  • 解释
    • 这行代码声明了 rclcpp 命名空间。rclcpp 是 ROS 2 的核心命名空间,包含了与客户端、服务、节点、发布者、订阅者等相关的功能。

detail::create_subscription 函数

namespace detail
{
template<
  typename MessageT,
  typename CallbackT,
  typename AllocatorT,
  typename SubscriptionT,
  typename MessageMemoryStrategyT,
  typename NodeParametersT,
  typename NodeTopicsT
>
typename std::shared_ptr<SubscriptionT>
create_subscription(
  NodeParametersT & node_parameters,
  NodeTopicsT & node_topics,
  const std::string & topic_name,
  const rclcpp::QoS & qos,
  CallbackT && callback,
  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
  ),
  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
    MessageMemoryStrategyT::create_default()
  )
)
{
函数头解析
  • 函数声明

    • 这是一个模板函数,接受多个模板参数用于灵活创建订阅者对象。它返回一个指向 SubscriptionT 类型对象的共享指针。
  • 参数解析

    • NodeParametersT & node_parameters: 节点参数接口的引用,用于声明 QoS 参数。
    • NodeTopicsT & node_topics: 节点话题接口的引用,用于处理节点的订阅功能。
    • const std::string & topic_name: 要订阅的主题名称。
    • const rclcpp::QoS & qos: QoS 配置,用于控制订阅者的行为。
    • CallbackT && callback: 处理接收到的消息的回调函数。
    • const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options: 订阅者选项,包括分配器和回调组配置。
    • typename MessageMemoryStrategyT::SharedPtr msg_mem_strat: 消息内存策略的共享指针,决定如何管理接收到的消息内存。
订阅者接口获取与话题统计
  using rclcpp::node_interfaces::get_node_topics_interface;
  auto node_topics_interface = get_node_topics_interface(node_topics);

  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>
  subscription_topic_stats = nullptr;

  if (rclcpp::detail::resolve_enable_topic_statistics(
      options,
      *node_topics_interface->get_node_base_interface()))
  {
  • 解释
    • get_node_topics_interface 函数从 node_topics 获取节点的 NodeTopicsInterface,这是处理节点话题接口的必要步骤。
    • 如果在 options 中启用了话题统计,则创建 SubscriptionTopicStatistics 对象,用于管理和发布统计数据。
统计数据发布检查
    if (options.topic_stats_options.publish_period <= std::chrono::milliseconds(0)) {
      throw std::invalid_argument(
              "topic_stats_options.publish_period must be greater than 0, specified value of " +
              std::to_string(options.topic_stats_options.publish_period.count()) + " ms");
    }
  • 解释
    • 如果启用了话题统计,但设置的发布周期小于或等于零,则抛出 std::invalid_argument 异常。统计数据的发布周期必须大于零,以确保数据定期发布。
统计数据发布者的创建
    std::shared_ptr<Publisher<statistics_msgs::msg::MetricsMessage>>
    publisher = rclcpp::detail::create_publisher<statistics_msgs::msg::MetricsMessage>(
      node_parameters,
      node_topics_interface,
      options.topic_stats_options.publish_topic,
      options.topic_stats_options.qos);
  • 解释
    • 创建一个 Publisher 对象,用于发布话题统计数据。create_publisher 函数创建并配置了发布者,使其能够根据 QoS 和其他选项发布统计数据。
统计数据定时器的设置
    subscription_topic_stats =
      std::make_shared<rclcpp::topic_statistics::SubscriptionTopicStatistics>(
      node_topics_interface->get_node_base_interface()->get_name(), publisher);

    std::weak_ptr<
      rclcpp::topic_statistics::SubscriptionTopicStatistics
    > weak_subscription_topic_stats(subscription_topic_stats);
    auto sub_call_back = [weak_subscription_topic_stats]() {
        auto subscription_topic_stats = weak_subscription_topic_stats.lock();
        if (subscription_topic_stats) {
          subscription_topic_stats->publish_message_and_reset_measurements();
        }
      };

    auto node_timer_interface = node_topics_interface->get_node_timers_interface();

    auto timer = create_wall_timer(
      std::chrono::duration_cast<std::chrono::nanoseconds>(
        options.topic_stats_options.publish_period),
      sub_call_back,
      options.callback_group,
      node_topics_interface->get_node_base_interface(),
      node_timer_interface
    );

    subscription_topic_stats->set_publisher_timer(timer);
  }
  • 解释
    • 创建 SubscriptionTopicStatistics 对象,并将其与 Publisher 关联,以管理统计数据的发布。
    • 使用 create_wall_timer 函数创建一个定时器,根据 publish_period 定期触发回调函数 sub_call_back,用于发布统计数据。
    • 将定时器设置到 subscription_topic_stats 对象中,以确保定时器和统计发布逻辑正确关联。
订阅者工厂的创建
  auto factory = rclcpp::create_subscription_factory<MessageT>(
    std::forward<CallbackT>(callback),
    options,
    msg_mem_strat,
    subscription_topic_stats
  );
  • 解释
    • 使用 create_subscription_factory 创建一个订阅者工厂对象 factory。这个工厂负责根据提供的选项、回调函数和内存策略创建订阅者对象。
QoS 参数声明与订阅者创建
  const rclcpp::QoS & actual_qos = options.qos_overriding_options.get_policy_kinds().size() ?
    rclcpp::detail::declare_qos_parameters(
    options.qos_overriding_options, node_parameters,
    node_topics_interface->resolve_topic_name(topic_name),
    qos, rclcpp::detail::SubscriptionQosParametersTraits{}) :
    qos;

  auto sub = node_topics_interface->create_subscription(topic_name, factory, actual_qos);
  node_topics_interface->add_subscription(sub, options.callback_group);

  return std::dynamic_pointer_cast<SubscriptionT>(sub);
}
  • 解释
    • 如果 options.qos_overriding_options 中包含了 QoS 参数的覆盖选项,那么会调用 declare_qos_parameters 函数。这会根据节点参数接口 node_parameters 和话题名 topic_name,结合传入的 QoS 设置 qos,声明或获取最终的 QoS 配置 actual_qos
    • 否则,直接使用传入的 qos 作为最终的 QoS 设置。
  auto sub = node_topics_interface->create_subscription(topic_name, factory, actual_qos);
  • 解释
    • 使用 node_topics_interface->create_subscription 函数,通过工厂对象 factory 和实际 QoS 设置 actual_qos 创建订阅者对象 sub。该对象负责在指定的主题上接收消息,并执行回调函数。
  node_topics_interface->add_subscription(sub, options.callback_group);
  • 解释
    • 将创建的订阅者 sub 添加到 node_topics_interface 中进行管理,并将其与指定的回调组 options.callback_group 关联起来。这有助于在接收到消息时,正确地调度回调函数执行。
  return std::dynamic_pointer_cast<SubscriptionT>(sub);
  • 解释
    • 使用 std::dynamic_pointer_cast 将订阅者对象转换为模板参数 SubscriptionT 指定的类型,并返回给调用者。这确保返回的订阅者指针是正确的类型,并且可以在需要时执行相应的操作。

create_subscription 函数的其他重载

create_subscription 函数有多个重载版本,以适应不同的需求。这些重载版本都调用了 detail::create_subscription 函数,确保在不同上下文中创建订阅者时具有一致的行为。

第一个重载版本
template<
  typename MessageT,
  typename CallbackT,
  typename AllocatorT = std::allocator<void>,
  typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType,
  typename NodeT>
typename std::shared_ptr<SubscriptionT>
create_subscription(
  NodeT & node,
  const std::string & topic_name,
  const rclcpp::QoS & qos,
  CallbackT && callback,
  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
  ),
  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
    MessageMemoryStrategyT::create_default()
  )
)
{
  return rclcpp::detail::create_subscription<
    MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>(
    node, node, topic_name, qos, std::forward<CallbackT>(callback), options, msg_mem_strat);
}
  • 解释
    • 这个重载版本适用于那些提供了 get_node_topics_interface()get_node_parameters_interface() 方法的 NodeT 类型。它简化了调用过程,用户只需传入节点对象、主题名称、QoS 和选项,即可创建订阅者。
第二个重载版本
template<
  typename MessageT,
  typename CallbackT,
  typename AllocatorT = std::allocator<void>,
  typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType>
typename std::shared_ptr<SubscriptionT>
create_subscription(
  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters,
  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics,
  const std::string & topic_name,
  const rclcpp::QoS & qos,
  CallbackT && callback,
  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
  ),
  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
    MessageMemoryStrategyT::create_default()
  )
)
{
  return rclcpp::detail::create_subscription<
    MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>(
    node_parameters, node_topics, topic_name, qos,
    std::forward<CallbackT>(callback), options, msg_mem_strat);
}
  • 解释
    • 这个重载版本直接接收节点参数接口 NodeParametersInterface 和节点话题接口 NodeTopicsInterface,适用于那些已经明确拆分出这些接口的场景。
    • 它确保了用户可以使用最灵活的方式创建订阅者,并且保持了与其他函数的接口一致性。

结束 rclcpp 命名空间与头文件保护

}  // namespace rclcpp

#endif  // RCLCPP__CREATE_SUBSCRIPTION_HPP_
  • 解释
    • 这部分代码关闭了 rclcpp 命名空间,并结束了头文件的防重复包含保护。

总结

create_subscription 函数为 ROS 2 提供了强大且灵活的订阅者创建功能。通过多个重载版本,用户可以根据具体需求选择最合适的方式创建订阅者。无论是处理简单的消息订阅,还是需要动态调整 QoS 参数,create_subscription 都能够有效地简化开发过程,同时确保代码的可读性和可维护性。

理解并掌握 create_subscription 函数的工作原理,可以帮助开发者在 ROS 2 中更高效地进行节点开发和消息处理。希望通过本文的逐行解析,您能够深入理解这些函数的设计思路,并在实际项目中灵活应用。如果您有任何问题或进一步的需求,欢迎留言讨论。

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值