-
Notifications
You must be signed in to change notification settings - Fork 37
Component Development
在阅读本章节内容前,确保已经熟悉 Quick Start 的内容,了解如何在开发环境中编写代码
一个典型的工作区包含以下目录`
-
src/: 所有源代码都放在这里。内含许多包,每个包都有一个独立的子目录。 -
build/: 编译时生成的目录,存放中间文件和可执行文件。 -
install/: 存放安装目录,编译后的可执行文件、库、配置文件等会被安装到这里。这也是 ROS2 系统实际运行时会使用到的文件。 -
log/: 编译日志文件。
rmcs_ws 就是一个工作区
包是构建 ROS2 系统的基本模块,而工作区则是管理这些包的容器,一个工作区可以包含多个包。每个包都必须位于一个工作区中才能被构建
在 RMCS 中,rmcs_ws/src/ 下的各个文件夹就是 ros2 包
写过嵌入式的都知道,事件运行都在一个 while(true) 中运行,不断循环,通过状态机和各种条件语句来跳转任务。而我们所使用的 RMCS 控制框架为应用层开发者提供了两个接口,update 和 interface,一个是单线程以 1khz 频率更新的函数,另一个是用于数据交换的指针包装器。
以底盘控制为例子,在运行过程中,我们接受遥控器杆量,键鼠操作量,一些传感器的观测量,在 update 函数中计算输出量,最后将这些结果输出到相应的控制器中,其中,输入量我们使用 InputInterface 来声明,表示这个变量受外部作用而更新,每次 update 这个值都可能被改变,而输出量则使用 OutputInterface 来声明,表示本模块会修改这个变量的数值,供下游的模块使用
依靠纯虚函数的运行时抽象能力,执行器可以在不知道 component 具体实现时实现 1khz 的集体更新,在程序运行期间实现数据流的传递。
而我们要关注的,则是如何具体实现一个 component。
通常来说,我们开发的各个模块代码都会放到 rmcs_core 这个包里面,这个包的结构是这样的
这时候开发组件不需要新建一个包,只需要将组件编写成 cpp 文件,然后注册到 plugins.xml,这样在根据 yaml 文件启动的时候就能找到这个组件了
但面对代码量较大的模块时,例如自瞄系统,为了解耦架构,方便协作开发,我们习惯性地会开一个新的 ros2 package 来存放代码。
-
首先进入工作空间的 src 目录下,注意,是工作空间的 src,而非一个独立代码包的 src,在我们的开发容器上,它往往位于这个目录:
/workspaces/RMCS/rmcs_ws/src/。 -
使用如下指令创建包,需要注意的是,
--build-type表示构建系统,不使用纯粹的cmake是因为有一些功能,例如发布自己包中的头文件,或是消息文件,可执行程序到终端中,需要依靠ament的拓展工具:
ros2 pkg create --build-type ament_cmake 包名- 向
CMakeLists.txt引入依赖:
仅使用 cmake 管理依赖的示例,这将会有一点啰唆:
# ...
# 设置 CPP 的语言标准和构建类型,并导出
# compile_commands.json 供 clangd 使用
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_STANDARD 23)
set(CMAKE_BUILD_TYPE "Release")
# ...
# 一般来说,项目是会使用上 ros2 的,如果
# 不使用 ros2 相关库可以不引入 rclcpp
find_package(rclcpp REQUIRED)
find_package(type_description_interfaces REQUIRED)
# 执行器的头文件,暴露了 component 的接口,我们的实现需要继承该接口
find_package(rmcs_executor REQUIRED)
# 自定义的一些消息类型,例如遥控器指令,机器人种类颜色等
find_package(rmcs_msgs REQUIRED)
# tf 树的实现和相关描述文件,但它是模板元特化版本的 tf
# 如果不知道 tf(transform tree)请查阅相关资料
find_package(fast_tf REQUIRED)
find_package(rmcs_description REQUIRED)
# ...
# 将下列头文件包含近本项目,使用 cmake 管理其他 ros2 包的依赖时,
# 头文件需要自己导入,相关变量的格式为 xxx_INCLUDE_DIRS
# 如果在 package.xml 导入依赖,则可以省略这些步骤,但很可能我们
# 需要在两处地方维护依赖列表,所以我推荐只在 cmake 上维护依赖,而
# 尽可能地减少 package.xml 的修改,事实上,在如今的开发流中,只有在初始化
# 项目时会修改 package.xml 的一些基础信息,后续开发则完全可以忽略这个
# 文件
include_directories(
# ...
${rclcpp_INCLUDE_DIRS}
${type_description_interfaces_INCLUDE_DIRS}
${rmcs_executor_INCLUDE_DIRS}
${rmcs_msgs_INCLUDE_DIRS}
${rmcs_description_INCLUDE_DIRS}
${fast_tf_INCLUDE_DIRS}
)
# ...
# 创建 component 的 target
add_library(
${COMPONENT_NAME} SHARED
# 这里是 .cc\.cpp 文件
# 他们是组件的代码实现,后续 COMPONENT_NAME 需要被添加到
# 各个配置文件中
# ...
)
target_link_libraries(
${COMPONENT_NAME}
# 这里是依赖,像是 opencv
# ...
# 上面引入的自定义 ros2 包中,rmcs_msgs,
# rmcs_description,fast_tf 只有头文件,
# 不需要 link,只需要 link rmcs_executor
# 自定义包的库文件名形如:xxx_LIBRARIES
${rmcs_executor_LIBRARIES}
# 和 rclcpp
rclcpp::rclcpp
)
# ...
# 导出组件的信息,就是依靠这个 cmake 函数
# 先 rmcs_executor 导出 plugins.xml 文件
# 而该文件便记录着组件的类型和名字
find_package(pluginlib REQUIRED)
pluginlib_export_plugin_description_file(
rmcs_executor plugins.xml
)
# 将包含 component 的动态链接库下载到指定目录中,
# executor 会根据 plugins.xml 来搜索相关文件
install(
TARGETS
${COMPONENT_NAME}
DESTINATION
lib/${PROJECT_NAME}
)
# ...
# 由 ament 进行 ros2 相关的打包
# 导出操作,是最后的收尾阶段
find_package(ament_cmake REQUIRED)
ament_package()一个完整的 cmake 示例在这里:rmcs_auto_aim_v2
也有较多依赖 ament 构建工具,使用 package.xml 来进行简单依赖管理的示例:rmcs_core/cmake,rmcs_core/package.xml
这两者都是可选,一般来说,使用纯 cmake 管理会较为麻烦一些,但有利于统一管理依赖,和进行颗粒度更小的包引入(比如使用 COMPONENTS)
使用 ament 需要用到大量非 cmake 原生语句来进行构建,此外还需要再 package.xml 声明依赖,有额外维护一个新文件的成本,但需要写的更少
值得注意的是,colcon build 的构建顺序完全地依赖于 package.xml 中的声明,并不会管 CMakeLists.txt 里面的依赖引入,所以如果单独开一个 ros2 包来开发组件,需要在 package.xml 中添加这么一句:
...
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<!-- 唯一添加的依赖,为了保证正常的构建顺序 -->
<depend>rmcs_executor</depend>
...模块构建好以后是以动态链接库(.so)的形式被下载到 install/ 目录中,执行器通过运行时动态加载 so 文件来加载模块,一般来说,要注册一个模块,需要以下几个步骤:
- 实现一个继承自
public rmcs_executor::Component的类作为模块程序入口同时重载update函数:
#include <rmcs_executor/component.hpp>
class Component final : public rmcs_executor::Component {
public:
void update() override {
....
}
}- 在该文件的末尾添加导出宏语句:
...
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(Component, rmcs_executor::Component)- 在该软件包的根目录下的
plugins.xml注册组件,COMPONENT_NAME是CMakeLists.txt中的组件动态链接库名字,一般是add_library的第一个参数,type和base_class_type则是对应上述宏的两个参数:
<library path="COMPONENT_NAME">
<class type="Component" base_class_type="rmcs_executor::Component">
<description>...</description>
</class>
...
</library>一个动态链接库可以导出多个组件,只要在后面添加 class 块即可,多个组件的 .cpp 文件需要构建成一个动态链接库,每一个 .cpp 文件都形如上述流程。
组件的导出形式是动态链接库,本身不具备运行能力,需要执行器加载后执行,所以我们需要配置执行器的加载什么组件:
rmcs_executor:
ros__parameters:
update_rate: 1000.0
components:
- Component -> component_name
component_name:
ros__parameters:
...上述是描述一个机器人运行系统的最小配置,我们需要关注 components: 这个配置项:
需要满足 ClassName -> component_name 这个形式,ClassName 是注册的类型名字,而 component_name 则是自定义的名字,一般用于 ros2 node 的命名,作为配置文件的 node name 来索引等,在程序中可以使用 get_component_name() 获取。
做完上述工作后,便可以构建项目,启动:
# 当然,你也可以使用正常的构建指令 colcon build
build-rmcs
ros2 launch rmcs_bringup rmcs.launch.py robot:=xxxx这里的 robot:=xxxx 便是配置文件的名字,不包括后缀,例如,sentry.yaml 的参数是 robot:=sentry。
在同一线程内,不考虑数据竞争,传递数据将变得十分简单(理想的数据传递环境),多个模块将按照依赖顺序 update,相互传递数据
首先需要声明接口:
// Component 仅导出为 rmcs_executor::Component 类型的动态链接库,
// 所以在 Component 中区分 `public` 和 `private` 是意义不大的,
// 所以所有函数及变量的声明都可以定义在 `public` 中,也不用特别分类
// 输出
OutputInterface<double> output_;
// 输入
InputInterface<double> input_;然后,在使用之前进行注册,一般是在构造函数中完成该操作:
// const std::string & name 参数名字
// OutputInterface<double> & interface 接口实例
// const double & args 默认值
register_output("/xxx/xxx/xxx", output_, 0);
// const std::string & name 参数名字
// InputInterface<rmcs_msgs::Switch> & interface 接口实例
// bool required = true 强制依赖
register_input("/xxx/xxx/xxx", input_);随后便可以在项目中使用了:
void update() override {
...
const auto& input = *input_;
auto& output = *output_;
output = 1.0;
...
}需要注意的是,两个模块不允许相互依赖,即模块的同时持有另一个模块的输入输出。
参数依靠 ros2 提供的工具读取,配置文件格式为 yaml,一般来说,我们写在 rmcs_bringup 目录下面的配置文件,也就是前面我们配置的那一个,多个 node 共享同一个配置文件,每个 node 的配置形如:
component_name:
ros__parameters:
string: "Hello World!"而在代码中,我们一般需要继承(或组合) rclcpp::Node 来引入 ros2 的相关功能,下面是一个完整示例:
// 秉持最小引入的原则,尽量不要使用 <rclcpp/rclcpp.hpp>
#include <rclcpp/node.hpp>
#include <rmcs_executor/component.hpp>
namespace rmcs {
class Component final
// 继承 Component 获得接口
: public rmcs_executor::Component
// 继承 Node 获得 ROS2 工具
, public rclcpp::Node {
public:
explicit Component() noexcept
// 使用 rmcs_executor::Component::get_component_name() 来获取配置文件中你自定义的
// component_name
// options 的定义在下面,使用 automatically_declare_parameters_from_overrides
// 来开启未声明参数可直接使用的特性,可以有效减少调用流程 @option-note
: Node{get_component_name(), options} {
// 使用 rclcpp::Node::get_paramter_or 等一系列方法来获取参数,一般我们使用下面这个
// 方法 <std::string> 限定了类型,倘若类型不匹配则会抛出异常,请额外注意
const auto string = get_parameter_or<std::string>("string", "Default string");
RCLCPP_INFO(get_logger(), "%s", string.c_str());
}
void update() override {
// ...
}
private:
// Node 的配置,说明如上 @option-note
static inline auto options =
rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true);
};
} // namespace rmcs
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rmcs::AutoAimComponent, rmcs_executor::Component)如果没有错误,运行后的log会出现这一句:
...
[rmcs_executor-1] [INFO] [1753951313.061248788] [component_name]: Hello World!
...
当然,我们更鼓励每个较大的模块单独使用一份配置文件,将配置解耦,但目前 RMCS 框架只能使用一份配置文件(对于一个进程来说),所以我们只能运行时动态加载配置文件,自己维护一份 yaml,不过我们现在就不操这份心,让所有进程内的节点共享同一份配置文件就好。
参数名字允许重复,后面的参数会覆盖前面相同的参数,使用 git 进行 rebase 和 merge 等操作时注意检查是否将两个版本的配置内容重合到了一起,该操作不会使程序出错,但会埋下隐患,请仔细检查
RMCS QQ 交流群:703702123