Hands On Custom Input Driver

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

本课从零写一个具体 driver：

UdpInputDriver

它监听 UDP 文本包。你可以用 nc -u 手动发包测试。

示例包：

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

driver 把它变成 raw source：

udp.vx
udp.vy
udp.yaw
udp.a
udp.mode

然后 YAML 再把这些 raw source 解释成业务 controls。

0. 为什么用 UDP 示例

UDP 示例比 CRSF 更适合教学：

不需要真实遥控器硬件。
可以用命令行发包。
能演示自定义 driver 的完整流程。
思路可以迁移到串口、CRSF、SBUS、TCP。

1. 设计 raw source

先不写代码，先定义 driver 输出：

udp.vx      [-1, 1]
udp.vy      [-1, 1]
udp.yaw     [-1, 1]
udp.a       0 或 1
udp.b       0 或 1
udp.mode    [-1, 1]

注意：driver 不输出 normal、sin_wave、btn_10=5。这些是业务层含义，放 YAML。

2. 在 YAML 声明 source

打开：

src/remote_controller/config/xbox_default.yaml

添加：

sources:
  udp:
    type: udp
    bind: 0.0.0.0
    port: 14550
    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, timeout_ms: 500, failsafe: 0.0}
      udp.b: {from: udp.b, timeout_ms: 500, failsafe: 0.0}
      udp.mode: {from: udp.mode, timeout_ms: 500, failsafe: 0.0}

后面我们会让 C++ config 读取 bind 和 port。

3. 把 UDP source 接到 controls

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

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

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

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

  udp.mode:
    type: enum
    source: udp.mode
    default: middle
    positions:
      low: [-1.0, -0.35]
      middle: [-0.34, 0.34]
      high: [0.35, 1.0]

混入现有移动输出：

outputs:
  analog:
    vel_des.x:
      controls: [move.vx, udp.move.vx]
      mix: max_abs
    vel_des.y:
      controls: [move.vy, udp.move.vy]
      mix: max_abs
    yawdot_des:
      controls: [move.yaw, udp.move.yaw]
      mix: max_abs

触发 sin_wave：

controls:
  command.sin_wave:
    type: bool
    expr:
      any:
        - [udp.mode=high, udp.button.a]
        - [keyboard.sin_wave]

outputs:
  edge:
    - output: btn_10=5
      when: [command.sin_wave]

4. 给 RemoteConfig 添加 UdpConfig

打开：

src/remote_controller/include/remote_controller/config.hpp

添加：

struct UdpConfig {
    std::string bind = "0.0.0.0";
    int port = 14550;
};

在 RemoteConfig 里添加：

UdpConfig udp;

5. 读取 YAML 中的 bind / port

打开：

src/remote_controller/src/config.cpp

在 load_sources() 中处理 type：

if (type == "udp") {
    config.udp.bind = get_or<std::string>(
        group_node,
        "bind",
        config.udp.bind);
    config.udp.port = get_or<int>(
        group_node,
        "port",
        config.udp.port);
    if (config.udp.port <= 0 || config.udp.port > 65535) {
        throw std::runtime_error("sources." + group + ".port must be between 1 and 65535");
    }
}

放置位置：

可以放在 type == "joystick" / type == "keyboard" 判断附近。
不影响 signals 的通用解析。

6. 新建 driver 头文件

新建：

src/remote_controller/include/remote_controller/udp_input_driver.hpp

内容：

#pragma once

#include <memory>
#include <mutex>

#include "remote_controller/input_driver.hpp"

namespace remote_controller {

std::unique_ptr<InputDriver> create_udp_input_driver(
    InputMapper &mapper,
    std::mutex &mapper_lock,
    DriverOutputHandler output_handler,
    DriverLogHandler log_handler);

}  // namespace remote_controller

7. 新建 driver cpp

新建：

src/remote_controller/src/udp_input_driver.cpp

先写完整骨架：

#include "remote_controller/udp_input_driver.hpp"

#include <atomic>
#include <cerrno>
#include <cstdint>
#include <cstdlib>
#include <cstring>
#include <map>
#include <sstream>
#include <stdexcept>
#include <string>
#include <sys/select.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <thread>
#include <unistd.h>
#include <utility>
#include <vector>

namespace remote_controller {
namespace {

class UdpInputDriver : public InputDriver {
public:
    UdpInputDriver(
        InputMapper &mapper,
        std::mutex &mapper_lock,
        DriverOutputHandler output_handler,
        DriverLogHandler log_handler)
        : mapper_(mapper),
          mapper_lock_(mapper_lock),
          output_handler_(std::move(output_handler)),
          log_handler_(std::move(log_handler))
    {
    }

    ~UdpInputDriver() override
    {
        stop();
    }

    std::string name() const override
    {
        return "udp";
    }

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

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

private:
    InputMapper &mapper_;
    std::mutex &mapper_lock_;
    DriverOutputHandler output_handler_;
    DriverLogHandler log_handler_;
    std::atomic<bool> stop_flag_{false};
    std::thread thread_;
    int fd_ = -1;

    void open_socket()
    {
        fd_ = socket(AF_INET, SOCK_DGRAM, 0);
        if (fd_ < 0) {
            throw std::runtime_error("udp socket create failed");
        }

        sockaddr_in addr{};
        addr.sin_family = AF_INET;
        addr.sin_port = htons(static_cast<uint16_t>(mapper_.config().udp.port));
        addr.sin_addr.s_addr = inet_addr(mapper_.config().udp.bind.c_str());

        if (bind(fd_, reinterpret_cast<sockaddr *>(&addr), sizeof(addr)) < 0) {
            close_socket();
            throw std::runtime_error("udp bind failed");
        }

        log("udp driver listening on " + mapper_.config().udp.bind + ":" +
            std::to_string(mapper_.config().udp.port));
    }

    void run()
    {
        while (!stop_flag_) {
            const int fd = fd_;
            if (fd < 0) {
                break;
            }

            fd_set fds;
            FD_ZERO(&fds);
            FD_SET(fd, &fds);

            timeval tv{};
            tv.tv_sec = 0;
            tv.tv_usec = 100000;

            const int ready = select(fd + 1, &fds, nullptr, nullptr, &tv);
            if (stop_flag_) {
                break;
            }
            if (ready == 0) {
                touch_runtime_sources();
                continue;
            }
            if (ready < 0) {
                if (errno == EINTR) {
                    continue;
                }
                log("udp select failed");
                continue;
            }

            char buffer[1024];
            const ssize_t len = recvfrom(fd, buffer, sizeof(buffer) - 1, 0, nullptr, nullptr);
            if (len <= 0) {
                continue;
            }
            buffer[len] = '\0';

            const auto values = parse_packet(buffer);
            std::vector<std::string> outputs;
            {
                const std::lock_guard<std::mutex> guard(mapper_lock_);
                for (const auto &item : values) {
                    append(outputs, mapper_.set_signal("udp." + item.first, item.second));
                }
                mapper_.touch_runtime_sources_with_prefix("udp.");
            }
            dispatch(outputs);
        }
    }

    std::map<std::string, double> parse_packet(const std::string &packet) const
    {
        std::map<std::string, double> values;
        std::istringstream stream(packet);
        std::string token;
        while (stream >> token) {
            const auto eq = token.find('=');
            if (eq == std::string::npos) {
                continue;
            }
            const std::string key = token.substr(0, eq);
            const double value = std::atof(token.substr(eq + 1).c_str());
            values[key] = clamp(value);
        }
        return values;
    }

    double clamp(double value) const
    {
        if (value < -1.0) {
            return -1.0;
        }
        if (value > 1.0) {
            return 1.0;
        }
        return value;
    }

    void append(std::vector<std::string> &dst, const std::vector<std::string> &src)
    {
        dst.insert(dst.end(), src.begin(), src.end());
    }

    void touch_runtime_sources()
    {
        const std::lock_guard<std::mutex> guard(mapper_lock_);
        mapper_.touch_runtime_sources_with_prefix("udp.");
    }

    void close_socket()
    {
        if (fd_ >= 0) {
            close(fd_);
            fd_ = -1;
        }
    }

    void dispatch(const std::vector<std::string> &outputs) const
    {
        if (!outputs.empty() && output_handler_) {
            output_handler_(outputs);
        }
    }

    void log(const std::string &message) const
    {
        if (log_handler_) {
            log_handler_(message);
        }
    }
};

}  // namespace

std::unique_ptr<InputDriver> create_udp_input_driver(
    InputMapper &mapper,
    std::mutex &mapper_lock,
    DriverOutputHandler output_handler,
    DriverLogHandler log_handler)
{
    return std::unique_ptr<InputDriver>(new UdpInputDriver(
        mapper,
        mapper_lock,
        std::move(output_handler),
        std::move(log_handler)));
}

}  // namespace remote_controller

这份代码为了教学尽量直观。生产环境可以继续增强：

支持逗号分隔。
检查未知 key。
支持 remote 地址过滤。
支持包序号和丢包统计。
支持 link quality。

8. 注册 factory

打开：

src/remote_controller/src/input_driver.cpp

加入：

#include "remote_controller/udp_input_driver.hpp"

在 create_input_driver() 中添加：

if (driver_type == "udp") {
    return create_udp_input_driver(
        mapper,
        mapper_lock,
        std::move(output_handler),
        std::move(log_handler));
}

9. 修改 CMake

打开：

src/remote_controller/CMakeLists.txt

把源文件加入 core library：

add_library(${PROJECT_NAME}_core
  src/config.cpp
  src/input_driver.cpp
  src/input_mapper.cpp
  src/motion_commands_adapter.cpp
  src/udp_input_driver.cpp
)

10. 编译

colcon build --symlink-install --packages-select remote_controller

加载环境：

source install/setup.bash

11. 启动 UDP driver

ros2 run remote_controller remote_controller \
  --driver udp \
  --config src/remote_controller/config/xbox_default.yaml

另一个终端观察：

ros2 topic echo /motion_commands

12. 用 nc 发包测试

发移动速度：

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

触发按钮：

printf 'a=1 mode=0.8\n' | nc -u -w0 127.0.0.1 14550

释放按钮：

printf 'a=0 mode=0.8\n' | nc -u -w0 127.0.0.1 14550

13. 为什么 select 超时很重要

如果 driver 永久阻塞在 recvfrom()：

Ctrl+C
  -> stop_flag_ = true
  -> 线程还卡在 recvfrom
  -> 程序退不出

所以示例使用：

select(fd_ + 1, &fds, nullptr, nullptr, &tv);

每 100ms 醒一次检查 stop_flag_。

14. 为什么要 touch_runtime_sources

如果 UDP 还在连接状态，但值没变化，不能触发 failsafe。

示例中：

mapper_.touch_runtime_sources_with_prefix("udp.");

这表示：

udp. 前缀的 source 连接还活着
只是值没有变化

如果长时间没有 UDP 包，这个示例在 select timeout 时仍然 touch。对于真实遥控器，你应该按协议决定：

如果协议有独立心跳包，收到心跳就 touch。
如果没有任何包，应该不 touch，让 timeout 生效。

教学版为了方便本机手动发包，可以把 timeout 调大，或者按需要改成“只有收到包才 touch”。

15. 教学版和生产版的区别

教学版：

select timeout 也 touch udp.

适合手动测试，不会因为你几秒没发包就 failsafe。

生产版：

只有收到有效遥控器帧或心跳时 touch udp.

适合真实断联保护。

你可以把 run() 中 timeout 分支改成：

if (ready == 0) {
    continue;
}

这样没有包时不会 touch，timeout_ms 会生效。

16. 本课检查清单

1. config.hpp 有 UdpConfig。
2. RemoteConfig 有 udp 字段。
3. config.cpp 读取 type=udp 的 bind/port。
4. udp_input_driver.hpp 存在。
5. udp_input_driver.cpp 实现 InputDriver。
6. input_driver.cpp factory 支持 driver_type == udp。
7. CMakeLists.txt 加入 src/udp_input_driver.cpp。
8. remote_controller 能 --driver udp 启动。
9. nc 发包后 /motion_commands 变化。
10. Ctrl+C 能退出。
11. timeout/failsafe 行为符合你的生产策略。

Hands On Custom Input Driver

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

0. 为什么用 UDP 示例

1. 设计 raw source

2. 在 YAML 声明 source

3. 把 UDP source 接到 controls

4. 给 RemoteConfig 添加 UdpConfig

5. 读取 YAML 中的 bind / port

6. 新建 driver 头文件

7. 新建 driver cpp

8. 注册 factory

9. 修改 CMake

10. 编译

11. 启动 UDP driver

12. 用 nc 发包测试

13. 为什么 select 超时很重要

14. 为什么要 touch_runtime_sources

15. 教学版和生产版的区别

16. 本课检查清单

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

快速开始

Mod 与状态开发

模型、推理与真机

输入系统

平台接入与控制时序

配置与 API 参考

框架架构与维护

Clone this wiki locally