虛擬序列埠與模擬下位機

本節用虛擬序列埠和模擬下位機跑通完整鏈路。沒有真實電機也可以驗證命令下發、反饋更新和失聯處理。

1.程式目標與協議約定

為了先看清執行緒與介面，使用換行分隔的 ASCII 教學協議：

上位机 → 下位机：CMD q1 q2\n
下位机 → 上位机：STATE q1 q2 v1 v2\n

q 為 rad，v 為 rad/s。位置範圍為 [-1, 1]。下位機每輪返回反饋，上位機發送最新目標。這個簡化協議沒有序號、CRC、裝置身份和使能確認，不能直接等同於完整實物協議。

2.示例 1：準備虛擬序列埠

在 Ubuntu 終端中安裝依賴：

sudo apt-get install -y libasio-dev socat

終端 1：

socat -d -d \
  PTY,raw,echo=0,link=/tmp/ros2_ctrl_pc \
  PTY,raw,echo=0,link=/tmp/ros2_ctrl_mcu

兩個路徑是同一對互通終端的別名。保持 socat 執行，不能把兩端路徑都交給上位機。

3.示例 2：編寫模擬下位機

由你建立 ~/ros2_control_humble_ws/tools/virtual_mcu.py，先執行 mkdir -p ~/ros2_control_humble_ws/tools，再儲存完整程式碼：

#!/usr/bin/env python3
import math
import os
import select
import sys
import termios
import time
import tty

fd = os.open(sys.argv[1], os.O_RDWR | os.O_NOCTTY | os.O_NONBLOCK)
tty.setraw(fd)
q, target = [0.0, 0.0], [0.0, 0.0]
buffer = b''
previous = time.monotonic()
last_command = previous
try:
    while True:
        ready, _, _ = select.select([fd], [], [], 0.02)
        if ready:
            buffer += os.read(fd, 256)
            if len(buffer) > 1024:
                buffer = b''
            while b'\n' in buffer:
                line, buffer = buffer.split(b'\n', 1)
                try:
                    fields = line.decode('ascii').split()
                    if len(fields) != 3 or fields[0] != 'CMD':
                        continue
                    values = [float(x) for x in fields[1:]]
                    if all(math.isfinite(x) and abs(x) <= 1.0 for x in values):
                        target = values
                        last_command = time.monotonic()
                except (ValueError, UnicodeError):
                    pass
        now = time.monotonic()
        dt = now - previous
        previous = now
        if now - last_command > 0.5:
            target = q.copy()  # 教学协议的看门狗：保持当前位置。
        alpha = 1.0 - math.exp(-dt / 0.2)
        velocity = []
        for i in range(2):
            step = alpha * (target[i] - q[i])
            q[i] += step
            velocity.append(step / dt)
        line = f'STATE {q[0]} {q[1]} {velocity[0]} {velocity[1]}\n'
        try:
            os.write(fd, line.encode('ascii'))
        except BlockingIOError:
            pass  # 丢弃过期状态，下一轮发送最新值。
finally:
    os.close(fd)

終端 2：

python3 ~/ros2_control_humble_ws/tools/virtual_mcu.py /tmp/ros2_ctrl_mcu

模擬器從目標逐漸產生反饋；超過 0.5 秒沒有合法命令則保持當前位置。這個處理僅表達教學中的位置保持策略，不能替代真實驅動器的停機邏輯。

4.示例 3：編寫序列埠硬體外掛

儲存為 learn_control_hardware/src/serial_system.cpp。它繼承從零編寫模擬硬體外掛的介面定義，替換配置、啟用、讀寫和通訊實現：

#include "learn_control_hardware/mini_system.hpp"
#include <asio.hpp>
#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <functional>
#include <istream>
#include <mutex>
#include <sstream>
#include <thread>
#include "pluginlib/class_list_macros.hpp"
namespace learn_control_hardware
{
class SerialSystem : public MiniSystem
{
  using Clock = std::chrono::steady_clock;
  struct Feedback {
    std::array<double, 2> q{}, v{};
    Clock::time_point stamp{};
    bool valid{false};
  };
public:
  SerialSystem() : port_(io_), timer_(io_), buffer_(256) {}
  ~SerialSystem() override {stop();}
  hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override
  {
    MiniSystem::on_configure(state);
    rx_ = Feedback{}; failed_ = false; enabled_ = false;
    try {
      const auto it = info_.hardware_parameters.find("port");
      if (it == info_.hardware_parameters.end()) {return hardware_interface::CallbackReturn::ERROR;}
      port_.open(it->second);
      port_.set_option(asio::serial_port_base::baud_rate(115200));
      port_.set_option(asio::serial_port_base::character_size(8));
      port_.set_option(asio::serial_port_base::parity(asio::serial_port_base::parity::none));
      port_.set_option(asio::serial_port_base::stop_bits(asio::serial_port_base::stop_bits::one));
      port_.set_option(asio::serial_port_base::flow_control(asio::serial_port_base::flow_control::none));
      receive(); tick();
      thread_ = std::thread([this]() {io_.run();});
      // 配置阶段允许有限等待。实时 read()/write() 中不做这种等待。
      for (int n = 0; n < 100; ++n) {
        {
          std::lock_guard<std::mutex> lock(rx_mutex_);
          if (rx_.valid) {return hardware_interface::CallbackReturn::SUCCESS;}
        }
        if (failed_) {break;}
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
      }
    } catch (const std::exception &) {failed_ = true;}
    stop(); return hardware_interface::CallbackReturn::ERROR;
  }
  hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override
  {
    std::lock_guard<std::mutex> lock(rx_mutex_);
    if (!rx_.valid || Clock::now() - rx_.stamp > std::chrono::milliseconds(500)) {
      return hardware_interface::CallbackReturn::ERROR;
    }
    for (std::size_t i = 0; i < 2; ++i) {
      position_[i] = rx_.q[i]; velocity_[i] = rx_.v[i]; command_[i] = rx_.q[i];
    }
    {std::lock_guard<std::mutex> tx_lock(tx_mutex_); target_ = rx_.q;}
    last_rx_ = rx_.stamp; enabled_ = true;
    return hardware_interface::CallbackReturn::SUCCESS;
  }
  hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override
  {
    enabled_ = false;
    return hardware_interface::CallbackReturn::SUCCESS;
  }
  hardware_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override
  {stop(); return hardware_interface::CallbackReturn::SUCCESS;}
  hardware_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) override
  {stop(); return hardware_interface::CallbackReturn::SUCCESS;}
  hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override
  {
    {
      std::unique_lock<std::mutex> lock(rx_mutex_, std::try_to_lock);
      if (lock.owns_lock() && rx_.valid) {
        for (std::size_t i = 0; i < 2; ++i) {
          position_[i] = rx_.q[i]; velocity_[i] = rx_.v[i];
        }
        last_rx_ = rx_.stamp;
      }
    }
    if (failed_ || Clock::now() - last_rx_ > std::chrono::milliseconds(500)) {
      enabled_ = false;
      return hardware_interface::return_type::ERROR;
    }
    return hardware_interface::return_type::OK;
  }
  hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override
  {
    if (failed_) {return hardware_interface::return_type::ERROR;}
    for (double q : command_) {
      if (!std::isfinite(q) || q < -1.0 || q > 1.0) {
        enabled_ = false; return hardware_interface::return_type::ERROR;
      }
    }
    std::unique_lock<std::mutex> lock(tx_mutex_, std::try_to_lock);
    if (lock.owns_lock()) {target_ = {command_[0], command_[1]};}
    return hardware_interface::return_type::OK;
  }
private:
  void receive()
  {
    asio::async_read_until(port_, buffer_, '\n',
      std::bind(&SerialSystem::received, this, std::placeholders::_1, std::placeholders::_2));
  }
  void received(const asio::error_code & ec, std::size_t)
  {
    if (ec) {failed_ = true; return;}
    std::istream stream(&buffer_); std::string line; std::getline(stream, line);
    std::istringstream parser(line); std::string tag, extra; Feedback data;
    if (parser >> tag >> data.q[0] >> data.q[1] >> data.v[0] >> data.v[1] &&
      tag == "STATE" && !(parser >> extra)) {
      bool valid = true;
      for (double x : data.q) {valid = valid && std::isfinite(x) && std::abs(x) <= 1.0;}
      for (double x : data.v) {valid = valid && std::isfinite(x);}
      if (valid) {
        data.valid = true; data.stamp = Clock::now();
        std::lock_guard<std::mutex> lock(rx_mutex_); rx_ = data;
      }
    }
    receive();
  }
  void tick()
  {
    timer_.expires_after(std::chrono::milliseconds(20));
    timer_.async_wait(std::bind(&SerialSystem::send_tick, this, std::placeholders::_1));
  }
  void send_tick(const asio::error_code & ec)
  {
    if (ec || failed_) {return;}
    if (enabled_ && !busy_) {
      std::array<double, 2> q;
      {std::lock_guard<std::mutex> lock(tx_mutex_); q = target_;}
      std::ostringstream stream; stream << "CMD " << q[0] << " " << q[1] << "\n";
      outgoing_ = stream.str(); busy_ = true;
      asio::async_write(port_, asio::buffer(outgoing_),
        std::bind(&SerialSystem::sent, this, std::placeholders::_1, std::placeholders::_2));
    }
    tick();
  }
  void sent(const asio::error_code & ec, std::size_t)
  {busy_ = false; if (ec) {failed_ = true;}}
  void stop()
  {
    enabled_ = false; io_.stop();
    if (thread_.joinable()) {thread_.join();}
    asio::error_code ec; timer_.cancel(ec); port_.close(ec);
    // 消费取消回调，下一次 configure 不继承上次操作。
    io_.restart(); io_.poll(); io_.restart();
    buffer_.consume(buffer_.size()); busy_ = false;
  }
  asio::io_context io_;
  asio::serial_port port_;
  asio::steady_timer timer_;
  asio::streambuf buffer_;
  std::thread thread_;
  std::mutex rx_mutex_, tx_mutex_;
  Feedback rx_;
  std::array<double, 2> target_{};
  Clock::time_point last_rx_{};
  std::atomic<bool> enabled_{false}, failed_{false};
  std::string outgoing_;
  bool busy_{false};
};
}
PLUGINLIB_EXPORT_CLASS(learn_control_hardware::SerialSystem, hardware_interface::SystemInterface)

4.1.關鍵函式與執行順序

1. on_configure 開啟序列埠，設定 115200、8N1、無流控，啟動接收與 20 ms 傳送排程，再啟動獨立執行緒。
2. 配置階段有限等待第一份有效反饋；沒有反饋則配置失敗。等待不放在週期 read 中。
3. on_activate 對齊實際反饋與目標，允許傳送命令。
4. read 使用 try-lock 取完整快照，檢查單調時鐘下的 0.5 秒反饋期限。
5. write 驗證目標，通過 try-lock 更新待發送快照。
6. Asio 執行緒生成報文，async_write 使用成員字串作為緩衝；busy 標記保證未完成時不改寫它。
7. stop 先停事件迴圈並等待執行緒退出，再關閉物件並消費取消回撥。

4.2.為什麼不能在回撥裡直接寫 position_

position_ 的地址已經匯出給控制器。通訊執行緒若直接改它，控制執行緒同時讀取就可能資料競爭。本實現先更新受保護的 rx_，再由 read 把快照寫入介面變數。

4.3.為什麼先解析再鎖 rx_mutex_

解析字串和數值校驗屬於通訊執行緒工作；鎖僅保護短暫的完整結構複製。控制執行緒用 try-lock，拿不到時不等待，繼續用仍在有效期內的舊反饋。

5.示例 4：補齊構建與外掛註冊

用以下完整內容替換 CMakeLists.txt：

cmake_minimum_required(VERSION 3.16)
project(learn_control_hardware)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(ament_cmake REQUIRED)
find_package(Threads REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
add_library(${PROJECT_NAME} SHARED src/mini_system.cpp src/serial_system.cpp)
target_compile_definitions(${PROJECT_NAME} PRIVATE ASIO_STANDALONE)
target_link_libraries(${PROJECT_NAME} Threads::Threads)
target_include_directories(${PROJECT_NAME} PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
ament_target_dependencies(${PROJECT_NAME}
  hardware_interface pluginlib rclcpp rclcpp_lifecycle)
pluginlib_export_plugin_description_file(hardware_interface hardware_plugins.xml)
install(TARGETS ${PROJECT_NAME} DESTINATION lib)
install(DIRECTORY include/ DESTINATION include)
install(DIRECTORY urdf config launch DESTINATION share/${PROJECT_NAME})
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
ament_export_dependencies(hardware_interface pluginlib rclcpp rclcpp_lifecycle)
ament_package()

用以下完整內容替換 hardware_plugins.xml：

<library path="learn_control_hardware">
  <class name="learn_control_hardware/MiniSystem"
         type="learn_control_hardware::MiniSystem"
         base_class_type="hardware_interface::SystemInterface">
    <description>Two joint position learning system</description>
  </class>
  <class name="learn_control_hardware/SerialSystem" type="learn_control_hardware::SerialSystem"
         base_class_type="hardware_interface::SystemInterface"/>
</library>

在 package.xml 中補充系統構建依賴 <build_depend>asio</build_depend>。本節的 apt 安裝確保標頭檔案已存在；不要把 standalone Asio 與 Boost.Asio 的錯誤碼型別混用。

建立 urdf/mini_serial.urdf：

<?xml version="1.0"?>
<robot name="mini">

  <link name="world"/>
  <link name="base_link">
    <inertial><mass value="1"/><inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.007"/></inertial>
    <visual><geometry><box size="0.2 0.2 0.1"/></geometry></visual>
  </link>
  <joint name="world_fixed" type="fixed"><parent link="world"/><child link="base_link"/></joint>

  <link name="link1">
    <inertial><origin xyz="0 0 0.2"/><mass value="1"/>
      <inertia ixx="0.0135" ixy="0" ixz="0" iyy="0.0135" iyz="0" izz="0.00042"/>
    </inertial>
    <visual><origin xyz="0 0 0.2"/><geometry><box size="0.05 0.05 0.4"/></geometry></visual>
    <collision><origin xyz="0 0 0.2"/><geometry><box size="0.05 0.05 0.4"/></geometry></collision>
  </link>
  <joint name="joint1" type="revolute">
    <parent link="base_link"/><child link="link1"/>
    <origin xyz="0 0 0.05"/><axis xyz="0 1 0"/>
    <limit lower="-1" upper="1" effort="20" velocity="2"/>
  </joint>

  <link name="link2">
    <inertial><origin xyz="0 0 0.2"/><mass value="1"/>
      <inertia ixx="0.0135" ixy="0" ixz="0" iyy="0.0135" iyz="0" izz="0.00042"/>
    </inertial>
    <visual><origin xyz="0 0 0.2"/><geometry><box size="0.05 0.05 0.4"/></geometry></visual>
    <collision><origin xyz="0 0 0.2"/><geometry><box size="0.05 0.05 0.4"/></geometry></collision>
  </link>
  <joint name="joint2" type="revolute">
    <parent link="link1"/><child link="link2"/>
    <origin xyz="0 0 0.4"/><axis xyz="0 1 0"/>
    <limit lower="-1" upper="1" effort="20" velocity="2"/>
  </joint>

  <ros2_control name="MiniSystem" type="system">
    <hardware><plugin>learn_control_hardware/SerialSystem</plugin><param name="port">/tmp/ros2_ctrl_pc</param></hardware>
    <joint name="joint1">
      <command_interface name="position"/>
      <state_interface name="position"><param name="initial_value">0.0</param></state_interface>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint2">
      <command_interface name="position"/>
      <state_interface name="position"><param name="initial_value">0.0</param></state_interface>
      <state_interface name="velocity"/>
    </joint>
  </ros2_control>
</robot>

6.編譯執行

保持 socat 與模擬下位機執行，停止其它機器人，終端 3：

cd ~/ros2_control_humble_ws
colcon build --symlink-install --packages-select learn_control_hardware
source install/setup.bash

ros2 launch learn_control_hardware mini.launch.py \
  model:=$(ros2 pkg prefix learn_control_hardware)/share/learn_control_hardware/urdf/mini_serial.urdf

終端 4：

source /opt/ros/humble/setup.bash
source ~/ros2_control_humble_ws/install/setup.bash

ros2 topic pub --once /forward_position_controller/commands \
  std_msgs/msg/Float64MultiArray "{data: [0.25, -0.15]}"
ros2 topic echo /joint_states

6.1.執行結果與超時檢查

正常執行時，反饋應逐漸接近 0.25、-0.15 rad。隨後在終端 2 按 Ctrl+C 停止模擬下位機，等待超過反饋有效期，再執行 ros2 control list_hardware_components 和 ros2 control list_hardware_interfaces。

按教程參考版本的實現，反饋過期會觸發 read 返回 ERROR，硬體轉為 unconfigured，命令介面顯示 unavailable、unclaimed。使用不同 Humble 補丁版本時，應同時核對實際狀態和管理器錯誤日誌。

這個狀態轉換不等於真實電機自動斷電。外掛停用後續教學命令傳送，下位機仍需要獨立超時與裝置級故障處理。停用時已經在傳輸的位元組也不能由一個布林值撤回。

6.2.恢復方法

學習階段重新啟動模擬下位機和整套 launch，確認初始反饋正常再發送目標。不要在裝置失聯後無條件自動清錯並繼續運動。

7.本節總結

序列埠只是硬體後端的一部分。控制器繼續操作同名介面；通訊執行緒擁有報文緩衝；控制執行緒擁有匯出的狀態與命令變數。

---

返回 Humble 總覽 · 上一篇：即時迴圈與非同步通訊 · 下一篇：單執行器實物接入