国内的外贸b2c网站百度官网下载安装到桌面上
术语
- DDS (Data Distribution Service): 用于实时系统的数据分发服务标准,是ROS 2底层通信的基础
- RMW (ROS Middleware): ROS中间件接口,提供与具体DDS实现无关的抽象API
- QoS (Quality of Service): 服务质量策略,控制通信的可靠性、历史记录、耐久性等属性
- 符号解析: 动态库加载过程中,查找和绑定函数指针的机制
1. 架构概述
ROS2采用分层设计,通过多层抽象实现高度灵活性和可插拔性。其调用链如下:
应用层(开发人员) → rclcpp(C++客户端库) → rcl(C语言客户端库) → rmw(中间件接口) → rmw_implementation(动态加载层) → 具体DDS实现
2. 发布者创建流程详解
2.1 用户代码层(rclcpp::Node::create_publisher)
通常使用以下代码创建发布者:
auto node = std::make_shared<rclcpp::Node>("publisher_node");
auto publisher = node->create_publisher<std_msgs::msg::String>("topic_name", 10); // 队列深度为10
2.2 rclcpp层(Node类)
在node.hpp
文件中对各模板函数进行声明,函数的具体实现在node_impl.hpp
文件中进行定义。
class Node : public std::enable_shared_from_this<Node>
{template<typename MessageT,typename AllocatorT = std::allocator<void>,typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>>std::shared_ptr<PublisherT>create_publisher(const std::string & topic_name,const rclcpp::QoS & qos,const PublisherOptionsWithAllocator<AllocatorT> & options =PublisherOptionsWithAllocator<AllocatorT>());
}
node_impl.hpp
文件中,node->create_publisher
调用一系列rclcpp函数:
template<typename MessageT, typename AllocatorT, typename PublisherT>
std::shared_ptr<PublisherT>
Node::create_publisher(const std::string & topic_name,const rclcpp::QoS & qos,const PublisherOptionsWithAllocator<AllocatorT> & options)
{return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(*this,extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),qos,options);
}
该方法调用create_publisher.hpp
文件的node_topics_interface->create_publisher
创建类型特定的工厂
template<typename MessageT,typename AllocatorT = std::allocator<void>,typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>,typename NodeParametersT,typename NodeTopicsT>
std::shared_ptr<PublisherT>
create_publisher(NodeParametersT & node_parameters,NodeTopicsT & node_topics,const std::string & topic_name,const rclcpp::QoS & qos,const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (rclcpp::PublisherOptionsWithAllocator<AllocatorT>())
)
{auto node_topics_interface = rclcpp::node_interfaces::get_node_topics_interface(node_topics);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::PublisherQosParametersTraits{}) :qos;// Create the publisher.auto pub = node_topics_interface->create_publisher(topic_name,rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(options),actual_qos);// Add the publisher to the node topics interface.node_topics_interface->add_publisher(pub, options.callback_group);return std::dynamic_pointer_cast<PublisherT>(pub);
}
node_topics_interface->create_publisher
实际调用node_topics.cpp
文件的NodeTopics::create_publisher
非模板方法,该方法使用publisher_factory.hpp
文件中工厂的create_typed_publisher
函数创建实际的发布者,创建的发布者进行后期初始化设置。
rclcpp::PublisherBase::SharedPtr
NodeTopics::create_publisher(const std::string & topic_name,const rclcpp::PublisherFactory & publisher_factory,const rclcpp::QoS & qos)
{// Create the MessageT specific Publisher using the factory, but return it as PublisherBase.return publisher_factory.create_typed_publisher(node_base_, topic_name, qos);
}
而create_typed_publisher
的实现如下:
template<typename MessageT, typename AllocatorT, typename PublisherT>
PublisherFactory
create_publisher_factory(const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
{PublisherFactory factory {// factory function that creates a MessageT specific PublisherT[options](rclcpp::node_interfaces::NodeBaseInterface * node_base,const std::string & topic_name,const rclcpp::QoS & qos) -> std::shared_ptr<PublisherT>{auto publisher = std::make_shared<PublisherT>(node_base, topic_name, qos, options);// This is used for setting up things like intra process comms which// require this->shared_from_this() which cannot be called from// the constructor.publisher->post_init_setup(node_base, topic_name, qos, options);return publisher;}};// return the factory now that it is populatedreturn factory;
}
创建特定的Publisher时,publisher.hpp
文件内部会转到具体的Publisher构造函数:
Publisher(rclcpp::node_interfaces::NodeBaseInterface * node_base,const std::string & topic,const rclcpp::QoS & qos,const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
: PublisherBase(node_base,topic,rclcpp::get_message_type_support_handle<MessageT>(),options.template to_rcl_publisher_options<MessageT>(qos)),options_(options),published_type_allocator_(*options.get_allocator()),ros_message_type_allocator_(*options.get_allocator())
{// 初始化内存分配器和事件回调
}
PublisherBase构造函数则进行RCL层的publisher初始化
PublisherBase::PublisherBase(rclcpp::node_interfaces::NodeBaseInterface * node_base,const std::string & topic,const rosidl_message_type_support_t & type_support,const rcl_publisher_options_t & publisher_options)
: node_base_(node_base),topic_(topic)
{// 创建rcl_publisher_t实例publisher_handle_ = std::shared_ptr<rcl_publisher_t>(new rcl_publisher_t, [node_handle](rcl_publisher_t * publisher) {if (rcl_publisher_fini(publisher, node_handle) != RCL_RET_OK) {RCLCPP_ERROR(/* ... */);}delete publisher;});*publisher_handle_.get() = rcl_get_zero_initialized_publisher();// 关键调用:初始化RCL层publisherrcl_ret_t ret = rcl_publisher_init(publisher_handle_.get(),node_base->get_rcl_node_handle(),&type_support,topic.c_str(),&publisher_options);if (ret != RCL_RET_OK) {// 错误处理}
}
2.3 rcl层
rcl_publisher_init
函数进一步处理:
rcl_ret_t
rcl_publisher_init(rcl_publisher_t * publisher,const rcl_node_t * node,const rosidl_message_type_support_t * type_support,const char * topic_name,const rcl_publisher_options_t * options
)
{// 参数验证和初始化// 向RMW层请求创建发布者// rmw_handle为rmw_publisher_t *类型publisher->impl->rmw_handle = rmw_create_publisher(rcl_node_get_rmw_handle(node),type_support,remapped_topic_name,&(options->qos),&(options->rmw_publisher_options));// 错误处理与返回值设置
}
2.4 rmw层(通过rmw_implementation)
此处进入rmw_implementation
包的functions.cpp
中,其中rmw_create_publisher
为rmw.h
文件中定义的接口函数:
RMW_INTERFACE_FN(rmw_create_publisher,rmw_publisher_t *, nullptr,5, ARG_TYPES(const rmw_node_t *, const rosidl_message_type_support_t *, const char *,const rmw_qos_profile_t *, const rmw_publisher_options_t *))
这个宏展开后生成:
#define RMW_INTERFACE_FN(name, ReturnType, error_value, _NR, ...) \void * symbol_ ## name = nullptr; \ReturnType name(EXPAND(ARGS_ ## _NR(__VA_ARGS__))) \{ \CALL_SYMBOL( \name, ReturnType, error_value, ARG_TYPES(__VA_ARGS__), \EXPAND(ARG_VALUES_ ## _NR(__VA_ARGS__))); \}
CALL_SYMBOL
宏进一步展开:
#define CALL_SYMBOL(symbol_name, ReturnType, error_value, ArgTypes, arg_values) \if (!symbol_ ## symbol_name) { \/* only necessary for functions called before rmw_init */ \//获取库中的函数符号symbol_ ## symbol_name = get_symbol(#symbol_name); \} \if (!symbol_ ## symbol_name) { \/* error message set by get_symbol() */ \return error_value; \} \typedef ReturnType (* FunctionSignature)(ArgTypes); \// 根据函数地址,调用加载的函数FunctionSignature func = reinterpret_cast<FunctionSignature>(symbol_ ## symbol_name); \return func(arg_values);
get_symbol
函数获取函数符号:
void *
get_symbol(const char * symbol_name)
{try {return lookup_symbol(get_library(), symbol_name);} catch (const std::exception & e) {RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("failed to get symbol '%s' due to %s",symbol_name, e.what());return nullptr;}
}
2.5 加载和符号查找
关键的lookup_symbol
函数负责从已加载的库中查找符号:
void *
lookup_symbol(std::shared_ptr<rcpputils::SharedLibrary> lib, const std::string & symbol_name)
{if (!lib) {if (!rmw_error_is_set()) {RMW_SET_ERROR_MSG("no shared library to lookup");} // else assume library loading failedreturn nullptr;}if (!lib->has_symbol(symbol_name)) {try {std::string library_path = lib->get_library_path();RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("failed to resolve symbol '%s' in shared library '%s'",symbol_name.c_str(), library_path.c_str());} catch (const std::exception & e) {RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("failed to resolve symbol '%s' in shared library due to %s",symbol_name.c_str(), e.what());}return nullptr;}// 返回找到的函数指针return lib->get_symbol(symbol_name);
}
get_library()
函数负责加载RMW实现库:
std::shared_ptr<rcpputils::SharedLibrary> get_library()
{if (!g_rmw_lib) {// 懒加载策略 - 首次使用时加载g_rmw_lib = load_library();}return g_rmw_lib;
}
2.6 具体DDS实现
在rmw_publisher.cpp
文件中,调用rmw_fastrtps_cpp::create_publisher
函数进行publisher创建。最终,通过符号解析得到的函数指针指向特定DDS实现(如FastDDS、CycloneDDS)提供的具体rmw_create_publisher
实现:
// 在FastDDS中的实现示例
rmw_publisher_t *
rmw_fastrtps_cpp::create_publisher(const CustomParticipantInfo * participant_info,const rosidl_message_type_support_t * type_supports,const char * topic_name,const rmw_qos_profile_t * qos_policies,const rmw_publisher_options_t * publisher_options,bool keyed,bool create_publisher_listener)
{rmw_publisher_t * publisher = rmw_fastrtps_cpp::create_publisher(participant_info,type_supports,topic_name,qos_policies,publisher_options,false,true);// 创建并返回rmw_publisher_t结构
}rmw_publisher_t *
rmw_fastrtps_cpp::create_publisher(const CustomParticipantInfo * participant_info,const rosidl_message_type_support_t * type_supports,const char * topic_name,const rmw_qos_profile_t * qos_policies,const rmw_publisher_options_t * publisher_options,bool keyed,bool create_publisher_listener)
{eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_;...
}
3. 层级交互总结
- rclcpp层:提供面向对象的友好API,管理C++对象生命周期
- rcl层:提供C语言API,处理资源分配、错误处理和参数验证
- rmw层:定义中间件抽象接口,与具体DDS无关
- rmw_implementation层:
- 使用环境变量决定加载哪个RMW实现
- 通过动态符号查找机制(
lookup_symbol
)获取函数指针 - 通过函数指针转发调用到实际DDS实现
- 具体DDS实现:执行真正的DDS操作,与网络通信
4. 实际应用示例
在ROS 2系统中,可以通过环境变量轻松切换底层DDS实现:
# 使用FastDDS (默认)
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp# 使用CycloneDDS
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp# 使用Connext DDS
export RMW_IMPLEMENTATION=rmw_connextdds# 查看当前使用的RMW实现
ros2 doctor --report | grep middleware