第 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、小車底盤控制都能接上。