MediaPipe 与 ROS 集成：机器人动作交互系统部署

核心节点实现

下面是一个简化的 pose_estimator.py 实现。注意这里有一个关键细节：OpenCV 读取的图片是 BGR 格式，而 MediaPipe 需要 RGB，因此必须进行颜色空间转换。

#!/usr/bin/env python
import rospy
import cv2
import numpy as np
import json
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from geometry_msgs.msg import PointStamped, Vector3
from std_msgs.msg import Header, String
import mediapipe as mp

class PoseEstimator:
    def __init__(self):
        self.bridge = CvBridge()
        # 初始化 MediaPipe Pose 解决方案
        self.mp_pose = mp.solutions.pose
        self.pose = self.mp_pose.Pose(
            static_image_mode=False,
            model_complexity=1,      # 平衡速度与精度
            enable_segmentation=False,
            min_detection_confidence=0.5,
            min_tracking_confidence=0.5
        )

        # 订阅相机图像
        self.image_sub = rospy.Subscriber("/camera/image_raw", Image, self.image_callback)
        
        # 发布关键点数据
        self.keypoint_pub = rospy.Publisher("/skeleton_pose", PointStamped, queue_size=10)
        self.skeleton_pub = rospy.Publisher("/full_skeleton", String, queue_size=10)
        
        rospy.loginfo("Pose Estimator Node Initialized")

    def image_callback(self, msg):
        try:
            # 将 ROS Image 消息转换为 OpenCV 格式
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        except Exception as e:
            rospy.logerr(f"Image conversion error: {e}")
            return

        # MediaPipe 需要 RGB 输入
        rgb_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
        results = self.pose.process(rgb_image)

        if results.pose_landmarks:
            h, w, _ = cv_image.shape
            keypoints = []

            for i, landmark in enumerate(results.pose_landmarks.landmark):
                px = int(landmark.x * w)
                py = int(landmark.y * h)
                pz = landmark.z * w  # 相对深度估算
                vis = landmark.visibility

                # 示例：仅发布鼻子作为单点参考
                if i == 0:
                    point_msg = PointStamped()
                    point_msg.header = Header(stamp=rospy.Time.now(), frame_id="camera_link")
                    point_msg.point.x = px
                    point_msg.point.y = py
                    point_msg.point.z = pz
                    self.keypoint_pub.publish(point_msg)

                keypoints.append({
                    "id": i,
                    "x": px,
                    "y": py,
                    "z": pz,
                    "visibility": float(vis)
                })

            # 发布完整骨架数据（JSON 字符串）
            skeleton_str = json.dumps(keypoints)
            str_msg = String()
            str_msg.data = skeleton_str
            self.skeleton_pub.publish(str_msg)

            # 可视化反馈（调试用）
            mp.solutions.drawing_utils.draw_landmarks(
                cv_image, results.pose_landmarks, self.mp_pose.POSE_CONNECTIONS)
            cv2.imshow("MediaPipe Pose", cv_image)
            cv2.waitKey(1)

if __name__ == '__main__':
    rospy.init_node('mediapipe_pose_node', anonymous=True)
    node = PoseEstimator()
    rospy.spin()

编译与运行

确保依赖已安装：

pip install mediapipe opencv-python rospy cv_bridge

赋予权限并启动：

chmod +x pose_estimator.py
roscore &
rosrun mediapipe_ros pose_estimator.py

新开终端验证话题：

rostopic echo /skeleton_pose

你应该能看到类似以下的输出结构：

header: seq: 123 stamp: secs: 1712345678 nsecs: 901234567 frame_id: "camera_link"
point: x: 320.0 y: 180.0 z: 45.2

动作识别与联动控制

有了关键点数据，就可以通过几何关系判断动作。例如判断'举手'：

def is_hand_raised(landmarks):
    """判断右手是否举起"""
    shoulder_y = landmarks[12]['y']  # 右肩索引
    wrist_y = landmarks[16]['y']     # 右手腕索引
    return wrist_y < shoulder_y       # 手腕 Y 值小于肩膀即视为举起

假设机器人支持 /cmd_vel 控制：

from geometry_msgs.msg import Twist

class RobotController:
    def __init__(self):
        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

    def move_forward(self):
        twist = Twist()
        twist.linear.x = 0.2  # 前进速度
        self.cmd_pub.publish(twist)

在主循环中加入条件判断，即可实现'抬手前进'的自然交互体验。

总结与建议

本文展示了如何将 MediaPipe 与 ROS 结合，打造一套可用于真实系统的动作交互框架。核心收获包括成功部署了本地化姿态检测服务，实现了 WebUI 与 ROS 双通道输出，并构建了'感知→决策→执行'的闭环原型。

在实际落地时，建议关注以下几点：

1. 性能优化：对于嵌入式设备，建议降低输入图像分辨率至 640x480 并启用 model_complexity=0。
2. 坐标转换：若需精确空间定位，请标定相机内参并使用 solvePnP 计算世界坐标。
3. 异常处理：增加超时机制和重连逻辑，防止长时间无响应导致系统卡死。
4. 安全策略：避免直接映射复杂动作指令，应加入确认机制或限速保护。