From 607b586070125e620a07b1b4155358428f8016d3 Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Fri, 24 Apr 2026 17:35:00 +0200 Subject: [PATCH 1/2] Fix when with pitch != width*BBP in Image Display Signed-off-by: Alejandro Hernandez Cordero --- .../displays/image/ros_image_texture.cpp | 61 ++++++++++++++++++- 1 file changed, 60 insertions(+), 1 deletion(-) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp index 347c0a9db..eb877112b 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp @@ -43,6 +43,8 @@ #include #include +#include + #include // NOLINT: cpplint cannot handle include order #include "sensor_msgs/image_encodings.hpp" @@ -135,6 +137,29 @@ void ROSImageTexture::setNormalizeFloatImage(bool normalize, double min, double max_ = max; } +// Bytes per pixel for encodings that have a fixed linear row layout. Returns +// 0 for encodings whose per-row layout is non-trivial (YUV 4:2:2 packed, NV12 +// planar) — those are handled explicitly by their converters using stride. +static size_t bytesPerPixelForEncoding(const std::string & encoding) +{ + using namespace sensor_msgs::image_encodings; // NOLINT + if (encoding == MONO8 || encoding == TYPE_8UC1 || encoding == TYPE_8SC1) {return 1;} + if (encoding == MONO16 || encoding == TYPE_16UC1 || encoding == TYPE_16SC1) {return 2;} + if (encoding == RGB8 || encoding == BGR8 || + encoding == TYPE_8UC3 || encoding == TYPE_8SC3) + { + return 3; + } + if (encoding == RGBA8 || encoding == BGRA8 || + encoding == TYPE_8UC4 || encoding == TYPE_8SC4) + { + return 4; + } + if (encoding == TYPE_32FC1) {return 4;} + if (encoding.rfind("bayer", 0) == 0) {return 1;} + return 0; +} + static double computeMedianOfSeveralFrames(std::deque & buffer, double value, unsigned median_frames) { @@ -169,8 +194,42 @@ bool ROSImageTexture::update() height_ = image->height; stride_ = image->step; + // If the publisher sent rows with trailing padding (step > width * bpp), + // repack into a contiguous buffer. Ogre::Image::loadRawData and the + // convertTo8bit rescale loop both assume packed rows — without this, + // each row drifts right by `padding` bytes, producing a diagonal shear. + // YUV/NV12 encodings are left alone because their converters honor stride + // directly. + std::vector repacked; + const uint8_t * data_ptr = image->data.data(); + size_t data_size = image->data.size(); + const size_t bpp = bytesPerPixelForEncoding(image->encoding); + if (bpp != 0) { + const size_t packed_row = static_cast(width_) * bpp; + if (stride_ > packed_row) { + if (image->data.size() < static_cast(stride_) * height_) { + RVIZ_COMMON_LOG_ERROR_STREAM( + "Image data size " << image->data.size() << + " smaller than step*height (" << stride_ * height_ << ")"); + return false; + } + repacked.resize(packed_row * height_); + for (uint32_t y = 0; y < height_; ++y) { + std::memcpy( + repacked.data() + y * packed_row, + image->data.data() + y * stride_, + packed_row); + } + data_ptr = repacked.data(); + data_size = repacked.size(); + // Reflect the repack so any downstream code reading stride_ sees the + // new (packed) layout. + stride_ = packed_row; + } + } + ImageData image_data = setFormatAndNormalizeDataIfNecessary( - image->encoding, image->data.data(), image->data.size()); + image->encoding, data_ptr, data_size); Ogre::Image ogre_image; try { From e1ad7af46084630e14a2d9b345293e73a29e56a3 Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Mon, 25 May 2026 18:44:33 +0200 Subject: [PATCH 2/2] Review feedback Signed-off-by: Alejandro Hernandez Cordero --- .../displays/image/ros_image_texture.cpp | 23 ++++++++----------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp index eb877112b..71d25bc87 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp @@ -142,22 +142,19 @@ void ROSImageTexture::setNormalizeFloatImage(bool normalize, double min, double // planar) — those are handled explicitly by their converters using stride. static size_t bytesPerPixelForEncoding(const std::string & encoding) { - using namespace sensor_msgs::image_encodings; // NOLINT - if (encoding == MONO8 || encoding == TYPE_8UC1 || encoding == TYPE_8SC1) {return 1;} - if (encoding == MONO16 || encoding == TYPE_16UC1 || encoding == TYPE_16SC1) {return 2;} - if (encoding == RGB8 || encoding == BGR8 || - encoding == TYPE_8UC3 || encoding == TYPE_8SC3) + namespace enc = sensor_msgs::image_encodings; + if (encoding == enc::YUV422 || encoding == enc::YUV422_YUY2 || + encoding == enc::UYVY || encoding == enc::YUYV || + encoding == enc::NV12 || encoding == enc::NV21 || encoding == enc::NV24) { - return 3; + return 0; } - if (encoding == RGBA8 || encoding == BGRA8 || - encoding == TYPE_8UC4 || encoding == TYPE_8SC4) - { - return 4; + try { + return static_cast(enc::numChannels(encoding)) * + enc::bitDepth(encoding) / 8; + } catch (const std::runtime_error &) { + return 0; // unknown encoding — skip repack } - if (encoding == TYPE_32FC1) {return 4;} - if (encoding.rfind("bayer", 0) == 0) {return 1;} - return 0; } static double