26赛季结束了，工程机器人只有MCU没有上位机，双臂14个电机、底盘8个电机再加上云台电机，全部放在STM32H7上2ms周期计算运行。双臂甚至为了便利直接抛弃了正逆运动学，而是直接提前录制路径点回放完成了取矿动作，自定义控制器是直接同构关节映射。新赛季规则修改，要从工程机器人变成重装机器人，甚至还要加枪管等，工程复杂度和控制的电机会进一步增多，纯MCU架构总会触及算力瓶颈。另外碰撞检测、重力补偿、路径规划等转移到上位机进行控制是大势所趋，尤其是面对未来可能是视觉自动对矿、神经网络或强化学习相关的探索，Linux上位机也是必须的。
使用Linux作为控制平台，情况会比MCU复杂很多。Linux平台有CPU、GPU、硬盘、内存、文件系统、进程、动态库等等，是一个完整的计算机操作系统，其中最大的难题就是进程，这和MCU完全不同，MCU上面始终都在运行同一个程序、同一个地址空间，最后进一个while(1)，或者FreeRTOS的循环，他没有进程的概念。我们要在Linux上做控制，应该只开一个进程做一切还是分很多进程呢？一个进程肯定不好，假如一行代码造成空指针崩溃，整个车都会疯，这太危险了，而且多个进程也更利于不同逻辑的解耦。如果分很多进程，进程间怎么传输数据呢？怎么协调他们之间的关系呢？怎么和下位机通信呢？这显然需要实现一套完整的进程间、设备间通信的基础设施，这是操作系统强大的算力带来的必要成本。
然而，早有人帮我们把这个基础设施都建起来了，这就是ROS2（Robot Operating System 2，机器人操作系统 2）。虽然叫这个名字，但ROS2本身不是操作系统，他是为了在Linux或其他重操作系统内核上建立的控制系统基础设施。网上的大部分教程和AI都喜欢直接抛一些概念，扔一大堆API和命令，但我这里更倾向于从意识流的角度分析这一大套抽象为什么会被构建出来。
底层：DDS
在没有ROS2的情况下，假如我有一个chassis进程，一个gimbal进程，他俩之间要传输数据。我们首先会想到使用TCP/UDP这些传输层协议，然后Linux内核网络栈完成socket传输，但到这里只是一堆字节，毫无抽象。接下来我们就要封装谁发送谁接收的模型、数据类型、丢包处理、数据先后顺序、优先级、上线时间等等，OMG（Object Management Group，对象管理组织）对这些制定了一个国际标准，这就是大名鼎鼎的DDS（Data Distribution Service，数据分配服务）。DDS标准规定了对数据处理和传输的许多处理细节，其核心的思想为Data-Centric Publish-Subscribe，其核心不在于传输，而是数据本身。在这个逻辑里，是谁给谁传的不重要，而是先有了数据，才有了这个数据的生产者和消费者，然后自然而然在他们之间建立联系，恰好，机器人天然就是一个有着大量数据源和大量数据消费者的动态系统。因此ROS2天然就可以沿用这一标准，就产生了RMW（ROS Middleware Interface，ROS中间件接口），在Humble版本中，他把ROS2底层的数据管理交给了任意符合DDS标准的具体中间件实现：
1
2
3
| ➜ ros2 doctor --report | grep -i middleware
RMW MIDDLEWARE
middleware name : rmw_cyclonedds_cpp
|
我们可以通过这个命令看到我们现在运行的DDS具体实现，我这里是Cyclone DDS。除了Cyclone DDS之外，还有Fast DDS、Connext DDS等实现，可以在环境变量里面改。
代码组织：Node、Executable、Package、Workspace
在DDS上层，ROS2就需要封装出发送、接收、处理数据的基本逻辑单元了，这就是Node。进程本身不会是基本单元，因为一个进程可能存在多个Node，ROS2希望区分Linux系统的概念和ROS2本身的逻辑概念，上层就不需要关心实际数据的传输还是进程间还是进程内的，并且让他们可以自由组合以整体优化或减少开销。从操作系统层面，下面以C++为例，我们的目标一定还是编译出几个可执行文件（Executable），Linux运行每个Executable产生一个运行时进程，然后再抽象出Node，并利用DDS实现他们之间的数据管理，此时我们就需要思考应该怎么组织这一套“可以编译出多个Executable的复杂系统”的代码。
ROS2给我们了现成答案。对于一个完整的机器人项目，代码的最小组织单位是Package，他可以编译出一个或多个Executables，甚至可以没有Executable完全用来存放资源。我们运行下面的命令生成一个最简单的Package，至于为什么要这么创建目录后面会提到：
1
2
3
4
5
6
7
| mkdir -p my_arm_ws/src
cd my_arm_ws/src
ros2 pkg create my_arm \
--build-type ament_cmake \
--dependencies rclcpp \
--node-name my_node
cd my_arm
|
一个Package会有单独的package.xml和CMakeLists.txt，最简单的Package是这样的：
1
2
3
4
5
| my_arm/
├── package.xml
├── CMakeLists.txt
└── src/
└── my_node.cpp
|
package.xml 描述了Package的基本信息，包括名称和依赖的Packages，比如：
1
2
3
4
5
6
7
8
9
10
| <package format="xxx">
<name>my_arm</name>
<version>0.0.1</version>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
</package>
|
这里的rclcpp全称是ROS Client Library for C++，是ROS底层专为C++提供的大量API的库，如果使用Python相应的就是rclpy，不过此时Executable就不再是“编译出来的二进制文件”，而是链接出的Python脚本。CMakeLists.txt我们已经非常熟悉了，在MCU项目中也要使用到，他指挥CMake生成.ninja或Makefile等，再指挥编译器完成编译，所以他就决定了这个Package要编译出几个Executables以及怎么编译：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
| find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
add_executable(my_node src/my_node.cpp)
ament_target_dependencies(
my_node
rclcpp
)
install(
TARGETS my_node
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
|
这里出现了MCU项目里没有的东西，那就是ament，这是ROS2给CMake开发的一个工具包，他可以让CMake认识Package元信息以及各种依赖关系。可以看到，这里产生了这个Package唯一一个Executable，就是my_node，非常有意思的是这里的install，他规定要把my_node的最终产物放在lib/${PROJECT_NAME}，这是什么地方？我们运行下面的命令来构建：
1
2
| cd ../..
colcon build
|
colcon也是ROS2的构建工具，全称是COLlective CONstruction，意为把一组Packages作为整体，按依赖关系组织构建。这个整体我们就称为Workspace，运行colcon build的目录就称为Workspace目录。跑完这个命令我们的my_arm_ws/目录下就会产生除了src/外的一大堆目录：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
| my_arm_ws/
├── src/
│ ├── my_arm/
│ │ ├── package.xml
│ │ ├── CMakeLists.txt
│ │ └── src/
│ │ └── my_node.cpp
│ └── other_package/
│ └── ...
├── install/
│ ├── my_arm/
│ │ └── lib/
│ │ └── my_arm/
│ │ └── my_node
│ └── other_package/
│ └── ...
├── log/
│ └── ...
└── build/
└── ...
|
从Workspace的视角看，刚才所述真正需要维护的代码都在src/下，然而编译产生的真正Executable在install/下。剩下还有log/和build/，分别存放日志和构建过程中的缓存，这些目录下层才是一个个Packages。接下来我们来改my_node.cpp，让他成为一个真正的Node：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
| #include <rclcpp/rclcpp.hpp>
int main(int argc, char * argv[])_
{
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("my_node");
RCLCPP_INFO(node->get_logger(), "Hello ROS 2!");
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
|
很简单，先引入rclcpp库然后初始化，把命令行参数透传进来。std::make_shared<rclcpp::Node>("my_node")是创建了一个真正的Node对象，使用C++的智能指针，RCLCPP_INFO()则是打印日志。我们嵌入式人可以这么理解，rclcpp::spin()是ROS2维护的一个类似while(1)的事件循环，让他持续保持运行。接下来我们回到Workspace目录再构建一遍：
1
2
| cd ../../..
colcon build
|
然后运行那个Node，当然准确来说是那个Executable，但是里面只有一个Node：
1
2
| source install/setup.bash
ros2 run my_arm my_node
|
这里的source会灌入一些环境变量，让ROS2知道当前Workspace的上下文，找到Executable的位置，当然使用zsh可以改成setup.zsh。Node跑起来后，我们就能看到终端吐出了日志并阻塞：
1
| [INFO] [....] [my_node]: Hello ROS 2!
|
如果在另一个终端ros2 node list则会看到/my_node，这证明我们的Node已经正式运行起来了。当然ROS2官方提供了rqt可以图形化观察Node和他们之间的各种联系，可以在另一个终端跑，就会看到：
甚至我们可以直接ps aux | grep my_node来找到那个真实的Linux进程，他会直接指向那个Executable。
数据及共享：Topic、Service、Parameter、Action
一个Node跑起来肯定不够，早就提到ROS2的重点在于不同Node之间的数据共享，接下来我们来研究。
Topic
Topic是ROS2当中最简单的数据共享模型，他直接继承了DDS的Data-Centric Publish-Subscribe特性，先定义一种数据，我们称为Topic，再用一个或多个Node发布（Publish）、一个或多个Node订阅（Subscribe），就实现了多个Node对多个Node的一种数据广播。
我们来定义一个Node talker：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
| #include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using namespace std::chrono_literals;
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("talker");
auto publisher =
node->create_publisher<std_msgs::msg::String>("/hello", 10);
auto timer = node->create_wall_timer(
1s,
[publisher]() {
std_msgs::msg::String msg;
msg.data = "hello";
publisher->publish(msg);
});
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
|
依旧是创建Node对象，然后创建Topic及其Publisher，这里的API是node->create_publisher<std_msgs::msg::String>("/hello", 10)，其意就是创建一个名为/hello的Topic，然后将Node talker作为Publisher。参数10可以理解成最多缓存最近10条。timer即为1s执行一次，std_msgs::msg::String 是ROS2已经定义好的一个消息类型，其中只有string data一个字段，最后publisher->publish(msg);完成消息发布。有意思的是，这里create_wall_timer()的第三个参数是一个lambda表达式实现的闭包对象，巧妙地把publisher透传进去。
接下来我们定义一个Node listener：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
| #include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("listener");
auto subscription =
node->create_subscription<std_msgs::msg::String>(
"/hello",
10,
[](const std_msgs::msg::String::SharedPtr msg) {
RCLCPP_INFO(
rclcpp::get_logger("listener"),
"I heard: %s",
msg->data.c_str()
);
});
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
|
create_subscription()非常易懂，第三个参数依旧是一个闭包，不过这里就是真正的回调函数，从参数拿到msg对象。回调只有底层DDS发现Topic /hello的Publisher发布新数据才会触发，这让我想到NVIC。
现在Package应该有两个Node，我们把他放进两个Executables，自然要改CMakeLists：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
| find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
add_executable(talker src/talker.cpp)
ament_target_dependencies(
talker
rclcpp
std_msgs
)
add_executable(listener src/listener.cpp)
ament_target_dependencies(
listener
rclcpp
std_msgs
)
install(
TARGETS
talker
listener
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
|
这里重点在于多了依赖，find_package(std_msgs REQUIRED)，两个Executables分别也要在他们的ament_target_dependencies加上std_msgs，包括package.xml：
1
2
| <depend>rclcpp</depend>
<depend>std_msgs</depend>
|
接下来colcon build、source install/setup.bash、ros2 run my_package talker、ros2 run my_package listener一条龙，把两个Node跑起来：
1
2
3
| [INFO] [...] [listener]: I heard: hello
[INFO] [...] [listener]: I heard: hello
[INFO] [...] [listener]: I heard: hello
|
这样一个最简单的Topic Publish-Subscribe就完成了，可以用ros2 topic list和rqt来观测他们。有意思的是，甚至可以停掉Node talker，直接用终端来Publish：
1
| ros2 topic pub /hello std_msgs/msg/String "{data: 'hello from cli'}" -r 1
|
可见，Subscriber根本不会关心是谁在Publish，只关心Topic实际数据是什么，这就是Data-Centric。
Service
Topic是纯粹的Data-Centric，完全不关心谁收到。但是实际机器人项目中一定有确认对方收到、等待结果的需求，ROS2向上封装了一个Service抽象来应对。Service同样定义了两个Node角色，分别是Client和Server，很形象，Client向Server发送请求（Request），Server收到后回复（Response），这就是Service Request-Response模型，可以有多个Client和一个Server。ROS2提供了一个很经典的Service数据类型，叫做example_interfaces/srv/AddTwoInts，是这样定义的：
1
2
3
4
| int64 a
int64 b
---
int64 sum
|
---上方就是Request的数据类型，下方是Response的数据类型。这种类似结构体的数据类型模板定义叫做Interface，ROS2有三类Interface，分别是.msg、.srv、.action，这里的AddTwoInts就是一个.srv实例，而前面讲到的std_msgs/msg/String是一个.msg实例，.action在后面讲解Action的时候会提到。Interface本身也在Package里，举个例子：
1
2
3
4
5
6
7
8
9
| my_robot_interfaces/
├── package.xml
├── CMakeLists.txt
├── msg/
│ └── ArmState.msg
├── srv/
│ └── ResetMotor.srv
└── action/
└── MoveArm.action
|
这里的ArmState.msg就称为my_robot_interfaces/msg/ArmState，可见前面的example_interfaces和std_msgs本身也是Package，不过是ROS2自带的基础Package。在C++代码中调用时，ROSIDL（ROS Interface Definition Language，ROS接口定义语言）会自动根据my_robot_interfaces/msg/ArmState里的Interface定义生成真实的my_robot_interfaces::msg::ArmState结构体来供我们使用。当然，存放Interface的Package一般不会存放真实的Node，这是一种好习惯，不然会产生依赖地狱。
我们来创建一个Node来做Server：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
| #include <memory>
#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node =
std::make_shared<rclcpp::Node>("add_server");
auto service =
node->create_service<example_interfaces::srv::AddTwoInts>(
"/add_two_ints",
[](const std::shared_ptr<
example_interfaces::srv::AddTwoInts::Request> request,
std::shared_ptr<
example_interfaces::srv::AddTwoInts::Response> response)
{
response->sum =
request->a + request->b;
}
);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
|
相同的样板代码不多赘述，create_service()很直观，从回调拿到request和response两个参数。接下来我们来创建Client：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
| #include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
using namespace std::chrono_literals;
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node =
std::make_shared<rclcpp::Node>("add_client");
auto client =
node->create_client<
example_interfaces::srv::AddTwoInts>(
"/add_two_ints");
while (!client->wait_for_service(1s)) {
RCLCPP_INFO(
node->get_logger(),
"Waiting for service..."
);
}
auto request =
std::make_shared<
example_interfaces::srv::AddTwoInts::Request>();
request->a = 10;
request->b = 20;
auto future =
client->async_send_request(request);
rclcpp::spin_until_future_complete(
node,
future
);
auto response = future.get();
RCLCPP_INFO(
node->get_logger(),
"Result: %ld",
response->sum
);
rclcpp::shutdown();
return 0;
}
|
首先是这里的while (!client->wait_for_service(1s))，这代表等待Server上线，如果没上线就每秒打印一句Waiting for service...，很好理解。有意思的在auto future = client->async_send_request(request);，这里代表异步发送Request，发送Request本身不会阻塞Node，可以换回一个future对象作为凭证未来兑换真正的Response。这里后面又使用了阻塞的写法，spin_until_future_complete() 显式阻塞程序运行直到获得Response。当然，也可以不阻塞，把Response到来的时机作为回调运行：
1
2
3
4
5
6
7
8
9
10
11
12
13
| auto response_callback =
[node](rclcpp::Client<AddTwoInts>::SharedFuture future) {
auto response = future.get();
RCLCPP_INFO(
node->get_logger(),
"Result: %ld",
response->sum
);
};
client->async_send_request(request, response_callback);
// Do something else
|
这样就可以体现真正的“异步”，在等待Response的时间做其他事。
接下来一样在CMakeLists和package.xml里加上新依赖example_interfaces，然后编译运行一条龙，就可以使用ros2 service list和rqt观察，当然依旧可以直接终端作为Client来发Request：
1
2
3
4
| ros2 service call \
/add_two_ints \
example_interfaces/srv/AddTwoInts \
"{a: 10, b: 20}"
|
结果一样：
1
2
| response:
example_interfaces.srv.AddTwoInts_Response(sum=30)
|
Parameter
Topic所承载的数据经常是实时不断变化的动态数据，就比如说当前的关节角，但Node一些功能配置肯定需要固定下来，比如提前调好的Kp/Kd参数。ROS2专为这种需求提供了Parameter，这是挂在Node上带名字和类型的一些运行时配置值，他是Node本身的属性。他主要强调运行时，不然硬编码每次修改值都要重新编译。我们举个最简单的例子：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
| #include <memory>
#include "rclcpp/rclcpp.hpp"
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("my_node");
node->declare_parameter("message", "hello");
auto message =
node->get_parameter("message").as_string();
RCLCPP_INFO(
node->get_logger(),
"message = %s",
message.c_str()
);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
|
declare_parameter()在这里声明了Parameter message，类型是string。get_parameter()则是读取当前的参数值。命令行可以直接get/set，比如：
1
2
| ros2 param set /my_node message abc
ros2 param get /my_node message
|
这里有一个问题，如果在get_parameter()之后再set，message对象本身不会再更新。如果要实时响应Parameter修改可以用回调：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
| node->declare_parameter("max_velocity", 2.0);
auto callback =
node->add_on_set_parameters_callback(
[node](const std::vector<rclcpp::Parameter> & parameters)
{
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
for (const auto & parameter : parameters) {
if (parameter.get_name() == "max_velocity") {
RCLCPP_INFO(
node->get_logger(),
"new max_velocity = %.2f",
parameter.as_double()
);
}
}
return result;
}
);
|
甚至可以拒绝修改，这里把value限制在0到10内：
1
2
3
4
5
6
7
8
9
| if (parameter.get_name() == "max_velocity") {
double value = parameter.as_double();
if (value <= 0.0 || value > 10.0) {
result.successful = false;
result.reason = "max_velocity must be between 0 and 10";
}
}
|
或者修改别人的Parameter：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
| auto client =
std::make_shared<rclcpp::AsyncParametersClient>(
node,
"/arm_controller"
);
while (!client->wait_for_service(std::chrono::seconds(1))) {
RCLCPP_INFO(
node->get_logger(),
"Waiting for parameter service..."
);
}
client->set_parameters({
rclcpp::Parameter("max_velocity", 1.5)
});
|
可以注意到这里出现了wait_for_service()，这是因为A修改B的Parameter本质上就是一个经典的Service Request-Response，A作为Client，修改的数据作为Request，B作为Server回复Response接受修改或拒绝修改，可见Parameter底层是大量通过Service实现的。
Action
Action顾名思义，就是一个有完整生命周期的行动任务，可以随时监控进度或中途取消。如果我们要自己实现这一套状态机，需要使用很多Topic和Service，但ROS2都帮我们封装好了，发出命令的是Client，执行任务的是Server。我们先看看我们之前提到的.action Interface的结构：
1
2
3
4
5
| Goal
---
Result
---
Feedback
|
这里出现了三个部分，Goal是最初的目标，Result的执行完实际返回的结果，Feedback是执行过程当中的反馈。依旧是action_tutorials_interfaces/action/Fibonacci给了我们一个斐波那契数列的例子：
1
2
3
4
5
| int32 order
---
int32[] sequence
---
int32[] partial_sequence
|
int32 order表示最初的目标，计算多长的斐波那契数列；int32[] sequence是最终完整的数列；int32[] partial_sequence是当前已经算出来的部分。很好理解。我们来创建一个最简单的Server：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
| #include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "action_tutorials_interfaces/action/fibonacci.hpp"
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using GoalHandleFibonacci =
rclcpp_action::ServerGoalHandle<Fibonacci>;
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node =
std::make_shared<rclcpp::Node>("fibonacci_server");
auto action_server =
rclcpp_action::create_server<Fibonacci>(
node,
"/fibonacci",
// 收到 Goal
[](const rclcpp_action::GoalUUID &,
std::shared_ptr<const Fibonacci::Goal> goal)
{
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
},
// 收到 Cancel 请求
[](const std::shared_ptr<GoalHandleFibonacci>)
{
return rclcpp_action::CancelResponse::ACCEPT;
},
// Goal 正式接受
[](const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
// 真正执行任务的逻辑
}
);
rclcpp::spin(node);
rclcpp::shutdown();
}
|
rclcpp_action::create_server()直接接受了三个闭包，三个匿名函数，分别是收到Goal之后的选择（执行或拒绝）、收到Cancel申请之后的选择（执行或拒绝），后面才是真正的任务逻辑。拒绝直接返回REJECT：
1
| return rclcpp_action::GoalResponse::REJECT;
|
1
| return rclcpp_action::CancelResponse::REJECT;
|
执行过程当中不断发布Feedback：
1
2
3
4
5
6
| auto feedback =
std::make_shared<Fibonacci::Feedback>();
feedback->partial_sequence = sequence;
goal_handle->publish_feedback(feedback);
|
最终返回成功的Result：
1
2
3
4
5
6
| auto result =
std::make_shared<Fibonacci::Result>();
result->sequence = sequence;
goal_handle->succeed(result);
|
中途Cancel后Server的处理，然后返回Result：
1
2
3
4
5
| if (goal_handle->is_canceling()) {
// Cancel 需要的收尾工作
goal_handle->canceled(result);
}
|
当然也可以中途执行失败：
1
| goal_handle->abort(result);
|
接下来我们来写Client：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
| #include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "action_tutorials_interfaces/action/fibonacci.hpp"
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using GoalHandleFibonacci =
rclcpp_action::ClientGoalHandle<Fibonacci>;
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node =
std::make_shared<rclcpp::Node>("fibonacci_client");
auto client =
rclcpp_action::create_client<Fibonacci>(
node,
"/fibonacci"
);
client->wait_for_action_server();
Fibonacci::Goal goal;
goal.order = 10;
rclcpp_action::Client<Fibonacci>::SendGoalOptions options;
// Server 是否接受 Goal
options.goal_response_callback =
[](const GoalHandleFibonacci::SharedPtr & goal_handle)
{
if (!goal_handle) {
// Goal 被拒绝
return;
}
// Goal 被接受
};
// 执行过程中的 Feedback
options.feedback_callback =
[](GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
// 处理 feedback
};
// 最终 Result
options.result_callback =
[](const GoalHandleFibonacci::WrappedResult & result)
{
// 根据 result.code 判断：
// SUCCEEDED / ABORTED / CANCELED
};
client->async_send_goal(goal, options);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
|
很直观易懂，准备三个回调闭包，分别是发起Goal的回调、Feedback更新的回调、拿到Result的回调，都是options的成员。接下来用client->async_send_goal(goal, options);异步发送请求。
真实项目架构概要
学习完了ROS2的最基本形态和通信接口，这只是整个上位机控制栈的起点，距离维护真实的机器人项目还相差甚远，至少还要学习Executor、TF、URDF、ros2_control、MoveIt2等等。但受限于笔记的篇幅不能太长，再加上我坚持纯手敲笔记精力有限，这里就简单把一个真实项目的组织形态做一个前瞻。
假设一个机械臂项目是smartrobot_arm，他的Workspace结构可能是这样的：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
| smartrobot_ws/
└── src/
├── smartrobot_interfaces/
│ ├── msg/
│ ├── srv/
│ ├── action/
│ ├── package.xml
│ └── CMakeLists.txt
├── smartrobot_description/
│ ├── urdf/
│ ├── meshes/
│ ├── rviz/
│ └── ...
├── smartrobot_hardware/
│ ├── include/
│ ├── src/
│ ├── config/
│ └── ...
├── smartrobot_control/
│ ├── src/
│ ├── config/
│ └── ...
├── smartrobot_moveit_config/
│ ├── config/
│ ├── launch/
│ ├── rviz/
│ └── ...
├── smartrobot_bringup/
│ ├── launch/
│ ├── config/
│ └── ...
└── smartrobot_vision/
├── src/
└── ...
|
可以看到，不同的Package应该是分工明确的，且一个Package只做一件事，保持高内聚。smartrobot_interfaces/不用多说，之前早已提到过，Interface Package就应该只存放Interface，定义通信的数据类型标准；smartrobot_description/则专门存放一些资源文件，可以理解为机械臂长什么样，其中urdf/和meshes/基本都可以从Solidworks等CAD软件直接导出；smartrobot_moveit_config/包括一些MoveIt配置，MoveIt是一个机械臂控制框架；smartrobot_bringup/保存了整个项目的启动脚本。这四个Package都是资源Package，并没有实际的Node，也不会产生install/下面的实际Executable。
smartrobot_hardware/则是真正适配硬件的部分，处理和下位机甚至和电机的实际通信；smartrobot_control/ 是实际控制相关逻辑，smartrobot_vision/可以是视觉算法。这些则是真正的逻辑Package，包含真实的Node。一个项目资源和逻辑相区分是一个好习惯，可以让依赖关系更清晰更可维护。
启动脚本和YAML
有意思的是这里出现了很多launch/，以是smartrobot_bringup/下面的launch/举例：
1
2
3
4
5
6
| smartrobot_bringup/
├── launch/
│ └── robot.launch.py
├── config/
│ └── controller.yaml
└── package.xml
|
整个项目是一个复杂系统，每次启动必然要启动一大堆Node，并给他们的Parameter赋上初值以及其他基本配置。为了防止每次启动都要开多个终端跑一大堆ros2 run，就自然产生了启动脚本。这些Python脚本就是用来组织编排不同Node的启动顺序，一次性启动多个Node，以及加载config/下面的这些yaml值的，举个最简单的robot.launch.py的例子：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
| from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
# 找到 smartrobot_bringup 安装后的 share 目录
package_dir = get_package_share_directory(
"smartrobot_bringup"
)
# 拼出 YAML 文件路径
controller_config = os.path.join(
package_dir,
"config",
"controller.yaml"
)
# 定义要启动的 Node
arm_controller = Node(
package="smartrobot_control",
executable="arm_controller",
name="arm_controller",
parameters=[
controller_config
],
output="screen"
)
state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="screen"
)
return LaunchDescription([
state_publisher,
arm_controller
])
|
ROS2启动脚本的核心函数就是generate_launch_description()，里面指定需要启动的Node，这里启动了两个Node arm_controller和state_publisher，最后必须返回LaunchDescription()来执行。有意思的是这里还设置了Parameter值，他被写在config/controller.yaml里：
1
2
3
4
5
6
7
| # config/controller.yaml
arm_controller:
ros__parameters:
control_rate: 100
max_velocity: 2.0
gravity_compensation: true
|
启动脚本关联的yaml本质上一种Parameter覆写，代码里的declare_parameter()初值将会被覆写：
1
2
| node->declare_parameter("control_rate", 1000);
node->declare_parameter("max_velocity", 1.0);
|
前面yaml里的参数将会真正生效。这本质上也是一种参数资源和代码逻辑的解耦，将大量Parameter设置抽离成专门的yaml文件方便修改。最后一个命令：
1
| ros2 launch smartrobot_bringup robot.launch.py
|
相当于同时启动两个Node并设置Parameter。真实的ROS2项目会远比这个复杂，启动脚本也会层层引用嵌套，相当于同时执行几十上百条命令，完成真正的Bootstrap。当然，启动脚本还有remap、namespace等等功能这里就不详细展开了。
这篇笔记作为上位机技术栈的入门起点，是我从MCU开始向miniPC机器人编程领域拓展的里程碑，上位机拥有MCU前所未有的算力和资源，是一个全新的世界。电控之所以称之为电控，核心在于控制，无论是用MCU控制还是上位机控制，还是上位机下位机同时存在，只是平台不同，目标是相同的。
