support rgba8 and bgra8 encodings by skipping alpha channel (#869)

Related with the change in ROS 1 https://github.com/ros-perception/image_pipeline/pull/671/files --------- Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
ros-perception · Jan 19, 2024 · 75638cb · 75638cb
1 parent d6d184e
commit 75638cb
Show file tree

Hide file tree

Showing 3 changed files with 64 additions and 0 deletions.
diff --git a/depth_image_proc/src/point_cloud_xyzrgb.cpp b/depth_image_proc/src/point_cloud_xyzrgb.cpp
@@ -207,11 +207,21 @@ void PointCloudXyzrgbNode::imageCb(
     green_offset = 1;
     blue_offset = 2;
     color_step = 3;
+  } else if (rgb_msg->encoding == sensor_msgs::image_encodings::RGBA8) {
+    red_offset = 0;
+    green_offset = 1;
+    blue_offset = 2;
+    color_step = 4;
   } else if (rgb_msg->encoding == sensor_msgs::image_encodings::BGR8) {
     red_offset = 2;
     green_offset = 1;
     blue_offset = 0;
     color_step = 3;
+  } else if (rgb_msg->encoding == sensor_msgs::image_encodings::BGRA8) {
+    red_offset = 2;
+    green_offset = 1;
+    blue_offset = 0;
+    color_step = 4;
   } else if (rgb_msg->encoding == sensor_msgs::image_encodings::MONO8) {
     red_offset = 0;
     green_offset = 0;

diff --git a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp
@@ -205,11 +205,21 @@ void PointCloudXyzrgbRadialNode::imageCb(
     green_offset = 1;
     blue_offset = 2;
     color_step = 3;
+  } else if (rgb_msg->encoding == sensor_msgs::image_encodings::RGBA8) {
+    red_offset = 0;
+    green_offset = 1;
+    blue_offset = 2;
+    color_step = 4;
   } else if (rgb_msg->encoding == sensor_msgs::image_encodings::BGR8) {
     red_offset = 2;
     green_offset = 1;
     blue_offset = 0;
     color_step = 3;
+  } else if (rgb_msg->encoding == sensor_msgs::image_encodings::BGRA8) {
+    red_offset = 2;
+    green_offset = 1;
+    blue_offset = 0;
+    color_step = 4;
   } else if (rgb_msg->encoding == sensor_msgs::image_encodings::MONO8) {
     red_offset = 0;
     green_offset = 0;

diff --git a/stereo_image_proc/src/stereo_image_proc/stereo_processor.cpp b/stereo_image_proc/src/stereo_image_proc/stereo_processor.cpp
@@ -214,6 +214,26 @@ void StereoProcessor::processPoints(
         }
       }
     }
+  } else if (encoding == enc::RGBA8) {
+    for (int32_t u = 0; u < dense_points_.rows; ++u) {
+      for (int32_t v = 0; v < dense_points_.cols; ++v) {
+        if (isValidPoint(dense_points_(u, v))) {
+          const cv::Vec4b & rgb = color.at<cv::Vec4b>(u, v);
+          int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
+          points.channels[0].values.push_back(*reinterpret_cast<float *>(&rgb_packed));
+        }
+      }
+    }
+  } else if (encoding == enc::BGRA8) {
+    for (int32_t u = 0; u < dense_points_.rows; ++u) {
+      for (int32_t v = 0; v < dense_points_.cols; ++v) {
+        if (isValidPoint(dense_points_(u, v))) {
+          const cv::Vec4b & bgr = color.at<cv::Vec4b>(u, v);
+          int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
+          points.channels[0].values.push_back(*reinterpret_cast<float *>(&rgb_packed));
+        }
+      }
+    }
   } else if (encoding == enc::BGR8) {
     for (int32_t u = 0; u < dense_points_.rows; ++u) {
       for (int32_t v = 0; v < dense_points_.cols; ++v) {
@@ -317,6 +337,18 @@ void StereoProcessor::processPoints2(
         }
       }
     }
+  } else if (encoding == enc::RGBA8) {
+    for (int32_t u = 0; u < dense_points_.rows; ++u) {
+      for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
+        if (isValidPoint(dense_points_(u, v))) {
+          const cv::Vec4b & rgb = color.at<cv::Vec4b>(u, v);
+          int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
+          memcpy(&points.data[i * points.point_step + 12], &rgb_packed, sizeof(int32_t));
+        } else {
+          memcpy(&points.data[i * points.point_step + 12], &bad_point, sizeof(float));
+        }
+      }
+    }
   } else if (encoding == enc::BGR8) {
     for (int32_t u = 0; u < dense_points_.rows; ++u) {
       for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
@@ -329,6 +361,18 @@ void StereoProcessor::processPoints2(
         }
       }
     }
+  } else if (encoding == enc::BGRA8) {
+    for (int32_t u = 0; u < dense_points_.rows; ++u) {
+      for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
+        if (isValidPoint(dense_points_(u, v))) {
+          const cv::Vec4b & bgr = color.at<cv::Vec4b>(u, v);
+          int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
+          memcpy(&points.data[i * points.point_step + 12], &rgb_packed, sizeof(int32_t));
+        } else {
+          memcpy(&points.data[i * points.point_step + 12], &bad_point, sizeof(float));
+        }
+      }
+    }
   } else {
     RCUTILS_LOG_WARN(
       "Could not fill color channel of the point cloud, unrecognized encoding '%s'",