将Gazebo的Log数据转化为csv格式

Why Needed ？

Gazebo提供了世界状态的录制功能，但是官方并没有提供一个简单的数据导出功能，自带的绘图记录仪有最大数据量的限制（170kb左右），严重影响了数据采集工作的进行。

What Done？

这里给出一个简单的录制软件，省去一部分的重复工作，可以将回放的LOG文件转化为CSV格式，便于下一步的数据处理。

HOW？

Gazebo发布的话题类型为ModelStates、LinkStates，前者是总体世界的状态，后者是某个机器人身上的关节状态
以ModelStates为例，ModelStates由标准ROS话题构成，分别是String、Twist、Pose。根据这一特性可以抽取所需要的模型的位姿变化数据。

Step

具体步骤如下：

1.在本脚本中修改保存文件地址，以及添加所需要的模型姿态。所需要修改位置已经使用注释标注出；
2.rosrun gazebo_ros gazebo -u - p {LOG_PATH}启动log文件
3.启动本文的py脚本；
4.点击Gazebo的播放按钮，等待log回放结束；
5.按下ctrl+c即可完成log文件录制；

Script

python2脚本，可用于python3运行

#!/usr/bin/env python
import pandas as pd
import os
import rospy

from std_msgs.msg import String
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Twist
from gazebo_msgs.msg import ModelStates

#如果你需要保存其他的数据，请在这里添加标签，并在Callback回调函数中添加需要保存的数据
dataSheet = pd.DataFrame(columns=['x','y','z'])

def callback(data):

    for i,x in enumerate(data.name):
    #修改这里来确定所需要记录的模型
        if x =='把这里修改为你的模型名字':
            # print(x)
            pose=data.pose[i]
            quat=pose.orientation
            pos=pose.position
            time=rospy.get_time()
            while time==0:
                time=rospy.get_time()
#如果你需要保存其他的数据，请在这里按照列标签添加你的数据
            dataSheet.loc[time]=[pos.x,pos.y,pos.z]
            print time#提供程序运行反馈
            break
    pass

rospy.init_node('get_gazebo',anonymous=True)
rospy.Subscriber('/gazebo/model_states',ModelStates,callback)
rospy.spin()
path = "把这里修改为你希望保存到的路径"
if not os.path.exists(path):
    os.makedirs(path)
#文件名会自动加上ROS时间戳，避免覆盖
dataSheet.to_csv(path+'/time_'+str(rospy.get_time())+'_data.csv')

print 'Data_recorded'