VINS-Fusion入口函数解析

入口函数

文件名称

rosNodeTest.cpp

主要流程

1. ROS节点初始化，初始化节点，初始化话题，可以通过registerPub()中发现节点下有哪些话题

2. 从config文件夹中读配置数据放入parameter.cpp文件中的变量

3. 将配置文件数据存入estimator全局对象

4. 注册发布者，这里的注册主要是将后端优化的数据发布到loopfusion进行回环检测。在processImage函数处理完毕位姿后就会发布。发布的时候是怎么做的呢？直接调用visualization中的pubxxx函数。在loop_fusion中可以看到如下代码就是在订阅发布的数据；这个数字表示每2000ms就接受一次消息。

   // 订阅各种传感器数据的话题并发布结果
   ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);
   ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback);
   ros::Subscriber sub_pose = n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback);
   ros::Subscriber sub_extrinsic = n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback);
   ros::Subscriber sub_point = n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback);
   ros::Subscriber sub_margin_point = n.subscribe("/vins_estimator/margin_cloud", 2000, margin_point_callback);
   

5. 接受rosbag中发布的话题数据，因为需要读图像，读取imu数据。主要是执行回调函数XXX_callback把数据拿到。这个里面feature_callback等就是回调函数。

   if (USE_IMU)
       {
           // 使用n.subscribe()函数订阅名为IMU_TOPIC的主题，设置队列大小为2000，
           // 并指定imu_callback作为接收到消息时的回调函数，同时使用ros::TransportHints().tcpNoDelay()设置传输选项。
           sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
       }
       // 创建特征数据订阅者
       ros::Subscriber sub_feature = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
       // 创建图像数据订阅者
       ros::Subscriber sub_img0 = n.subscribe(IMAGE0_TOPIC, 100, img0_callback);
       ros::Subscriber sub_img1;
   

6. 执行回调函数后，把数据放入一个队列缓冲区。这里的缓冲区上锁了，防止新来的数据改变队列。因为上锁了，这个缓冲区推入消息数据的时候就不会更改。有可能这里在给img0_buf放数据的时候，其他的线程还在用。防止其他线程改变imge0_buf

   // 图像回调函数，在此函数中将图像消息加入图像消息队列
   void img0_callback(const sensor_msgs::ImageConstPtr &img_msg)
   {
       // 对消息队列加锁，保证线程安全
       m_buf.lock();
       img0_buf.push(img_msg);
       m_buf.unlock();
   }
   

7. 进入线程sync_process。我感觉这个线程应该是同步处理的。在主函数中只要知道，程序在不断地把数据推送到数据缓冲区，另外的线程在不断地拿缓冲区的数据进行数据处理