在 ROS（Robot Operating System）中，使用功能包（package）来组织 C++ 节点是一种常见且有效的方式，它能让代码结构更清晰、便于管理和复用。

1. 环境准备

确保已经安装了 ROS，这里以 ROS 2 Humble 为例，并且已经正确配置好了开发环境。

2. 创建工作空间

工作空间是 ROS 项目的根目录，用于存放多个功能包。首先创建一个工作空间目录，并在其中创建 src 子目录用于存放功能包。

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

3. 创建功能包

使用 ros2 pkg create 命令创建一个新的功能包。假设功能包名为 demo_cpp_pkg，依赖项为 rclcpp（ROS 2 C++ 客户端库）。

ros2 pkg create --build-type ament_cmake demo_cpp_pkg --dependencies rclcpp

该命令会在 src 目录下创建一个名为 demo_cpp_pkg 的功能包，包含基本的目录结构和配置文件。

4. 功能包目录结构

创建完成后，demo_cpp_pkg 功能包的基本目录结构如下：

demo_cpp_pkg/
├── CMakeLists.txt
├── include/
│   └── demo_cpp_pkg/
├── package.xml
└── src/

• CMakeLists.txt：CMake 构建脚本，用于配置项目的编译和安装规则。
• include/：存放头文件的目录。
• package.xml：功能包的元数据文件，包含功能包的名称、版本、依赖项等信息。
• src/：存放源文件的目录。

5. 编写 C++ 节点代码

在 src 目录下创建一个新的 C++ 源文件，例如 cpp_node.cpp，并编写一个简单的 ROS 2 节点代码。

#include <rclcpp/rclcpp.hpp>int main(int argc, char * argv[])
{rclcpp::init(argc, argv);auto node = rclcpp::Node::make_shared("cpp_node");RCLCPP_INFO(node->get_logger(), "Hello, ROS 2!");rclcpp::spin(node);rclcpp::shutdown();return 0;
}

这段代码创建了一个简单的 ROS 2 节点，节点名称为 cpp_node，并在启动时输出一条日志信息。

6. 配置 CMakeLists.txt

打开 CMakeLists.txt 文件，添加以下内容来编译和安装 cpp_node 节点。

# 添加可执行文件
add_executable(cpp_node src/cpp_node.cpp)
# 链接ROS 2库
ament_target_dependencies(cpp_node rclcpp)# 安装可执行文件
install(TARGETScpp_nodeDESTINATION lib/${PROJECT_NAME}
)

上述代码中，add_executable 用于定义可执行文件，ament_target_dependencies 用于链接 ROS 2 库，install 用于将可执行文件安装到指定目录。

7. 编译功能包

回到工作空间根目录，使用 colcon build 命令编译功能包。        

cd ~/ros2_ws
colcon build --packages-select demo_cpp_pkg

--packages-select 选项用于指定只编译 demo_cpp_pkg 功能包。

8. 运行 C++ 节点

编译完成后，需要 source 工作空间的环境变量，然后运行 cpp_node 节点。

source install/setup.bash
ros2 run demo_cpp_pkg cpp_node

source install/setup.bash 用于加载工作空间的环境变量，ros2 run 用于运行指定功能包中的节点。

9. 总结

通过以上步骤，成功使用功能包组织了一个 C++ 节点。可以按照类似的方法在功能包中添加更多的节点和功能，同时可以根据需要在 package.xml 中添加更多的依赖项，在 CMakeLists.txt 中配置更复杂的编译和安装规则。