在小鱼手眼标定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运行就可以啦!
附上效果图