From e7dec17d983ba2dc59b6b991fbc5cab1f7ec0358 Mon Sep 17 00:00:00 2001 From: Adel Fakih Date: Mon, 15 Feb 2021 06:16:46 -0500 Subject: [PATCH] Added a custom syncer for depth streaming at 6HZ and Color at 30Hz --- realsense2_camera/CMakeLists.txt | 7 +- .../realsense2_camera/realsense_node.h | 112 +++++++++++++++++- realsense2_camera/launch/rs_camera.launch | 10 +- realsense2_camera/src/realsense_node.cpp | 2 +- 4 files changed, 123 insertions(+), 8 deletions(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 9d015730a6..c0175cf961 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/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( diff --git a/realsense2_camera/include/realsense2_camera/realsense_node.h b/realsense2_camera/include/realsense2_camera/realsense_node.h index 60d089a295..e2f33dc5df 100644 --- a/realsense2_camera/include/realsense2_camera/realsense_node.h +++ b/realsense2_camera/include/realsense2_camera/realsense_node.h @@ -37,9 +37,119 @@ #include #include +#include +#include +#include + + namespace realsense2_camera { +class DepthSyncer { + + struct SyncerImplementation { + SyncerImplementation() { + bundler = std::make_unique([&](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 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 bundle; + std::unique_ptr bundler; + std::function on_frame_callbak; + rs2::frame_queue q; + std::deque depth; + std::deque color; + bool got_depth = false; + bool got_color = false; + + boost::accumulators::accumulator_set> acc; + }; + +public: + DepthSyncer(){ + _impl = std::make_shared(); + } + + DepthSyncer(const DepthSyncer& other) + { + _impl = other._impl; + } + + void start(std::function on_frame) + { + _impl->Start(on_frame); + } + + void operator()(rs2::frame f) + { + _impl->Invoke(f); + } +private: + std::shared_ptr _impl; + +}; + class RealSenseParamManagerBase; template 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 _depth_aligned_image; std::map _depth_aligned_encoding; diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index ba5e775532..fcb30b0bb3 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -24,18 +24,18 @@ - - - + + + - + - + diff --git a/realsense2_camera/src/realsense_node.cpp b/realsense2_camera/src/realsense_node.cpp index f85dd409f0..ebbbd41101 100644 --- a/realsense2_camera/src/realsense_node.cpp +++ b/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::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();