added plugin

voxblox_msgs/msg/Mesh.msg

@@ -0,0 +1,6 @@
+std_msgs/Header header
+
+# Index of meshed points in block map
+int64[3] index
+
+voxblox_msgs/Triangle[] triangles

voxblox_msgs/msg/Triangle.msg

@@ -0,0 +1,17 @@
+# Colored triangle used in meshing
+
+# Position
+float32[3] x
+float32[3] y
+float32[3] z
+
+# Normals
+float32[3] nx
+float32[3] ny
+float32[3] nz
+
+# Color
+uint8[3] r
+uint8[3] g
+uint8[3] b
+uint8[3] a

voxblox_msgs/package.xml

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

voxblox_ros/launch/sim.launch

@@ -0,0 +1,18 @@
+<launch>
+   <node name="simulation_eval" pkg="voxblox_ros" type="simulation_eval" output="screen" args="-alsologtostderr" clear_params="true">
+    <remap from="pointcloud" to="/camera/depth_registered/points"/>
+    <param name="tsdf_voxel_size" value="0.1" />
+    <param name="tsdf_voxels_per_side" value="16" />
+    <param name="voxel_carving_enabled" value="true" />
+    <param name="color_mode" value="normals" />
+    <param name="use_tf_transforms" value="false" />
+    <param name="update_mesh_every_n_sec" value="1.0" />
+    <param name="min_time_between_msgs_sec" value="0.2" />
+    <param name="method" value="fast" />
+    <param name="generate_esdf" value="true" />
+    <param name="use_const_weight" value="false" />
+    <param name="allow_clear" value="false" />
+    <param name="verbose" value="true" />
+  </node>
+
+</launch>

voxblox_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(voxblox_rviz_plugin)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+set(CMAKE_MACOSX_RPATH 0)
+add_definitions(-std=c++11 -Wno-sign-compare -Wno-unused-value)
+
+## This setting causes Qt's "MOC" generation to happen automatically.
+set(CMAKE_AUTOMOC ON)
+
+## This plugin includes Qt widgets, so we must include Qt.
+## We'll use the version that rviz used so they are compatible.
+if(rviz_QT_VERSION VERSION_LESS "5")
+  message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
+  find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui)
+  ## pull in all required include dirs, define QT_LIBRARIES, etc.
+  include(${QT_USE_FILE})
+  qt4_wrap_cpp(MOC_FILES
+    ${INCLUDE_FILES_QT}
+  )
+else()
+  message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
+  find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
+  ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies
+  set(QT_LIBRARIES Qt5::Widgets)
+  QT5_WRAP_CPP(MOC_FILES
+    ${INCLUDE_FILES_QT}
+  )
+endif()
+
+## Avoid Qt signals and slots defining "emit", "slots", etc.
+add_definitions(-DQT_NO_KEYWORDS)
+
+add_library(${PROJECT_NAME}
+  src/voxblox_mesh_display.cc
+  src/voxblox_mesh_visual.cc
+  ${MOC_FILES}
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${QT_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()

voxblox_rviz_plugin/include/voxblox_rviz_plugin/voxblox_mesh_display.h

@@ -0,0 +1,35 @@
+#ifndef VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_DISPLAY_H
+#define VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_DISPLAY_H
+
+#include <rviz/message_filter_display.h>
+#include <voxblox_msgs/Mesh.h>
+
+namespace Ogre {
+class SceneNode;
+}
+
+namespace voxblox_rviz_plugin {
+
+class VoxbloxMeshVisual;
+
+class VoxbloxMeshDisplay
+    : public rviz::MessageFilterDisplay<voxblox_msgs::Mesh> {
+  Q_OBJECT
+ public:
+  VoxbloxMeshDisplay();
+  virtual ~VoxbloxMeshDisplay();
+
+ protected:
+  virtual void onInitialize();
+
+  virtual void reset();
+
+ private:
+  void processMessage(const voxblox_msgs::Mesh::ConstPtr& msg);
+
+  std::unique_ptr<VoxbloxMeshVisual> visual_;
+};
+
+}  // namespace voxblox_rviz_plugin
+
+#endif  // VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_DISPLAY_H

voxblox_rviz_plugin/include/voxblox_rviz_plugin/voxblox_mesh_visual.h

@@ -0,0 +1,39 @@
+#ifndef VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_VISUAL_H
+#define VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_VISUAL_H
+
+#include <OGRE/OgreManualObject.h>
+
+#include <voxblox/core/block_hash.h>
+#include <voxblox_msgs/Mesh.h>
+
+namespace Ogre {
+class Vector3;
+class Quaternion;
+class ManualObject;
+}
+
+namespace voxblox_rviz_plugin {
+
+// Visualizes a single grid_map_msgs::GridMap message.
+class VoxbloxMeshVisual {
+ public:
+  VoxbloxMeshVisual(Ogre::SceneManager* scene_manager,
+                    Ogre::SceneNode* parent_node);
+  virtual ~VoxbloxMeshVisual();
+
+  void setMessage(const voxblox_msgs::Mesh::ConstPtr& msg);
+
+  // Set the coordinate frame pose.
+  void setFramePosition(const Ogre::Vector3& position);
+  void setFrameOrientation(const Ogre::Quaternion& orientation);
+
+ private:
+  Ogre::SceneNode* frame_node_;
+  Ogre::SceneManager* scene_manager_;
+
+  voxblox::AnyIndexHashMapType<Ogre::ManualObject*>::type object_map_;
+};
+
+}  // namespace voxblox_rviz_plugin
+
+#endif  // VOXBLOX_RVIZ_PLUGIN_VOXBLOX_MESH_VISUAL_H

voxblox_rviz_plugin/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>voxblox_rviz_plugin</name>
+  <version>0.0.0</version>
+  <description>RViz plugin for displaying incremental voxblox mesh messages.</description>
+  <maintainer email="[email protected]">Zachary Taylor</maintainer>
+  <license>BSD</license>
+  <author>Zachary Taylor</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>rviz</depend>
+  <depend>voxblox</depend>
+  <depend>voxblox_msgs</depend>
+  <export>
+      <rviz plugin="${prefix}/plugin_description.xml"/>
+  </export>
+</package>

voxblox_rviz_plugin/plugin_description.xml

@@ -0,0 +1,8 @@
+<library path="lib/libvoxblox_rviz_plugin">
+  <class name="voxblox_rviz_plugin/VoxbloxMesh"
+         type="voxblox_rviz_plugin::VoxbloxMeshDisplay"
+         base_class_type="rviz::Display">
+    <description>Displays incremental voxblox mesh messages.</description>
+    <message_type>voxblox_msgs/Mesh</message_type>
+  </class>
+</library>

voxblox_rviz_plugin/src/voxblox_mesh_display.cc

@@ -0,0 +1,53 @@
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+
+#include <tf/transform_listener.h>
+
+#include <rviz/frame_manager.h>
+#include <rviz/visualization_manager.h>
+
+#include "voxblox_rviz_plugin/voxblox_mesh_display.h"
+#include "voxblox_rviz_plugin/voxblox_mesh_visual.h"
+
+namespace voxblox_rviz_plugin {
+
+VoxbloxMeshDisplay::VoxbloxMeshDisplay() {}
+
+void VoxbloxMeshDisplay::onInitialize() { MFDClass::onInitialize(); }
+
+VoxbloxMeshDisplay::~VoxbloxMeshDisplay() {}
+
+void VoxbloxMeshDisplay::reset() {
+  MFDClass::reset();
+  visual_.reset();
+}
+
+void VoxbloxMeshDisplay::processMessage(
+    const voxblox_msgs::Mesh::ConstPtr& msg) {
+  // Here we call the rviz::FrameManager to get the transform from the
+  // fixed frame to the frame in the header of this Imu message.  If
+  // it fails, we can't do anything else so we return.
+  Ogre::Quaternion orientation;
+  Ogre::Vector3 position;
+  if (!context_->getFrameManager()->getTransform(
+          msg->header.frame_id, msg->header.stamp, position, orientation)) {
+    ROS_DEBUG("Error transforming from frame '%s' to frame '%s'",
+              msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
+    return;
+  }
+
+  if (visual_ == nullptr) {
+    visual_.reset(
+        new VoxbloxMeshVisual(context_->getSceneManager(), scene_node_));
+  }
+
+  // Now set or update the contents of the chosen visual.
+  visual_->setMessage(msg);
+  visual_->setFramePosition(position);
+  visual_->setFrameOrientation(orientation);
+}
+
+}  // namespace voxblox_rviz_plugin
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(voxblox_rviz_plugin::VoxbloxMeshDisplay, rviz::Display)

voxblox_rviz_plugin/src/voxblox_mesh_visual.cc

@@ -0,0 +1,72 @@
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+
+#include "voxblox_rviz_plugin/voxblox_mesh_visual.h"
+
+namespace voxblox_rviz_plugin {
+
+VoxbloxMeshVisual::VoxbloxMeshVisual(Ogre::SceneManager* scene_manager,
+                                     Ogre::SceneNode* parent_node) {
+  scene_manager_ = scene_manager;
+  frame_node_ = parent_node->createChildSceneNode();
+}
+
+VoxbloxMeshVisual::~VoxbloxMeshVisual() {
+  // Destroy all the objects
+  for (std::pair<const voxblox::BlockIndex, Ogre::ManualObject*> ogre_object :
+       object_map_) {
+    scene_manager_->destroyManualObject(ogre_object.second);
+  }
+}
+
+void VoxbloxMeshVisual::setMessage(const voxblox_msgs::Mesh::ConstPtr& msg) {
+  const voxblox::BlockIndex index(msg->index[0], msg->index[1], msg->index[2]);
+
+  Ogre::ManualObject* ogre_object;
+  const voxblox::AnyIndexHashMapType<Ogre::ManualObject*>::type::const_iterator
+      it = object_map_.find(index);
+  if (it != object_map_.end()) {
+    ogre_object = it->second;
+    ogre_object->clear();
+  } else {
+    std::string object_name = std::to_string(index.x()) + std::string(" ") +
+                              std::to_string(index.y()) + std::string(" ") +
+                              std::to_string(index.z());
+    ogre_object = scene_manager_->createManualObject(object_name);
+    object_map_.insert(std::make_pair(index, ogre_object));
+
+    frame_node_->attachObject(ogre_object);
+  }
+
+  size_t nVertices = msg->triangles.size();
+  ogre_object->estimateVertexCount(3 * msg->triangles.size());
+  ogre_object->begin("BaseWhiteNoLighting",
+                     Ogre::RenderOperation::OT_TRIANGLE_LIST);
+
+  for (const voxblox_msgs::Triangle& triangle : msg->triangles) {
+    for (size_t i = 0; i < 3; ++i) {
+      ogre_object->position(triangle.x[i], triangle.y[i], triangle.z[i]);
+      ogre_object->normal(triangle.nx[i], triangle.ny[i], triangle.nz[i]);
+
+      constexpr float color_conv_factor = 1 / 255;
+      ogre_object->colour(
+          color_conv_factor * static_cast<float>(triangle.r[i]),
+          color_conv_factor * static_cast<float>(triangle.g[i]),
+          color_conv_factor * static_cast<float>(triangle.b[i]),
+          color_conv_factor * static_cast<float>(triangle.a[i]));
+    }
+  }
+
+  ogre_object->end();
+}
+
+void VoxbloxMeshVisual::setFramePosition(const Ogre::Vector3& position) {
+  frame_node_->setPosition(position);
+}
+
+void VoxbloxMeshVisual::setFrameOrientation(
+    const Ogre::Quaternion& orientation) {
+  frame_node_->setOrientation(orientation);
+}
+
+}  // namespace voxblox_rviz_plugin