相机引导机器人移动

Python 版本 （最简单、最适合视觉程序），把相机算出的 X/Y/Z 偏移 填进去，机器人就会实时往焊缝方向动。

一、核心代码：相机偏移 → ROS Twist 指令

完全匹配 Servo 配置：

python

运行

#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist

# ===================== 【你只需要改这里】 =====================
# 相机算出的偏移量 (单位：米，手眼标定后统一在 base_link 坐标系下)
dx = 0.0    # X 方向偏移（前后）
dy = 0.0    # Y 方向偏移（左右）
dz = 0.0    # Z 方向偏移（上下）

# 旋转偏移（焊接一般不用，保持 0）
drx = 0.0
dry = 0.0
drz = 0.0
# ============================================================

def servo_publisher():
    rospy.init_node('camera_vision_servo')
    
    # 【关键】发布到你 Servo 配置的话题：delta_twist_cmds
    pub = rospy.Publisher('/servo_server/delta_twist_cmds', Twist, queue_size=1)
    
    rate = rospy.Rate(30)  # 30Hz 频率，匹配你的 Servo 配置
    
    rospy.loginfo("✅ 相机引导 Servo 已启动，发送实时偏移指令...")

    while not rospy.is_shutdown():
        twist = Twist()
        
        # 相机偏移量 → 机器人实时速度指令
        twist.linear.x = dx
        twist.linear.y = dy
        twist.linear.z = dz
        
        twist.angular.x = drx
        twist.angular.y = dry
        twist.angular.z = drz

        pub.publish(twist)
        rate.sleep()

if __name__ == '__main__':
    try:
        servo_publisher()
    except rospy.ROSInterruptException:
        pass

二、这个代码怎么用？（超简单）

1. 把相机算出的 base_link 下的 X/Y/Z 偏移 填进 dx/dy/dz
2. 运行这个节点
3. 机器人就会实时、平滑、安全地向焊缝移动

---

三、【工业版】：带平滑、限幅、防抖动

视觉引导必须用这个，更稳、不抖、不冲：

#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist

class VisionServo:
    def __init__(self):
        rospy.init_node('vision_servo_controller')
        self.pub = rospy.Publisher('/servo_server/delta_twist_cmds', Twist, queue_size=1)
        self.rate = rospy.Rate(30)

        # 安全速度限制（米/秒），和你 Servo 配置对应
        self.max_linear_speed = 0.15  
        self.max_angular_speed = 0.2

    def limit_speed(self, val, max_val):
        """限幅，防止超速"""
        return max(-max_val, min(val, max_val))

    def run(self, dx, dy, dz, drx=0, dry=0, drz=0):
        twist = Twist()
        
        twist.linear.x = self.limit_speed(dx, self.max_linear_speed)
        twist.linear.y = self.limit_speed(dy, self.max_linear_speed)
        twist.linear.z = self.limit_speed(dz, self.max_linear_speed)
        
        twist.angular.x = self.limit_speed(drx, self.max_angular_speed)
        twist.angular.y = self.limit_speed(dry, self.max_angular_speed)
        twist.angular.z = self.limit_speed(drz, self.max_angular_speed)

        self.pub.publish(twist)
        self.rate.sleep()

if __name__ == '__main__':
    vs = VisionServo()
    rospy.loginfo("✅ 相机视觉引导伺服已启动")
    
    while not rospy.is_shutdown():
        # --- 【相机输出】 ---
        dx = 0.0  # 替换成你的相机X偏移
        dy = 0.0  # 替换成你的相机Y偏移
        dz = 0.0  # 替换成你的相机Z偏移
        
        vs.run(dx, dy, dz)

---

四、最重要的对应关系

Servo 配置：

yaml

robot_link_command_frame: base_link
ee_frame_name: YB_Link
cartesian_command_in_topic: ~/delta_twist_cmds

我给你的代码：

• 发布到：/servo_server/delta_twist_cmds
• 坐标系：base_link
• 单位：米
• 效果：相机算出多少偏移，机器人就往哪走

完全匹配！

---

五、只需要做一件事

把相机输出的 XYZ 偏移量赋值给代码里的

plaintext

dx = 相机输出X
dy = 相机输出Y
dz = 相机输出Z

机器人就实现了：

相机实时引导机器人跟踪焊缝