手眼标定handeye-calib：发布PoseStamped类型机械臂姿态信息给手眼标定功能包

在小鱼手眼标定handeye-calib功能包中有非常重要的一步，就是需要把当前机械臂的姿态信息以PoseStamped形式发布。

笔者找了一圈貌似也没找到现成的程序，那就自己写一个叭。

# -*- coding: utf-8 -*-
#!/usr/bin/env python

import rospy
import sys
import moveit_commander
from geometry_msgs.msg import PoseStamped

def publish_arm_pose():
    rospy.init_node('publish_arm_pose', anonymous=True)
    pose_pub = rospy.Publisher('/arm_pose', PoseStamped, queue_size=10)

    # 初始化 moveit_commander
    moveit_commander.roscpp_initialize(sys.argv)
    robot = moveit_commander.RobotCommander()
    group_name = "arm"  # 这里指定你的机械臂控制组的名称
    move_group = moveit_commander.MoveGroupCommander(group_name)

    rate = rospy.Rate(10)  # 发布频率为每秒10次

    while not rospy.is_shutdown():
        # 获取当前机械臂的位姿
        current_pose = move_group.get_current_pose().pose

        # 创建一个 PoseStamped 消息
        pose_msg = PoseStamped()
        pose_msg.header.stamp = rospy.Time.now()
        pose_msg.header.frame_id = "link_5"  # 根据你的机械臂实际情况设置正确的 frame_id

        # 设置机械臂的位置和姿态信息
        pose_msg.pose = current_pose

        # 发布机械臂位置和姿态数据
        pose_pub.publish(pose_msg)

        rate.sleep()

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

将group_name替换成你的机械臂控制组的名称，再将pose_msg.header.frame_id根据你的机械臂实际情况设置正确的 frame_id，直接python运行就可以啦！

附上效果图