diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 883d72f..93fcf73 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -64,6 +64,7 @@ #include "jpeg_decoder.h" #include #include +#include #if defined(ROS_JAZZY) || defined(ROS_IRON) #include @@ -133,8 +134,8 @@ const std::map STREAM_TYPE_TO_FRAME_TYPE = { }; typedef struct { - uint8_t mode; - uint16_t fps; + uint8_t mode; + uint16_t fps; } cs_param_t; class OBCameraNode { diff --git a/orbbec_camera/tools/metadata_export_files.hpp b/orbbec_camera/tools/metadata_export_files.hpp index e94c32b..6a3da51 100644 --- a/orbbec_camera/tools/metadata_export_files.hpp +++ b/orbbec_camera/tools/metadata_export_files.hpp @@ -67,7 +67,9 @@ class MetadataExportFiles : public rclcpp::Node { } void load_parameters() { - std::ifstream file("install/orbbec_camera/share/orbbec_camera/config/metadataexport/metadata_export_params.json"); + std::ifstream file( + "install/orbbec_camera/share/orbbec_camera/config/metadataexport/" + "metadata_export_params.json"); if (!file.is_open()) { RCLCPP_ERROR(this->get_logger(), "Failed to open JSON file."); return; @@ -106,8 +108,7 @@ class MetadataExportFiles : public rclcpp::Node { left_ir_sync_ = std::make_shared>( MySyncPolicy(10), *left_ir_image_sub_, *left_ir_metadata_sub_); - left_ir_sync_->setMaxIntervalDuration( - rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + left_ir_sync_->setMaxIntervalDuration(rclcpp::Duration(0, 100 * 1000000)); // 100 ms left_ir_sync_->registerCallback( std::bind(&MetadataExportFiles::left_ir_metadata_sync_callback, this, _1, _2)); @@ -120,8 +121,7 @@ class MetadataExportFiles : public rclcpp::Node { right_ir_sync_ = std::make_shared>( MySyncPolicy(10), *right_ir_image_sub_, *right_ir_metadata_sub_); - right_ir_sync_->setMaxIntervalDuration( - rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + right_ir_sync_->setMaxIntervalDuration(rclcpp::Duration(0, 100 * 1000000)); // 100 ms right_ir_sync_->registerCallback( std::bind(&MetadataExportFiles::right_ir_metadata_sync_callback, this, _1, _2)); @@ -134,7 +134,7 @@ class MetadataExportFiles : public rclcpp::Node { depth_sync_ = std::make_shared>( MySyncPolicy(10), *depth_image_sub_, *depth_metadata_sub_); - depth_sync_->setMaxIntervalDuration(rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + depth_sync_->setMaxIntervalDuration(rclcpp::Duration(0, 100 * 1000000)); // 100 ms depth_sync_->registerCallback( std::bind(&MetadataExportFiles::depth_metadata_sync_callback, this, _1, _2)); @@ -147,7 +147,7 @@ class MetadataExportFiles : public rclcpp::Node { color_sync_ = std::make_shared>( MySyncPolicy(10), *color_image_sub_, *color_metadata_sub_); - color_sync_->setMaxIntervalDuration(rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + color_sync_->setMaxIntervalDuration(rclcpp::Duration(0, 100 * 1000000)); // 100 ms color_sync_->registerCallback( std::bind(&MetadataExportFiles::color_metadata_sync_callback, this, _1, _2)); @@ -380,9 +380,10 @@ class MetadataExportFiles : public rclcpp::Node { } std::ostringstream ss; - ss << serial_path << "/" << std::to_string(frame_index) << "_" << "0000" << "_" - << camera_timestamp_us << "_" << timestamp_us.str() << "_" << prefix << "_" << resolution - << ".png"; + ss << serial_path << "/" << std::to_string(frame_index) << "_" + << "0000" + << "_" << camera_timestamp_us << "_" << timestamp_us.str() << "_" << prefix << "_" + << resolution << ".png"; cv::imwrite(ss.str(), cv_ptr->image); } void save_metadata_to_file(const ImageMetadata &metadata, const std::string &prefix) { diff --git a/orbbec_camera/tools/metadata_save_files.hpp b/orbbec_camera/tools/metadata_save_files.hpp index 4d4861a..73b7aa0 100644 --- a/orbbec_camera/tools/metadata_save_files.hpp +++ b/orbbec_camera/tools/metadata_save_files.hpp @@ -133,8 +133,7 @@ class MetadataSaveFiles : public rclcpp::Node { left_ir_sync_ = std::make_shared>( MySyncPolicy(10), *left_ir_image_sub_, *left_ir_metadata_sub_); - left_ir_sync_->setMaxIntervalDuration( - rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + left_ir_sync_->setMaxIntervalDuration(rclcpp::Duration(0, 100 * 1000000)); // 100 ms left_ir_sync_->registerCallback( std::bind(&MetadataSaveFiles::left_ir_metadata_sync_callback, this, _1, _2)); @@ -147,8 +146,7 @@ class MetadataSaveFiles : public rclcpp::Node { right_ir_sync_ = std::make_shared>( MySyncPolicy(10), *right_ir_image_sub_, *right_ir_metadata_sub_); - right_ir_sync_->setMaxIntervalDuration( - rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + right_ir_sync_->setMaxIntervalDuration(rclcpp::Duration(0, 100 * 1000000)); // 100 ms right_ir_sync_->registerCallback( std::bind(&MetadataSaveFiles::right_ir_metadata_sync_callback, this, _1, _2)); @@ -161,7 +159,7 @@ class MetadataSaveFiles : public rclcpp::Node { depth_sync_ = std::make_shared>( MySyncPolicy(10), *depth_image_sub_, *depth_metadata_sub_); - depth_sync_->setMaxIntervalDuration(rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + depth_sync_->setMaxIntervalDuration(rclcpp::Duration(0, 100 * 1000000)); // 100 ms depth_sync_->registerCallback( std::bind(&MetadataSaveFiles::depth_metadata_sync_callback, this, _1, _2));