ROS2 学习记录（一）——ROS2 的话题通信与服务通信
文章目录
一个月前我还在学习 ROS，彼时的学习记录里使用的是 ROS Noetic 发行版。直到最近上手一块 RDK 开发板，并且使用预装的 ROS2 时我才意识到，是时候重新学一下 ROS2 的使用了。ROS2 从命令到接口再到框架，几乎与 ROS1 完全不同。
例如在 ROS1 中，一个包下可以同时存在 C++ 节点和 Python 节点，但是在 ROS2 中，一个包中默认只能有 C++ 节点或者 Python 节点；ROS2 使用 setup.py 作为 Python 节点的配置文件；ROS1 使用 catkin 作为构建工具，ROS2 中使用 colcon 作为构建工具；ROS2 使用去中心化架构，不需要 ROS Core 调度；ROS2 取消了 devel 目录……
当然，即使 ROS2 在 ROS1 的基础上做了重大更新，但是开发 ROS 项目的流程没有变，工作空间-包-节点的组织结构没有变，ROS 通信模型也没有变。现在，我就需要按照两个月前学习 ROS1 的路径，重新学习 ROS2。
创建并初始化工作空间
创建一个工作空间目录 ros2-project，并在该目录下运行 colcon build，得到一个初始化的工作空间，该目录下应该有 build、install、log、src 四个目录（devel 目录被取消了）。我们后续所有的包都会存储在 src 目录下。
ROS2 话题通信
C++ 实现
创建一个 C++ 包
在 ROS1 中，通过 catkin_create_pkg 命令就能直接创建一个包，而不需要考虑这个包里会存放哪些节点。然而在 ROS2 中，我们必须提前考虑好，这个包里的节点是要用 C++ 编写还是用 Python 编写。以存放 C++ 节点的包为例，创建包的命令是
1ros2 pkg create --build-type ament_cmake cpp_pkg --dependencies rclcpp std_msgs
在这条命令中，--build-type 参数表示构建类型，ament_cmake 代表 C++ 节点，ament_python 代表 Python 节点；cpp_pkg 是创建的包名；--dependencies 参数表示依赖项，rclcpp 是 ROS2 与 C++ 的接口库，对应 ROS1 中的 roscpp，std_msgs 与 ROS1 中相同，表示标准消息类型。
编写发布者节点
ROS2 中的节点需要继承 ROS2 提供的节点类，在 C++ 中为 rclcpp::Node。
1#include "rclcpp/rclcpp.hpp"
2#include "std_msgs/msg/string.hpp"
3
4using namespace std::chrono_literals; // 启用时间自变量，例如 500ms
5
6class MinimalPublisher : public rclcpp::Node { // 继承 rclcpp::Node 类，表明这是一个 C++ ROS2 节点
7 public:
8 MinimalPublisher() : Node("minimal_publisher"), count_(0) {
9 publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10); // 创建发布者，发布到 topic 话题，队列大小为 10
10 timer_ = this->create_wall_timer(
11 500ms, std::bind(&MinimalPublisher::callback, this)); // 创建定时器，每 500ms 调用一次 callback 函数
12 }
13
14 private:
15 void callback() {
16 auto message = std_msgs::msg::String(); // 创建 std_msgs::msg::String 类型的消息
17 message.data = "Hello, World: " + std::to_string(count_++); // 设置消息内容
18 RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); // 打印发布信息
19 publisher_->publish(message); // 发布消息
20 }
21
22 rclcpp::TimerBase::SharedPtr timer_; // 定时器（智能指针管理）
23 rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_; // 发布者（智能指针管理）
24 size_t count_; // 计数器
25};
26
27int main(int argc, char **argv) {
28 rclcpp::init(argc, argv); // 初始化 ROS2
29 rclcpp::spin(std::make_shared<MinimalPublisher>()); // 运行节点，直到节点退出
30 rclcpp::shutdown(); // 关闭 ROS2
31 return 0;
32}
编写订阅者节点
1#include "rclcpp/rclcpp.hpp"
2#include "std_msgs/msg/string.hpp"
3
4class MinimalSubscriber : public rclcpp::Node { // 继承 rclcpp::Node 类，表明这是一个 C++ ROS2 节点
5 public:
6 MinimalSubscriber() : Node("minimal_subscriber") {
7 // 创建 Subscription 对象，监听 topic 话题，队列大小为 10
8 subscription_ = this->create_subscription<std_msgs::msg::String>(
9 "topic", 10, std::bind(&MinimalSubscriber::callback, this, std::placeholders::_1));
10 }
11
12 private:
13 void callback(const std_msgs::msg::String::SharedPtr message) {
14 RCLCPP_INFO(this->get_logger(), "Received: '%s'", message->data.c_str()); // 打印接收到的消息
15 }
16 rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
17};
18
19int main(int argc, char **argv) {
20 rclcpp::init(argc, argv); // 初始化 ROS2
21 rclcpp::spin(std::make_shared<MinimalSubscriber>()); // 运行节点
22 rclcpp::shutdown(); // 关闭 ROS2
23 return 0;
24}
修改 CMakeLists.txt 文件并编译运行
ROS2 中的 CMakeLists.txt 文件与 ROS1 中不同，需要使用 ament_cmake 作为构建类型，并且需要指定依赖项。在模板文档末尾添加如下内容：
1add_executable(publisher src/publisher.cpp)
2ament_target_dependencies(publisher rclcpp std_msgs)
3
4add_executable(subscriber src/subscriber.cpp)
5ament_target_dependencies(subscriber rclcpp std_msgs)
6
7install(TARGETS
8 publisher
9 subscriber
10 DESTINATION lib/${PROJECT_NAME})
随后在工作空间根目录运行 colcon build 命令编译包，如果只需要编译 cpp_pkg 包，则可通过 colcon build --packages-select cpp_pkg 来指定。
编译完成后，在两个终端中分别运行 ros2 run cpp_pkg publisher 和 ros2 run cpp_pkg subscriber 命令，即可启动两个节点，并且无需 ROS Core 调度。
Python 实现
创建一个 Python 包
创建 Python 包的命令如下：
1ros2 pkg create --build-type ament_python py_pkg --dependencies rclpy std_msgs
Python 包的结构如下：
1.
2├── package.xml
3├── py_pkg
4│ └── __init__.py
5├── resource
6│ └── py_pkg
7├── setup.cfg
8├── setup.py
9└── test
10 ├── test_copyright.py
11 ├── test_flake8.py
12 └── test_pep257.py
不难发现，此 Python 包的结构与 C++ 包完全不同。该 Python 包中的脚本置于 pk_pkg/py_pkg/ 下，__init__.py 文件用于标识该目录为 Python 包，setup.py 文件用于构建该 Python 包。
编写发布者节点
1import rclpy
2from rclpy.node import Node
3from std_msgs.msg import String
4
5
6class MinimalPublisher(Node): # 继承 Node 类，表示这是一个 ROS2 节点
7
8 def __init__(self):
9 super().__init__("minimal_publisher") # 初始化节点，命名为 minimal_publisher
10 self.publisher_ = self.create_publisher(
11 String, "topic", 10
12 ) # 创建发布者，发布到 topic 话题，队列大小为 10
13 self.timer = self.create_timer(
14 0.5, self.callback
15 ) # 创建定时器，每隔 0.5 秒执行一次回调函数
16 self.count = 1 # 计数器
17
18 def callback(self):
19 message = String() # 创建 String 消息
20 message.data = "Hello, World: " + str(self.count) # 设置消息内容
21 self.publisher_.publish(message) # 发布消息
22 self.get_logger().info(f"Publishing: '{message.data}'") # 打印日志
23 self.count += 1
24
25
26def main(args=None):
27 rclpy.init(args=args) # 初始化 rclpy
28 minimal_publisher = MinimalPublisher() # 创建节点对象
29 rclpy.spin(minimal_publisher) # 运行节点
30 minimal_publisher.destroy_node() # 销毁节点
31 rclpy.shutdown() # 关闭 rclpy
32
33
34if __name__ == "__main__":
35 main()
编写订阅者节点
1import rclpy
2from rclpy.node import Node
3from std_msgs.msg import String
4
5
6class MinimalSubscriber(Node):
7
8 def __init__(self):
9 super().__init__("minimal_subscriber")
10 self.subscription = self.create_subscription(
11 String, "topic", self.callback, 10
12 ) # 创建订阅者，订阅 topic，队列大小为 10
13
14 def callback(self, message):
15 self.get_logger().info(f"Received: '{message.data}'")
16
17
18def main(args=None):
19 rclpy.init(args=args) # 初始化 ROS 2 上下文
20 minimal_subscriber = MinimalSubscriber() # 创建订阅者节点实例
21 rclpy.spin(minimal_subscriber) # 运行节点（阻塞式）
22 minimal_subscriber.destroy_node() # 销毁节点
23 rclpy.shutdown() # 关闭 ROS 2
24
25
26if __name__ == "__main__":
27 main()
修改 setup.py 文件并编译运行
setup.py 的原始内容如下：
1from setuptools import setup
2
3package_name = "py_pkg"
4
5setup(
6 name="py_pkg",
7 version="0.0.0",
8 packages=["py_pkg"],
9 data_files=[
10 ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
11 ("share/" + package_name, ["package.xml"]),
12 ],
13 install_requires=["setuptools"],
14 zip_safe=True,
15 maintainer="jackgdn",
16 maintainer_email="li_zhong_yao@foxmail.com",
17 description="TODO: Package description",
18 license="TODO: License declaration",
19 tests_require=["pytest"],
20 entry_points={
21 "console_scripts": [],
22 },
23)
修改时需要在 entry_points.console_scripts 添加节点，即 "publisher = py_pkg.publisher:main" 和 "subscriber = py_pkg.subscriber:main"。以前者为例，最先出现的 publisher 是编译目标名称，py_pkg 是包名，第二次出现的 publisher 是脚本名，main 是程序入口点。修改完成的 setup.py 如下：
1from setuptools import setup
2
3package_name = "py_pkg"
4
5setup(
6 name="py_pkg",
7 version="0.0.0",
8 packages=["py_pkg"],
9 data_files=[
10 ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
11 ("share/" + package_name, ["package.xml"]),
12 ],
13 install_requires=["setuptools"],
14 zip_safe=True,
15 maintainer="jackgdn",
16 maintainer_email="li_zhong_yao@foxmail.com",
17 description="TODO: Package description",
18 license="TODO: License declaration",
19 tests_require=["pytest"],
20 entry_points={
21 "console_scripts": [
22 "publisher = py_pkg.publisher:main",
23 "subscriber = py_pkg.subscriber:main",
24 ],
25 },
26)
随后在工作空间根目录运行 colcon build 命令编译包，如果只需要编译 py_pkg 包，则可通过 colcon build --packages-select py_pkg 来指定。
编译完成后，在两个终端中分别运行 ros2 run py_pkg publisher 和 ros2 run py_pkg subscriber 命令，即可启动两个节点。
ROS2 服务通信
C++ 实现
在 package.xml 中添加依赖
在服务通信部分，我选择直接在刚刚创建的包中添加两个新节点，用于实现简单的服务通信。我使用的是 example_interfaces 包中的 AddTwoInts 服务类型，该服务的请求为两个 int32 类型整数，响应为一个 int32 类型的整数。由于在创建该包时没有添加 example_interfaces 包作为依赖项，因此现在需要手动将该依赖添加到 package.xml 文件中。
1<depend>example_interfaces</depend>
编写服务端节点
和话题通信一样，服务通信也需要继承 ROS2 提供的节点类。
1#include "example_interfaces/srv/add_two_ints.hpp"
2#include "rclcpp/rclcpp.hpp"
3
4using std::placeholders::_1;
5using std::placeholders::_2;
6
7class MinimalServer : public rclcpp::Node {
8 public:
9 // 构造函数，初始化节点并创建服务
10 MinimalServer() : Node("add_two_ints_server") {
11 service_ = this->create_service<example_interfaces::srv::AddTwoInts>("service",
12 std::bind(&MinimalServer::handle, this, _1, _2));
13 RCLCPP_INFO(this->get_logger(), "Server ready.");
14 }
15
16 private:
17 // 处理服务请求的回调函数
18 void handle(const example_interfaces::srv::AddTwoInts::Request::SharedPtr request,
19 const example_interfaces::srv::AddTwoInts::Response::SharedPtr response) {
20 response->sum = request->a + request->b;
21 RCLCPP_INFO(this->get_logger(), "Request: a=%ld, b=%ld", request->a, request->b);
22 }
23
24 // 服务的共享指针
25 rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service_;
26};
27
28// 主函数，初始化 ROS 2，创建并自旋服务器节点，最后关闭
29int main(int argc, char **argv) {
30 rclcpp::init(argc, argv);
31 rclcpp::spin(std::make_shared<MinimalServer>());
32 rclcpp::shutdown();
33 return 0;
34}
编写客户端节点
1#include "example_interfaces/srv/add_two_ints.hpp"
2#include "rclcpp/rclcpp.hpp"
3
4using namespace std::chrono_literals;
5
6// 定义一个继承自 rclcpp::Node 的类，用于实现一个最小化的客户端
7class MinimalClient : public rclcpp::Node {
8 public:
9 // 构造函数，初始化节点并创建客户端和服务定时器
10 MinimalClient() : Node("mininal_client") {
11 client_ = this->create_client<example_interfaces::srv::AddTwoInts>("service");
12 timer_ = this->create_wall_timer(500ms, std::bind(&MinimalClient::send_request, this));
13 }
14
15 private:
16 // 定时器回调函数，处理服务响应
17 void timer_callback(const rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture future) {
18 RCLCPP_INFO(this->get_logger(), "Response: %ld", future.get()->sum);
19 }
20
21 // 发送请求到服务端
22 void send_request() {
23 while (!client_->wait_for_service(1s)) {
24 if (!rclcpp::ok()) {
25 RCLCPP_ERROR(this->get_logger(), "Interrupted.");
26 return;
27 } else {
28 RCLCPP_INFO(this->get_logger(), "Service not available.");
29 }
30 }
31
32 auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
33 request->a = i_++;
34 request->b = i_++;
35 client_->async_send_request(request, std::bind(&MinimalClient::timer_callback, this, std::placeholders::_1));
36 }
37
38 rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client_; // 服务客户端指针
39 rclcpp::TimerBase::SharedPtr timer_; // 定时器指针
40 size_t i_ = 0; // 用于生成请求数据的计数器
41};
42
43// 主函数，初始化 ROS 2 环境，创建并运行客户端节点，最后关闭 ROS 2 环境
44int main(int argc, char **argv) {
45 rclcpp::init(argc, argv);
46 rclcpp::spin(std::make_shared<MinimalClient>());
47 rclcpp::shutdown();
48 return 0;
49}
修改 CMakeLists.txt 文件并编译运行
在 CMakeLists.txt 中，我们也需要添加 example_interfaces 作为依赖项：
1find_package(example_interfaces REQUIRED)
随后添加编译目标：
1add_executable(server src/server.cpp)
2ament_target_dependencies(server rclcpp example_interfaces)
3add_executable(client src/client.cpp)
4ament_target_dependencies(client rclcpp example_interfaces)
5
6install(TARGETS
7 publisher
8 subscriber
9 server
10 client
11 DESTINATION lib/${PROJECT_NAME})
使用 colcon build 命令编译。
Python 实现
在 package.xml 中添加依赖
和 C++ 实现一样，我们需要在 package.xml 中添加 example_interfaces 作为依赖项。
1<depend>example_interfaces</depend>
整体来看，这个 XML 文件是这样：
1<?xml version="1.0"?>
2<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3<package format="3">
4 <name>py_pkg</name>
5 <version>0.0.0</version>
6 <description>TODO: Package description</description>
7 <maintainer email="li_zhong_yao@foxmail.com">jackgdn</maintainer>
8 <license>TODO: License declaration</license>
9
10 <depend>rclpy</depend>
11 <depend>std_msgs</depend>
12 <depend>example_interfaces</depend>
13
14 <test_depend>ament_copyright</test_depend>
15 <test_depend>ament_flake8</test_depend>
16 <test_depend>ament_pep257</test_depend>
17 <test_depend>python3-pytest</test_depend>
18
19 <export>
20 <build_type>ament_python</build_type>
21 </export>
22</package>
编写服务端节点
1import rclpy
2from example_interfaces.srv import AddTwoInts
3from rclpy.node import Node
4
5
6class MinimalServer(Node):
7 # 初始化函数，设置节点名称并创建服务
8 def __init__(self):
9 super().__init__("minimal_server")
10 self.service = self.create_service(AddTwoInts, "service", self.handle_add)
11 self.get_logger().info("Server ready.")
12
13 # 处理加法请求的回调函数
14 def handle_add(self, request, response):
15 response.sum = request.a + request.b
16 self.get_logger().info(f"Request: a={request.a}, b={request.b}")
17 return response
18
19
20# 主函数，初始化 ROS2 环境，创建服务节点并保持运行
21def main(args=None):
22 rclpy.init(args=args)
23 server = MinimalServer()
24 rclpy.spin(server)
25 rclpy.shutdown()
26
27
28if __name__ == "__main__":
29 main()
编写客户端节点
1import rclpy
2from example_interfaces.srv import AddTwoInts
3from rclpy.node import Node
4
5
6class MinimalClient(Node):
7 # 初始化节点，设置客户端并启动定时器
8 def __init__(self):
9 super().__init__("minimal_client")
10 self.client = self.create_client(AddTwoInts, "service")
11 self.i = 0
12 self.timer = self.create_timer(0.5, self.send_request)
13
14 # 处理服务响应的回调函数
15 def timer_callback(self, future):
16 response = future.result()
17 self.get_logger().info(f"Response: {response.sum}")
18
19 # 发送请求到服务端
20 def send_request(self):
21 while not self.client.wait_for_service(timeout_sec=1.0):
22 if not rclpy.ok():
23 self.get_logger().error("Interrupted.")
24 return
25 else:
26 self.get_logger().info("Service not available.")
27
28 request = AddTwoInts.Request()
29 request.a = self.i
30 request.b = self.i + 1
31 self.i += 2
32
33 self.future = self.client.call_async(request)
34 self.future.add_done_callback(self.timer_callback)
35
36
37def main(args=None):
38 # 初始化 ROS 2 客户端库
39 rclpy.init(args=args)
40 client = MinimalClient()
41 # 运行客户端节点直到手动停止
42 rclpy.spin(client)
43 # 关闭 ROS 2 客户端库
44 rclpy.shutdown()
45
46
47if __name__ == "__main__":
48 main()
修改 setup.py 文件并编译运行
修改后的 setup.py 文件如下：
1from setuptools import setup
2
3package_name = "py_pkg"
4
5setup(
6 name="py_pkg",
7 version="0.0.0",
8 packages=["py_pkg"],
9 data_files=[
10 ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
11 ("share/" + package_name, ["package.xml"]),
12 ],
13 install_requires=["setuptools"],
14 zip_safe=True,
15 maintainer="jackgdn",
16 maintainer_email="li_zhong_yao@foxmail.com",
17 description="TODO: Package description",
18 license="TODO: License declaration",
19 tests_require=["pytest"],
20 entry_points={
21 "console_scripts": [
22 "publisher = py_pkg.publisher:main",
23 "subscriber = py_pkg.subscriber:main",
24 "server = py_pkg.server:main",
25 "client = py_pkg.client:main",
26 ],
27 },
28)
使用 colcon build 命令编译。