ros_gazebo/turtlebot3 室内仿真导航，提取camera/image和pose位姿真值，并将topic 时间同步对齐，最后制作成kitti 格式的数据集。

（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