虛擬串口與模擬下位機

用虛擬串口連接上位機插件和模擬下位機。可以完整觀察目標下發、狀態返回與反饋超時。

1.協議與目標

CMD q1 q2\n
STATE q1 q2 v1 v2\n

q 為 rad，v 為 rad/s，目標範圍 [-1,1]。這是換行分隔的教學協議，沒有設備身份、序號、CRC 或使能確認；真實設備適配見單執行器實物適配。

2.示例 1：創建虛擬串口

sudo apt-get install -y libasio-dev socat
socat -d -d PTY,raw,echo=0,link=/tmp/jazzy_ctrl_pc \
  PTY,raw,echo=0,link=/tmp/jazzy_ctrl_mcu

保持 socat 運行。一端給上位機，另一端給模擬下位機。

3.示例 2：完整模擬下位機

創建 tools 目錄，將代碼保存為 ~/ros2_control_jazzy_ws/tools/virtual_mcu.py：

mkdir -p ~/ros2_control_jazzy_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_jazzy_ws/tools/virtual_mcu.py /tmp/jazzy_ctrl_mcu

模擬器持續產生反饋，超過 0.5 秒沒有合法命令時保持當前位置。這是教學位置策略，不代表任意驅動器的停機行為。

4.示例 3：完整串口硬件插件

保存 src/serial_system.cpp：

#include "jazzy_control_hardware/two_joint_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 jazzy_control_hardware
{
class SerialSystem : public TwoJointSystem
{
  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
  {
    TwoJointSystem::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) {
      q_[i] = rx_.q[i]; dq_[i] = rx_.v[i];
      set_state(position_name_[i], q_[i]); set_state(velocity_name_[i], dq_[i]);
      set_command(position_name_[i], 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) {
          q_[i] = rx_.q[i]; dq_[i] = rx_.v[i];
          set_state(position_name_[i], q_[i]); set_state(velocity_name_[i], dq_[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;}
    std::array<double, 2> next{};
    for (std::size_t i = 0; i < 2; ++i) {
      const double q = get_command(position_name_[i]);
      next[i] = q;
      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_ = next;}
    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(jazzy_control_hardware::SerialSystem, hardware_interface::SystemInterface)

4.1.執行過程與函數説明

on_configure 在非週期階段打開串口，設置 115200、8N1、無流控，啓動讀寫任務和獨立線程，有限等待第一份合法反饋。

on_activate 檢查反饋時效，將框架狀態與初始命令對齊，再允許發送。read 通過 try-lock 取得整組反饋，然後 set_state 更新框架接口；write 從 get_command 取完整目標，驗證後交給發送快照。

Asio 線程擁有報文字符串。busy 保證已有異步寫完成前不修改發送緩衝；讀緩衝有容量上限，錯誤會停止正常收發。

stop 先停止事件循環並等待線程退出，再取消和關閉資源。傳給回調的 this 必須在回調執行期間有效，因此線程不能在對象析構以後繼續運行。

4.2.為什麼解析在鎖外完成

字符串拆分和數值驗證屬於通信線程工作，鎖只保護短暫的結構複製。控制線程取不到鎖時繼續使用尚未過期的快照，不等待整段報文處理。

5.示例 4：最終構建與註冊文件

替換完整 CMakeLists.txt：

cmake_minimum_required(VERSION 3.22)
project(jazzy_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/two_joint_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 launch config 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="jazzy_control_hardware">
  <class name="jazzy_control_hardware/TwoJointSystem"
    type="jazzy_control_hardware::TwoJointSystem"
    base_class_type="hardware_interface::SystemInterface"/>
  <class name="jazzy_control_hardware/SerialSystem" type="jazzy_control_hardware::SerialSystem"
    base_class_type="hardware_interface::SystemInterface"/>
</library>

package.xml 在 export 之前增加系統構建依賴 <build_depend>asio</build_depend>。standalone Asio 使用 asio::error_code，不與 Boost.Asio 的類型混用。

保存完整 urdf/two_joint_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>jazzy_control_hardware/SerialSystem</plugin><param name="port">/tmp/jazzy_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_jazzy_ws
colcon build --symlink-install --packages-select jazzy_control_hardware
source install/setup.bash

ros2 launch jazzy_control_hardware two_joint.launch.py \
  model:=$(ros2 pkg prefix jazzy_control_hardware)/share/jazzy_control_hardware/urdf/two_joint_serial.urdf

終端 4：

source /opt/ros/jazzy/setup.bash
source ~/ros2_control_jazzy_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

反饋應接近 [0.25,-0.15]。隨後停止模擬下位機，等待超過反饋有效期，再查看：

ros2 control list_hardware_components
ros2 control list_hardware_interfaces
ros2 control list_controllers

read 應報告反饋過期，插件禁止繼續發送普通目標。具體錯誤後的狀態以所選框架版本和日誌為準；重新啓動前要恢復可信反饋。

停止上位機發送不等於真實電機斷電，已經進入傳輸的字節也不能由布爾標記撤回。實物需要設備協議確認及獨立看門狗。

---

返回 Jazzy 總覽 · 上一篇：實時循環與異步通信 · 下一篇：單執行器實物適配