第 19.1.6 節
机器人工程写法与 ROS2 集成
0瀏覽次數0訪問次數--跳出率--平均停留
前面几节讲的是 Boost.Asio 本体。
这一节讲你真正做机器人项目时应该怎么组织代码，尤其是：串口驱动、TCP/UDP 通信模块、ROS2 节点之间的关系。
核心原则：
不要在 ROS2 回调里写长时间阻塞的串口 read / TCP read。
通信模块应该自己用 Asio 异步运行。
ROS2 节点只负责发布、订阅、参数、生命周期和业务逻辑。
本节仍然使用 std::bind。
示例 1：普通 main() 中模拟机器人协议解析
程序目标
很多下位机串口协议都是一行文本，例如：
ODOM 1.0 2.0 0.5
BAT 24.3
IMU 0.1 0.2 9.8
本例先不接串口，只在普通 main() 里模拟解析这些字符串。
完整代码
#include <iostream>
#include <sstream>
#include <string>
void parse_line(const std::string& line)
{
std::istringstream iss(line);
std::string type;
iss >> type;
if (type == "ODOM")
{
double x = 0.0;
double y = 0.0;
double yaw = 0.0;
iss >> x >> y >> yaw;
std::cout << "解析里程计：x = " << x
<< "，y = " << y
<< "，yaw = " << yaw << std::endl;
}
else if (type == "BAT")
{
double voltage = 0.0;
iss >> voltage;
std::cout << "解析电池电压：" << voltage << " V" << std::endl;
}
else if (type == "IMU")
{
double ax = 0.0;
double ay = 0.0;
double az = 0.0;
iss >> ax >> ay >> az;
std::cout << "解析 IMU 加速度："
<< ax << ", " << ay << ", " << az << std::endl;
}
else
{
std::cout << "未知协议行：" << line << std::endl;
}
}
int main()
{
// 程序从 main 函数开始执行，下面的语句会按顺序运行。
std::cout << "main：开始模拟协议解析" << std::endl;
parse_line("ODOM 1.0 2.0 0.5");
parse_line("BAT 24.3");
parse_line("IMU 0.1 0.2 9.8");
parse_line("HELLO STM32");
std::cout << "main：结束" << std::endl;
// 返回 0 表示程序正常结束。
return 0;
}
运行结果：见下方“运行输出与时间顺序”；如果示例涉及定时器、线程、网络或外部设备，具体时间和顺序可能会随环境略有变化。
编译运行
g++ demo1_protocol_parse.cpp -o demo1_protocol_parse -std=c++17
./demo1_protocol_parse
运行输出
main：开始模拟协议解析
解析里程计：x = 1，y = 2，yaw = 0.5
解析电池电压：24.3 V
解析 IMU 加速度：0.1, 0.2, 9.8
未知协议行：HELLO STM32
main：结束
本示例需要注意的点
在接串口之前，先把协议解析函数单独写好、单独测试。这样调试效率更高。
不要一上来就把：
串口读取
协议解析
ROS2 发布
控制逻辑
全部混在一个函数里。
示例 2：类里封装机器人协议解析器
程序目标
把示例 1 改成类封装，为后面接串口回调做准备。
完整代码
#include <iostream>
#include <sstream>
#include <string>
class RobotProtocolParser
{
public:
void parse_line(const std::string& line)
{
std::istringstream iss(line);
std::string type;
iss >> type;
if (type == "ODOM")
{
parse_odom(iss);
}
else if (type == "BAT")
{
parse_battery(iss);
}
else
{
std::cout << "未知协议行：" << line << std::endl;
}
}
private:
void parse_odom(std::istringstream& iss)
{
double x = 0.0;
double y = 0.0;
double yaw = 0.0;
iss >> x >> y >> yaw;
std::cout << "RobotProtocolParser：ODOM x = " << x
<< "，y = " << y
<< "，yaw = " << yaw << std::endl;
}
void parse_battery(std::istringstream& iss)
{
double voltage = 0.0;
iss >> voltage;
std::cout << "RobotProtocolParser：BAT voltage = "
<< voltage << " V" << std::endl;
}
};
int main()
{
// 程序从 main 函数开始执行，下面的语句会按顺序运行。
RobotProtocolParser parser;
std::cout << "main：开始解析" << std::endl;
parser.parse_line("ODOM 0.1 0.2 0.3");
parser.parse_line("BAT 23.8");
parser.parse_line("ERROR abc");
std::cout << "main：结束" << std::endl;
// 返回 0 表示程序正常结束。
return 0;
}
运行结果：见下方“运行输出与时间顺序”；如果示例涉及定时器、线程、网络或外部设备，具体时间和顺序可能会随环境略有变化。
编译运行
g++ demo2_protocol_parser_class.cpp -o demo2_protocol_parser_class -std=c++17
./demo2_protocol_parser_class
运行输出
main：开始解析
RobotProtocolParser：ODOM x = 0.1，y = 0.2，yaw = 0.3
RobotProtocolParser：BAT voltage = 23.8 V
未知协议行：ERROR abc
main：结束
本示例需要注意的点
协议解析器不应该依赖 Boost.Asio，也不应该依赖 ROS2。
它最好是一个纯 C++ 类。这样你可以用普通 main() 或单元测试直接验证它。
示例 3：Asio 串口读取 + 协议解析器组合
程序目标
把前面的异步串口读取和协议解析器组合起来：
串口收到一行
SerialRobotDriver::on_read() 被调用
取出字符串
交给 RobotProtocolParser 解析
继续注册下一次串口读取
完整代码
#include <boost/asio.hpp>
#include <boost/system/error_code.hpp>
#include <functional>
#include <iostream>
#include <istream>
#include <sstream>
#include <string>
class RobotProtocolParser
{
public:
void parse_line(const std::string& line)
{
std::istringstream iss(line);
std::string type;
iss >> type;
if (type == "ODOM")
{
double x = 0.0;
double y = 0.0;
double yaw = 0.0;
iss >> x >> y >> yaw;
std::cout << "解析 ODOM：x = " << x
<< "，y = " << y
<< "，yaw = " << yaw << std::endl;
}
else if (type == "BAT")
{
double voltage = 0.0;
iss >> voltage;
std::cout << "解析 BAT：" << voltage << " V" << std::endl;
}
else
{
std::cout << "未知数据：" << line << std::endl;
}
}
};
class SerialRobotDriver
{
public:
SerialRobotDriver(boost::asio::io_context& io, const std::string& port_name)
: serial_(io)
{
boost::system::error_code ec;
serial_.open(port_name, ec);
if (ec)
{
std::cout << "打开串口失败：" << ec.message() << std::endl;
return;
}
serial_.set_option(boost::asio::serial_port_base::baud_rate(115200));
std::cout << "SerialRobotDriver：串口打开成功" << std::endl;
start_read();
}
private:
void start_read()
{
boost::asio::async_read_until(serial_,
buffer_,
'\n',
std::bind(&SerialRobotDriver::on_read,
this,
std::placeholders::_1,
std::placeholders::_2));
}
void on_read(const boost::system::error_code& ec, std::size_t bytes_transferred)
{
if (ec)
{
std::cout << "串口读取错误：" << ec.message() << std::endl;
return;
}
std::istream is(&buffer_);
std::string line;
std::getline(is, line);
std::cout << "收到串口行，bytes = " << bytes_transferred
<< "，line = " << line << std::endl;
parser_.parse_line(line);
start_read();
}
private:
// serial_port 表示串口设备，后续读写都通过它完成。
boost::asio::serial_port serial_;
boost::asio::streambuf buffer_;
RobotProtocolParser parser_;
};
int main(int argc, char* argv[])
{
// 程序从 main 函数开始执行，argc/argv 用来接收命令行参数。
std::string port_name = "/dev/pts/3";
if (argc >= 2)
{
port_name = argv[1];
}
// io_context 是 Asio 的事件循环对象，异步任务需要靠它调度。
boost::asio::io_context io;
SerialRobotDriver driver(io, port_name);
std::cout << "main：开始 io.run()" << std::endl;
// 启动事件循环，前面注册的异步任务会在这里被调度执行。
io.run();
std::cout << "main：io.run() 返回" << std::endl;
return 0;
}
运行结果：见下方“运行输出与时间顺序”；如果示例涉及定时器、线程、网络或外部设备，具体时间和顺序可能会随环境略有变化。
编译运行
终端 1：创建虚拟串口。
socat -d -d pty,raw,echo=0 pty,raw,echo=0
终端 2：运行程序。
g++ demo3_serial_robot_driver.cpp -o demo3_serial_robot_driver -std=c++17 -lboost_system -pthread
./demo3_serial_robot_driver /dev/pts/3
终端 3：发送模拟下位机数据。
echo "ODOM 1.0 2.0 0.5" > /dev/pts/4
echo "BAT 24.2" > /dev/pts/4
运行输出与时间顺序
程序启动后输出：
SerialRobotDriver：串口打开成功
main：开始 io.run()
发送第一行后输出：
收到串口行，bytes = 17，line = ODOM 1.0 2.0 0.5
解析 ODOM：x = 1，y = 2，yaw = 0.5
发送第二行后输出：
收到串口行，bytes = 9，line = BAT 24.2
解析 BAT：24.2 V
本示例需要注意的点
这就是一个最小机器人串口驱动雏形。
但它还没有做：
- 串口断开重连；
- 二进制帧解析；
- CRC 校验；
- 数据转 ROS2 topic；
- cmd_vel 写回下位机；
- 多线程安全。
这些可以在这个结构上继续加。
示例 4：ROS2 节点里使用 Asio 的推荐结构
程序目标
这个示例展示推荐结构，不要求你现在立刻完整运行。
推荐思路：
ROS2 节点线程：负责 rclcpp::spin()
Asio 线程：负责 io_context.run()
串口回调：收到数据后更新成员变量或 post 给 ROS2 逻辑
ROS2 timer：定期发布 odom / battery / imu
代码骨架
#include <boost/asio.hpp>
#include <functional>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
class RobotNode : public rclcpp::Node
{
public:
RobotNode()
: Node("robot_node"),
guard_(boost::asio::make_work_guard(io_))
{
publisher_ = this->create_publisher<std_msgs::msg::String>("robot_status", 10);
ros_timer_ = this->create_wall_timer(
std::chrono::seconds(1),
std::bind(&RobotNode::on_ros_timer, this));
asio_thread_ = std::thread(std::bind(&RobotNode::run_asio, this));
std::cout << "RobotNode：启动完成" << std::endl;
}
~RobotNode()
{
guard_.reset();
io_.stop();
if (asio_thread_.joinable())
{
asio_thread_.join();
}
}
private:
void run_asio()
{
std::cout << "Asio 线程：io.run() 开始" << std::endl;
// 启动事件循环，前面注册的异步任务会在这里被调度执行。
io_.run();
std::cout << "Asio 线程：io.run() 返回" << std::endl;
}
void on_ros_timer()
{
std_msgs::msg::String msg;
msg.data = "robot alive";
publisher_->publish(msg);
RCLCPP_INFO(this->get_logger(), "publish: %s", msg.data.c_str());
}
private:
// io_context 是 Asio 的事件循环对象，异步任务需要靠它调度。
boost::asio::io_context io_;
boost::asio::executor_work_guard<boost::asio::io_context::executor_type> guard_;
// 创建子线程，让这部分代码和 main 线程并发运行。
std::thread asio_thread_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr ros_timer_;
};
int main(int argc, char* argv[])
{
// 程序从 main 函数开始执行，argc/argv 用来接收命令行参数。
rclcpp::init(argc, argv);
std::shared_ptr<RobotNode> node = std::make_shared<RobotNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
典型运行输出与时间顺序
节点启动后输出类似：
RobotNode：启动完成
Asio 线程：io.run() 开始
之后 ROS2 timer 每 1 秒输出一次：
[INFO] [robot_node]: publish: robot alive
[INFO] [robot_node]: publish: robot alive
[INFO] [robot_node]: publish: robot alive
按 Ctrl+C 退出后：
Asio 线程：io.run() 返回
本示例需要注意的点
ROS2 里也有 timer：
create_wall_timer(...)
Boost.Asio 里也有 timer：
boost::asio::steady_timer
它们不是一回事。
一般建议：
ROS2 周期发布 topic -> 用 ROS2 timer
串口/TCP/UDP 超时、重连、心跳 -> 用 Asio timer
为什么不要在 ROS2 回调里阻塞读串口
不要这样：
订阅 cmd_vel 的回调里
直接 while 循环 read 串口
因为 ROS2 executor 可能被你阻塞，其他 topic、timer、service 都会受影响。
更好的方式是：
Asio 负责异步收发
ROS2 回调只把命令投递给 Asio 模块
示例 5：ROS2 回调里把任务投递给 Asio
程序目标
展示一种常见结构：ROS2 收到命令后，不直接写串口，而是把写操作投递给 Asio 线程。
代码片段
void on_cmd_vel(const std_msgs::msg::String::SharedPtr msg)
{
std::string command = msg->data + "\n";
boost::asio::post(io_,
std::bind(&RobotNode::send_command_in_asio_thread,
this,
command));
}
void send_command_in_asio_thread(const std::string& command)
{
std::cout << "Asio 线程中发送命令：" << command;
// 这里调用 SerialWriter::async_send(command)
// 或者调用你自己的串口驱动发送队列
}
典型输出与时间顺序
当 ROS2 收到一条命令：
cmd_vel 0.2 0.0
ROS2 回调线程会立刻把任务投递出去。随后 Asio 线程执行：
Asio 线程中发送命令：cmd_vel 0.2 0.0
本示例需要注意的点
boost::asio::post() 可以把任务安全地投递到 Asio 事件循环。
如果你的串口对象只在 Asio 线程里访问，那么可以减少很多加锁问题。
机器人工程中的推荐分层
推荐结构：
RobotNode ROS2 节点层
├── SerialRobotDriver 串口通信层
│ ├── async_read_until 异步读取
│ ├── write_queue 异步写队列
│ └── steady_timer 超时/心跳/重连
├── RobotProtocolParser 协议解析层
└── RobotState 数据缓存层
不要写成：
一个 node.cpp 里面塞满：
串口打开
read
write
协议解析
odom 发布
cmd_vel 订阅
PID 控制
TF 发布
参数读取
这样后期会非常难维护。
常见坑总结
坑 1：io.run() 提前返回
原因通常是没有异步任务，也没有 work_guard。
解决：
auto guard = boost::asio::make_work_guard(io);
坑 2：回调不执行
常见原因：
- 没有调用
io.run()； io.run()已经返回；- 异步对象提前析构；
- buffer 提前析构；
- 串口没收到分隔符，例如
async_read_until(..., '\n')但下位机没发换行。
坑 3：类里绑定成员函数写错
正确写法：
std::bind(&ClassName::callback,
this,
std::placeholders::_1,
std::placeholders::_2)
如果是 shared_ptr 管理生命周期：
std::bind(&Session::on_read,
shared_from_this(),
std::placeholders::_1,
std::placeholders::_2)
坑 4：异步写时 buffer 是局部变量
错误思路：
函数里创建 std::string
把它交给 async_write
函数返回后 string 销毁
异步写还没完成
推荐：
用类成员变量
用 shared_ptr
用 write_queue
坑 5：TCP 当成一包一包的数据
TCP 是字节流，没有消息边界。机器人自定义协议必须设计分帧。
坑 6：UDP 以为一定可靠
UDP 可能丢包、乱序、重复。重要命令不要无脑依赖单次 UDP。
你后面可以怎么继续扩展
学完这套后，可以继续做三个小项目：
- 串口下位机协议驱动
Linux 上位机通过 Asio 串口读 STM32 发来的 odom，并发送 cmd_vel。 - TCP 调试服务器
写一个 TCP server，手机或电脑通过网络连接，发送调试命令控制小车。 - UDP 心跳与状态广播
小车每 1 秒广播一次自身状态，调试工具自动发现设备。
这三个项目和 ROS2、Nav2、ros2_control、小车底盘控制都能接上。