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))
        # print ("end-effector print out",frame_info)
        cmd = geometry_msgs.msg.TransformStamped()
        cmd = frame_info
        end_effector_pos.publish(cmd)

    except (tf2_ros.TransformException):
        rate.sleep()
        continue

rate.sleep()

3. chmod +x nodes/mico_tf_listener.py

4. 如果不知道机械臂末端的frame叫什么，可以查看tf中定义的frame：

rosrun tf view_frames

过几秒会产生frames.pdf在运行路径下。（运行该命令应先运行gazebo: roslaunch kinova_gazebo robot_launch.launch m1n6s300）

5. 找到kinova_gazebo/launch/robot_launch.launch文件， 在最后添加：

6. 最后，你在运行gazebo时候就会调用你新建的node接受tf的信息，发布末端的位置和角度信息到话题：/end_effector_pos

rostopic echo /end_effector_pos

得到类似的：

At time 40.762
- Translation: [-0.195, 0.085, 0.625]
- Rotation: in Quaternion [-0.009, 0.998, 0.056, 0.016]
            in RPY (radian) [3.029, 0.033, -3.125]
            in RPY (degree) [173.571, 1.908, -179.078]
At time 40.762
- Translation: [-0.195, 0.085, 0.625]
- Rotation: in Quaternion [-0.009, 0.998, 0.056, 0.016]
            in RPY (radian) [3.029, 0.033, -3.125]
            in RPY (degree) [173.571, 1.908, -179.078]
At time 40.802
- Translation: [-0.195, 0.085, 0.625]
- Rotation: in Quaternion [-0.009, 0.998, 0.056, 0.016]
            in RPY (radian) [3.029, 0.033, -3.125]
            in RPY (degree) [173.571, 1.908, -179.078]
.....
.....