Skip to content

Commit

Permalink
Fix foxy build error
Browse files Browse the repository at this point in the history
  • Loading branch information
jj committed Oct 23, 2024
1 parent a508cf6 commit b3dbed6
Show file tree
Hide file tree
Showing 3 changed files with 17 additions and 17 deletions.
5 changes: 3 additions & 2 deletions orbbec_camera/include/orbbec_camera/ob_camera_node.h
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,7 @@
#include "jpeg_decoder.h"
#include <std_msgs/msg/string.hpp>
#include <fcntl.h>
#include <unistd.h>

#if defined(ROS_JAZZY) || defined(ROS_IRON)
#include <cv_bridge/cv_bridge.hpp>
Expand Down Expand Up @@ -133,8 +134,8 @@ const std::map<OBStreamType, OBFrameType> STREAM_TYPE_TO_FRAME_TYPE = {
};

typedef struct {
uint8_t mode;
uint16_t fps;
uint8_t mode;
uint16_t fps;
} cs_param_t;

class OBCameraNode {
Expand Down
21 changes: 11 additions & 10 deletions orbbec_camera/tools/metadata_export_files.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -106,8 +108,7 @@ class MetadataExportFiles : public rclcpp::Node {

left_ir_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
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));
Expand All @@ -120,8 +121,7 @@ class MetadataExportFiles : public rclcpp::Node {

right_ir_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
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));
Expand All @@ -134,7 +134,7 @@ class MetadataExportFiles : public rclcpp::Node {

depth_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
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));
Expand All @@ -147,7 +147,7 @@ class MetadataExportFiles : public rclcpp::Node {

color_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
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));
Expand Down Expand Up @@ -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) {
Expand Down
8 changes: 3 additions & 5 deletions orbbec_camera/tools/metadata_save_files.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -133,8 +133,7 @@ class MetadataSaveFiles : public rclcpp::Node {

left_ir_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
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));
Expand All @@ -147,8 +146,7 @@ class MetadataSaveFiles : public rclcpp::Node {

right_ir_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
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));
Expand All @@ -161,7 +159,7 @@ class MetadataSaveFiles : public rclcpp::Node {

depth_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
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));
Expand Down

0 comments on commit b3dbed6

Please sign in to comment.