i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-11 15:09:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

add frame loss judgment but service switch color or depth does not work

Browse Source
This commit is contained in:
jj
2024-10-21 14:46:31 +08:00
parent 0ad2053a06
commit b1d5b8ae18
4 changed files with 65 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/launch/multi_camera_synced_multi_save_rgbir.launch.py
+12 -13
View File
@@ -17,9 +17,9 @@ def generate_launch_description():
), ),
launch_arguments={ launch_arguments={
"camera_name": "front_camera", "camera_name": "front_camera",
"usb_port": "2-2", "usb_port": "2-7",
"device_num": "1", "device_num": "2",
"sync_mode": "secondary", "sync_mode": "software_triggering",
"enable_left_ir":"true", "enable_left_ir":"true",
}.items(), }.items(),
) )
@@ -42,11 +42,10 @@ def generate_launch_description():
), ),
launch_arguments={ launch_arguments={
"camera_name": "right_camera", "camera_name": "right_camera",
"usb_port": "gmsl2-3", "usb_port": "2-6",
"device_num": "2", "device_num": "2",
"sync_mode": "secondary", "sync_mode": "hardware_triggering",
"config_file_path": config_file_path, "enable_left_ir":"true",
"enable_gmsl_trigger": "false",
}.items(), }.items(),
) )
# rear_camera = IncludeLaunchDescription( # rear_camera = IncludeLaunchDescription(
@@ -71,18 +70,18 @@ def generate_launch_description():
{ {
# The port number should be filled in according to the order of the port numbers above. # The port number should be filled in according to the order of the port numbers above.
# "usb_ports": ["gmsl2-1","gmsl2-2","gmsl2-3"], # "usb_ports": ["gmsl2-1","gmsl2-2","gmsl2-3"],
"image_number": "100", "image_number": "20",
"usb_ports": ["2-2"], "usb_ports": ["2-7","2-6"],
"ir_topics": [ "ir_topics": [
"/front_camera/left_ir/image_raw", "/front_camera/left_ir/image_raw",
# "/left_camera/left_ir/image_raw", # "/left_camera/left_ir/image_raw",
# "/right_camera/left_ir/image_raw", "/right_camera/left_ir/image_raw",
# "/rear_camera/left_ir/image_raw", # "/rear_camera/left_ir/image_raw",
], ],
"color_topics": [ "color_topics": [
"/front_camera/color/image_raw", "/front_camera/color/image_raw",
# "/left_camera/color/image_raw", # "/left_camera/color/image_raw",
# "/right_camera/color/image_raw", "/right_camera/color/image_raw",
# "/rear_camera/color/image_raw", # "/rear_camera/color/image_raw",
], ],
} }
@@ -97,9 +96,9 @@ def generate_launch_description():
period=0.5, period=0.5,
actions=[ actions=[
# TimerAction(period=0.5, actions=[GroupAction([rear_camera])]), # TimerAction(period=0.5, actions=[GroupAction([rear_camera])]),
# TimerAction(period=0.5, actions=[GroupAction([right_camera])]), TimerAction(period=0.5, actions=[GroupAction([right_camera])]),
# TimerAction(period=0.5, actions=[GroupAction([left_camera])]), # TimerAction(period=0.5, actions=[GroupAction([left_camera])]),
TimerAction(period=0.5, actions=[GroupAction([front_camera])]), TimerAction(period=1.0, actions=[GroupAction([front_camera])]),
# The primary camera should be launched at last # The primary camera should be launched at last
], ],
), ),
orbbec_camera/src/ob_camera_node.cpp
+5
View File
@@ -1734,6 +1734,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
auto color_frame = frame_set->getFrame(OB_FRAME_COLOR); auto color_frame = frame_set->getFrame(OB_FRAME_COLOR);
if (isGemini335PID(pid)) { if (isGemini335PID(pid)) {
depth_frame = processDepthFrameFilter(depth_frame); depth_frame = processDepthFrameFilter(depth_frame);
bool depth_aligned = false;
if (depth_frame) { if (depth_frame) {
frame_set->pushFrame(depth_frame); frame_set->pushFrame(depth_frame);
} }
@@ -1742,6 +1743,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
auto new_frame_set = new_frame->as<ob::FrameSet>(); auto new_frame_set = new_frame->as<ob::FrameSet>();
CHECK_NOTNULL(new_frame_set.get()); CHECK_NOTNULL(new_frame_set.get());
frame_set = new_frame_set; frame_set = new_frame_set;
depth_aligned = true;
} else { } else {
RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame"); RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame");
return; return;
@@ -1751,6 +1753,9 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
"Depth registration is disabled or align filter is null or depth frame is " "Depth registration is disabled or align filter is null or depth frame is "
"null or color frame is null"); "null or color frame is null");
} }
if (depth_registration_ && align_filter_ && !depth_aligned) {
return;
}
} }
if (enable_stream_[COLOR] && color_frame) { if (enable_stream_[COLOR] && color_frame) {
orbbec_camera/tools/multi_save_rgbir_node.cpp
+2 -3
View File
@@ -18,8 +18,7 @@
int main(int argc, char **argv) { int main(int argc, char **argv) {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
auto node = std::make_shared<MultiCameraSubscriber>(); auto node = std::make_shared<MultiCameraSubscriber>();
rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 20); rclcpp::spin(node);
executor.add_node(node); rclcpp::shutdown();
executor.spin();
return 0; return 0;
} }
orbbec_camera/tools/multi_save_rgbir_node.hpp
+46 -115
View File
@@ -24,10 +24,10 @@ class MultiCameraSubscriber : public rclcpp::Node {
int vid = device_info->vid(); int vid = device_info->vid();
int pid = device_info->pid(); int pid = device_info->pid();
serial_numbers_[usb_port] = serial; serial_numbers_[usb_port] = serial;
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":vid: " << std::hex << vid); RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_save_rgbir_node"), ":vid: " << std::hex << vid);
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":pid: " << std::hex << pid); RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_save_rgbir_node"), ":pid: " << std::hex << pid);
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":serial: " << serial); RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_save_rgbir_node"), ":serial: " << serial);
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":usb_port: " << usb_port); RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_save_rgbir_node"), ":usb_port: " << usb_port);
color_frame_counters_[count] = 0; color_frame_counters_[count] = 0;
ir_frame_counters_[count] = 0; ir_frame_counters_[count] = 0;
count++; count++;
@@ -43,19 +43,17 @@ class MultiCameraSubscriber : public rclcpp::Node {
this->declare_parameter<std::vector<std::string>>("ir_topics", std::vector<std::string>()); this->declare_parameter<std::vector<std::string>>("ir_topics", std::vector<std::string>());
this->declare_parameter<std::vector<std::string>>("color_topics", std::vector<std::string>()); this->declare_parameter<std::vector<std::string>>("color_topics", std::vector<std::string>());
this->declare_parameter<std::vector<std::string>>("usb_ports", std::vector<std::string>()); this->declare_parameter<std::vector<std::string>>("usb_ports", std::vector<std::string>());
this->declare_parameter<std::string>("image_number", "100");
std::vector<std::string> ir_topics_ = this->get_parameter("ir_topics").as_string_array(); std::vector<std::string> ir_topics_ = this->get_parameter("ir_topics").as_string_array();
std::vector<std::string> color_topics_ = this->get_parameter("color_topics").as_string_array(); std::vector<std::string> color_topics_ = this->get_parameter("color_topics").as_string_array();
std::vector<std::string> usb_params_ = this->get_parameter("usb_ports").as_string_array(); std::vector<std::string> usb_params_ = this->get_parameter("usb_ports").as_string_array();
image_number_ = this->get_parameter("image_number").as_string();
auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data));
for (size_t i = 0; i < usb_params_.size(); i++) { for (size_t i = 0; i < usb_params_.size(); i++) {
usb_numbers_[i] = usb_params_[i]; usb_numbers_[i] = usb_params_[i];
usb_index_map_[usb_params_[i]] = i; usb_index_map_[usb_params_[i]] = i;
} }
reentrant_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
for (const auto &pair : serial_numbers_) { for (const auto &pair : serial_numbers_) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"usb_port: " << pair.first << ", serial: " << pair.second); "usb_port: " << pair.first << ", serial: " << pair.second);
@@ -71,48 +69,37 @@ class MultiCameraSubscriber : public rclcpp::Node {
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"color_topic: " << color_topics_[i]); "color_topic: " << color_topics_[i]);
rclcpp::SubscriptionOptions ir_sub_options; auto ir_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
ir_sub_options.callback_group = reentrant_callback_group_; this, ir_topics_[i], custom_qos.get_rmw_qos_profile());
auto color_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
this, color_topics_[i], custom_qos.get_rmw_qos_profile());
rclcpp::SubscriptionOptions color_sub_options; ir_sub->registerCallback([this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) {
color_sub_options.callback_group = reentrant_callback_group_; this->irCallback(msg, i);
});
auto ir_sub = this->create_subscription<sensor_msgs::msg::Image>( color_sub->registerCallback([this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) {
ir_topics_[i], custom_qos, this->colorCallback(msg, i);
[this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) { });
this->irCallback(msg, i);
},
ir_sub_options);
auto color_sub = this->create_subscription<sensor_msgs::msg::Image>(
color_topics_[i], custom_qos,
[this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) {
this->colorCallback(msg, i);
},
color_sub_options);
ir_subscribers_.push_back(ir_sub); ir_subscribers_.push_back(ir_sub);
color_subscribers_.push_back(color_sub); color_subscribers_.push_back(color_sub);
ir_image_buffers_.emplace_back();
color_image_buffers_.emplace_back();
ir_current_timestamp_buffers_.emplace_back();
color_current_timestamp_buffers_.emplace_back();
ir_timestamp_buffers_.emplace_back();
color_timestamp_buffers_.emplace_back();
} }
} }
private: private:
std::string generateFolderName(const std::string &serial_number, size_t serial_index) { std::string generateFolderName(const sensor_msgs::msg::Image::ConstSharedPtr &color_msg,
const std::string &serial_number, size_t serial_index) {
std::string color_resolution =
std::to_string(color_msg->width) + "x" + std::to_string(color_msg->height);
std::string color_encoding = color_msg->encoding;
std::string frame_rate = "30fps"; std::string frame_rate = "30fps";
std::string folder_name = "Star-AE-OFF-ir-" + ir_resolution + "-" + "y8" + "-rgb-" + std::string folder_name = "Star-AE-OFF-ir-" + color_resolution + "-" + "y8" + "-rgb-" +
color_resolution + "-" + "mjpg" + "-" + frame_rate; color_resolution + "-" + "mjpg" + "-" + frame_rate;
std::string path = std::string("multicamera_sync/output/") + folder_name + "/" + std::string path = std::string("multicamera_sync/output/") + folder_name + "/" +
"TotalModeFrames/" + "/" + "SN" + serial_number + "_Index" + "TotalModeFrames/" + "/" + "SN" + serial_number + "_Index" +
std::to_string(serial_index); std::to_string(serial_index);
std::filesystem::create_directories(path); std::filesystem::create_directories(path);
return path; return path;
} }
@@ -134,87 +121,44 @@ class MultiCameraSubscriber : public rclcpp::Node {
return timestamp.str(); return timestamp.str();
} }
void irCallback(const sensor_msgs::msg::Image::ConstSharedPtr &image, size_t index) {
void saveAlignedImages(size_t index) {
images_saved_ = true;
auto &ir_images = ir_image_buffers_[index];
auto &ir_current_timestamps = ir_current_timestamp_buffers_[index];
auto &ir_timestamps = ir_timestamp_buffers_[index];
auto &color_images = color_image_buffers_[index];
auto &color_current_timestamps = color_current_timestamp_buffers_[index];
auto &color_timestamps = color_timestamp_buffers_[index];
if (ir_images.size() < static_cast<size_t>(std::stoi(image_number_)) ||
color_images.size() < static_cast<size_t>(std::stoi(image_number_))) {
return;
}
auto usb_iter = usb_index_map_.find(usb_numbers_[index]); auto usb_iter = usb_index_map_.find(usb_numbers_[index]);
auto serial_iter = serial_numbers_.find(usb_numbers_[index]); auto serial_iter = serial_numbers_.find(usb_numbers_[index]);
int usb_index = usb_iter->second; int usb_index = usb_iter->second;
std::string serial_index = serial_iter->second; std::string serial_index = serial_iter->second;
for (size_t i = 0; i < static_cast<size_t>(std::stoi(image_number_)); ++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) + "_d" + ir_current_timestamps[i] + "_f" +
std::to_string(i) + "_s" + ir_timestamps[i] + "_.jpg";
cv::imwrite(ir_filename, ir_images[i]);
RCLCPP_INFO(this->get_logger(), "Saved IR image to: %s", ir_filename.c_str());
std::string color_filename = folder + "/color_SN" + serial_index + "_Index" +
std::to_string(usb_index) + "_d" + color_current_timestamps[i] +
"_f" + std::to_string(i) + "_s" + color_timestamps[i] + "_.jpg";
cv::imwrite(color_filename, color_images[i]);
RCLCPP_INFO(this->get_logger(), "Saved Color image to: %s", color_filename.c_str());
}
ir_images.clear();
color_images.clear();
ir_current_timestamps.clear();
color_current_timestamps.clear();
ir_timestamps.clear();
color_timestamps.clear();
rclcpp::shutdown();
}
void irCallback(const sensor_msgs::msg::Image::ConstSharedPtr &image, size_t index) {
cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image; cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
std::string current_timestamp_ir = getCurrentTimestamp(image); std::string timestamp_ir = getCurrentTimestamp(image);
std::string timestamp_ir = getTimestamp(); size_t frame_index = ir_frame_counters_[index]++;
ir_image_buffers_[index].push_back(ir_mat); std::string folder = generateFolderName(image, serial_index, usb_index);
ir_current_timestamp_buffers_[index].push_back(current_timestamp_ir); std::string filename = folder + "/ir#left_SN" + serial_index + "_Index" +
ir_timestamp_buffers_[index].push_back(timestamp_ir); std::to_string(usb_index) + "_d" + timestamp_ir + "_f" +
ir_resolution = std::to_string(image->width) + "x" + std::to_string(image->height); std::to_string(frame_index) + "_s" + getTimestamp() + "_.jpg";
if (!images_saved_ && cv::imwrite(filename, ir_mat);
ir_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_)) && RCLCPP_INFO(this->get_logger(), "Saved IR image for camera to: %s", filename.c_str());
color_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_))) {
saveAlignedImages(index);
}
} }
void colorCallback(const sensor_msgs::msg::Image::ConstSharedPtr &image, size_t index) { void colorCallback(const sensor_msgs::msg::Image::ConstSharedPtr &image, size_t index) {
auto usb_iter = usb_index_map_.find(usb_numbers_[index]);
auto serial_iter = serial_numbers_.find(usb_numbers_[index]);
int usb_index = usb_iter->second;
std::string serial_index = serial_iter->second;
cv::Mat color_mat = cv_bridge::toCvCopy(image, image->encoding)->image; cv::Mat color_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
cv::Mat corrected_image; cv::Mat corrected_image;
cv::cvtColor(color_mat, corrected_image, cv::COLOR_RGB2BGR); cv::cvtColor(color_mat, corrected_image, cv::COLOR_RGB2BGR);
std::string current_timestamp_color = getCurrentTimestamp(image); std::string timestamp_color = getCurrentTimestamp(image);
std::string timestamp_color = getTimestamp(); size_t frame_index = color_frame_counters_[index]++;
std::string folder = generateFolderName(image, serial_index, usb_index);
color_image_buffers_[index].push_back(corrected_image); std::string filename = folder + "/color_SN" + serial_index + "_Index" +
color_current_timestamp_buffers_[index].push_back(current_timestamp_color); std::to_string(usb_index) + "_d" + timestamp_color + "_f" +
color_timestamp_buffers_[index].push_back(timestamp_color); std::to_string(frame_index) + "_s" + getTimestamp() + "_.jpg";
color_resolution = std::to_string(image->width) + "x" + std::to_string(image->height); cv::imwrite(filename, corrected_image);
if (!images_saved_ && RCLCPP_INFO(this->get_logger(), "Saved Color image for camera to: %s", filename.c_str());
ir_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_)) &&
color_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_))) {
saveAlignedImages(index);
}
} }
rclcpp::CallbackGroup::SharedPtr reentrant_callback_group_; std::vector<std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>>>
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> ir_subscribers_; ir_subscribers_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> color_subscribers_; std::vector<std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>>>
color_subscribers_;
std::map<std::string, int> usb_index_map_; std::map<std::string, int> usb_index_map_;
std::map<std::string, std::string> serial_numbers_; std::map<std::string, std::string> serial_numbers_;
@@ -226,17 +170,4 @@ class MultiCameraSubscriber : public rclcpp::Node {
std::vector<std::string> usb_params_; std::vector<std::string> usb_params_;
std::vector<std::string> ir_topics_; std::vector<std::string> ir_topics_;
std::vector<std::string> color_topics_; std::vector<std::string> color_topics_;
std::string image_number_;
std::vector<std::vector<cv::Mat>> ir_image_buffers_;
std::vector<std::vector<cv::Mat>> color_image_buffers_;
std::vector<std::vector<std::string>> ir_current_timestamp_buffers_;
std::vector<std::vector<std::string>> color_current_timestamp_buffers_;
std::vector<std::vector<std::string>> ir_timestamp_buffers_;
std::vector<std::vector<std::string>> color_timestamp_buffers_;
std::string color_resolution;
std::string ir_resolution;
bool images_saved_;
}; };