深入解析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 中更高效地进行节点开发和消息处理。希望通过本文的逐行解析,您能够深入理解这些函数的设计思路,并在实际项目中灵活应用。如果您有任何问题或进一步的需求,欢迎留言讨论。