（1）ros_gazebo turtlebot3 室内环境导航仿真。参考ubuntu 16.04+ros kinetic + gazebo+ aws-robotics 室内环境导航仿真_May Wang的博客-CSDN博客
$ cd catkin_ws $ source develop/setup.bash $ expore TURTLEBOT3_MODEL=waffle_pi $ roslaunch navigation_simulation small_house_turtlebot_navigation.launch follow_route:=false dynamic_route:=true gui:=true
(2) 录制topic camera/rgb/image_raw 和topic /tf 的bag包。（tf中odom到map 的trandform默认为机器人相对地图的位姿）
$ rosbag record /tf /camera/rgb/image_raw -o out.bag
(3) 播放该rosbag包，查看bag信息。
rosbag play <bagfile> rosbag play <bagfile> --topic /topic1 rosbag play out_2022-04-01-15-43-05.bag --topic /tf $ rosbag info out.bag
（3）从该rosbag中提取出图片和机器人的位姿信息。
------------------------------------------------------------------------------------------------
（尝试过程，可省略）将bag中的pose 的topic 信息提取为csv文件
rosbag record /gazebo/model_states -o gazebo_state.bag rostopic echo -b gazebo_state.bag -p /gazebo/model_states > gazebo_state.txt
------------------------------------------------------------------------------------------------
（尝试过程，可省略）gazebo中获得任意model的groundtruth的pose
rosservice call /gazebo/get_model_state "model_name: 'turtlebot3_waffle_pi' relative_entity_name: 'world'" header: seq: 2 stamp: secs: 609 nsecs: 288000000 frame_id: "world" pose: position: x: -0.266953255634 y: -2.79826728758 z: -0.0010085243088 orientation: x: -0.000302331025397 y: -0.00156445147662 z: 0.188558351213 w: -0.982060695311 twist: linear: x: -0.238763637508 y: 0.0951505227823 z: 8.2381658963e-05 angular: x: -0.000829139385973 y: -0.000544168135714 z: -0.0354440816541 success: True status_message: "GetModelState: got properties"
--------------------------------------------------------------------------------------
（4）通过tf topic 获取位姿相对麻烦，故通过具有发布机器人位姿真值功能的gazebo插件libgazebo_ros_p3d.so，添加位姿真值的topic /ground_truth/state 实现。
获取机器人turtlebot3 在gazebo 中仿真导航时的位姿真值_May Wang的博客-CSDN博客
（5）重新录制 包含图片和位姿真值的bag包。
$ cd catkin_ws $ source develop/setup.bash $ expore TURTLEBOT3_MODEL=waffle_pi $ roslaunch navigation_simulation small_house_turtlebot_navigation.launch follow_route:=true gui:=true
新终端
rosrun rqt_image_view rqt_image_view
如图：
新终端
rosbag record /groundtruth/state /camera/rgb/image_raw -o out.bag
（6）（可忽略，尝试过程）从bag中提取图片和pose信息，图片以时间戳命名，pose信息保存为txt文件。
新建文件夹:rosbag_data，新建文档get_image.py,如下：
# coding:utf-8 # get_image.py import roslib; import rosbag import rospy import cv2 from sensor_msgs.msg import Image from cv_bridge import CvBridge from cv_bridge import CvBridgeError # 存放解析输出图片的位置 path = '/home/wsx/0A_rosbag_data/image/' class ImageCreator(): def __init__(self): self.bridge = CvBridge() # 要读取的bag文件； with rosbag.Bag('nav_image_pose.bag', 'r') as bag: for topic, msg, t in bag.read_messages(): # 图像的topic； if topic == "/camera/rgb/image_raw": try: cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") except CvBridgeError as e: print e timestr = "%.1f" % msg.header.stamp.to_sec() # %.6f表示小数点后带有6位，可根据精确度需要修改； image_name = timestr + ".jpg" # 图像命名：时间戳.jpg cv2.imwrite(path + image_name, cv_image) # 保存； if __name__ == '__main__': # rospy.init_node(PKG) try: image_creator = ImageCreator() except rospy.ROSInterruptException: pass
提取图片，新建终端
wsx@hello:~/0A_rosbag_data$ python get_image.py
结果如下：
提取pose， 每一帧都保存为一个txt文件，方便后面进行时间戳对齐。
rostopic echo -b nav_image_pose.bag -p /ground_truth/state > pose.txt
（7）(可忽略，尝试过程)时间戳对齐，参考激光雷达LiDAR和相机的.bag数据解析与对齐_Summer_crown的博客-CSDN博客。
(8) 将（5）中录制的包中的两个topic ：image_raw和ground_truth/state（分别为sensor_msgs::image 和nav_msgs::odometry 类型）同步时间戳对齐，并保存为kitti 数据格式。
rosbag包存放在 0A_rosbag_data 中，参考https://github.com/zhanghm1995/ROSKittiWriter，放入catkin_ws/src 中， 添加 nav_msgs :: odometry 类的groundtruth topic以及与image_raw topic的相关时间同步和读写保存操作，然后catkin_make。（参考我修改后的代码）
在0A_rosbag_data下建立 kitti_data/pose 和image 两个文件夹。
打开两个终端，运行
wsx@hello:~/0A_rosbag_data$ rosbag play nav_image_pose.bag
和
wsx@hello:~/catkin_ws$ roslaunch ros_kitti_writer kitti_writer_standalone.launch
结果如下：
[ WARN] [1649321211.383564639]: Begin saving data 13152 [ WARN] [1649321211.418604931]: Begin saving data 13153 [ WARN] [1649321211.455254096]: Begin saving data 13154 [ WARN] [1649321211.464548000]: Begin saving data 13155 [ WARN] [1649321211.498690634]: Begin saving data 13156 [ERROR] [1649321211.508932578]: Waiting for image and lidar topics!!! [ WARN] [1649321211.542629590]: Begin saving data 13157 [ WARN] [1649321211.586601073]: Begin saving data 13158 [ WARN] [1649321211.630599996]: Begin saving data 13159 [ WARN] [1649321211.639320367]: Begin saving data 13160 [ WARN] [1649321211.675267830]: Begin saving data 13161 [ WARN] [1649321211.709811994]: Begin saving data 13162 [ WARN] [1649321211.750695039]: Begin saving data 13163 [ WARN] [1649321211.797838768]: Begin saving data 13164 [ INFO] [1649321211.841893824]: Sync_Callback [ WARN] [1649321211.926622021]: Begin saving data 13165 [ WARN] [1649321211.970652961]: Begin saving data 13166 [ WARN] [1649321211.979278670]: Begin saving data 13167
