启元 T1 Pro 的 SDK 里没有导航模块，官方示例 carry-clothes 的检测器是一段返回空列表的占位代码。本文记录 color_seek 的完整开发过程：用 HSV 颜色检测，配上针孔测距与比例控制，让机器人在狭窄场地里自己找到指定颜色的目标并停在 0.6m 外。全部逻辑先在 WSL 的 --sim 里验证（1.20m → 0.68m 收敛，218 帧，零振荡），再打包 .rpk 经 PrimeConsole 部署上板。实测数据来自 Ubuntu 22.04.5 + ROS2 Humble 环境，代码开源可复现。

代码仓库地址： color_seek:基于 ROS2 与 HSV 颜色检测的机器人目标搜索项目 - AtomGit

目录

• 一、环境配置与部署

• 二、HSV 检测与现场标定

• 三、视觉伺服的三个参数

• 四、代码结构框架设计

• 五、调试实录：记录我遇到的两个坑

• 六、上板：从仿真到部署的三段路

• 七、常见问题排查

• 八、需要注意的限制

• 结语 / 参考资料

一、环境配置与部署

在我写第一行控制代码之前，我把环境链路整个过了一遍。这一步虽然似浪费时间，小编也是通过查资料、取各个官网查询问题做了近2个小时，后面各个开发都需要这个环境，所以还是很重要的。

实测环境：

组件	版本	说明
Windows 11 + WSL2	22H2	镜像网络模式，WSL 与机器人同网段
Ubuntu	22.04.5 LTS	官方 SDK 指定版本
ROS 2	Humble	desktop 安装
T1 SDK	v1.0.0.0	colcon 编译，4 个包
OpenCV	4.13.0（源码编译）	SDK 内置依赖

SDK 的编译是整条链路的地基。t1-sdk 在 WSL 里 colcon 编译，4 个包（aimdk_msgs、aimdk_opencv、ruckig_for_primebot、aimdk_examples_cpp）一次跑完 5 分 49 秒。中间那个 ippicv 下载 SSL 报错只是警告，编译没有被掐断，最后 Summary 里 4 packages finished——消息包和服务定义就都齐了：

WSL 里 colcon build 的完整输出：4 packages finished，SDK 与 aimdk_msgs 消息包就绪。

模型侧的验证靠 t1-urdf：RViz 里能看到完整的 T1 模型和关节列表，Gazebo 里能把机器人 spawn 进世界。这两步过了，说明 URDF 和仿真链路是通的：

Gazebo 中成功加载 T1 Pro 模型，仿真世界就绪。

--dry-run 的输出表明：机器人头部相机的 RTSP 流拉通了（[rtsp] connected: rtsp://192.168.100.198:2554/live_head_stereo_left），以及运行计划打印正常——模式、颜色、停车距离、限速、伺服频率一行齐全。

官方文档 附近写了无线场景可以用 rtsp://<IP>:2554/live_head_stereo_left 访问相机，但同一份文档也说了“暂不支持在 Windows/Mac 下进行开发”。我实际测下来的结论是：WiFi 下相机流间歇可用，PC 与机器人同网段时多数时候能拉到 640×480 的实时画面，偶发握手失败，强制 RTSP over TCP 后稳定得多

接口层对应的处理是：FFMPEG 环境变量锁死 TCP 传输、5 秒流超时，外加三次重试——每次失败间隔 1 秒：

os.environ["OPENCV_FFMPEG_CAPTURE_OPTIONS"] = (
    "rtsp_transport;tcp|stimeout;5000000")
for attempt in range(3):
    self._cap = cv2.VideoCapture(self._rtsp_url)
    if self._cap.isOpened():
        self.node.get_logger().info(f"[rtsp] connected: {self._rtsp_url}")
        break
    self.node.get_logger().warn(f"[rtsp] open failed ({attempt + 1}/3)")
    time.sleep(1.0)

三次都拉不开就放弃 RTSP，capture_image() 会自动落到备选路径——走 CaptureJpegImage 服务单帧取图。两条取图路径在接口层做了透明封装，策略层只管调 capture_image()。

二、HSV 检测与现场标定

机器人“找到”一个东西，前提是它先“看见”。我发现官方示例 carry-clothes 的 vision.py 里有一个 Detector 接口，读一遍源码就会发现，官方给的是占位实现——不管输入什么画面，返回的都是空列表。伺服、测距、控制的骨架都在，唯独“画面里到底有没有目标”这一段要自己写。

官方的这段占位实现长这样：

官方示例 carry-clothes 的 vision.py：Detector 接口与默认的 HeuristicDetector 占位实现——detect() 收到任何画面都返回空列表，注释里写明“请接入真实检测模型后上机”。

我们用最直接的办法：HSV 颜色阈值。

HSV_RANGES = {
    "red": [((0, 80, 80), (10, 255, 255)), ((170, 80, 80), (180, 255, 255))],
    "green": [((35, 60, 60), (85, 255, 255))],
    "blue": [((100, 80, 80), (130, 255, 255))],
}

红色跨越了色相的 0°，需要拆成两段。选目标物时我们做过一轮测算：把候选目标（一块砖红色封面的笔记本）的照片按 HSV 分布统计，结果如下：

目标物颜色分布与 HSV 阈值对照：色相 99% 落在 0~10°、饱和度 94% 高于 80、明度 97% 高于 80，三项全部命中预设 red 阈值——这块封皮参数不用调整。

换成颜色更杂的目标，需要现场标定。为此我加了一个 --detect-test 模式：取一帧、检测、画绿框、存图，不发起运动指令。先在真机上完成这个，等它框住了目标再让它走：

python3 main.py --detect-test --color red

阈值分割 → 5×5 形态学开运算去噪点、闭运算补空洞 → 取最大连通域 → 输出它的中心偏移和像素高度。下面是完整实现：

class ColorDetector:
    def detect(self, jpeg_bytes):
        buf = np.frombuffer(jpeg_bytes, dtype=np.uint8)
        frame = cv2.imdecode(buf, cv2.IMREAD_COLOR)
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
​
        mask = None
        for low, high in self.ranges:
            part = cv2.inRange(hsv, np.array(low), np.array(high))
            mask = part if mask is None else cv2.bitwise_or(mask, part)
​
        kernel = np.ones((5, 5), np.uint8)
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
​
        contours, _ = cv2.findContours(
            mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        best = max(contours, key=cv2.contourArea)
        if cv2.contourArea(best) < self.min_area_ratio * w * h:
            return []
​
        x, y, bw, bh = cv2.boundingRect(best)
        cx = (x + bw / 2.0 - w / 2.0) / (w / 2.0)
        cy = (h / 2.0 - (y + bh / 2.0)) / (h / 2.0)
        return [Detection(self.color, confidence, cx, cy, float(bh))]

几个设计点：

• 坐标归一化：cx 以画面中心为 0、右为正，cy 上为正。这样“偏了多少”和相机分辨率解耦，640×480 和 1920×1080 的流走同一套控制参数

• 置信度按面积给：min(1.0, area / (0.25 * w * h))，目标占画面四分之一就算满置信。它不参与判定，只在日志里给“框得稳不稳”一个直观读数

• min_area_ratio = 0.002：面积小于画面千分之二的连通域直接丢弃。真机上远处的小色块、反光噪点全靠这一刀过滤，没有它伺服循环会被噪声目标牵着走

官方示例 carry-clothes 在仿真里是这么收敛的：走到洗衣机前 0.60m 停下，9.2 秒完成放衣服任务。我们要做的只是把它的空检测器换成真的：

官方示例 carry-clothes 的 --sim 运行实录：[drive] 序列里距离从 1.19m 一路收敛，[servo] 已到位：距离 0.60m 方位 -0.7°——官方伺服骨架的完整工作样貌。

三、视觉伺服的三个参数

目标在画面里了，接下来是让它走过去。整个闭环如下图所示：

color_seek 视觉伺服闭环：取图、检测、测距、控制、下发速度，50Hz 一圈；停车判据是距离误差和角度误差同时进容差。

三个核心参数，下面是介绍：

没有深度相机、没有激光，测距用的是最朴素的针孔模型，如何让机器人获取距离：

distance = FOCAL_PX * target_height_m / height_px

目标物的实际高度是非常重要的。我们默认按 A4 纸短边 0.21m 标定，换成 213×290mm 立放的笔记本时传 --target-height 0.29。如果这个参数写错，比如：换成一块 12cm 高的饮料盒（和原物体，也就是笔记本），距离会被高估近一倍，机器人会直接冲过停车点怼上去，那就无法有效识别。

那么机器人的速度给多少？ 比例控制，两个增益各管一个方向：

forward = clip(k_forward * (distance - standoff), -max_forward, max_forward)
yaw = clip(k_yaw * radians(angle), -max_yaw, max_yaw)

k_forward=0.6、k_yaw=1.2 沿用官方示例的取值；max_forward 我们从官方的 0.35 压到 0.2——狭窄场地里慢就是快。

落到代码上就是两个纯函数，一个把检测框翻译成距离和方位角，一个把误差翻译成速度：

def estimate(self, det):
    if det.height_px > 1e-3:
        distance = FOCAL_PX * self.cfg.target_height_m / det.height_px
        distance = max(0.2, min(12.0, distance))
    else:
        distance = -1.0
    angle = det.cx * (FOV_DEG / 2.0)
    return distance, angle
​
def control(self, distance, angle):
    c = self.cfg
    derr = distance - c.standoff_m
    forward = max(-c.max_forward, min(c.max_forward, c.k_forward * derr))
    yaw = max(-c.max_yaw, min(c.max_yaw, math.radians(c.k_yaw * angle)))
    arrived = (abs(derr) <= c.dist_tolerance_m
               and abs(angle) <= c.angle_tolerance_deg)
    return forward, yaw, arrived

真正骨架是真机主循环——50Hz 绝对节拍，丢目标原地慢转搜索，丢失超 2 秒放弃：

while time.monotonic() - start < timeout_s:
    tick = time.monotonic()
    image = self.bot.capture_image()
    dets = detector.detect(image) if image else []
    if not dets:
        if time.monotonic() - last_seen > LOST_TIMEOUT_S:
            break
        self.bot.drive(0.0, 0.0, 0.25)
    else:
        last_seen = time.monotonic()
        distance, angle = self.estimate(dets[0])
        forward, yaw, arrived = self.control(distance, angle)
        self.bot.drive(forward, 0.0, yaw)
        if arrived:
            self.bot.stop()
            self.bot.say(f"找到{self.color}目标啦")
            self.bot.play_emotion(EMOTION["great_joy"])
            self.bot.play_led(LED["twinkle"])
            self.bot.release_input_source()
            return True
    gap = time.monotonic() - tick
    if gap < dt:
        time.sleep(dt - gap)

三个细节：节拍用绝对时间（tick + dt），不用“干完活睡固定时长”——检测耗时波动会累积成频率漂移，走跑控制对 50Hz 的要求是持续、不是平均；丢目标先原地慢转（0.25 rad/s）再谈放弃，盲走只会离目标更远；收尾必须 release_input_source，需要把控制权还回去，否则遥控和语音都进不来。

那么什么时候停？ 距离误差 ≤0.08m 且角度误差 ≤6° 判定到位。--sim 的实测输出把整个过程写得很清晰：

--sim 实测：SimWorld 从 1.20m、偏 15° 起步，218 帧收敛到 0.68m 停车。注意最后一行——停在 0.68 而不是 0.60，因为 0.6+0.08 的容差内即判到位。

把收敛过程画成曲线，形态更直观：

 收敛曲线（按 --sim 实测数据重绘）：距离单调递减、无回摆；角度（红线）同步归零。末段 0.71 → 0.68 两步收尾。

有一个细节非常重要：仿真里距离起点读数是 1.20m 还是 1.66m，取决于 --target-height 与仿真世界参数是否一致。第一版代码里 SimDetector 的目标高度写死 0.21m，命令行传 0.29 就会出现“实际 1.2 米、估出来 1.66 米”的偏差。改成跟随配置之后，仿真读数才和真机标定值对得上——仿真的价值就在于提前暴露这类标定错误。

四、代码结构框架设计

color_seek/
├── main.py            # 入口：参数解析、--dry-run/--sim/--detect-test/--record
├── manifest.json      # .rpk 技能声明（8 项原子能力）
├── pack_rpk.py        # 打包脚本（manifest + checksums → .rpk）
├── src/
│   ├── seek.py        # 伺服任务：SimWorld 仿真 / 真机循环 / CSV 记录
│   ├── iface.py       # SDK 接口封装：速度发布、动作切换、输入源、TTS/表情/灯带
│   └── detector.py    # HSV 检测器 + SimDetector（仿真世界）
└── cfg/pkg.yaml       # PrimeConsole 读取的技能身份

入口层把两种运行方式分开：命令行调试走 argparse，部署上板走固定入口 run()：

def run(sim=False, **kwargs):
    if not rclpy.ok():
        rclpy.init()
    node = rclpy.create_node("color_seek")
    cfg = ServoConfig(standoff_m=kwargs.get("standoff_m", 0.6),
                      max_forward=kwargs.get("max_forward", 0.2),
                      target_height_m=kwargs.get("target_height_m", 0.29))
    task = SeekTask(node, color=kwargs.get("color", "red"), cfg=cfg,
                    record_csv=kwargs.get("record_csv", False),
                    image_source=kwargs.get("image_source", "service"))
    task.bot.sim = sim
    try:
        arrived = task.run(sim=sim, timeout_s=kwargs.get("timeout_s", 60.0))
    finally:
        task.close()
        if rclpy.ok():
            rclpy.shutdown()

注意部署入口默认 image_source="service"：包跑在机器人板上，板内回环取图走 CaptureJpegImage 服务比 RTSP 稳；--target-height 默认值也换成真机主目标 0.29m（立放笔记本）。

iface.py 里最难缠的是动作切换的状态等待——SetMcAction 发出去只是受理，状态要从 TRANSITION(200) 轮询到 RUNNING(100) 才算切完，中途发速度指令全部无效：

def switch_action(self, action_id, desc, timeout_s=30.0):
    gap = time.monotonic() - self._last_set_action
    if gap < 0.6:
        time.sleep(0.6 - gap)
    req = SetMcAction.Request()
    req.command.action.value = action_id
    req.command.action_desc = desc
    resp = self._call(self.cli_set_action, req, timeout_sec=3.0)
    self._last_set_action = time.monotonic()
​
    deadline = time.monotonic() + timeout_s
    while time.monotonic() < deadline:
        q = GetMcAction.Request()
        r = self._call(self.cli_get_action, q, timeout_sec=1.0, retries=1)
        if r is not None and r.info.status.value == ACTION_RUNNING:
            return True
        time.sleep(0.2)
    return False

开头的 0.6 秒间隔护栏是给 SetMcAction 的调用限制留的——两次间隔小于 500ms 会返回 -10001，接口层记下上次调用时间自动补齐，策略层永远不用操心这个数。

分层的原则是把“和 SDK 对话的部分”和“控制策略的部分”隔开：iface.py 里所有 topic 名、QoS、500ms 节流、零速补发这些脏活都在，seek.py 只写“取图→算误差→发速度”的策略。后来 color_seek 和 crosstalk 的部署包出问题时，要改的只有 manifest 层，这样可以不用修改策略。

五、调试实录：记录我遇到的两个坑

坑一：距离标定错，仿真里可以看见，真机上就是一次碰撞。 针孔测距的误差方向很恶劣——目标被估矮了，距离会算远（提前停）；估高了，距离算近（冲过去）。上面说的 1.66m 偏差就是在仿真里抓到的。如果没这层验证，第一次真机跑就是拿机器人做碰撞测试。

坑二：包能校验、不能部署。 color_seek 和 crosstalk 两个包第一次部署全部失败，报 skillid unknown。拿部署成功的 touch_game 包做 diff，唯一结构差异是 cfg/pkg.yaml——PrimeConsole 的技能身份是从这个文件的 package_name 字段读的，缺了它，校验报告显示“通过”，部署按钮点了没反应。补上之后一次通过。这个坑的教训是：校验通过和部署成功之间，还隔着一层身份识别。

六、上板：从仿真到部署的三段路

整个开发路径可以切成三段，每段有明确的产物：

第一段在 WSL 里完成逻辑验证；第二段打包，.rpk 本质是 zip 加一份校验清单，八份文件：

pack_rpk.py 一键打包，输出包大小与文件数。

包内容清单：manifest.json（能力声明）、cfg/pkg.yaml（技能身份）、assets/icon.png、META-INF/checksums.json（sha256 校验）加源码四件套。

第三段在 PrimeConsole 里完成：导入 .rpk → 七维校验（files/errors/warnings/imports/sdk_calls/security/runtime）→ 部署到机器人。

color_seek 的验证报告：skillid 正确解析为 color_seek，原子能力列表与验证记录双双“通过”，部署成功。

部署完成意味着 Skill 跑在机器人板上，ROS2 通信走板内回环——WiFi 下的所有限制全部绕开。这也是我们把开发重心放在“仿真验证逻辑、部署验证链路”这条路线上的原因。下面是原子能力展示：

{
  "skillId": "color_seek",
  "name": "color_seek",
  "version": "1.0.0",
  "robotModel": "q1",
  "description": "视觉寻物：机器人扫描锁定指定颜色目标并走近播报方位，狭窄场地实测，限速 0.2m/s",
  "atomicCapabilities": [
    {
      "id": "motion.emotion",
      "label": "表情播放",
      "entry": "prime.motion.emotion"
    },
    {
      "id": "motion.preset",
      "label": "预设动作",
      "entry": "prime.motion.preset"
    },
    {
      "id": "tts.speak",
      "label": "语音合成",
      "entry": "prime.tts.speak"
    },
    {
      "id": "led.control",
      "label": "LED 灯效",
      "entry": "prime.led.control"
    }
  ]
}

本次二次开发已经发布skill（“视觉寻物”），欢迎大家体验：

七、常见问题排查

现象	原因	处理
ros2 topic list 收不到 HAL 数据	BMS 等 HAL 接口是 Best Effort，订阅端默认 Reliable	用 qos_profile_sensor_data
走跑指令发了机器人不动	速度指令中断超 1s 脱锁停机	保持 ≥50Hz 持续下发，循环用绝对节拍
SetMcAction 返回 -10001	两次调用间隔小于 500ms	接口层记录上次调用时间自动补齐
动作切换后指令被忽略	状态还在 TRANSITION(200)	轮询 GetMcAction 到 RUNNING(100) 再发速度
控制权抢不回来	SDK 输入源没释放	任务结束调 release_input_source
RTSP 间歇连不上	默认 UDP 传输不稳定	rtsp_transport;tcp + 超时 + 重试三次
目标找到了但停太远/冲过去	--target-height 与实际高度不符	按目标物实际高度重新标定

八、需要注意的限制

• HSV 阈值对光照敏感，哑光材质、避免强逆光，否则检测率明显下降

• 针孔测距只在 1m 内可靠，远了只适合粗定位

• 没有避障逻辑，也没有电量护栏——真机测试需要 1.5m 内清空、人随时待命

结语

color_seek 做的事可以一句话概括：在没有导航模块的 SDK 上，用相机和颜色检测把“走到目标前”这件事自己搭出来。从官方留空的检测器出发，到 --sim 收敛、到 .rpk 上板，每一步都有实测输出记录，可以说这次二次开发是成功的。

本文使用的 SDK 与消息包均来自启元机器人开发者社区——官方在 AtomGit 开源了 t1-sdk（含 aimdk_msgs 消息包）与 carry-clothes 示例技能，本文的伺服骨架即改造自后者，检测部分为需要自行开发实现。

参考资料

1. 启元机器人开发者社区（SDK/文档下载）：启元机器人开发者社区 · 一站式具身智能研发平台

2. 官方 t1-sdk 仓库（含 aimdk_msgs 消息包）：t1-sdk:基于 ROS2 与 FastDDS 的机器人二次开发 SDK 项目 - AtomGit

3. 官方 carry-clothes 示例技能（视觉伺服范式来源）：https://atomgit.com/primebot/carry-clothes

4. 本文配套仓库 color_seek：color_seek:基于 ROS2 与 HSV 颜色检测的机器人目标搜索项目 - AtomGit