diff --git a/orbbec_camera/CMakeLists.txt b/orbbec_camera/CMakeLists.txt index fe843c3a..3624216f 100644 --- a/orbbec_camera/CMakeLists.txt +++ b/orbbec_camera/CMakeLists.txt @@ -196,6 +196,7 @@ add_orbbec_executable(topic_statistics_node tools/topic_statistics.cpp) add_orbbec_executable(ob_benchmark_node tools/ob_benchmark.cpp) add_orbbec_executable(435le_example_node examples/435le_new_interface/camera_example_node.cpp) add_orbbec_executable(service_benchmark_node scripts/service_benchmark_node.cpp) +add_orbbec_executable(image_sync_example_node examples/multi_camera_time_sync/image_sync_example_node.cpp) install( PROGRAMS @@ -250,7 +251,7 @@ if(DEFINED ENV{BUILDING_PACKAGE}) install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/scripts/99-obsensor-libusb.rules DESTINATION /etc/udev/rules.d) endif() -install(TARGETS list_devices_node list_depth_work_mode_node list_camera_profile_mode_node topic_statistics_node service_benchmark_node ob_benchmark_node 435le_example_node set_device_ip DESTINATION lib/${PROJECT_NAME}/ +install(TARGETS list_devices_node list_depth_work_mode_node list_camera_profile_mode_node topic_statistics_node service_benchmark_node ob_benchmark_node 435le_example_node set_device_ip image_sync_example_node DESTINATION lib/${PROJECT_NAME}/ ) if(BUILD_TESTING) diff --git a/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp b/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp new file mode 100644 index 00000000..3bcef813 --- /dev/null +++ b/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp @@ -0,0 +1,232 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using std::placeholders::_1; +using std::placeholders::_2; +using std::placeholders::_3; +using std::placeholders::_4; +using std::placeholders::_5; +using std::placeholders::_6; +using std::placeholders::_7; +using std::placeholders::_8; + +class ImageSyncNode : public rclcpp::Node { + public: + ImageSyncNode() + : Node("image_sync_node"), + diff_sum_(0.0), + count_(0), + max_diff_(0.0), + min_diff_(std::numeric_limits::max()) { + rclcpp::QoS qos(rclcpp::KeepLast(10)); + qos.reliable(); + + camera_01_color_sub_.subscribe(this, "/camera_01/color/image_raw", qos.get_rmw_qos_profile()); + camera_01_depth_sub_.subscribe(this, "/camera_01/depth/image_raw", qos.get_rmw_qos_profile()); + camera_02_color_sub_.subscribe(this, "/camera_02/color/image_raw", qos.get_rmw_qos_profile()); + camera_02_depth_sub_.subscribe(this, "/camera_02/depth/image_raw", qos.get_rmw_qos_profile()); + camera_03_color_sub_.subscribe(this, "/camera_03/color/image_raw", qos.get_rmw_qos_profile()); + camera_03_depth_sub_.subscribe(this, "/camera_03/depth/image_raw", qos.get_rmw_qos_profile()); + camera_04_color_sub_.subscribe(this, "/camera_04/color/image_raw", qos.get_rmw_qos_profile()); + camera_04_depth_sub_.subscribe(this, "/camera_04/depth/image_raw", qos.get_rmw_qos_profile()); + + sync_ = + std::make_shared(SyncPolicy(10), camera_01_color_sub_, camera_01_depth_sub_, + camera_02_color_sub_, camera_02_depth_sub_, camera_03_color_sub_, + camera_03_depth_sub_, camera_04_color_sub_, camera_04_depth_sub_); + + // sync_->setAgePenalty(0.5); + sync_->registerCallback( + std::bind(&ImageSyncNode::sync_callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); + + RCLCPP_INFO(this->get_logger(), "image sync node started."); + } + + private: + double diff_sum_; + size_t count_; + double max_diff_; + double min_diff_; + + using SyncPolicy = message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::Image, + sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::Image, + sensor_msgs::msg::Image, sensor_msgs::msg::Image>; + using Sync = message_filters::Synchronizer; + + message_filters::Subscriber camera_01_color_sub_; + message_filters::Subscriber camera_01_depth_sub_; + message_filters::Subscriber camera_02_color_sub_; + message_filters::Subscriber camera_02_depth_sub_; + message_filters::Subscriber camera_03_color_sub_; + message_filters::Subscriber camera_03_depth_sub_; + message_filters::Subscriber camera_04_color_sub_; + message_filters::Subscriber camera_04_depth_sub_; + + std::shared_ptr sync_; + + void sync_callback(const sensor_msgs::msg::Image::ConstSharedPtr &img01_c, + const sensor_msgs::msg::Image::ConstSharedPtr &img01_d, + const sensor_msgs::msg::Image::ConstSharedPtr &img02_c, + const sensor_msgs::msg::Image::ConstSharedPtr &img02_d, + const sensor_msgs::msg::Image::ConstSharedPtr &img03_c, + const sensor_msgs::msg::Image::ConstSharedPtr &img03_d, + const sensor_msgs::msg::Image::ConstSharedPtr &img04_c, + const sensor_msgs::msg::Image::ConstSharedPtr &img04_d) { + std::vector images; + std::vector timestamps; + std::vector camera_names = {"camera_01", "camera_01", "camera_02", "camera_02", + "camera_03", "camera_03", "camera_04", "camera_04"}; + + std::vector image_types = {"color", "depth", "color", "depth", + "color", "depth", "color", "depth"}; + try { + images.push_back(cv_bridge::toCvShare(img01_c, "bgr8")->image.clone()); + images.push_back(cv_bridge::toCvShare(img01_d, "16UC1")->image.clone()); + images.push_back(cv_bridge::toCvShare(img02_c, "bgr8")->image.clone()); + images.push_back(cv_bridge::toCvShare(img02_d, "16UC1")->image.clone()); + images.push_back(cv_bridge::toCvShare(img03_c, "bgr8")->image.clone()); + images.push_back(cv_bridge::toCvShare(img03_d, "16UC1")->image.clone()); + images.push_back(cv_bridge::toCvShare(img04_c, "bgr8")->image.clone()); + images.push_back(cv_bridge::toCvShare(img04_d, "16UC1")->image.clone()); + + timestamps = {img01_c->header.stamp.sec + img01_c->header.stamp.nanosec * 1e-9, + img01_d->header.stamp.sec + img01_d->header.stamp.nanosec * 1e-9, + img02_c->header.stamp.sec + img02_c->header.stamp.nanosec * 1e-9, + img02_d->header.stamp.sec + img02_d->header.stamp.nanosec * 1e-9, + img03_c->header.stamp.sec + img03_c->header.stamp.nanosec * 1e-9, + img03_d->header.stamp.sec + img03_d->header.stamp.nanosec * 1e-9, + img04_c->header.stamp.sec + img04_c->header.stamp.nanosec * 1e-9, + img04_d->header.stamp.sec + img04_d->header.stamp.nanosec * 1e-9}; + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + for (size_t i = 0; i < images.size(); i++) { + if (images[i].channels() == 1) { + cv::Mat tmp; + images[i].convertTo(tmp, CV_8U, 255.0 / 10000.0); + cv::applyColorMap(tmp, images[i], cv::COLORMAP_JET); + } + // std::string ts_text = "ts: " + std::to_string(timestamps[i]); + std::string ts_text = + camera_names[i] + " " + image_types[i] + " stamp: " + std::to_string(timestamps[i]); + cv::putText(images[i], ts_text, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1.0, + cv::Scalar(0, 0, 255), 2); + } + + int margin = 10; + int row1_height = std::max({images[0].rows, images[1].rows, images[2].rows, images[3].rows}); + int row2_height = std::max({images[4].rows, images[5].rows, images[6].rows, images[7].rows}); + + int row1_width = images[0].cols + margin + images[1].cols + margin + images[2].cols + margin + + images[3].cols; + int row2_width = images[4].cols + margin + images[5].cols + margin + images[6].cols + margin + + images[7].cols; + + int canvas_width = std::max(row1_width, row2_width); + int canvas_height = row1_height + margin + row2_height; + + cv::Mat canvas(canvas_height, canvas_width, CV_8UC3, cv::Scalar(30, 30, 30)); + + int x_offset = 0; + int y_offset = 0; + for (int i = 0; i < 4; ++i) { + images[i].copyTo(canvas(cv::Rect(x_offset, y_offset, images[i].cols, images[i].rows))); + x_offset += images[i].cols + margin; + } + + x_offset = 0; + y_offset = row1_height + margin; + for (int i = 4; i < 8; ++i) { + images[i].copyTo(canvas(cv::Rect(x_offset, y_offset, images[i].cols, images[i].rows))); + x_offset += images[i].cols + margin; + } + + int screen_width = 1800; + int screen_height = 800; + + double scale_w = static_cast(screen_width) / canvas.cols; + double scale_h = static_cast(screen_height) / canvas.rows; + double scale = std::min(1.0, std::min(scale_w, scale_h)); + + cv::Mat display; + if (scale < 1.0) { + cv::resize(canvas, display, cv::Size(), scale, scale); + } else { + display = canvas; + } + cv::imshow("Time Synced Cameras", display); + cv::waitKey(1); + + std::cout << std::fixed; + std::cout << "===========================================================" << std::endl; + + // Camera01 + std::cout << "Camera01 stamp: color= " << std::setprecision(6) << timestamps[0] + << " depth= " << std::setprecision(6) << timestamps[1] + << " Delay relative to camera01(color): color= " << std::setprecision(3) + << (timestamps[0] - timestamps[0]) * 1000.0 << " ms" + << " depth= " << std::setprecision(3) << (timestamps[1] - timestamps[0]) * 1000.0 + << " ms" << std::endl; + + // Camera02 + std::cout << "Camera02 stamp: color= " << std::setprecision(6) << timestamps[2] + << " depth= " << std::setprecision(6) << timestamps[3] + << " Delay relative to camera01(color): color= " << std::setprecision(3) + << (timestamps[2] - timestamps[0]) * 1000.0 << " ms" + << " depth= " << std::setprecision(3) << (timestamps[3] - timestamps[0]) * 1000.0 + << " ms" << std::endl; + + // Camera03 + std::cout << "Camera03 stamp: color= " << std::setprecision(6) << timestamps[4] + << " depth= " << std::setprecision(6) << timestamps[5] + << " Delay relative to camera01(color): color= " << std::setprecision(3) + << (timestamps[4] - timestamps[0]) * 1000.0 << " ms" + << " depth= " << std::setprecision(3) << (timestamps[5] - timestamps[0]) * 1000.0 + << " ms" << std::endl; + + // Camera04 + std::cout << "Camera04 stamp: color= " << std::setprecision(6) << timestamps[6] + << " depth= " << std::setprecision(6) << timestamps[7] + << " Delay relative to camera01(color): color= " << std::setprecision(3) + << (timestamps[6] - timestamps[0]) * 1000.0 << " ms" + << " depth= " << std::setprecision(3) << (timestamps[7] - timestamps[0]) * 1000.0 + << " ms" << std::endl; + double cur = 0.0; + double base_t = timestamps[0]; // Camera01 color + for (size_t i = 0; i < timestamps.size(); ++i) { + double diff = std::fabs(timestamps[i] - base_t) * 1000.0; + cur = std::max(cur, diff); + } + diff_sum_ += cur; + count_++; + max_diff_ = std::max(max_diff_, cur); + min_diff_ = std::min(min_diff_, cur); + double avg_diff = diff_sum_ / count_; + std::cout << "\nImage Timestamp Difference Statistics" << std::endl; + std::cout << "cur: " << cur << " ms" + << " avg: " << avg_diff << " ms" + << " max: " << max_diff_ << " ms" + << " min: " << min_diff_ << " ms" << std::endl; + } +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file