Work In Progress Custom sync 30 6

realsense2_camera/CMakeLists.txt

@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(realsense2_camera)
 
+set (CMAKE_CXX_STANDARD 14)
+
 option(BUILD_WITH_OPENMP "Use OpenMP" OFF)
 option(SET_USER_BREAK_AT_STARTUP "Set user wait point in startup (for debug)" OFF)
 
@@ -17,6 +19,8 @@ find_package(catkin REQUIRED COMPONENTS
     diagnostic_updater
     )
 
+find_package( Boost REQUIRED)
+
 if(BUILD_WITH_OPENMP)
     find_package(OpenMP)
     if(NOT OpenMP_FOUND)
@@ -62,7 +66,8 @@ include_directories(
     include
     ${catkin_INCLUDE_DIRS}
     ${realsense_INCLUDE_DIR}
-    )
+    ${Boost_INCLUDE_DIRS}
+)
 
 # Generate dynamic reconfigure options from .cfg files
 generate_dynamic_reconfigure_options(

realsense2_camera/include/realsense2_camera/realsense_node.h

@@ -37,9 +37,119 @@
 #include <diagnostic_updater/diagnostic_updater.h>
 #include <diagnostic_updater/update_functions.h>
 
+#include <boost/accumulators/accumulators.hpp>
+#include <boost/accumulators/statistics.hpp>
+#include <iostream>
+
+
 namespace realsense2_camera
 {
 
+class DepthSyncer {
+
+    struct SyncerImplementation {
+        SyncerImplementation() {
+            bundler = std::make_unique<rs2::processing_block>([&](rs2::frame f, rs2::frame_source& src) {
+                    bundle.push_back(f);
+                    if (bundle.size() == 2)
+            {
+                    auto fs = src.allocate_composite_frame(bundle);
+                    src.frame_ready(fs);
+                    bundle.clear();
+        }
+        });
+        }
+
+        void Start(std::function<void(rs2::frame)> on_frame)
+        {
+            on_frame_callbak = on_frame;
+            bundler->start(q);
+        }
+
+        void Invoke(rs2::frame f)
+        {
+            const auto stream_type = f.get_profile().stream_type();
+            if (stream_type == RS2_STREAM_DEPTH) {
+                depth.push_back(f);
+                got_depth = true;
+            }
+            if (stream_type == RS2_STREAM_COLOR) {
+                color.push_back(f);
+                got_color = true;
+            }
+
+            if (got_color && got_depth){
+                const auto& d = depth.front();
+                auto best = color.back();
+                auto best_diff = std::abs(d.get_timestamp() - color.back().get_timestamp());
+                for (const auto& c : color) {
+                    const auto diff = std::abs(d.get_timestamp() - c.get_timestamp());
+                    if (diff < best_diff) {
+                        best_diff = diff;
+                        best = c;
+                    }
+                }
+
+                if (best_diff < 20 || color.back().get_timestamp() > depth.back().get_timestamp()) {
+                    acc(best_diff);
+
+                    ROS_INFO_STREAM("[Sync Error: " << best_diff << "] [Average Sync Error: " << boost::accumulators::mean(acc) <<
+                                    "] Variance of Sync Error: " << boost::accumulators::variance(acc) << "]");
+                    ROS_INFO_STREAM("Depth queue size " << depth.size());
+
+                    if (depth.size() > 1) {
+                      throw std::runtime_error("Depth queue size should not go over 1");
+                    }
+
+                    bundler->invoke(d);
+                    bundler->invoke(best);
+
+                    depth.clear();
+                    color.clear();
+                    got_color = false;
+                    got_depth = false;
+                    rs2::frameset fs = q.wait_for_frame();
+                    on_frame_callbak(fs);
+                }
+            }
+        }
+
+        std::vector<rs2::frame> bundle;
+        std::unique_ptr<rs2::processing_block> bundler;
+        std::function<void(rs2::frame)> on_frame_callbak;
+        rs2::frame_queue q;
+        std::deque<rs2::frame> depth;
+        std::deque<rs2::frame> color;
+        bool got_depth = false;
+        bool got_color = false;
+
+        boost::accumulators::accumulator_set<double, boost::accumulators::features<boost::accumulators::tag::mean, boost::accumulators::tag::variance>> acc;
+    };
+
+public:
+    DepthSyncer(){
+        _impl = std::make_shared<SyncerImplementation>();
+    }
+
+    DepthSyncer(const DepthSyncer& other)
+    {
+        _impl = other._impl;
+    }
+
+    void start(std::function<void(rs2::frame)> on_frame)
+    {
+        _impl->Start(on_frame);
+    }
+
+    void operator()(rs2::frame f)
+    {
+        _impl->Invoke(f);
+    }
+private:
+    std::shared_ptr<SyncerImplementation> _impl;
+
+};
+
 class RealSenseParamManagerBase;
 template<uint16_t Model>
 class RealSenseParamManager;
@@ -226,7 +336,7 @@ class RealSenseParamManager;
         bool _sync_frames;
         bool _pointcloud;
         bool _use_ros_time;
-        rs2::asynchronous_syncer _syncer;
+        DepthSyncer _syncer;
 
         std::map<stream_index_pair, cv::Mat> _depth_aligned_image;
         std::map<stream_index_pair, std::string> _depth_aligned_encoding;

realsense2_camera/launch/rs_camera.launch

@@ -24,18 +24,18 @@
   <arg name="enable_color"        default="true"/>
 
   <arg name="fisheye_fps"         default="30"/>
-  <arg name="depth_fps"           default="30"/>
-  <arg name="infra1_fps"          default="30"/>
-  <arg name="infra2_fps"          default="30"/>
+  <arg name="depth_fps"           default="6"/>
+  <arg name="infra1_fps"          default="6"/>
+  <arg name="infra2_fps"          default="6"/>
   <arg name="color_fps"           default="30"/>
   <arg name="gyro_fps"            default="1000"/>
   <arg name="accel_fps"           default="1000"/>
   <arg name="enable_imu"          default="true"/>
 
   <arg name="enable_pointcloud"   default="false"/>
-  <arg name="enable_sync"         default="false"/>
+  <arg name="enable_sync"         default="true"/>
   <arg name="enable_ros_time"     default="false"/>
-  <arg name="align_depth"         default="false"/>
+  <arg name="align_depth"         default="true"/>
 
   <group ns="$(arg camera)">
     <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">

realsense2_camera/src/realsense_node.cpp

@@ -146,7 +146,7 @@ void RealSenseNode::getDevice() {
         ROS_INFO_STREAM("publish topics from rosbag file: " << _rosbag_filename.c_str());
         auto pipe = std::make_shared<rs2::pipeline>();
         rs2::config cfg;
-        cfg.enable_device_from_file(_rosbag_filename.c_str(), false);
+        cfg.enable_device_from_file(_rosbag_filename.c_str());
         cfg.enable_all_streams();
         pipe->start(cfg); //File will be opened in read mode at this point
         auto _device = pipe->get_active_profile().get_device();