Triangle lists support textures (#719)

* Triangle lists support textures

Signed-off-by: Greg Balke <greg@openrobotics.org>

* Uncrustify Format

Signed-off-by: Greg Balke <greg@openrobotics.org>

* Texture loaded from message directly

Signed-off-by: Greg Balke <greg@openrobotics.org>

* Update to support latest marker message format

Signed-off-by: Greg Balke <greg@openrobotics.org>

* Update rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

* Remove unused includes

Signed-off-by: Greg Balke <greg@openrobotics.org>

* Adjust string matching to meet updated spec texture_embedded:// -> embedded://

Signed-off-by: Greg Balke <greg@openrobotics.org>

* Comment on constant

Signed-off-by: Greg Balke <greg@openrobotics.org>

* Fix errors

Signed-off-by: Greg Balke <greg@openrobotics.org>

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

rviz_default_plugins/CMakeLists.txt

@@ -67,6 +67,7 @@ find_package(nav_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(resource_retriever REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
@@ -249,6 +250,7 @@ ament_target_dependencies(rviz_default_plugins
   resource_retriever
   rviz_common
   rviz_rendering
+  sensor_msgs
   tf2
   tf2_geometry_msgs
   tf2_ros
@@ -269,6 +271,7 @@ ament_export_dependencies(
   rclcpp
   resource_retriever
   rviz_ogre_vendor
+  sensor_msgs
   tf2
   tf2_geometry_msgs
   tf2_ros

rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/triangle_list_marker.hpp

@@ -38,6 +38,7 @@
 
 #include "rviz_default_plugins/displays/marker/markers/marker_base.hpp"
 #include "rviz_default_plugins/visibility_control.hpp"
+#include "sensor_msgs/msg/image.hpp"
 
 // This is necessary because of using stl types with this display. Nevertheless, if you are
 // experiencing problems when subclassing this class, please make sure ROS2 and your code were
@@ -77,6 +78,7 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC TriangleListMarker : public MarkerBase
   Ogre::ManualObject * manual_object_;
   Ogre::MaterialPtr material_;
   std::string material_name_;
+  std::string texture_name_;
 
 private:
   bool wrongNumberOfPoints(const MarkerConstSharedPtr & new_message);
@@ -92,8 +94,14 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC TriangleListMarker : public MarkerBase
   bool fillManualObjectAndDetermineAlpha(MarkerConstSharedPtr new_message) const;
   void updateMaterial(const MarkerConstSharedPtr & new_message, bool any_vertex_has_alpha) const;
 
+  void loadTexture(const MarkerConstSharedPtr & new_message) const;
+
   bool hasVertexColors(MarkerConstSharedPtr new_message) const;
   bool hasFaceColors(MarkerConstSharedPtr new_message) const;
+  bool hasTexture(MarkerConstSharedPtr new_message) const;
+  bool textureEmbedded(MarkerConstSharedPtr new_message) const;
+
+  std::string getTextureName(MarkerConstSharedPtr new_message) const;
 };
 
 }  // namespace markers

rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp

@@ -30,18 +30,24 @@
 
 #include "rviz_default_plugins/displays/marker/markers/triangle_list_marker.hpp"
 
+#include <string>
 #include <vector>
 
-#include <OgreSceneNode.h>
-#include <OgreSceneManager.h>
+#include <OgreDataStream.h>
+#include <OgreImage.h>
 #include <OgreManualObject.h>
 #include <OgreMaterialManager.h>
+#include <OgreResourceGroupManager.h>
+#include <OgreSceneNode.h>
+#include <OgreSceneManager.h>
 #include <OgreTextureManager.h>
 #include <OgreTechnique.h>
 
+#include "resource_retriever/retriever.hpp"
 #include "rviz_rendering/mesh_loader.hpp"
 #include "rviz_rendering/material_manager.hpp"
 #include "rviz_common/display_context.hpp"
+#include "rviz_common/load_resource.hpp"
 #include "rviz_common/logging.hpp"
 #include "rviz_common/uniform_string_stream.hpp"
 
@@ -140,8 +146,11 @@ void TriangleListMarker::initializeManualObject(
   manual_object_ = context_->getSceneManager()->createManualObject(ss.str());
   scene_node_->attachObject(manual_object_);
 
-  ss << "Material";
-  material_name_ = ss.str();
+  texture_name_ = ss.str() + std::string("Texture");
+  if (textureEmbedded(new_message)) {
+    texture_name_ += getTextureName(new_message);
+  }
+  material_name_ = ss.str() + std::string("Material");
   material_ = rviz_rendering::MaterialManager::createMaterialWithLighting(material_name_);
   material_->setCullingMode(Ogre::CULL_NONE);
   handler_ = rviz_common::interaction::createSelectionHandler<MarkerSelectionHandler>(
@@ -217,6 +226,12 @@ bool TriangleListMarker::fillManualObjectAndDetermineAlpha(
           new_message->colors[i / 3].b,
           new_message->color.a * new_message->colors[i / 3].a);
       }
+
+      if (hasTexture(new_message)) {
+        manual_object_->textureCoord(
+          new_message->uv_coordinates[i + c].u,
+          new_message->uv_coordinates[i + c].v);
+      }
     }
   }
   return any_vertex_has_alpha;
@@ -249,6 +264,36 @@ void TriangleListMarker::updateMaterial(
     material_->getTechnique(0)->setSceneBlending(Ogre::SBT_REPLACE);
     material_->getTechnique(0)->setDepthWriteEnabled(true);
   }
+
+  if (hasTexture(new_message) && textureEmbedded(new_message)) {
+    // If the texture is already loaded, delete it.
+    Ogre::ResourcePtr texture = Ogre::TextureManager::getSingleton().getByName(
+      texture_name_, "rviz_rendering");
+    if (texture != NULL) {
+      Ogre::TextureManager::getSingleton().remove(texture);
+    }
+
+    loadTexture(new_message);
+    material_->getTechnique(0)->setLightingEnabled(true);
+    material_->setReceiveShadows(false);
+    material_->setCullingMode(Ogre::CULL_NONE);
+    material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name_);
+    material_->getTechnique(0)->getPass(0)->setSceneBlending(
+      Ogre::SBT_TRANSPARENT_ALPHA);
+  }
+}
+
+void TriangleListMarker::loadTexture(const MarkerBase::MarkerConstSharedPtr & new_message) const
+{
+  // Have to pass non-const date to MemoryDataStream so copy into non-const buffer.
+  std::vector<unsigned char> texture = new_message->texture.data;
+  Ogre::DataStreamPtr data_stream(new Ogre::MemoryDataStream(
+      texture.data(),
+      texture.size(), false, true));
+  Ogre::Image img;
+  img.load(data_stream, new_message->texture.format);
+  Ogre::TextureManager::getSingleton().loadImage(
+    texture_name_, "rviz_rendering", img, Ogre::TEX_TYPE_2D);
 }
 
 bool TriangleListMarker::hasFaceColors(const MarkerBase::MarkerConstSharedPtr new_message) const
@@ -261,6 +306,34 @@ bool TriangleListMarker::hasVertexColors(const MarkerBase::MarkerConstSharedPtr
   return new_message->colors.size() == new_message->points.size();
 }
 
+bool TriangleListMarker::hasTexture(const MarkerBase::MarkerConstSharedPtr new_message) const
+{
+  return !new_message->texture_resource.empty() &&
+         new_message->uv_coordinates.size() == new_message->points.size();
+}
+
+bool TriangleListMarker::textureEmbedded(const MarkerBase::MarkerConstSharedPtr new_message) const
+{
+  return !new_message->texture_resource.empty() &&
+         new_message->texture_resource.find("embedded://") == 0;
+}
+
+std::string TriangleListMarker::getTextureName(const MarkerBase::MarkerConstSharedPtr new_message)
+const
+{
+  if (new_message->texture_resource.empty()) {
+    return "";
+  }
+
+  size_t index = new_message->texture_resource.find("://");
+  if (index == std::string::npos) {
+    return "";
+  }
+
+  // Index past "://" by adding 3 to the result of find.
+  return new_message->texture_resource.substr(index + 3);
+}
+
 S_MaterialPtr TriangleListMarker::getMaterials()
 {
   S_MaterialPtr materials;