From b30097c0918eb8e381d96a2178f39b8acdddf4f5 Mon Sep 17 00:00:00 2001 From: Greg Balke <11024792+gbalke@users.noreply.github.com> Date: Fri, 27 Aug 2021 15:57:03 -0700 Subject: [PATCH] Triangle lists support textures (#719) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Triangle lists support textures Signed-off-by: Greg Balke * Uncrustify Format Signed-off-by: Greg Balke * Texture loaded from message directly Signed-off-by: Greg Balke * Update to support latest marker message format Signed-off-by: Greg Balke * Update rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp Co-authored-by: Alejandro Hernández Cordero * Remove unused includes Signed-off-by: Greg Balke * Adjust string matching to meet updated spec texture_embedded:// -> embedded:// Signed-off-by: Greg Balke * Comment on constant Signed-off-by: Greg Balke * Fix errors Signed-off-by: Greg Balke Co-authored-by: Alejandro Hernández Cordero --- rviz_default_plugins/CMakeLists.txt | 3 + .../marker/markers/triangle_list_marker.hpp | 8 ++ .../marker/markers/triangle_list_marker.cpp | 81 ++++++++++++++++++- 3 files changed, 88 insertions(+), 4 deletions(-) diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index 692ec30e7..da14b26f1 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/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 diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/triangle_list_marker.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/triangle_list_marker.hpp index cea36bf57..b477d4df7 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/triangle_list_marker.hpp +++ b/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 diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp index 9a1db2f11..d84d4880f 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp +++ b/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 #include -#include -#include +#include +#include #include #include +#include +#include +#include #include #include +#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( @@ -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 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;