i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-07 13:37:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

Color flow adds rgba and bgra

Browse Source
This commit is contained in:
jj
2024-10-29 20:23:06 +08:00
parent df144fd0d8
commit 3ea2ddae13
2 changed files with 25 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/d2c_viewer.cpp
+6 -5
View File
@@ -52,16 +52,17 @@ void D2CViewer::messageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& r
rgb_msg->height, depth_msg->width, depth_msg->height);
return;
}
auto rgb_img_ptr = cv_bridge::toCvCopy(rgb_msg, sensor_msgs::image_encodings::RGB8);
auto rgb_encode = (rgb_msg->step == 5760) ? sensor_msgs::image_encodings::RGB8 : sensor_msgs::image_encodings::RGBA8;
auto gray_type = (rgb_msg->step == 5760) ? cv::COLOR_GRAY2RGB : cv::COLOR_GRAY2RGBA;
auto rgb_img_ptr = cv_bridge::toCvCopy(rgb_msg, rgb_encode);
auto depth_img_ptr = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_16UC1);
cv::Mat gray_depth, depth_img, d2c_img;
depth_img_ptr->image.convertTo(gray_depth, CV_8UC1);
cv::cvtColor(gray_depth, depth_img, cv::COLOR_GRAY2RGB);
depth_img.setTo(cv::Scalar(255, 255, 0), depth_img);
cv::cvtColor(gray_depth, depth_img, gray_type);
depth_img.setTo(cv::Scalar(255, 255, 0 ), depth_img);
cv::bitwise_or(rgb_img_ptr->image, depth_img, d2c_img);
sensor_msgs::msg::Image::SharedPtr d2c_msg =
cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::RGB8, d2c_img)
.toImageMsg();
cv_bridge::CvImage(std_msgs::msg::Header(), rgb_encode, d2c_img).toImageMsg();
d2c_msg->header = rgb_msg->header;
d2c_viewer_pub_->publish(*d2c_msg);
}
orbbec_camera/src/ob_camera_node.cpp
+19 -6
View File
@@ -68,7 +68,7 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
d2c_viewer_ = std::make_unique<D2CViewer>(node_, rgb_qos, depth_qos);
}
if (enable_stream_[COLOR]) {
rgb_buffer_ = new uint8_t[width_[COLOR] * height_[COLOR] * 3];
rgb_buffer_ = new uint8_t[width_[COLOR] * height_[COLOR] * 4];
}
if (enable_colored_point_cloud_ && enable_stream_[DEPTH] && enable_stream_[COLOR]) {
rgb_point_cloud_buffer_size_ = width_[COLOR] * height_[COLOR] * sizeof(OBColorPoint);
@@ -716,8 +716,19 @@ void OBCameraNode::setupProfiles() {
fps_[elem] = static_cast<int>(selected_profile->getFps());
format_[elem] = selected_profile->getFormat();
updateImageConfig(elem);
images_[elem] =
cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0));
if (selected_profile->format() == OB_FORMAT_BGRA) {
images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
encoding_[elem] = sensor_msgs::image_encodings::BGRA8;
unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
} else if (selected_profile->format() == OB_FORMAT_RGBA) {
images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
encoding_[elem] = sensor_msgs::image_encodings::RGBA8;
unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
}
else {
images_[elem] =
cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0));
}
RCLCPP_INFO_STREAM(logger_,
" stream "
<< stream_name_[elem]
@@ -1834,7 +1845,7 @@ std::shared_ptr<ob::Frame> OBCameraNode::softwareDecodeColorFrame(
if (frame->getFormat() == OB_FORMAT_RGB || frame->getFormat() == OB_FORMAT_BGR) {
return frame;
}
if (frame->getFormat() == OB_FORMAT_RGB || frame->getFormat() == OB_FORMAT_BGR) {
if (frame->getFormat() == OB_FORMAT_RGBA || frame->getFormat() == OB_FORMAT_BGRA) {
return frame;
}
if (frame->getFormat() == OB_FORMAT_Y16 || frame->getFormat() == OB_FORMAT_Y8) {
@@ -1906,7 +1917,7 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
return false;
}
CHECK_NOTNULL(buffer);
memcpy(buffer, video_frame->getData(), video_frame->getDataSize());
memcpy(rgb_buffer_, video_frame->getData(), video_frame->getDataSize());
return true;
}
return true;
@@ -2048,7 +2059,9 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
RCLCPP_ERROR(logger_, "color frame is not decoded");
return;
}
if (frame->getType() == OB_FRAME_COLOR) {
if (frame->getType() == OB_FRAME_COLOR && frame->format() != OB_FORMAT_Y8 &&
frame->format() != OB_FORMAT_Y16 && frame->format() != OB_FORMAT_BGRA &&
frame->format() != OB_FORMAT_RGBA && image_publishers_[COLOR]->get_subscription_count() > 0) {
memcpy(image.data, rgb_buffer_, video_frame->getWidth() * video_frame->getHeight() * 3);
} else {
memcpy(image.data, video_frame->getData(), video_frame->getDataSize());