Gazebo+tf 跟踪机械臂末端轨迹

本文以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))