Fix memory access in case of 3-byte pixel formats (#1519)

ros-visualization · Jun 23, 2020 · e5ef587 · e5ef587
1 parent 171eb3c
commit e5ef587
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 8 deletions.
diff --git a/src/rviz/selection/selection_manager.cpp b/src/rviz/selection/selection_manager.cpp
@@ -575,24 +575,27 @@ void SelectionManager::setHighlightRect(Ogre::Viewport* viewport, int x1, int y1
   highlight_rectangle_->setCorners(nx1, ny1, nx2, ny2);
 }
 
-void SelectionManager::unpackColors(const Ogre::PixelBox& box, V_CollObject& pixels)
+void SelectionManager::unpackColors(Ogre::PixelBox& box, V_CollObject& pixels)
 {
   int w = box.getWidth();
   int h = box.getHeight();
 
   pixels.clear();
   pixels.reserve(w * h);
+  size_t size = Ogre::PixelUtil::getMemorySize(1, 1, 1, box.format);
 
   for (int y = 0; y < h; y++)
   {
     for (int x = 0; x < w; x++)
     {
-      uint32_t pos = (x + y * w) * 4;
-
-      uint32_t pix_val = *(uint32_t*)((uint8_t*)box.data + pos);
-      uint32_t handle = colorToHandle(box.format, pix_val);
-
-      pixels.push_back(handle);
+      if (size == 4) // In case of a 4-byte color format, we can directly process the 32-bit values
+      {
+        uint32_t pos = (x + y * w) * 4;
+        uint32_t pix_val = *(uint32_t*)((uint8_t*)box.data + pos);
+        pixels.push_back(colorToHandle(box.format, pix_val));
+      }
+      else // otherwise perform "official" transformation into float-based Ogre::ColourValue and back
+        pixels.push_back(colorToHandle(box.getColourAt(x, y, 1)));
     }
   }
 }

diff --git a/src/rviz/selection/selection_manager.h b/src/rviz/selection/selection_manager.h
@@ -275,7 +275,7 @@ private Q_SLOTS:
               unsigned texture_width,
               unsigned textured_height);
 
-  void unpackColors(const Ogre::PixelBox& box, V_CollObject& pixels);
+  void unpackColors(Ogre::PixelBox& box, V_CollObject& pixels);
 
   void setDepthTextureSize(unsigned width, unsigned height);