Fix potencial leak / seg fault (#726)

* Fix potencial leak / seg fault

Signed-off-by: ahcorde <[email protected]>

* Avoid make copies

Signed-off-by: ahcorde <[email protected]>

* make linters happy

Signed-off-by: ahcorde <[email protected]>

* Added feedback

Signed-off-by: ahcorde <[email protected]>

* Heavily revamp ros_image_texture.cpp

The main goal here is to stop using a pointer
from a freed std::vector.  Instead, we allocate
memory on the heap and delete it when this piece
of code no longer needs it (Ogre copies it internally).

Along the way, we do a huge cleanup of this code to
make it easier to understand and fix some obvious
problems with it:

1. Improve the locking.
2. Make some methods const.
3. Move implementations in the C++ file.
4. Remove unnecessary functions.
5. Rename a few things to make their function clearer.

Signed-off-by: Chris Lalancette <[email protected]>

* Fix an incorrect cast.

Signed-off-by: Chris Lalancette <[email protected]>

---------

Signed-off-by: ahcorde <[email protected]>
Signed-off-by: Chris Lalancette <[email protected]>
Co-authored-by: Chris Lalancette <[email protected]>

rviz_default_plugins/include/rviz_default_plugins/displays/image/ros_image_texture.hpp

@@ -60,14 +60,25 @@ class UnsupportedImageEncoding : public std::runtime_error
   {}
 };
 
-struct ImageData
+struct ImageData final
 {
-  ImageData(std::string encoding, const uint8_t * data_ptr, size_t size);
+  ImageData(
+    Ogre::PixelFormat pixformat,
+    const uint8_t * data_ptr,
+    size_t data_size_in_bytes,
+    bool take_ownership);
+
+  ~ImageData();
 
-  std::string encoding_;
   Ogre::PixelFormat pixel_format_;
   const uint8_t * data_ptr_;
-  size_t size_;
+  size_t size_in_bytes_;
+  // Depending on the input format of the data from the ROS message, we may or may not need to do
+  // some kind of conversion.  In the case where we do *not* do a conversion, we directly use the
+  // data pointer from the sensor_msgs::msg::Image::ConstSharedPtr and don't do any allocations for
+  // performance reasons.  In the case where we *do* a conversion, we allocate memory and then this
+  // class will take ownership of the data and will be responsible for freeing it.
+  bool has_ownership_;
 };
 
 class ROSImageTexture : public ROSImageTextureIface
@@ -86,19 +97,19 @@ class ROSImageTexture : public ROSImageTextureIface
   void clear() override;
 
   RVIZ_DEFAULT_PLUGINS_PUBLIC
-  const Ogre::String getName() override {return texture_->getName();}
+  const Ogre::String getName() const override;
 
   RVIZ_DEFAULT_PLUGINS_PUBLIC
-  const Ogre::TexturePtr & getTexture() override {return texture_;}
+  const Ogre::TexturePtr & getTexture() override;
 
   RVIZ_DEFAULT_PLUGINS_PUBLIC
   const sensor_msgs::msg::Image::ConstSharedPtr getImage() override;
 
   RVIZ_DEFAULT_PLUGINS_PUBLIC
-  uint32_t getWidth() override {return width_;}
+  uint32_t getWidth() const override;
 
   RVIZ_DEFAULT_PLUGINS_PUBLIC
-  uint32_t getHeight() override {return height_;}
+  uint32_t getHeight() const override;
 
   // automatic range normalization
   RVIZ_DEFAULT_PLUGINS_PUBLIC
@@ -111,21 +122,16 @@ class ROSImageTexture : public ROSImageTextureIface
   void setMedianFrames(unsigned median_frames) override;
 
 private:
-  template<typename T>
-  std::vector<uint8_t> normalize(const T * image_data, size_t image_data_size);
-  template<typename T>
-  std::vector<uint8_t> createNewNormalizedBuffer(
-    const T * image_data, size_t image_data_size, T minValue, T maxValue) const;
-  double computeMedianOfSeveralFrames(std::deque<double> & buffer, double new_value);
-  void updateBuffer(std::deque<double> & buffer, double value) const;
-  double computeMedianOfBuffer(const std::deque<double> & buffer) const;
   template<typename T>
   void getMinimalAndMaximalValueToNormalize(
-    const T * image_data, size_t image_data_size, T & minValue, T & maxValue);
+    const T * data_ptr, size_t num_elements, T & min_value, T & max_value);
+  template<typename T>
+  ImageData convertTo8bit(const uint8_t * data_ptr, size_t data_size_in_bytes);
+  ImageData convertYUV422ToRGBData(const uint8_t * data_ptr, size_t data_size_in_bytes);
+  ImageData convertYUV422_YUY2ToRGBData(const uint8_t * data_ptr, size_t data_size_in_bytes);
 
-  bool fillWithCurrentImage(sensor_msgs::msg::Image::ConstSharedPtr & image);
-  ImageData setFormatAndNormalizeDataIfNecessary(ImageData image_data);
-  void loadImageToOgreImage(const ImageData & image_data, Ogre::Image & ogre_image) const;
+  ImageData setFormatAndNormalizeDataIfNecessary(
+    const std::string & encoding, const uint8_t * data_ptr, size_t data_size_in_bytes);
 
   sensor_msgs::msg::Image::ConstSharedPtr current_image_;
   std::mutex mutex_;
@@ -137,7 +143,6 @@ class ROSImageTexture : public ROSImageTextureIface
   uint32_t width_;
   uint32_t height_;
   uint32_t stride_;
-  std::shared_ptr<std::vector<uint8_t>> bufferptr_;
 
   // fields for float image running median computation
   bool normalize_;

rviz_default_plugins/include/rviz_default_plugins/displays/image/ros_image_texture_iface.hpp

@@ -51,12 +51,12 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC ROSImageTextureIface
   virtual bool update() = 0;
   virtual void clear() = 0;
 
-  virtual const Ogre::String getName() = 0;
+  virtual const Ogre::String getName() const = 0;
   virtual const Ogre::TexturePtr & getTexture() = 0;
   virtual const sensor_msgs::msg::Image::ConstSharedPtr getImage() = 0;
 
-  virtual uint32_t getWidth() = 0;
-  virtual uint32_t getHeight() = 0;
+  virtual uint32_t getWidth() const = 0;
+  virtual uint32_t getHeight() const = 0;
 
   // automatic range normalization
   virtual void setNormalizeFloatImage(bool normalize) = 0;