Wiki

機器人工程寫法與 ROS2 整合

更新於 2023-10-05約 4 分鐘閱讀

本文由簡體中文內容確定性轉換，並受版本化術語表保護。

前面幾節講的是 Boost.Asio 本體。
這一節講你真正做機器人專案時應該怎麼組織程式碼，尤其是：序列埠驅動、TCP/UDP 通訊模組、ROS2 節點之間的關係。

核心原則：

不要在 ROS2 回调里写长时间阻塞的串口 read / TCP read。
通信模块应该自己用 Asio 异步运行。
ROS2 节点只负责发布、订阅、参数、生命周期和业务逻辑。

本節仍然使用 std::bind。

1.示例 1：普通 `main()` 中模擬機器人協議解析

1.1.程式目標

很多下位機序列埠協議都是一行文本，例如：

ODOM 1.0 2.0 0.5
BAT 24.3
IMU 0.1 0.2 9.8

本例先不接序列埠，只在普通 main() 裡模擬解析這些字串。

1.2.完整程式碼

#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;
}

執行結果：見下方“執行輸出與時間順序”；如果示例涉及定時器、執行緒、網路或外部裝置，具體時間和順序可能會隨環境略有變化。

1.3.編譯執行

g++ demo1_protocol_parse.cpp -o demo1_protocol_parse -std=c++17
./demo1_protocol_parse

1.4.執行輸出

main：开始模拟协议解析
解析里程计：x = 1，y = 2，yaw = 0.5
解析电池电压：24.3 V
解析 IMU 加速度：0.1, 0.2, 9.8
未知协议行：HELLO STM32
main：结束

1.5.本示例需要注意的點

在接序列埠之前，先把協議解析函式單獨寫好、單獨測試。這樣除錯效率更高。

不要一上來就把：

串口读取
协议解析
ROS2 发布
控制逻辑

全部混在一個函數里。

2.示例 2：類裡封裝機器人協議解析器

2.1.程式目標

把示例 1 改成類封裝，為後面接序列埠回撥做準備。

2.2.完整程式碼

#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;
}

執行結果：見下方“執行輸出與時間順序”；如果示例涉及定時器、執行緒、網路或外部裝置，具體時間和順序可能會隨環境略有變化。

2.3.編譯執行

g++ demo2_protocol_parser_class.cpp -o demo2_protocol_parser_class -std=c++17
./demo2_protocol_parser_class

2.4.執行輸出

main：开始解析
RobotProtocolParser：ODOM x = 0.1，y = 0.2，yaw = 0.3
RobotProtocolParser：BAT voltage = 23.8 V
未知协议行：ERROR abc
main：结束

2.5.本示例需要注意的點

協議解析器不應該依賴 Boost.Asio，也不應該依賴 ROS2。

它最好是一個純 C++ 類。這樣你可以用普通 main() 或單元測試直接驗證它。

3.示例 3：Asio 序列埠讀取 + 協議解析器組合

3.1.程式目標

把前面的非同步序列埠讀取和協議解析器組合起來：

串口收到一行
SerialRobotDriver::on_read() 被调用
取出字符串
交给 RobotProtocolParser 解析
继续注册下一次串口读取

3.2.完整程式碼

#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;
}

執行結果：見下方“執行輸出與時間順序”；如果示例涉及定時器、執行緒、網路或外部裝置，具體時間和順序可能會隨環境略有變化。

3.3.編譯執行

終端 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

3.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

3.5.本示例需要注意的點

這就是一個最小機器人序列埠驅動雛形。

但它還沒有做：

序列埠斷開重連；
二進位制幀解析；
CRC 校驗；
資料轉 ROS2 topic；
cmd_vel 寫回下位機；
多執行緒安全。

這些可以在這個結構上繼續加。

4.示例 4：ROS2 節點裡使用 Asio 的推薦結構

4.1.程式目標

這個示例展示推薦結構，不要求你現在立刻完整執行。

推薦思路：

ROS2 节点线程：负责 rclcpp::spin()
Asio 线程：负责 io_context.run()
串口回调：收到数据后更新成员变量或 post 给 ROS2 逻辑
ROS2 timer：定期发布 odom / battery / imu

4.2.程式碼骨架

#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;
}

4.3.典型執行輸出與時間順序

節點啟動後輸出類似：

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() 返回

4.4.本示例需要注意的點

ROS2 裡也有 timer：

create_wall_timer(...)

Boost.Asio 裡也有 timer：

boost::asio::steady_timer

它們不是一回事。

一般建議：

ROS2 周期发布 topic -> 用 ROS2 timer
串口/TCP/UDP 超时、重连、心跳 -> 用 Asio timer

4.5.為什麼不要在 ROS2 回撥裡阻塞讀序列埠

不要這樣：

订阅 cmd_vel 的回调里
直接 while 循环 read 串口

因為 ROS2 executor 可能被你阻塞，其他 topic、timer、service 都會受影響。

更好的方式是：

Asio 负责异步收发
ROS2 回调只把命令投递给 Asio 模块

5.示例 5：ROS2 回撥裡把任務投遞給 Asio

5.1.程式目標

展示一種常見結構：ROS2 收到命令後，不直接寫序列埠，而是把寫操作投遞給 Asio 執行緒。

5.2.程式碼片段

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)
    // 或者调用你自己的串口驱动发送队列
}

5.3.典型輸出與時間順序

當 ROS2 收到一條命令：

cmd_vel 0.2 0.0

ROS2 回撥執行緒會立刻把任務投遞出去。隨後 Asio 執行緒執行：

Asio 线程中发送命令：cmd_vel 0.2 0.0

5.4.本示例需要注意的點

boost::asio::post() 可以把任務安全地投遞到 Asio 事件迴圈。

如果你的序列埠物件只在 Asio 執行緒裡訪問，那麼可以減少很多加鎖問題。

6.機器人工程中的推薦分層

推薦結構：

RobotNode                  ROS2 节点层
├── SerialRobotDriver      串口通信层
│   ├── async_read_until   异步读取
│   ├── write_queue        异步写队列
│   └── steady_timer       超时/心跳/重连
├── RobotProtocolParser    协议解析层
└── RobotState             数据缓存层

不要寫成：

一个 node.cpp 里面塞满：
串口打开
read
write
协议解析
odom 发布
cmd_vel 订阅
PID 控制
TF 发布
参数读取

這樣後期會非常難維護。

7.常見坑總結

7.1.坑 1：`io.run()` 提前返回

原因通常是沒有非同步任務，也沒有 work_guard。

解決：

auto guard = boost::asio::make_work_guard(io);

7.2.坑 2：回撥不執行

常見原因：

沒有呼叫 io.run()；
io.run() 已經返回；
非同步物件提前析構；
buffer 提前析構；
序列埠沒收到分隔符，例如 async_read_until(..., '\n') 但下位機沒發換行。

7.3.坑 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)

7.4.坑 4：非同步寫時 buffer 是區域性變數

錯誤思路：

函数里创建 std::string
把它交给 async_write
函数返回后 string 销毁
异步写还没完成

7.5.坑 5：TCP 當成一包一包的資料

TCP 是位元組流，沒有訊息邊界。機器人自定義協議必須設計分幀。

7.6.坑 6：UDP 以為一定可靠

UDP 可能丟包、亂序、重複。重要命令不要無腦依賴單次 UDP。

8.你後面可以怎麼繼續擴充套件

學完這套後，可以繼續做三個小專案：

序列埠下位機協議驅動
Linux 上位機通過 Asio 序列埠讀 STM32 發來的 odom，併發送 cmd_vel。
TCP 除錯伺服器
寫一個 TCP server，手機或電腦通過網路連線，傳送除錯命令控制小車。
UDP 心跳與狀態廣播
小車每 1 秒廣播一次自身狀態，除錯工具自動發現裝置。

這三個專案和 ROS2、Nav2、ros2_control、小車底盤控制都能接上。

1.示例 1：普通 main() 中模擬機器人協議解析

1.1.程式目標

1.2.完整程式碼

1.3.編譯執行

1.4.執行輸出

1.5.本示例需要注意的點

2.示例 2：類裡封裝機器人協議解析器

2.1.程式目標

2.2.完整程式碼

2.3.編譯執行

2.4.執行輸出

2.5.本示例需要注意的點

3.示例 3：Asio 序列埠讀取 + 協議解析器組合

3.1.程式目標

3.2.完整程式碼

3.3.編譯執行

3.4.執行輸出與時間順序

3.5.本示例需要注意的點

4.示例 4：ROS2 節點裡使用 Asio 的推薦結構

4.1.程式目標

4.2.程式碼骨架

4.3.典型執行輸出與時間順序

4.4.本示例需要注意的點

4.5.為什麼不要在 ROS2 回撥裡阻塞讀序列埠

5.示例 5：ROS2 回撥裡把任務投遞給 Asio

5.1.程式目標

5.2.程式碼片段

5.3.典型輸出與時間順序

5.4.本示例需要注意的點

6.機器人工程中的推薦分層

7.常見坑總結

7.1.坑 1：io.run() 提前返回

7.2.坑 2：回撥不執行

7.3.坑 3：類裡繫結成員函式寫錯

7.4.坑 4：非同步寫時 buffer 是區域性變數

7.5.坑 5：TCP 當成一包一包的資料

7.6.坑 6：UDP 以為一定可靠

8.你後面可以怎麼繼續擴充套件

1.示例 1：普通 `main()` 中模擬機器人協議解析

7.1.坑 1：`io.run()` 提前返回