虚拟串口与模拟下位机

用虚拟串口连接上位机插件和模拟下位机。可以完整观察目标下发、状态返回与反馈超时。

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