Hands On Custom Input Driver

手把手 6：从零写一个 UDP 自定义遥控器驱动

本课以 UDP 文本包为例，但目标是掌握当前的多设备驱动框架。完整接口说明见 自定义遥控器驱动参考。

示例包：

vx=0.4 vy=0.0 yaw=-0.2 a=1 mode=0.8

目标数据流：

UDP packet
  -> UdpInputDriver
  -> udp.vx / udp.vy / udp.yaw / udp.a / udp.mode
  -> sources / controls / outputs
  -> MotionCommands

1. 定义候选设备

UDP 设备不是“文件是否存在”就能判断在线。建议 driver 根据最近一次格式正确的 UDP 包维护健康时间；is_available() 在活跃期间据此判断。若希望 UDP 在尚未收到包时也被选中，可让 driver 在 is_available() 中非阻塞地创建/轮询 socket，并缓存最近一包。

inputs:
  selection:
    scan_interval_ms: 100
    promote_stable_ms: 500

sources:
  udp_remote:
    type: udp
    priority: 20
    bind: 0.0.0.0
    port: 14550
    ready_timeout_ms: 1000
    loss_timeout_ms: 500
    cooldown_ms: 1000
    signals:
      udp.vx:
        from: udp.vx
        timeout_ms: 500
        failsafe: 0.0
      udp.vy:
        from: udp.vy
        timeout_ms: 500
        failsafe: 0.0
      udp.yaw:
        from: udp.yaw
        timeout_ms: 500
        failsafe: 0.0
      udp.a:
        from: udp.a
      udp.mode:
        from: udp.mode

这里的两个超时不要混淆：

• loss_timeout_ms：设备级断连。is_available() 连续失败后，管理器清空设备、发布一次零运动命令并切换。
• signals.*.timeout_ms：字段级保护。仅将过期字段写成 failsafe，不触发设备切换。

UDP 每包包含完整遥控状态时，通常只需要 loss_timeout_ms；timeout_ms 适合字段确实可能独立停更的协议。

2. 接入 controls

controls:
  udp.move.vx:
    type: analog
    inputs:
      - source: udp.vx
    alpha: 0.05

  udp.move.vy:
    type: analog
    inputs:
      - source: udp.vy
    alpha: 0.05

  udp.move.yaw:
    type: analog
    inputs:
      - source: udp.yaw
    alpha: 0.05

  udp.button.a:
    type: bool
    threshold: 0.5
    inputs:
      - source: udp.a

由于候选设备严格独占，也可以直接把 source 写进已有 move.vx 的 sources 列表；只会有当前活动设备在更新其 raw signal。

3. 建立文件

src/remote_controller/src/drivers/udp_input_driver.cpp

并在 CMakeLists.txt 的 remote_controller_core 源文件列表显式加入：

src/drivers/udp_input_driver.cpp

不要创建或修改旧的 src/input_driver.cpp，它已被拆分为 src/drivers/ 中的注册表和具体驱动。

4. 实现最小 driver

#include "remote_controller/drivers/input_driver.hpp"

#include <atomic>
#include <chrono>
#include <thread>

namespace remote_controller {

class UdpInputDriver : public InputDriverBase {
public:
    using InputDriverBase::InputDriverBase;

    std::string name() const override { return config_.name; }

    bool is_available() override
    {
        // 未激活时打开并非阻塞轮询 socket，激活后继续检查最近
        // 一次合法包；不能只看 UDP socket 是否已创建。
        return poll_health_nonblocking();
    }

    bool is_ready() const override { return ready_; }

    void start() override
    {
        stop();
        stop_flag_ = false;
        ready_ = false;
        thread_ = std::thread(&UdpInputDriver::run, this);
    }

    void stop() override
    {
        stop_flag_ = true;
        close_socket();
        if (thread_.joinable()) {
            thread_.join();
        }
        ready_ = false;
    }

private:
    std::thread thread_;
    std::atomic<bool> ready_{false};
    std::chrono::steady_clock::time_point last_valid_packet_{};

    void run()
    {
        while (!stop_flag_) {
            const Packet packet = read_next_valid_packet();
            if (!packet.valid) {
                continue;
            }
            set_signal("udp.vx", packet.vx);
            set_signal("udp.vy", packet.vy);
            set_signal("udp.yaw", packet.yaw);
            set_signal("udp.a", packet.a ? 1.0 : 0.0);
            set_signal("udp.mode", packet.mode);
            last_valid_packet_ = std::chrono::steady_clock::now();
            ready_ = true;
        }
    }
};

}  // namespace remote_controller

read_next_valid_packet() 应使用非阻塞 socket 与 select() / poll()。每次超时检查 stop_flag_；绝不能永久阻塞在 recvfrom()。

设备配置可从 config_ 读取：

const std::string bind = config_.options.at("bind");
const int port = std::stoi(config_.options.at("port"));

实际代码应对缺少 option、端口范围和数据包字段做校验。

5. 注册 factory

在 main.cpp 中、构造 COMPublisher 前注册类型：

remote_controller::register_input_driver_factory(
    "udp",
    [](const remote_controller::InputDeviceConfig &config,
       remote_controller::InputMapper &mapper,
       std::mutex &mapper_lock,
       remote_controller::DriverOutputHandler output_handler,
       remote_controller::DriverLogHandler log_handler) {
        return std::unique_ptr<remote_controller::InputDriver>(
            new remote_controller::UdpInputDriver(
                config, mapper, mapper_lock,
                std::move(output_handler), std::move(log_handler)));
    });

未注册 type: udp 时，节点仅会记录 warning 并回退到下一个候选设备。

6. 编译与验证

colcon build --packages-select remote_controller
source install/setup.bash

观察日志：

input candidate available: udp_remote
starting input candidate: udp_remote
input candidate ready; accepting commands: udp_remote

发包：

printf 'vx=0.4 vy=0.0 yaw=-0.2 a=1 mode=0.8\n' | nc -u -w0 127.0.0.1 14550

再检查：

1. 高优先级 UDP 设备稳定后是否抢占当前设备。
2. 停止合法 UDP 包后，是否在 loss_timeout_ms 后发布一次零运动命令。
3. 恢复数据时，是否在 ready 后才能控制。
4. 切换期间按住的 edge 命令是否必须释放再按才触发。
5. Ctrl+C 是否能在一个 poll 周期内退出。