启元Q1机器人SDK实操：跑通触摸消息，用ROS 2图形界面完成调试与模型展示

打工仔折腾 AI

391人浏览 · 2026-10-25 16:29:37

打工仔折腾 AI · 2026-10-25 16:29:37 发布

场景与目标

做机器人交互开发时，纯终端命令行的调试效率很低。触摸传感器是常见的输入源，但底层SDK通常只返回原生协议包（比如按键矩阵扫描码或电容屏原始坐标）。如果直接把这些数据散落在各个业务逻辑里，后续做行为树规划或者多模态融合会很痛苦。

我的目标很明确：把启元Q1的触摸事件标准化，接入ROS 2话题总线；在RViz2里同时看到机器人三维模型和触摸触发位置；整个链路要能跑通、能断点、能观察消息频率。不需要追求UI多华丽，重点是数据流不断、坐标系不乱、回调不阻塞。

环境基线与依赖梳理

ROS 2的版本选型直接决定了API走向。我目前的工作机跑的是 Ubuntu 22.04 + ROS 2 Humble，Python 3.10。这个组合对 rclpy 的协程支持和类型注解兼容性最好。如果你的设备是 Iron 或 Rolling，部分消息类的构造函数签名会有微调，但整体架构不变。

依赖清单按必要性排序：

ros-humble-rclpy：ROS 2 Python客户端库
ros-humble-sensor-msgs：包含 TouchEvent 标准消息定义
ros-humble-geometry-msgs：用于发布坐标变换（TF）
ros-humble-rviz2：图形化调试工具
pyyaml：读取自定义的关节限位或屏幕映射参数（可选）

这里需要明确一点：启元Q1官方SDK的具体类名、初始化函数和连接端口，不同固件版本可能存在差异。我无法逐行核对最新文档，因此下文以“硬件抽象层返回原始触控字典”为前提进行封装。实际对接时，只需把SDK返回的数据结构映射到字典即可，通信层的写法完全一致。

触摸事件的采集与ROS 2发布逻辑

触摸事件的核心不在于“拿到数据”，而在于“稳定上报”。硬件中断或轮询周期往往很快，如果每次触发都直接调用 publisher.publish()，ROS 2底层的DDS队列很容易被撑满，进而引发丢包或节点挂起。

我的做法是在节点内部做一层节流与状态去重。只有当触摸状态真正发生变化（按下→离开，或按压深度超过阈值）时才发布有效消息，并附带时间戳。

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import TouchEvent
from std_msgs.msg import Header
import time

class TouchPublisherNode(Node):
    def __init__(self):
        super().__init__('q1_touch_publisher')
        
        # 使用默认QoS，保证基本可靠性，避免高频刷屏导致DDS积压
        qos_profile = rclpy.qos.QoSProfile(
            reliability=rclpy.qos.Reliability.BEST_EFFORT,
            durability=rclpy.qos.Durability.TRANSIENT_LOCAL,
            history=rclpy.qos.HistoryPolicy.KEEP_LAST,
            depth=1
        )
        self.pub = self.create_publisher(TouchEvent, '/q1/touch_event', qos_profile)
        
        self._last_state = -1
        self._debounce_ms = 80  # 硬件消抖与逻辑防抖合并
        self._last_trigger_time = 0
        
        # 模拟SDK数据源，实际替换为 Q1_SDK.get_touch_raw()
        self.touch_data_source = {'x': 0.0, 'y': 0.0, 'pressure': 0.0, 'state': 0}
        
        self.timer = self.create_timer(0.05, self.poll_hw_and_publish)
        self.get_logger().info('触摸发布节点已启动，目标话题: /q1/touch_event')

    def poll_hw_and_publish(self):
        # 1. 获取原始数据（此处为占位，实际调用SDK）
        raw = self.touch_data_source  
        
        # 2. 状态变化检测 + 时间消抖
        current_time = self.get_clock().now().nanoseconds / 1e9
        if current_time - self._last_trigger_time < self._debounce_ms / 1000.0:
            return
            
        current_state = int(raw['state'])
        if current_state == self._last_state and current_state != 0:
            return  # 保持按压中且未超消抖窗口，跳过重复发布
            
        self._last_trigger_time = current_time
        self._last_state = current_state
        
        # 3. 构建标准消息
        msg = TouchEvent()
        msg.header = Header()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'touch_panel_frame'
        
        # 映射状态：0-RELEASED, 1-PRESSED, 2-ESCAPE
        msg.state = int(raw['state'])
        msg.point.x = float(raw['x'])
        msg.point.y = float(raw['y'])
        msg.pressure = float(raw['pressure'])
        
        self.pub.publish(msg)
        self.get_logger().debug(f'发布触摸事件: {msg.state}, ({msg.point.x:.2f}, {msg.point.y:.2f})')

这段代码有几个设计考量：

QoS 设为 BEST_EFFORT。触摸事件属于控制流而非关键遥测，允许偶尔丢失比阻塞主线程更合理。如果后续要接安全急停逻辑，再改为 RELiable。
_debounce_ms 不是随便写的。电容屏驱动层通常会输出毛刺，软件层做一次简单的低频过滤能大幅降低DDS负载。
frame_id 固定为 touch_panel_frame。这在后面做坐标变换时会起作用，否则RViz2无法自动将触摸点投影到模型上。

图形化调试：RViz2 加载模型与实时渲染

数据跑通后，下一步是可视化。RViz2 的配置不需要从零写 XML，直接通过 UI 添加显示项更高效。

操作步骤：

打开终端执行 ros2 run rviz2 rviz2。
左侧面板点击 Add → 搜索 RobotModel → 添加。在下方 Topic 栏填入你的 URDF 模型加载节点所发布的话题（通常是 /robot_description），或者直接修改 rviz2 启动参数传入文件路径。
再次 Add → 搜索 Marker 或 PointCloud。这里我用 Marker 演示更直观。设置 Marker Topic 为 /q1/touch_marker，形状选 SPHERE，大小设 0.02，颜色调亮。
确保 Global Options 中的 Fixed Frame 与机器人根连杆名称一致（例如 base_link）。

为了让触摸点能在模型屏幕上正确显示，我需要写一个极小的转换节点，把 TouchEvent 里的屏幕像素坐标换算成机器人物理坐标，并发布为一个球体 Marker。

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import TouchEvent
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Pose
from std_msgs.msg import Header
import math

class TouchToMarkerNode(Node):
    def __init__(self):
        super().__init__('touch_to_marker')
        self.sub = self.create_subscription(
            TouchEvent, '/q1/touch_event', self.on_touch, 10
        )
        self.marker_pub = self.create_publisher(Marker, '/q1/touch_marker', 10)
        
        # 屏幕分辨率与物理尺寸映射比例（需根据实际Q1屏幕参数填写）
        self.screen_w_px = 800
        self.screen_h_px = 480
        self.physical_w_m = 0.15  # 米
        self.physical_h_m = 0.09
        
        self.get_logger().info('触摸转Marker节点就绪')

    def on_touch(self, msg: TouchEvent):
        if msg.state == 0:  # 抬起时清空标记
            self.publish_marker(msg, clear=True)
            return
            
        # 归一化坐标 -> 物理坐标
        norm_x = msg.point.x / self.screen_w_px
        norm_y = msg.point.y / self.screen_h_px
        
        phys_x = norm_x * self.physical_w_m
        phys_y = norm_y * self.physical_h_m
        phys_z = 0.0  # 假设屏幕平面为Z=0
        
        self.publish_marker(msg, clear=False, pos=(phys_x, phys_y, phys_z))

    def publish_marker(self, touch_msg, clear=False, pos=None):
        marker = Marker()
        marker.header = touch_msg.header
        marker.ns = "touch_visualization"
        marker.id = 0
        marker.type = Marker.SPHERE
        marker.action = Marker.ADD if not clear else Marker.DELETE
        marker.lifetime.sec = 0  # 持久化直到被覆盖或删除
        
        if not clear and pos:
            pose = Pose()
            pose.position.x, pose.position.y, pose.position.z = pos
            marker.pose = pose
            marker.scale.x = 0.015
            marker.color.a = 0.8
            marker.color.r = 1.0
            marker.color.g = 0.2
            
        self.marker_pub.publish(marker)

关键点在于 action 字段。很多初学者写完 ADD 后发现屏幕上一个红点消失不了，新点叠旧点上。ROS 2 的 Marker 机制依赖 DELETE 动作或重新发布相同 id 的新消息来清理。我在抬起事件中强制发送 DELETE，保证了视觉干净。

坐标系对齐与TF发布的坑

RViz2 能显示模型，不代表坐标系统一。如果 touch_panel_frame 和 base_link 之间没有 TF 关系，触摸点会悬浮在空间任意位置，或者干脆飘出视野。

我在系统中维护了一个静态 TF 广播器，专门负责描述触摸面板相对于机身底盘的位置。这一步必须做，否则后续做手指引导机械臂抓取时，所有逆解算都会偏移。

import rclpy
from rclpy.node import Node
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
import numpy as np

class TouchPanelTFBroadcaster(Node):
    def __init__(self):
        super().__init__('panel_tf_broadcaster')
        self.tf_broadcaster = StaticTransformBroadcaster(self)
        
        # 手动测量或查阅手册获取面板中心相对底盘中心的偏移与姿态
        # 此处示例：面板安装在顶部，X偏移0.1m，Y无偏移，Z偏移0.15m，绕Y轴旋转-90度
        offset = np.array([0.10, 0.0, 0.15])
        rot_euler = np.array([0.0, -np.pi/2, 0.0])
        
        quat = self.euler_to_quat(rot_euler)
        
        tfm = TransformStamped()
        tfm.header.stamp = self.get_clock().now().to_msg()
        tfm.header.frame_id = 'base_link'
        tfm.child_frame_id = 'touch_panel_frame'
        tfm.transform.translation.x, tfm.transform.translation.y, tfm.transform.translation.z = offset.tolist()
        tfm.transform.rotation.x, tfm.transform.rotation.y, tfm.transform.rotation.z, tfm.transform.rotation.w = quat.tolist()
        
        self.tf_broadcaster.sendTransform(tfm)
        self.get_logger().info('已发布 touch_panel_frame 静态坐标变换')

    @staticmethod
    def euler_to_quat(euler):
        cy, sy = math.cos(euler[2]/2), math.sin(euler[2]/2)
        cx, sx = math.cos(euler[0]/2), math.sin(euler[0]/2)
        cz, sz = math.cos(euler[1]/2), math.sin(euler[1]/2)
        w = cy*cx*cz + sy*sx*sz
        x = cy*sx*cz - sy*cx*sz
        y = cy*cx*sz + sy*sx*cz
        z = sy*cx*cz - cy*sx*sz
        return [x, y, z, w]

注意：StaticTransformBroadcaster 只需要在节点初始化时发一次。不要放在定时器里循环发，否则会污染 TF 树日志。另外，frame_id 必须与发布触摸消息时的 header.frame_id 完全一致，大小写敏感。我一开始在这里栽过跟头，差了个下划线，RViz2 就报 Invalid frame ID，排查了半小时才定位到字符串匹配问题。