Multi-mesh visualization support and rviz plugin improvements

voxblox_msgs/msg/LayerWithTrajectory.msg

@@ -0,0 +1,2 @@
+voxblox_msgs/Layer layer
+nav_msgs/Path trajectory

voxblox_msgs/msg/MultiMesh.msg

@@ -0,0 +1,14 @@
+# A mesh message that support multiple instances of meshes.
+
+# header of the MultiMesh message. This header is also used for the 'mesh' field when read by the rviz plugin.
+std_msgs/Header header
+
+# Each submap is discerned by its namespace. These can be hierarchical using '/' as a separator.
+string name_space
+
+# sets the opacity of the entire mesh.
+uint8 alpha
+
+# The message containing the voxblox mesh.
+# NOTE: if 'mesh.mesh_blocks' is empty the according visual will be cleared.
+voxblox_msgs/Mesh mesh

voxblox_msgs/package.xml

@@ -18,4 +18,5 @@
   <depend>message_generation</depend>
   <depend>message_runtime</depend>
   <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
 </package>

voxblox_rviz_plugin/CMakeLists.txt

@@ -33,9 +33,19 @@ endif()
 ## Avoid Qt signals and slots defining "emit", "slots", etc.
 add_definitions(-DQT_NO_KEYWORDS)
 
-set(HEADER_FILES include/voxblox_rviz_plugin/voxblox_mesh_display.h include/voxblox_rviz_plugin/voxblox_mesh_visual.h)
+set(HEADER_FILES
+        include/voxblox_rviz_plugin/voxblox_mesh_display.h
+        include/voxblox_rviz_plugin/voxblox_multi_mesh_display.h
+        include/voxblox_rviz_plugin/voxblox_mesh_visual.h
+        include/voxblox_rviz_plugin/material_loader.h
+        )
 
-set(SRC_FILES src/voxblox_mesh_display.cc src/voxblox_mesh_visual.cc )
+set(SRC_FILES
+        src/voxblox_mesh_display.cc
+        src/voxblox_multi_mesh_display.cc
+        src/voxblox_mesh_visual.cc
+        src/material_loader.cc
+        )
 
 cs_add_library(${PROJECT_NAME}
   ${SRC_FILES}

voxblox_rviz_plugin/content/materials/voxblox.material

@@ -0,0 +1,82 @@
+// material for (transparent) meshes, based on 'BaseWhiteNoLighting'
+material VoxbloxMaterialTransparent
+{
+	receive_shadows on
+	transparency_casts_shadows off
+	technique
+	{
+		lod_index 0
+		scheme Default
+		pass
+		{
+			lighting off
+			max_lights 8
+			start_light 0
+			iteration once
+			point_size 1
+			point_sprites off
+			point_size_attenuation off
+			point_size_min 0
+			point_size_max 0
+			scene_blend alpha_blend
+			depth_check on
+			alpha_rejection always_pass 0
+			alpha_to_coverage off
+			transparent_sorting on
+			depth_write off
+			depth_func less_equal
+			depth_bias 0 0
+			iteration_depth_bias 0
+			light_scissor off
+			light_clip_planes off
+			cull_hardware clockwise
+			cull_software back
+			shading gouraud
+			polygon_mode solid
+			polygon_mode_overrideable on
+			normalise_normals off
+			fog_override false
+		}
+	}
+}
+
+material VoxbloxMaterial
+{
+	receive_shadows on
+	transparency_casts_shadows off
+	technique
+	{
+		lod_index 0
+		scheme Default
+		pass
+		{
+			lighting off
+			max_lights 8
+			start_light 0
+			iteration once
+			point_size 1
+			point_sprites off
+			point_size_attenuation off
+			point_size_min 0
+			point_size_max 0
+			scene_blend one zero
+			depth_check on
+			alpha_rejection always_pass 0
+			alpha_to_coverage off
+			transparent_sorting on
+			depth_write on
+			depth_func less_equal
+			depth_bias 0 0
+			iteration_depth_bias 0
+			light_scissor off
+			light_clip_planes off
+			cull_hardware clockwise
+			cull_software back
+			shading gouraud
+			polygon_mode solid
+			polygon_mode_overrideable on
+			normalise_normals off
+			fog_override false
+		}
+	}
+}

voxblox_rviz_plugin/include/voxblox_rviz_plugin/material_loader.h

@@ -0,0 +1,21 @@
+#ifndef VOXBLOX_RVIZ_PLUGIN_MATERIAL_LOADER_H_
+#define VOXBLOX_RVIZ_PLUGIN_MATERIAL_LOADER_H_
+
+namespace voxblox_rviz_plugin {
+
+/**
+ * This class simply loads custom ogre materials for visualization upon startup.
+ */
+class MaterialLoader {
+ public:
+  static void loadMaterials();
+
+ private:
+  MaterialLoader() = default;
+
+  static bool materials_loaded_;
+};
+
+}  // namespace voxblox_rviz_plugin
+
+#endif  // VOXBLOX_RVIZ_PLUGIN_MATERIAL_LOADER_H_

voxblox_rviz_plugin/include/voxblox_rviz_plugin/voxblox_mesh_display.h

@@ -18,17 +18,21 @@ class VoxbloxMeshDisplay
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   VoxbloxMeshDisplay();
-  virtual ~VoxbloxMeshDisplay();
+  virtual ~VoxbloxMeshDisplay() = default;
 
  protected:
-  virtual void onInitialize();
-
-  virtual void reset();
+  void reset() override;
+  void fixedFrameChanged() override;
 
  private:
-  void processMessage(const voxblox_msgs::Mesh::ConstPtr& msg);
+  void processMessage(const voxblox_msgs::Mesh::ConstPtr& msg) override;
+  bool updateTransformation(ros::Time stamp);
 
   std::unique_ptr<VoxbloxMeshVisual> visual_;
+
+  // Allows the user to still clear the mesh by clicking this property
+  rviz::BoolProperty visible_property_;
+  Q_SLOT void visibleSLOT();
 };
 
 }  // namespace voxblox_rviz_plugin

voxblox_rviz_plugin/include/voxblox_rviz_plugin/voxblox_mesh_visual.h

@@ -1,32 +1,48 @@
 #ifndef VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_VISUAL_H_
 #define VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_VISUAL_H_
 
+#include <limits>
+#include <map>
+#include <string>
+
 #include <OGRE/OgreManualObject.h>
 
 #include <voxblox/core/block_hash.h>
 #include <voxblox_msgs/Mesh.h>
+#include <voxblox_msgs/MultiMesh.h>
 
 namespace voxblox_rviz_plugin {
 
 /// Visualizes a single voxblox_msgs::Mesh message.
 class VoxbloxMeshVisual {
  public:
   VoxbloxMeshVisual(Ogre::SceneManager* scene_manager,
-                    Ogre::SceneNode* parent_node);
+                    Ogre::SceneNode* parent_node, std::string name_space = "");
   virtual ~VoxbloxMeshVisual();
 
-  void setMessage(const voxblox_msgs::Mesh::ConstPtr& msg);
+  void setMessage(const voxblox_msgs::Mesh::ConstPtr& msg,
+                  uint8_t alpha = std::numeric_limits<uint8_t>::max());
+
+  // enable / disable visibility
+  void setEnabled(bool enabled);
 
   /// Set the coordinate frame pose.
-  void setFramePosition(const Ogre::Vector3& position);
-  void setFrameOrientation(const Ogre::Quaternion& orientation);
+  void setPose(const Ogre::Vector3& position,
+               const Ogre::Quaternion& orientation);
+
+  void setFrameId(const std::string& frame_id) { frame_id_ = frame_id; }
+  const std::string& getFrameId() { return frame_id_; }
 
  private:
   Ogre::SceneNode* frame_node_;
   Ogre::SceneManager* scene_manager_;
 
   unsigned int instance_number_;
   static unsigned int instance_counter_;
+  std::string name_space_;  // this is the id used by multi-mesh messages
+  bool is_enabled_;
+  std::string frame_id_;  // the frame this mesh is in, newer messages will
+                          // overwrite this
 
   voxblox::AnyIndexHashMapType<Ogre::ManualObject*>::type object_map_;
 };

voxblox_rviz_plugin/include/voxblox_rviz_plugin/voxblox_multi_mesh_display.h

@@ -0,0 +1,78 @@
+#ifndef VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MULTI_MESH_DISPLAY_H_
+#define VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MULTI_MESH_DISPLAY_H_
+
+#include <map>
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+#include <rviz/message_filter_display.h>
+#include <voxblox_msgs/MultiMesh.h>
+
+#include "voxblox_rviz_plugin/voxblox_mesh_visual.h"
+
+namespace voxblox_rviz_plugin {
+
+class VoxbloxMeshVisual;
+class VisibilityField;
+
+class VoxbloxMultiMeshDisplay
+    : public rviz::MessageFilterDisplay<voxblox_msgs::MultiMesh> {
+  Q_OBJECT
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  VoxbloxMultiMeshDisplay();
+  ~VoxbloxMultiMeshDisplay() override = default;
+  void updateVisible();
+
+ protected:
+  void reset() override;
+  void fixedFrameChanged() override;
+
+  // Override subscribe to enable a custom queue size of more than 10.
+  static constexpr uint32_t kSubscriberQueueLength = 1000;
+  void subscribe() override;
+  void onInitialize() override;
+
+  // Automatically update the mesh poses based on their frame_ids.
+  void update(float wall_dt, float ros_dt) override;
+
+ private:
+  void processMessage(const voxblox_msgs::MultiMesh::ConstPtr& msg) override;
+  bool updateTransformation(VoxbloxMeshVisual* visual, ros::Time stamp);
+  void updateAllTransformations();
+
+  // The set of all visuals, identified by namespace.
+  std::unordered_map<std::string, VoxbloxMeshVisual> visuals_;
+
+  // The root of the visibility tree.
+  std::unique_ptr<VisibilityField> visibility_fields_;
+  Q_SLOT void visibleSlot();
+
+  // Keep track of the time that elapsed since we last updated the submap poses,
+  // such that we can throttle these updates to a reasonable rate.
+  float dt_since_last_update_;
+};
+
+// Allow the user to show hide sets of submaps based on the name spaces.
+class VisibilityField : public rviz::BoolProperty {
+  Q_OBJECT
+ public:
+  VisibilityField(const std::string& name, rviz::BoolProperty* parent,
+                  VoxbloxMultiMeshDisplay* master);
+  void addField(const std::string& field_name);
+  void removeField(const std::string& field_name);
+  bool isEnabled(const std::string& field_name);
+
+ private:
+  Q_SLOT void visibleSlot();
+  VoxbloxMultiMeshDisplay* master_;
+  std::unordered_map<std::string, std::unique_ptr<VisibilityField>> children_;
+  bool hasNameSpace(const std::string& name, std::string* ns,
+                    std::string* sub_name);
+};
+
+}  // namespace voxblox_rviz_plugin
+
+#endif  // VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MULTI_MESH_DISPLAY_H_

voxblox_rviz_plugin/plugin_description.xml

@@ -5,4 +5,11 @@
     <description>Displays incremental voxblox mesh messages.</description>
     <message_type>voxblox_msgs/Mesh</message_type>
   </class>
+
+  <class name="voxblox_rviz_plugin/VoxbloxMultiMesh"
+         type="voxblox_rviz_plugin::VoxbloxMultiMeshDisplay"
+         base_class_type="rviz::Display">
+    <description>Displays incremental voxblox mesh messages for multiple meshes.</description>
+    <message_type>voxblox_msgs/MultiMesh</message_type>
+  </class>
 </library>

voxblox_rviz_plugin/src/material_loader.cc

@@ -0,0 +1,29 @@
+#include "voxblox_rviz_plugin/material_loader.h"
+
+#include <OgreResourceGroupManager.h>
+#include <ros/package.h>
+
+namespace voxblox_rviz_plugin {
+
+bool MaterialLoader::materials_loaded_ = false;
+
+void MaterialLoader::loadMaterials() {
+  if (materials_loaded_) {
+    return;
+  }
+  // first instance loads a custom ogre material that supports transparent
+  // colors.
+  std::string path =
+      ros::package::getPath("voxblox_rviz_plugin") + "/content/materials";
+  Ogre::ResourceGroupManager::getSingletonPtr()->createResourceGroup(
+      "VoxbloxMaterials");
+  Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
+      path, "FileSystem", "VoxbloxMaterials", true);
+  Ogre::ResourceGroupManager::getSingletonPtr()->initialiseResourceGroup(
+      "VoxbloxMaterials");
+  Ogre::ResourceGroupManager::getSingletonPtr()->loadResourceGroup(
+      "VoxbloxMaterials");
+  materials_loaded_ = true;
+}
+
+}  // namespace voxblox_rviz_plugin