跳到正文

Wiki

虚拟串口与模拟下位机

约 5 分钟阅读

本节用虚拟串口和模拟下位机跑通完整链路。没有真实电机也可以验证命令下发、反馈更新和失联处理。

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 总览 · 上一篇：实时循环与异步通信 · 下一篇：单执行器实物接入