C++AI
ROS 基础教程：使用 C++ 编写节点
本文详细介绍了在 ROS 中使用 C++ 编写节点的基础流程。内容包括 ROS 节点与 Master 的概念、roscpp 接口简介、编写并发布话题的 C++ 代码示例、CMakeLists.txt 配置与编译方法、节点运行与调试、自定义头文件的引用方式（当前包及跨包），以及如何使用 launch 文件批量启动多个节点。文章通过具体代码和操作步骤，帮助读者掌握 ROS 节点开发的核心技能。
本文详细介绍了在 ROS 中使用 C++ 编写节点的基础流程。内容包括 ROS 节点与 Master 的概念、roscpp 接口简介、编写并发布话题的 C++ 代码示例、CMakeLists.txt 配置与编译方法、节点运行与调试、自定义头文件的引用方式（当前包及跨包），以及如何使用 launch 文件批量启动多个节点。文章通过具体代码和操作步骤，帮助读者掌握 ROS 节点开发的核心技能。
在 ROS 的世界里，最小的进程单元就是节点（node）。一个功能包里可以有多个可执行文件，可执行文件在运行之后就成了一个进程 (process)，这个进程在 ROS 中就叫做节点。从程序角度来说，node 就是一个可执行文件（通常为 C++ 编译生成的可执行文件、Python 脚本）被执行，加载到了内存之中。
从功能角度来说，通常一个 node 负责机器人的某一个单独的功能。由于机器人的功能模块非常复杂，我们往往不会把所有功能都集中到一个 node 上，而会采用分布式的方式，把鸡蛋放到不同的篮子里。例如有一个 node 来控制底盘轮子的运动，有一个 node 驱动摄像头获取图像，有一个 node 驱动激光雷达，有一个 node 根据传感器信息进行路径规划……这样做可以降低程序发生崩溃的可能性，试想一下如果把所有功能都写到一个程序中，模块间的通信、异常处理将会很麻烦。
由于机器人的元器件很多，功能庞大，因此实际运行时往往会运行众多的 node，负责感知世界、控制运动、决策和计算等功能。那么如何合理的进行调配、管理这些 node？这就要利用 ROS 提供给我们的节点管理器 master，master 在整个网络通信架构里相当于管理中心，管理着各个 node。node 首先在 master 处进行注册，之后 master 会将该 node 纳入整个 ROS 程序中。node 之间的通信也是先由 master 进行'牵线'，才能两两的进行点对点通信。当 ROS 程序启动时，第一步首先启动 master，由节点管理器处理依次启动 node。
接下来我们来介绍如何用 C++ 编写一个 Node。
ROS 为机器人开发者们提供了不同语言的编程接口，比如 C++ 接口叫做 roscpp，Python 接口叫做 rospy，Java 接口叫做 rosjava。尽管语言不通，但这些接口都可以用来创建 topic、service、param，实现 ROS 的通信功能。Client Library 有点类似开发中的 Helper Class，把一些常用的基本功能做了封装。
目前 ROS 支持的 Client Library 包括：
目前最常用的只有 roscpp 和 rospy，而其余的语言版本基本都还是测试版。
大致介绍了 roscpp 的一些接口之后，我们来编写第一个 ROS 节点。
当执行一个 ROS 程序，就被加载到了内存中，就成为了一个进程，在 ROS 里叫做节点。每一个 ROS 的节点尽管功能不同，但都有必不可少的一些步骤，比如初始化、销毁，需要通行的场景通常都还需要节点的句柄。这一节我们来学习 Node 最基本的一些操作。
在第二章节中我们建立了一个 ros_workspace 的工作空间同时在工作空间中创建了一个命名为 test_pkg 的功能包。本节中我们继续在 test_pkg 中创建一个节点。
在路径 ros_workspace/src/test_pkg/src 下创建一个 cpp 文件命名为：test.cpp。
在终端输入以下命令进入指定路径：
cd ~/ros_workspace/src/test_pkg/src
在终端输入以下命令创建 test.cpp:
touch test.cpp
在终端输入以下命令打开 test.cpp：
gedit test.cpp
输入以下代码：
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sstream>
int main(int argc, char** argv)
{
ros::init(argc, argv, "talker");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
ros::Rate loop_rate(10);
int count = 0;
while(ros::ok())
{
std_msgs::String msg;
std::stringstream ss;
ss << "hello world" << count;
msg.data = ss.str();
ROS_INFO("%s", msg.data.c_str());
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
上述测试代码创建了一个话题，并向其中发布'hello world'。
在完成了节点源码编写之后，需要修改 test_pkg 包下的 CMakeLists.txt 文件 (具体路径为：ros_workspace/src/test_pkg/CMakeLists.txt)，将编辑的源码生成可执行文件。
需要修改以下两条指令：
add_executable(${PROJECT_NAME}_node src/test.cpp)
该条指令用于指定将该功能包 src 目录下的哪个源文件编译为可执行程序，其中：
${PROJECT_NAME}_node 为生成可执行程序的名字，该名字可以任意指定。src/test.cpp 为编译要使用的源码的文件名。target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)
该条指令用于指定所使用的一些链接库。其中：${PROJECT_NAME}_node 是在 add_executable 中生成的可执行文件。
图 3.1 CMakeLists.txt 修改
当修改完 CMakeLists.txt 文件后，就可以在工作空间的根目录下使用 catkin_make 进行编译了。
cd ~/ros_workspace
catkin_make
编译成功如下图 3.2 所示：
图 3.2 编译完成
编译生成的可执行文件位于：~/ros_workspace/devel/lib/test_pkg/ 路径下，命名为：test_pkg_node。大家可以自行查看是否有该文件生成。
打开一个终端输入以下命令：
roscore // 打开 Master
图 3.3 打开 master
重新打开一个终端输入以下命令运行 test_pkg_node 节点。
rosrun test_pkg test_pkg_node
正确运行如下图 3.4 所示：
图 3.4 正确运行节点
注意：用 rosrun 运行节点之前一定要先运行 roscore 不然会报错。
ros 提供了用于处理 node 的命令 rosnode。
rosnode 命令的详细作用列表如下：
在编写代码时我们经常会需要引用头文件，引用公用的头文件很容易，因为它们已经在标准库头文件路径中。但是如果要引用自定义的头文件就稍微麻烦点，我们首先查看软件包的目录结构，需要在正确的目录下创建头文件并修改 CMakeLists.txt 文件这样才能正确编译，下面来举例说明：
为了方便说明我们来创建一个头文件。
在路径：~/ros_workspace/src/test_pkg/include/test_pkg 下创建 test_pkg.h
打开终端输入以下命令进入要创建头文件的目录：
cd ~/ros_workspace/src/test_pkg/include/test_pkg
输入以下命令创建头文件：
touch test_pkg.h
输入以下命令打开创建的头文件：
gedit test_pkg.h
输入以下代码：
#ifndef _TEST_PKG_
#define _TEST_PKG_
#define TEST_PKG_VER "1.0.0"
#define INIT_COUNT 100
int addTwoNum(int a, int b);
#endif
完成后保存关闭头文件，修改 test.cpp，如下代码：
#include <ros/ros.h>
#include <std_msgs/String.h>
#include "test_pkg/test_pkg.h" //自定义头文件
#include <sstream>
int addTwoNum(int a, int b)
{
return a + b;
}//头文件函数实现部分
int main(int argc, char* argv[])
{
ros::init(argc, argv, "talker");//这个名字是给系统看的
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
ros::Rate loop_rate(10);
int count = INIT_COUNT;//调用了头文件中的宏定义
ROS_INFO("test_pkg version:%s", TEST_PKG_VER);
while (ros::ok())
{
std_msgs::String msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();
ROS_INFO("%s", msg.data.c_str());
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
源码修改完成之后需要修改 test_pkg 中的 CMakeLists.txt 文件。如下图所示（注意修改后要保存）：
图 3.5 CMakeLists.txt 修改
此处修改是添加了一个头文件的包含路径：include。当然，此处的 include 是一个相对路径，指的是当前功能包 test_pkg 下的 include。这样编译器在编译源码的时候会到 include 文件下查找我们自定义的头文件 test_pkg/test_pkg.h。
到此我们就完成如何在当前包下引用自定义头文件的代码修改以及配置修改。现在我们需要对源码重新编译。
cd ~/ros_workspace
catkin_make
编译完成之后运行：
roscore
rosrun test_pkg test_pkg_node
正确运行如下图 3.6 所示，成功的输出了头文件中定义的版本号：version: 1.0.0。以及从 hello world 100 开始输出。
图 3.6 成功引用了自定义头文件
在一些情况我们需要引用其他软件包中提供的函数或宏定义，这样可以一定程度上减少我们在两个节点之间需要进行通信的话题个数，下面我们通过举例来进行说明。为了方便说明我们需要在之前创建的工作空间中创建一个新的 package 并命名为 my_pkg。如下图 3.7 所示：
图 3.7 创建 my_pkg 功能包
如何通过 catkin_create_pkg 命令来创建功能包前面已经讲过，这里要注意的是我们创建 my_pkg 的目的是要引用 test_pkg 中的自定义头文件，因此这里创建 my_pkg 的时候要对 test_pkg 进行依赖。
在路径 my_pkg/src 路径下创建源码文件 my_pkg.cpp。写入如下代码：
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sstream>
#include "test_pkg/test_pkg.h" //自定义头文件
int main(int argc, char* argv[])
{
ros::init(argc, argv, "my_pkg");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("my_chatter", 1000);
ros::Rate loop_rate(10);
int count = INIT_COUNT; //调用了头文件中的宏定义
ROS_INFO("test_pkg version:%s, init count:%d", TEST_PKG_VER, INIT_COUNT); /**/将其他软件包头文件中声明的宏定义打印出来**
while (ros::ok())
{
std_msgs::String msg;
std::stringstream ss;
ss << "my_pkg " << count;
msg.data = ss.str();
ROS_INFO("%s", msg.data.c_str());
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
完成源码编写之后首先我们来修改 my_pkg 的 CMakeLists.txt 文件。如下图 3.8 所示，在 my_pkg 的 CMakeLists.txt 指定了要编译的源码。
图 3.8 my_pkg CMakeLists.txt 修改
其次来修改 test_pkg 的 CMakeLists.txt 文件，如下图 3.9 所示。做如下修改的主要目的是通知其他软件包当前软件包含有自定义头文件，在 include 目录下。这样其他包在引用头文件的时候就可以到这个地方查找。
图 3.9 test_pkg CMakeLists.txt 修改
对工作空间进行编译：
cd ~/ros_workspace/
catkin_make
编译完成之后可以运行节点查看结果。
roscore
rosrun my_pkg my_pkg_node
运行结果如下图 3.10 所示，打印了头文件中定义的宏定义说明调用头文件成功。
图 3.10 my_pkg_node 测试结果
大家也可以自行修改以下头文件中的宏定义，重新运行节点查看输出结果。
在 ROS 中一个节点程序一般只能完成功能单一的任务，但是一个完整的 ROS 机器人一般由很多个节点程序同时运行、相互协作才能完成复杂的任务，因此这就要求在启动机器人时就必须要启动很多个节点程序，一般的 ROS 机器人由十几个节点程序组成，复杂的几十个都有可能。
这就要求我们必须高效率的启动很多节点，而不是通过 rosrun 命令来依次启动十几个节点程序，launch 文件 就是为解决这个需求而生，launch 文件就是一个 xml 格式的脚本文件，我们把需要启动的节点都写进 launch 文件 中，这样我们就可以通过 roslaunch 工具来调用 launch 文件，执行这个脚本文件就一次性启动所有的节点程序。
launch 文件同样也遵循着 xml 格式规范，是一种标签文本，它的格式包括以下标签：
<launch> <node> <include> <machine> <env-loader> <param> <rosparam> <arg> <remap> <group> 为了方便说明我们创建一个功能包来进行讲解。
cd ~/ros_workspace/src/
catkin_create_pkg robot_bringup
一般每一个机器人工程都包含一个命名为 xxx_bringup 的功能包，该功能包用来存放该机器人的各种启动文件。
robot_bringup 功能包创建成功后在功能的目录下创建一个 launch 文件夹并在 launch 文件夹下创建一个命名为 startup.launch。
cd ~/ros_workspace/src/robot_bringup/
mkdir launch
cd launch
touch startup.launch
每个 XML 文件都必须要包含一个根元素，根元素由一对 launch 标签定义： … 。
打开 startup.launch 输入以下内容如图 3.14 所示：
3.14 空的 launch 文件
标签的上一级根标签为标签，用于启动一个 ROS 节点。启动一个节点的写法如下：
<node pkg="package-name" type="executable-name" name="node-name" />
标签下有若干属性，至少要包含三个属性：pkg，type，name。Pkg 属性指定了要运行的节点属于哪个功能包，type 是指要运行节点的可执行文件的名称，name 属性给节点指派了名称，它将覆盖任何通过调用 ros::init 来赋予节点的名称。
现在打开 startup.launch 文件在里面运行 test_pkg_node 以及 my_pkg_node 两个节点。代码如下：
<launch>
<node pkg="test_pkg" type="test_pkg_node" name="test_pkg_node" output="screen"/>
<node pkg="my_pkg" type="my_pkg_node" name="my_pkg_node" output="screen"/>
</launch>
其中，output='screen'属性可以将单个节点的标准输出到终端而不是存储在日志文件中。
编译工作空间并运行 startup.launch 文件。
cd ~/ros_workspace/
catkin_make
运行 launch 的命令格式：
roslaunch 包名称 launch 文件名称
运行 startup.launch 文件。
roslaunch robot_bringup startup.launch
两个节点输出内容打印到屏幕 |
图 3.15 运行结果
除了上述的 pkg，name，type，output 属性以外还有一些常用属性需要掌握。
在当前 launch 文件中调用另一个 launch 文件，方便代码的复用，可以使用包含（include）标签：
<include file="$(find package-name)/launch-file-name">
由于直接输入路径信息很繁琐且容易出错，大多数包含元素都使用查找（find）命令搜索功能包的位置来替代直接输入绝对路径。
为了方便说明我们创建一个功能包 third_pkg 并用 robot_bringup 功能包中的 launch 物件来调用该功能包中的 launch 文件。
cd ~/ros_workspace/src/
catkin_create_pkg third_pkg std_msgs roscpp rospy
在 third_pkg/src 路径下创建 third_pkg.cpp 源文件，打开 third_pkg.cpp 输入以下测试代码：
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sstream>
int main(int argc, char** argv)
{
ros::init(argc, argv, "third_pkg");
ros::NodeHandle n; // 更新话题的消息格式为自定义的消息格式
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("third_pkg", 1000);
ros::Rate loop_rate(10);
int count = 0;
while(ros::ok())
{
std_msgs::String msg;
std::stringstream ss;
ss << "third pkg:" << count;
msg.data = ss.str();
ROS_INFO("%s", msg.data.c_str());
chatter_pub.publish(msg); //将消息发布到话题中
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
编辑源码之后同样要修改 CMakeLists.txt，将 third_pkg.cpp 编译为可执行代码。如下图所示：
图 3.19 CMakeLists.txt 修改
编译工作空间。
在/third_pkg 路径下创建一个 launch 文件夹：
cd ~/ros_workspace/src/third_pkg
mkdir launch
在 third_pkg/launch 路径下新建一个 third_pkg.launch，并输入以下代码：
<launch>
<node pkg="third_pkg" type="third_pkg_node" name="third_pkg_node" output="screen"/>
</launch>
打开 robot_bringup 功能包下的 startup.launch 文件，修改该如下：
<launch>
<node pkg="test_pkg" type="test_pkg_node" name="test_pkg_node" respawn="true" output="screen"/>
<node pkg="my_pkg" type="my_pkg_node" name="my_pkg_node" required="true" output="screen"/>
<include file="$(find third_pkg)/launch/third_pkg.launch"/>
</launch>
运行 startup.launch
roslaunch robot_bringup startup.launch
运行了三个节点 |
图 3.20 包含其他 launch 文件运行结果
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML 转 Markdown 互为补充。 在线工具，Markdown 转 HTML在线工具，online
将 HTML 片段转为 GitHub Flavored Markdown，支持标题、列表、链接、代码块与表格等；浏览器内处理，可链接预填。 在线工具，HTML 转 Markdown在线工具，online