Added support for adding static image underlay of overlay.

Author: peci1

CMakeLists.txt

@@ -148,6 +148,7 @@ add_library(${PROJECT_NAME}_distortion
   src/compositors/OgreCameraDistortion.cc
   src/compositors/OgreInvertColors.cc
   src/compositors/OgreOutline.cc
+  src/compositors/OgreStaticImage.cc
 )
 target_link_libraries(${PROJECT_NAME}_distortion PUBLIC
   ${catkin_LIBRARIES} OPENCV OGRE Eigen3::Eigen

include/robot_model_renderer/RobotModelRenderer.hpp

@@ -26,12 +26,14 @@
 #include <robot_model_renderer/compositors/OgreCameraDistortion.hpp>
 #include <robot_model_renderer/compositors/OgreInvertColors.hpp>
 #include <robot_model_renderer/compositors/OgreOutline.hpp>
+#include <robot_model_renderer/compositors/OgreStaticImage.hpp>
 #include <robot_model_renderer/pinhole_camera.hpp>
 #include <robot_model_renderer/ogre_helpers/render_system.hpp>
 #include <robot_model_renderer/robot/link_updater.hpp>
 #include <robot_model_renderer/robot/robot.hpp>
 #include <robot_model_renderer/robot/shape_filter.hpp>
 #include <robot_model_renderer/robot/shape_inflation_registry.hpp>
+#include <robot_model_renderer/types.hpp>
 #include <urdf/model.h>
 
 namespace Ogre
@@ -49,16 +51,6 @@ class Camera;
 namespace robot_model_renderer
 {
 
-/**
- * \brief Mode of robot model rendering.
- */
-enum class RenderingMode
-{
-  NORMAL,  //!< Normal mode, visual meshes use their textures, collision meshes use red or link color.
-  COLOR,  //!< All meshes are rendered with the specified color (with lighting effects).
-  MASK,  //!< All meshes are rendered as a binary mask (robot = white, background = black).
-};
-
 /**
  * \brief Configuration of RobotModelRenderer.
  */
@@ -95,6 +87,10 @@ struct RobotModelRendererConfig
   cv::InterpolationFlags upscalingInterpolation {cv::INTER_LINEAR};
   double renderImageScale {1.0};
   size_t maxRenderImageSize {0u};
+
+  cv::Mat staticMaskImage;
+  std::string staticMaskImageEncoding;  //!< Encoding from sensor_msgs/image_encodings.h . If empty, BGR(A) is assumed.
+  bool staticMaskIsBackground {true};  //!< If false, the static mask image will be drawn over the rendered image.
 };
 
 struct RenderErrors
@@ -130,6 +126,9 @@ class RobotModelRenderer : public cras::HasLogger
 
 protected:
   virtual void updateOgreCamera();
+  virtual cras::expected<cv::Mat, std::string> renderInner(const ros::Time& time, RenderErrors& errors);
+
+  virtual bool hasOverlays() const;
 
   LinkUpdater* linkUpdater;
 
@@ -148,13 +147,27 @@ class RobotModelRenderer : public cras::HasLogger
   Ogre::SceneManager* scene_manager_ {nullptr};
   Ogre::Light* default_light_ {nullptr};
   Ogre::SceneNode* scene_node_ {nullptr};
+
   Ogre::TexturePtr tex_;
   Ogre::RenderTarget* rt_ {nullptr};
   Ogre::Camera* camera_ {nullptr};
   Ogre::Viewport* viewPort_ {nullptr};
+
   OgreCameraDistortion distortionPass_;
   OgreInvertColors invertColorsPass_;
   OgreOutline outlinePass_;
+
+  Ogre::SceneManager* overlay_scene_manager_ {nullptr};
+  Ogre::SceneNode* overlay_scene_node_ {nullptr};
+  Ogre::SharedPtr<Ogre::Rectangle2D> overlay_;
+  Ogre::TexturePtr overlay_tex_;
+  Ogre::TexturePtr overlay_scene_tex_;
+  Ogre::RenderTarget* overlay_rt_ {nullptr};
+  Ogre::Camera* overlay_camera_ {nullptr};
+  Ogre::Viewport* overlay_viewPort_ {nullptr};
+
+  OgreStaticImage staticImagePass_;
+
   int cvImageType;
 };
 

include/robot_model_renderer/RosCameraRobotModelRenderer.hpp

@@ -70,6 +70,9 @@ struct RosCameraRobotModelRendererConfig
   cv::InterpolationFlags upscalingInterpolation {cv::INTER_LINEAR};
   double renderImageScale {1.0};
   size_t maxRenderImageSize {0u};
+
+  sensor_msgs::Image staticMaskImage;
+  bool staticMaskIsBackground {true};  //!< If false, the static mask image will be drawn over the rendered image.
 };
 
 /**

include/robot_model_renderer/c_api.h

@@ -83,6 +83,14 @@ struct robot_model_renderer_RobotModelRendererConfig
   int upscalingInterpolation;
   double renderImageScale;
   size_t maxRenderImageSize;
+
+  size_t staticMaskImageWidth;
+  size_t staticMaskImageHeight;
+  size_t staticMaskImageStep;
+  int staticMaskImageCVType;
+  void* staticMaskImage;
+  const char* staticMaskImageEncoding;  //!< Encoding from sensor_msgs/image_encodings.h . If empty, BGR(A) is assumed.
+  bool staticMaskIsBackground;  //!< If false, the static mask image will be drawn over the rendered image.
 };
 
 struct ros_Time

include/robot_model_renderer/compositors/OgreStaticImage.hpp

@@ -0,0 +1,68 @@
+// SPDX-License-Identifier: BSD-3-Clause
+// SPDX-FileCopyrightText: Czech Technical University in Prague
+
+#pragma once
+
+/**
+ * \file
+ * \brief OGRE compositor drawing a static image under or over the scene.
+ * \author Martin Pecka
+ */
+
+#include <memory>
+
+#include <opencv2/core/mat.hpp>
+
+#include <OgrePixelFormat.h>
+
+#include <cras_cpp_common/log_utils.h>
+
+#include <robot_model_renderer/types.hpp>
+
+namespace Ogre
+{
+
+class Camera;
+
+}
+
+namespace robot_model_renderer
+{
+
+/**
+ * \brief Ogre implementation static image compositor.
+ */
+class OgreStaticImage : public cras::HasLogger
+{
+public:
+  OgreStaticImage(const cras::LogHelperPtr& log, const cv::Mat& staticImage, Ogre::PixelFormat staticImageFormat,
+    Ogre::PixelFormat sceneFormat, bool staticImageIsBackground, RenderingMode renderingMode,
+    const Ogre::ColourValue& colorModeColor);
+
+  virtual ~OgreStaticImage();
+
+  /**
+   * \brief Destroy the pass and unregister it where needed.
+   */
+  void Destroy();
+
+  /**
+   * \brief Create the render pass.
+   */
+  void CreateRenderPass();
+
+  /**
+   * \brief Set the ogre camera that the render pass applies to
+   *
+   * \param[in] camera Pointer to the ogre camera.
+   */
+  virtual void SetCamera(Ogre::Camera* camera);
+
+protected:
+  Ogre::Camera* ogreCamera = nullptr;  //!< Pointer to the ogre camera
+
+  struct Implementation;
+  std::unique_ptr<Implementation> dataPtr;  //!< PIMPL data
+};
+
+}

include/robot_model_renderer/types.hpp

@@ -0,0 +1,26 @@
+// SPDX-License-Identifier: BSD-3-Clause
+// SPDX-FileCopyrightText: Czech Technical University in Prague
+
+#pragma once
+
+/**
+ * \file
+ * \brief Common types.
+ * \author Martin Pecka
+ */
+
+
+namespace robot_model_renderer
+{
+
+/**
+ * \brief Mode of robot model rendering.
+ */
+enum class RenderingMode
+{
+  NORMAL,  //!< Normal mode, visual meshes use their textures, collision meshes use red or link color.
+  COLOR,  //!< All meshes are rendered with the specified color (with lighting effects).
+  MASK,  //!< All meshes are rendered as a binary mask (robot = white, background = black).
+};
+
+}

nodelets/robot_model_renderer_nodelet.cpp

@@ -14,10 +14,14 @@
 #include <thread>
 #include <vector>
 
+#include <opencv2/imgcodecs.hpp>
+
 #include <urdf/model.h>
 
 #include <cras_cpp_common/nodelet_utils.hpp>
 #include <cras_cpp_common/param_utils.hpp>
+#include <cras_cpp_common/string_utils.hpp>
+#include <cv_bridge/cv_bridge.h>
 #include <image_transport/image_transport.h>
 #include <pluginlib/class_list_macros.h>
 #include <nodelet/nodelet.h>
@@ -203,6 +207,41 @@ class RobotModelRendererNodelet : public cras::Nodelet
     }
   }
 
+  std::string bestImageEncodingFromCvMat(const cv::Mat staticMaskImage)
+  {
+    const auto nchannels = staticMaskImage.channels();
+    const auto depth = staticMaskImage.depth();
+    if (depth == CV_8U && nchannels == 1)
+      return sensor_msgs::image_encodings::MONO8;
+    else if (depth == CV_8U && nchannels == 3)
+      return sensor_msgs::image_encodings::BGR8;  // OpenCV always loads in BGR order
+    else if (depth == CV_8U && nchannels == 4)
+      return sensor_msgs::image_encodings::BGRA8;  // OpenCV always loads in BGR order
+    else if (depth == CV_16U && nchannels == 1)
+      return sensor_msgs::image_encodings::MONO16;
+    else if (depth == CV_16U && nchannels == 3)
+      return sensor_msgs::image_encodings::BGR16;  // OpenCV always loads in BGR order
+    else if (depth == CV_16U && nchannels == 4)
+      return sensor_msgs::image_encodings::BGRA16;  // OpenCV always loads in BGR order
+    else if (depth == CV_32F && nchannels == 1)
+      return sensor_msgs::image_encodings::TYPE_32FC1;
+    else if (depth == CV_32F && nchannels == 2)
+      return sensor_msgs::image_encodings::TYPE_32FC2;
+    else if (depth == CV_32F && nchannels == 3)
+      return sensor_msgs::image_encodings::TYPE_32FC3;
+    else if (depth == CV_32F && nchannels == 4)
+      return sensor_msgs::image_encodings::TYPE_32FC4;
+    else if (depth == CV_64F && nchannels == 1)
+      return sensor_msgs::image_encodings::TYPE_64FC1;
+    else if (depth == CV_64F && nchannels == 2)
+      return sensor_msgs::image_encodings::TYPE_64FC2;
+    else if (depth == CV_64F && nchannels == 3)
+      return sensor_msgs::image_encodings::TYPE_64FC3;
+    else if (depth == CV_64F && nchannels == 4)
+      return sensor_msgs::image_encodings::TYPE_64FC4;
+    return "";
+  }
+
   void onInitialize()
   {
     const auto& publicParams = this->publicParams();
@@ -258,6 +297,8 @@ class RobotModelRendererNodelet : public cras::Nodelet
 
     RosCameraRobotModelRendererConfig config;
 
+    cras::TempLocale l(LC_ALL, "en_US.UTF-8");
+
     config.renderingMode = params->getParam("rendering_mode", config.renderingMode);
 
     if (config.renderingMode == RenderingMode::MASK)
@@ -291,6 +332,20 @@ class RobotModelRendererNodelet : public cras::Nodelet
     config.renderImageScale = params->getParam("render_image_scale", config.renderImageScale);
     config.maxRenderImageSize = params->getParam("max_render_image_size", config.maxRenderImageSize, "px (0 = none)");
 
+    const auto staticMaskImageFile = params->getParam("static_mask_image_file", "");
+    const auto staticMaskImageFileEncoding = params->getParam("static_mask_image_encoding", "");
+    config.staticMaskIsBackground = params->getParam("static_mask_is_background", config.staticMaskIsBackground);
+
+    const auto staticMaskImage = cv::imread(staticMaskImageFile, cv::IMREAD_UNCHANGED);
+    if (staticMaskImage.total() > 0)
+    {
+      auto encoding = staticMaskImageFileEncoding;
+      if (encoding.empty())
+        encoding = bestImageEncodingFromCvMat(staticMaskImage);
+      cv_bridge::CvImage maskImage({}, encoding, staticMaskImage);
+      config.staticMaskImage = *maskImage.toImageMsg();
+    }
+
     const auto inflationPadding = params->getParamVerbose("body_model/inflation/padding", 0.0, "m");
     const auto inflationScale = params->getParamVerbose("body_model/inflation/scale", 1.0);
     config.shapeInflationRegistry = std::make_shared<ShapeInflationRegistry>(inflationScale, inflationPadding);

ogre_media/materials/programs/static_image_fs.glsl

@@ -0,0 +1,50 @@
+// SPDX-License-Identifier: BSD-3-Clause
+// SPDX-FileCopyrightText: Czech Technical University in Prague
+
+uniform sampler2D rt_scene; // The RTT from the compositor
+uniform sampler2D static_image; // The RTT from the compositor
+uniform sampler2D orig_static_image; // The RTT from the compositor
+uniform int is_background;
+uniform vec4 background_color;
+uniform int rendering_mode;
+uniform vec4 color_mode_color;
+
+const int RM_NORMAL = 0;
+const int RM_COLOR = 1;
+const int RM_MASK = 2;
+
+float length(vec4 vec)
+{
+    return  sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z + vec.w * vec.w);
+}
+
+void main()
+{
+    vec2 uv = gl_TexCoord[0].xy;
+    vec4 scene_color = texture2D(rt_scene, uv);
+    vec4 static_color = texture2D(static_image, uv);
+    vec4 orig_static_color = texture2D(orig_static_image, uv);
+
+    vec4 final_static_color;
+    if (rendering_mode == RM_NORMAL)
+        final_static_color = static_color;
+    else if (rendering_mode == RM_COLOR)
+        final_static_color = color_mode_color;
+    else if (rendering_mode == RM_MASK)
+        final_static_color = vec4(1.0, 1.0, 1.0, 1.0);
+
+    if (is_background == 0)
+    {
+        if (orig_static_color.a < 0.5 || orig_static_color == background_color)
+            gl_FragColor = scene_color;
+        else
+            gl_FragColor = final_static_color;
+    }
+    else
+    {
+        if (scene_color.a < 0.5)
+            gl_FragColor = final_static_color;
+        else
+            gl_FragColor = scene_color;
+    }
+}

ogre_media/materials/scripts/outline.compositor

@@ -5,9 +5,9 @@ compositor Outline
 {
     technique
     {
-        texture rt0 target_width_scaled 0.25 target_height_scaled 0.25 PF_R8G8B8A8
-        texture blurred target_width_scaled 0.25 target_height_scaled 0.25 PF_R8G8B8A8
-        texture rt_scene target_width target_height PF_R8G8B8A8
+        texture rt0 target_width_scaled 0.25 target_height_scaled 0.25 PF_A8R8G8B8
+        texture blurred target_width_scaled 0.25 target_height_scaled 0.25 PF_A8R8G8B8
+        texture rt_scene target_width target_height PF_A8R8G8B8
 
         // Pass 1: Render the scene to our temporary texture.
         target rt_scene

ogre_media/materials/scripts/static_image.compositor

@@ -0,0 +1,26 @@
+// SPDX-License-Identifier: BSD-3-Clause
+// SPDX-FileCopyrightText: Czech Technical University in Prague
+
+compositor StaticImage
+{
+    technique
+    {
+        texture rt_scene target_width target_height PF_A8R8G8B8
+
+        target rt_scene
+        {
+            input previous
+        }
+
+        target_output
+        {
+            input none
+
+            pass render_quad
+            {
+                material StaticImageMat
+                input 0 rt_scene
+            }
+        }
+    }
+}