本文以Kinova Mico 机械臂为例,示例如何跟踪gazebo中机械臂末端(end-effector)的位置。
1. 在kinova_gazebo中建立nodes文件夹 (如果已经存在该文件夹则跳过)
2. 在nodes文件夹下建立文件:mico_tf_listener.py, 复制下面code,粘贴到该文件中
#!/usr/bin/env python
import rospy
import tf2_ros
import geometry_msgs.msg
rospy.init_node('mico_tf_listener')
# create a tf2_ros type buffer
tfBuffer = tf2_ros.Buffer()
# create a TF2 transform listener object. Save data into tfBuffer
listener = tf2_ros.TransformListener(tfBuffer)
end_effector_pos = rospy.Publisher('/end_effector_pos', geometry_msgs.msg.TransformStamped, queue_size=1)
rate = rospy.Rate(100) # rate at 1 hz
while not rospy.is_shutdown():
try:
# Get last ( Time(0) )transform from the world frame to frame m1n6s200_end_effector
frame_info = tfBuffer.lookup_transform('world', 'm1n6s200_end_effector', rospy.Time(0))