diff --git a/README.MD b/README.MD index 140ebab3..3894d1a4 100644 --- a/README.MD +++ b/README.MD @@ -115,20 +115,14 @@ Here is the device support list of main branch (v1.x) and v2-main branch (v2.x): - - **Note**: If you do not find your device, please contact our FAE or sales representative for help. **Definition**: 1. recommended for new designs: we will provide full supports with new features, bug fix and performance optimization; - 2. full maintenance: we will provide bug fix support; - 3. limited maintenance: we will provide critical bug fix support; - 4. not supported: we will not support specific device in this version; - 5. to be supported: we will add support in the near future. ## Table of Contents @@ -454,6 +448,10 @@ The following are the launch parameters available: the firmware, and if hardware logging is desired, it should also be set to `true`. - `log_level` : SDK log level, the default value is `info`, the optional values are `debug`, `info`, `warn`, `error`, `fatal`. - `enable_color_undistortion`: Enables color undistortion, the default value is `false`. Note that our color cameras exhibit minimal distortion, and typically, undistortion is not necessary. +- `interleave_ae_mode` : Set laser or hdr interleave. +- `interleave_frame_enable` : Whether to enable interleave frame mode. +- `interleave_skip_enable` : Whether to enable skip frames. +- `interleave_skip_index` : Set skip pattern IR or flood IR. **IMPORTANT**: *Please carefully read the instructions regarding software filtering settings at [this link](https://www.orbbec.com/docs/g330-use-depth-post-processing-blocks/). If you are uncertain, do not modify @@ -585,6 +583,9 @@ to `true` in the stream that corresponds to the argument of the launch file. - `/camera/toggle_color` - `/camera/toggle_depth` - `/camera/toggle_ir` +- `/camera/set_reset_timestamp` +- `/camera/set_sync_interleaverlaser` +- `/camera/set_sync_hosttime` ## All available topics @@ -767,18 +768,18 @@ Currently, the following devices are supported by the OrbbecSDK ROS2 Wrapper v2- For optimal performance, we strongly recommend updating to the latest firmware version. This ensures that you benefit from the most recent enhancements and bug fixes. -| Product List | Minimal Firmware Version | **Launch File** | -| :-------------- | :--------------- | :-------------------------- | -| Astra2 | 2.8.20 | astra2.launch.py | -| Femto Mega | 1.1.7/1.2.7 | femto_mega.launch.py | -| Femto Bolt | 1.0.6/1.0.9 | femto_bolt.launch.py | -| Gemini 2 | 1.4.60 /1.4.76 | gemini2.launch.py | -| Gemini 2 L | 1.4.32 | gemini2L.launch.py | -| Gemini 335 | 1.2.20 | gemini_330_series.launch.py | -| Gemini 335L | 1.2.20 | gemini_330_series.launch.py | -| Gemini 335Lg | 1.3.46 | gemini_330_series.launch.py | -| Gemini 336 | 1.2.20 | gemini_330_series.launch.py | -| Gemini 336L | 1.2.20 | gemini_330_series.launch.py | +| Product List | Minimal Firmware Version | **Launch File** | +| :----------- | :----------------------- | :-------------------------- | +| Astra2 | 2.8.20 | astra2.launch.py | +| Femto Mega | 1.1.7/1.2.7 | femto_mega.launch.py | +| Femto Bolt | 1.0.6/1.0.9 | femto_bolt.launch.py | +| Gemini 2 | 1.4.60 /1.4.76 | gemini2.launch.py | +| Gemini 2 L | 1.4.32 | gemini2L.launch.py | +| Gemini 335 | 1.2.20 | gemini_330_series.launch.py | +| Gemini 335L | 1.2.20 | gemini_330_series.launch.py | +| Gemini 335Lg | 1.3.46 | gemini_330_series.launch.py | +| Gemini 336 | 1.2.20 | gemini_330_series.launch.py | +| Gemini 336L | 1.2.20 | gemini_330_series.launch.py | All launch files are essentially similar, with the primary difference being the default values of the parameters set for different models within the same series. Differences in USB standards, such as USB 2.0 versus USB 3.0, may require adjustments to these parameters. If you encounter a startup failure, please carefully review the specification manual. Pay special attention to the resolution settings in the launch file, as well as other parameters, to ensure compatibility and optimal performance. diff --git a/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json b/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json index 47c546dc..764bd46d 100644 --- a/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json +++ b/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json @@ -1,26 +1,13 @@ { "save_rgbir_params": { "time_domain": "device", - "image_number": "100", "usb_ports": [ "2-3", "2-1" ], - "ir_topics": [ - "/G330_0/left_ir/image_raw", - "/G330_1/left_ir/image_raw" - ], - "left_ir_metadata_topic": [ - "/G330_0/left_ir/metadata", - "/G330_1/left_ir/metadata" - ], - "color_topics": [ - "/G330_0/color/image_raw", - "/G330_1/color/image_raw" - ], - "color_metadata_topic": [ - "/G330_0/color/metadata", - "/G330_1/color/metadata" + "camera_name": [ + "G330_0", + "G330_1" ] } } \ No newline at end of file diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 0b00bcd8..64d0dc42 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -302,8 +302,6 @@ class OBCameraNode { void getLdpMeasureDistanceCallback(const std::shared_ptr& request, std::shared_ptr& response); - void setSYNCImmediatelyCallback(const std::shared_ptr& request, - std::shared_ptr& response); void setRESETTimestampCallback(const std::shared_ptr& request, std::shared_ptr& response); void setSYNCInterleaveLaserCallback(const std::shared_ptr& request, @@ -460,7 +458,6 @@ class OBCameraNode { rclcpp::Service::SharedPtr set_fan_work_mode_srv_; rclcpp::Service::SharedPtr toggle_sensors_srv_; rclcpp::Service::SharedPtr get_ldp_measure_distance_srv_; - rclcpp::Service::SharedPtr set_sync_immediately_srv_; rclcpp::Service::SharedPtr set_reset_timestamp_srv_; rclcpp::Service::SharedPtr set_interleaver_laser_sync_srv_; rclcpp::Service::SharedPtr set_sync_host_time_srv_; diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index c42acbd0..b8163066 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -169,11 +169,6 @@ void OBCameraNode::setupCameraCtrlServices() { std::shared_ptr response) { getLdpMeasureDistanceCallback(request, response); }); - set_sync_immediately_srv_ = node_->create_service( - "set_sync_immediately", [this](const std::shared_ptr request, - std::shared_ptr response) { - setSYNCImmediatelyCallback(request, response); - }); set_reset_timestamp_srv_ = node_->create_service( "set_reset_timestamp", [this](const std::shared_ptr request, std::shared_ptr response) { @@ -786,25 +781,6 @@ void OBCameraNode::setIRLongExposureCallback( } } -void OBCameraNode::setSYNCImmediatelyCallback( - const std::shared_ptr& request, - std::shared_ptr& response) { - (void)request; - try { - TRY_EXECUTE_BLOCK(device_->timerSyncWithHost()); - response->success = true; - } catch (const ob::Error& e) { - response->message = e.getMessage(); - response->success = false; - } catch (const std::exception& e) { - response->message = e.what(); - response->success = false; - } catch (...) { - response->message = "unknown error"; - response->success = false; - } -} - void OBCameraNode::setRESETTimestampCallback( const std::shared_ptr& request, std::shared_ptr& response) { diff --git a/orbbec_camera/tools/multi_save_rgbir_node.hpp b/orbbec_camera/tools/multi_save_rgbir_node.hpp index fcf228ef..b25283f0 100644 --- a/orbbec_camera/tools/multi_save_rgbir_node.hpp +++ b/orbbec_camera/tools/multi_save_rgbir_node.hpp @@ -7,7 +7,7 @@ #include #include #include -#include +#include #include namespace orbbec_camera { namespace tools { @@ -18,10 +18,7 @@ struct ImageMetadata { class MultiCameraSubscriber : public rclcpp::Node { public: - MultiCameraSubscriber() : Node("multi_camera_subscriber") { - device_init(); - currenttimes_ = getCurrentTimes(); - } + MultiCameraSubscriber() : Node("multi_camera_subscriber") { device_init(); } void device_init() { try { auto context = std::make_unique(); @@ -62,7 +59,7 @@ class MultiCameraSubscriber : public rclcpp::Node { "usb_port: " << pair.first << ", index: " << pair.second); } auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); - capture_control_sub_ = this->create_subscription( + capture_control_sub_ = this->create_subscription( "start_capture", custom_qos, std::bind(&MultiCameraSubscriber::controlCaptureCallback, this, std::placeholders::_1)); } @@ -80,24 +77,29 @@ class MultiCameraSubscriber : public rclcpp::Node { } nlohmann::json json_data; file >> json_data; - time_domain_= json_data["save_rgbir_params"]["time_domain"].get(); - time_domain_ =(time_domain_ == "device") ? "_d" : (time_domain_ == "global" ? "_g" : "_unknown"); - image_number_ = json_data["save_rgbir_params"]["image_number"].get(); + time_domain_ = json_data["save_rgbir_params"]["time_domain"].get(); + time_domain_ = + (time_domain_ == "device") ? "_d" : (time_domain_ == "global" ? "_g" : "_unknown"); usb_params_ = json_data["save_rgbir_params"]["usb_ports"].get>(); - left_ir_metadata_topic_ = - json_data["save_rgbir_params"]["left_ir_metadata_topic"].get>(); - ir_topics_ = json_data["save_rgbir_params"]["ir_topics"].get>(); - color_topics_ = json_data["save_rgbir_params"]["color_topics"].get>(); - color_metadata_topic_ = - json_data["save_rgbir_params"]["color_metadata_topic"].get>(); + camera_name_ = json_data["save_rgbir_params"]["camera_name"].get>(); + left_ir_topics_.resize(camera_name_.size()); + left_ir_metadata_topic_.resize(camera_name_.size()); + color_topics_.resize(camera_name_.size()); + color_metadata_topic_.resize(camera_name_.size()); + for (size_t i = 0; i < camera_name_.size(); ++i) { + left_ir_topics_[i] = "/" + camera_name_[i] + "/left_ir/image_raw"; + left_ir_metadata_topic_[i] = "/" + camera_name_[i] + "/left_ir/metadata"; + color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw"; + color_metadata_topic_[i] = "/" + camera_name_[i] + "/color/metadata"; + } } void topic_init() { auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - "color_topic: " << ir_topics_.size()); - for (size_t i = 0; i < ir_topics_.size(); ++i) { + "camera_name_.size(): " << camera_name_.size()); + for (size_t i = 0; i < camera_name_.size(); ++i) { RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), - "ir_topic: " << ir_topics_[i]); + "left_ir_topic: " << left_ir_topics_[i]); RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "left_ir_metadata_topic_: " << left_ir_metadata_topic_[i]); RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), @@ -112,7 +114,7 @@ class MultiCameraSubscriber : public rclcpp::Node { color_sub_options.callback_group = reentrant_callback_group_; auto ir_sub = this->create_subscription( - ir_topics_[i], custom_qos, + left_ir_topics_[i], custom_qos, [this, i](std::shared_ptr msg) { this->irCallback(msg, i); }, @@ -142,17 +144,17 @@ class MultiCameraSubscriber : public rclcpp::Node { color_subscribers_.push_back(color_sub); color_meta_subscribers_.push_back(color_metadata_sub); - ir_image_buffers_.resize(ir_topics_.size()); - color_image_buffers_.resize(ir_topics_.size()); - ir_current_timestamp_buffers_.resize(ir_topics_.size()); - color_current_timestamp_buffers_.resize(ir_topics_.size()); - ir_timestamp_buffers_.resize(ir_topics_.size()); - color_timestamp_buffers_.resize(ir_topics_.size()); - left_ir_metadata_.exposure_buffs.resize(ir_topics_.size()); - left_ir_metadata_.gain_buffs.resize(ir_topics_.size()); - color_metadata_.exposure_buffs.resize(ir_topics_.size()); - color_metadata_.gain_buffs.resize(ir_topics_.size()); - callback_called_ = std::vector(ir_topics_.size(), false); + ir_image_buffers_.resize(left_ir_topics_.size()); + color_image_buffers_.resize(left_ir_topics_.size()); + ir_current_timestamp_buffers_.resize(left_ir_topics_.size()); + color_current_timestamp_buffers_.resize(left_ir_topics_.size()); + ir_timestamp_buffers_.resize(left_ir_topics_.size()); + color_timestamp_buffers_.resize(left_ir_topics_.size()); + left_ir_metadata_.exposure_buffs.resize(left_ir_topics_.size()); + left_ir_metadata_.gain_buffs.resize(left_ir_topics_.size()); + color_metadata_.exposure_buffs.resize(left_ir_topics_.size()); + color_metadata_.gain_buffs.resize(left_ir_topics_.size()); + callback_called_ = std::vector(left_ir_topics_.size(), false); } } std::string getCurrentTimes() { @@ -204,11 +206,11 @@ class MultiCameraSubscriber : public rclcpp::Node { auto &color_meta_exposure = color_metadata_.exposure_buffs[index]; auto &color_meta_gain = color_metadata_.gain_buffs[index]; callback_called_[index] = true; - if (ir_images.size() < static_cast(std::stoi(image_number_)) || - color_images.size() < static_cast(std::stoi(image_number_))) { + if (ir_images.size() < static_cast(is_saving_images_) || + color_images.size() < static_cast(is_saving_images_)) { return; } - RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "index:"<second; @@ -218,12 +220,13 @@ class MultiCameraSubscriber : public rclcpp::Node { } std::string serial_index = serial_iter->second; - for (size_t i = 0; i < static_cast(std::stoi(image_number_)); i++) { + for (size_t i = 0; i < static_cast(is_saving_images_); i++) { std::string folder = generateFolderName(serial_index, usb_index); std::string ir_filename = folder + "/ir#left_SN" + serial_index + "_Index" + - std::to_string(usb_index) + time_domain_ + ir_current_timestamps[i] + "_f" + - std::to_string(i) + "_s" + ir_timestamps[i] + "_e" + - left_ir_meta_exposure[i] + "_g" + left_ir_meta_gain[i] + "_.jpg"; + std::to_string(usb_index) + time_domain_ + + ir_current_timestamps[i] + "_f" + std::to_string(i) + "_s" + + ir_timestamps[i] + "_e" + left_ir_meta_exposure[i] + "_g" + + left_ir_meta_gain[i] + "_.jpg"; if (ir_images[i].empty()) { RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over "); continue; @@ -231,10 +234,10 @@ class MultiCameraSubscriber : public rclcpp::Node { cv::imwrite(ir_filename, ir_images[i]); - std::string color_filename = folder + "/color_SN" + serial_index + "_Index" + - std::to_string(usb_index) + time_domain_ + color_current_timestamps[i] + - "_f" + std::to_string(i) + "_s" + color_timestamps[i] + "_e" + - color_meta_exposure[i] + "_g" + color_meta_gain[i] +"_.jpg"; + std::string color_filename = + folder + "/color_SN" + serial_index + "_Index" + std::to_string(usb_index) + + time_domain_ + color_current_timestamps[i] + "_f" + std::to_string(i) + "_s" + + color_timestamps[i] + "_e" + color_meta_exposure[i] + "_g" + color_meta_gain[i] + "_.jpg"; if (color_images[i].empty()) { continue; } @@ -257,24 +260,26 @@ class MultiCameraSubscriber : public rclcpp::Node { std::all_of(callback_called_.begin(), callback_called_.end(), [](bool v) { return v; }); if (all_true) { RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over "); + is_saving_images_ = 0; ir_image_buffers_.clear(); ir_current_timestamp_buffers_.clear(); ir_timestamp_buffers_.clear(); color_image_buffers_.clear(); color_current_timestamp_buffers_.clear(); color_timestamp_buffers_.clear(); - rclcpp::shutdown(); + // rclcpp::shutdown(); } } - void controlCaptureCallback(const std_msgs::msg::Bool::SharedPtr msg) { + void controlCaptureCallback(const std_msgs::msg::Int32::SharedPtr msg) { + currenttimes_ = getCurrentTimes(); is_saving_images_ = msg->data; topic_init(); RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "jjjj " << is_saving_images_); } void irCallback(std::shared_ptr image, size_t index) { std::lock_guard lock(image_mutex_); - if (!callback_called_[index] && is_saving_images_) { + if (!callback_called_[index] && static_cast(is_saving_images_)) { cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image; std::string current_timestamp_ir = getCurrentTimestamp(image); std::string timestamp_ir = getTimestamp(); @@ -284,15 +289,15 @@ class MultiCameraSubscriber : public rclcpp::Node { ir_resolution_ = std::to_string(image->width) + "x" + std::to_string(image->height); RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), ":ir: " << index << ":" << ir_image_buffers_[index].size()); - if (ir_image_buffers_[index].size() >= static_cast(std::stoi(image_number_)) && - color_image_buffers_[index].size() >= static_cast(std::stoi(image_number_))) { + if (ir_image_buffers_[index].size() >= static_cast(is_saving_images_) && + color_image_buffers_[index].size() >= static_cast(is_saving_images_)) { saveAlignedImages(index); } } } void colorCallback(std::shared_ptr image, size_t index) { std::lock_guard lock(image_mutex_); - if (!callback_called_[index] && is_saving_images_) { + if (!callback_called_[index] && static_cast(is_saving_images_)) { cv::Mat color_mat = cv_bridge::toCvCopy(image, image->encoding)->image; cv::Mat corrected_image; cv::cvtColor(color_mat, corrected_image, cv::COLOR_RGB2BGR); @@ -304,8 +309,8 @@ class MultiCameraSubscriber : public rclcpp::Node { color_resolution_ = std::to_string(image->width) + "x" + std::to_string(image->height); RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), ":color: " << index << ":" << color_image_buffers_[index].size()); - if (ir_image_buffers_[index].size() >= static_cast(std::stoi(image_number_)) && - color_image_buffers_[index].size() >= static_cast(std::stoi(image_number_))) { + if (ir_image_buffers_[index].size() >= static_cast(is_saving_images_) && + color_image_buffers_[index].size() >= static_cast(is_saving_images_)) { saveAlignedImages(index); } } @@ -333,7 +338,7 @@ class MultiCameraSubscriber : public rclcpp::Node { color_meta_subscribers_; std::vector::SharedPtr> ir_subscribers_; std::vector::SharedPtr> color_subscribers_; - rclcpp::Subscription::SharedPtr capture_control_sub_; + rclcpp::Subscription::SharedPtr capture_control_sub_; std::map usb_index_map_; std::map serial_numbers_; @@ -343,11 +348,11 @@ class MultiCameraSubscriber : public rclcpp::Node { size_t count = 0; std::vector usb_params_; + std::vector camera_name_; std::vector left_ir_metadata_topic_; std::vector color_metadata_topic_; - std::vector ir_topics_; + std::vector left_ir_topics_; std::vector color_topics_; - std::string image_number_; std::string time_domain_; std::vector> ir_image_buffers_; @@ -363,7 +368,7 @@ class MultiCameraSubscriber : public rclcpp::Node { std::string ir_resolution_; std::string currenttimes_; - bool is_saving_images_ = false; + int is_saving_images_ = 0; ImageMetadata left_ir_metadata_ = ImageMetadata(); ImageMetadata color_metadata_ = ImageMetadata();