【新书介绍】《ROS 2机器人系统开发实践（视频教学版）》-CSDN博客
ROS 2机器人系统开发实践（视频教学版） 梁伟、李园奇、李海慧 清华大学出版社【行情 报价 价格 评测】-京东
2.2 ROS 2功能包节点编写
功能包（Package）是ROS 2中负责组织代码、配置文件及依赖关系的基本单元。节点的编写与运行均需要在功能包下实现。一个规范的功能包应包含源代码文件，以及Python构建配置文件（package.xml和setup.py）或C++程序的构建配置文件（CMakeLists.txt）等。本节将分别介绍如何使用Python和C++创建功能包，以及相关配置文件的编写方法。
2.2.1 Python实现功能包节点编写
Python以其开发效率高、语法简洁的优势，适合我们快速实现ROS 2节点功能的编写。本小节将介绍如何使用Python代码创建一个话题发布节点，以及Python构建相关文件package.xml和setup.py的配置方法。
首先，在工作空间chapter2_ws下，通过ROS 2功能包创建命令ros2 pkg create创建一个名为ros_python_pkg的Python功能包，命令如下：
#创建功能包
ros2 pkg create ros_python_pkg --build-type ament_python --license Apache-2.0
在以上命令中，参数ros_python_pkg表示所创建的功能包名称，--build-type ament_python表示构建功能包的类型为ament_python，--license Apache-2.0表示声明功能包的开源协议。
我们在终端中执行上述命令后，系统将在工作空间chapter2_ws下创建一个名为ros_python_pkg的文件夹，并同时在该文件夹内生成默认的文件及子目录，如图2.19所示。
图2.19 创建Python功能包ros_python_pkg
新创建的功能包ros_python_pkg的文件结构及各自作用如表2.3所示。
表2.3 功能包文件结构及作用
|----------------|--------------------|
| 功能包文件夹或文件 | 用 途 |
| resource | 资源文件目录。例如，图标、程序配置等 |
| ros_python_pkg | 代码目录，与功能包名称一致 |
| test | 测试文件目录 |
| LICENSE | 声明功能包的开源协议 |
| package.xml | 功能包元信息配置文件 |
| setup.cfg | 程序运行入口点配置文件 |
| setup.py | Python构建配置文件 |
打开Python代码目录ros_python_pkg，在右键菜单中选择"新建文件"并命名为python_node.py，如图2.20所示。
打开代码文件python_node.py，输入2.1.2节中编写的Python节点代码，如下所示：
import rclpy
from rclpy.node import Node
def main():
rclpy.init()
node = Node("python_Node")
node.get_logger().info("节点python_Node已启动")
rclpy.spin(node)
rclpy.shutdown()
备注：与2.1.2节中的代码不同，此处删除了主函数调用部分的代码。本小节将通过配置setup.py来调用入口函数main()。
图2.20 新建Python代码文件
由于在程序中调用了ROS 2库rclpy，因此需要打开Python配置清单文件package.xml，添加依赖声明，代码如下：
<package format="3">
...
<license>Apache-2.0</license>
<depend>rclpy</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
...
</package>
依赖声明添加完成后，再打开配置文件setup.py，找到节点console_scripts并注册Python程序入口函数，配置代码如下：
...
'console_scripts': [
'python_node=ros_python_pkg.python_node:main',
],
...
以上代码的含义是：当执行python_node命令时，系统将执行功能包目录ros_python_pkg下代码文件python_node.py中的main函数。至此，我们已完成Python节点代码编写及相关配置。
接下来，使用构建工具colcon对功能包进行构建，命令如下：
colcon build
在使用colcon构建功能包之前，为了避免2.1节中使用过的CMake配置文件CMakeLists.txt被colcon误读取，我们需要首先确认已删除或重命名该配置文件，然后执行ROS 2构建命令。执行后的效果如图2.21所示。
图2.21 构建Python功能包
功能包构建完成后，工作空间chapter2_ws中会新增三个文件夹，分别为build、install和log，它们的作用分别是：build存放构建过程中产生的中间文件，install存放构建结果（包括可执行文件、库文件等），log存放构建过程中产生的日志文件。其中，打开install文件夹，可以看到构建好的功能包和可执行文件，如图2.22所示。
图2.22 构建完成的功能包及可执行文件
接下来，使用ROS 2启动构建好的Python节点，命令如下：
ros2 run ros_python_pkg python_node
以上命令的作用是使用ros2 run运行构建好的节点文件。但是当我们执行上述命令后，会发现终端中输出找不到功能包的错误提示，如图2.23所示。
图2.23 提示找不到功能包
命令ros2 run是通过系统环境变量AMENT_PREFIX_PATH的值来查找功能包的，其默认值为ROS 2 的安装目录，而功能包ros_python_pkg位于工作空间下的install目录中。因此，我们需要通过修改环境变量AMENT_PREFIX_PATH的值，帮助ros2 run找到该功能包。
我们可以打开install目录下的脚本文件setup.bash，其中已包含新创建的功能包路径。通过命令source运行该脚本即可修改环境变量AMENT_PREFIX_PATH的值，修改后可使用echo命令检查是否修改成功，命令如下：
source install/setup.bash
echo $AMENT_PREFIX_PATH
执行完成后，若系统变量值中出现了已构建的功能包路径，则说明环境变量修改成功，如图2.24所示。
图2.24 修改并检查系统变量值
修改完成后，再次使用ros2 run运行节点python_node，即可正常运行，如图2.25所示。
图2.25 重新运行节点并查看节点列表
需要注意的是，系统环境变量值的修改只对当前终端有效。如果新开终端，必须先运行source命令修改环境变量值后再运行节点，否则会出现上述找不到功能包的错误。
本小节主要介绍了Python实现功能包节点中如何创建功能包、包中各文件的作用，以及通过配置文件声明依赖、注册入口函数等相关操作，并着重介绍了构建与执行节点时出现的问题及处理方法。尤其需要注意的是，为了防止构建工具colcon误读取2.1节中的CMake配置文件CMakeLists.txt导致构建错误，务必先将该文件删除或重命名，再执行colcon构建。
2.2.2 C++实现功能包节点编写
在2.2.1节中，使用Python代码创建了功能包节点并通过修改环境变量值实现了ROS 2启动节点。本小节将使用编译型语言C++创建功能包、节点并运行节点。
在工作空间chapter2_ws下通过ROS 2功能包创建命令ros2 pkg create创建一个C++功能包ros_c++_pkg，命令如下：
ros2 pkg create ros_c++_pkg --build-type ament_cmake --license Apache-2.0
以上命令参数ament_cmake的作用是创建C++功能包ros_c++_pkg，该功能包中的文件结构如图2.26所示。
图2.26 创建的C++功能包及其文件结构
功能包ros_c++_pkg中有两个文件夹和三个文件，各自作用如表2.4所示。
表2.4 功能包文件结构及作用
|----------------|-----------|
| 功能包文件夹或文件 | 用 途 |
| include | 头文件目录 |
| src | 源文件目录 |
| CMakeLists.txt | C++构建配置文件 |
| LICENSE | 声明功能包开源协议 |
| package.xml | 元信息配置文件 |
在源文件目录中，新建一个文件并命名为c++_node.cpp，如图2.27所示。
图2.27 创建C++代码文件c++_node.cpp
打开已创建的C++代码文件，编写用于创建和运行ROS 2节点的C++代码，示例如下：
#include "rclcpp/rclcpp.hpp"
int main(int argc,char **argv)
{
rclcpp::init(argc,argv);
auto node=std::make_shared<rclcpp::Node>("cpp_colcon_node");
RCLCPP_INFO(node->get_logger(),"C++ 节点已启动");
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
打开功能包ros_c++_pkg下的配置文件CMakeLists.txt并写入配置代码，如下所示：
cmake_minimum_required(VERSION 3.8)
project(ros_c++_pkg)
add_executable(cpp_node src/c++_node.cpp) #源代码文件路径一定要准确
find_package(rclcpp REQUIRED) #查找rclcpp头文件和库
ament_target_dependencies(cpp_node rclcpp) #为c++_node添加依赖
install(TARGETS #将c++_node复制到install目录下
cpp_node
DESTINATION lib/${PROJECT_NAME}
)
target_include_directories(cpp_node PUBLIC ${rclcpp_INCLUDE_DIRS})
target_link_libraries(cpp_node ${rclcpp_LIBRARIES})
其中，install(TARGETS cpp_node DESTINATION lib/${PROJECT_NAME})的作用是，将编译生成的可执行文件cpp_node复制到路径install/ros_cpp_pkg/lib/ros_cpp_pkg下，以便命令ros2 run能够找到该节点并正常运行。
接下来，打开C++功能包ros_cpp_pkg中src目录下的配置文件package.xml，并添加对rclcpp的依赖声明，示例如下：
...
<license>Apache-2.0</license>
depend>rclpy</depend>
<buildtool_depend>ament_cmake</buildtool_depen
<test_depend>ament_lint_auto</test_depend>
...
完成以上配置后，使用构建工具colcon构建生成可执行文件，命令如下：
colcon build
构建完成后，install目录下将生成名为ros_cpp_pkg的文件夹，同时生成的可执行文件cpp_node将被复制到ros_cpp_pkg/lib/ros_cpp_pkg目录下，如图2.28所示。
图2.28 生成的可执行文件被复制到目录install下
接下来，操作步骤与2.2.1节运行Python节点时类似：首先运行install/setup.bash脚本，修改系统环境变量AMENT_PREFIX_PATH的值，然后使用ros2 run命令即可找到并运行节点cpp_node，具体命令如下：
source install/setup.bash
ros2 run ros_c++_pkg cpp_node
运行上述命令后，我们可以看到C++节点已运行，如图2.29所示。
图2.29 C++节点已运行
虽然这里是使用ROS 2构建工具colcon对C++节点程序进行构建编译的，但实质上colcon仍是调用cmake和make命令来完成代码的编译与生成。
本小节主要介绍了如何使用C++编写ROS 2节点的创建与运行，以及配置文件CMakeLists.txt的修改等相关操作。我们可以参照下载资源中本章的具体项目代码进行修改，重点掌握C++实现过程中的配置方法。
