MeshDisplay.hpp

Go to the documentation of this file.

/*
 *  Software License Agreement (BSD License)
 *
 *  Robot Operating System code by the University of Osnabrück
 *  Copyright (c) 2015, University of Osnabrück
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   1. Redistributions of source code must retain the above
 *      copyright notice, this list of conditions and the following
 *      disclaimer.
 *
 *   2. Redistributions in binary form must reproduce the above
 *      copyright notice, this list of conditions and the following
 *      disclaimer in the documentation and/or other materials provided
 *      with the distribution.
 *
 *   3. Neither the name of the copyright holder nor the names of its
 *      contributors may be used to endorse or promote products derived
 *      from this software without specific prior written permission.
 *
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 *  TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 *  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 *  OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 *  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 *  OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 *  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *
 *
 *  MeshDisplay.hpp
 *
 *
 *  authors:
 *
 *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
 *    Henning Deeken <hdeeken@uni-osnabrueck.de>
 *    Marcel Mrozinski
 *    Nils Oesting
 *    Kristin Schmidt <krschmidt@uni-osnabrueck.de>
 *    Jan Philipp Vogtherr <jvogtherr@uni-osnabrueck.de>
 *    Malte kleine Piening <malte@klpiening.de>
 */
 
#ifndef MESH_DISPLAY_HPP
#define MESH_DISPLAY_HPP
 
#include <Types.hpp>
#include <MeshVisual.hpp>
 
#include <vector>
#include <memory>
#include <queue>
 
#include <string>
#include <math.h>
#include <algorithm>
 
#include <QMessageBox>
#include <QApplication>
#include <QIcon>
 
#include <ros/ros.h>
#include <ros/console.h>
#include <rviz/viewport_mouse_event.h>
#include <rviz/visualization_manager.h>
#include <rviz/visualization_frame.h>
#include <rviz/geometry.h>
 
#include <rviz/display_context.h>
#include <rviz/frame_manager.h>
#include <rviz/display.h>
 
#ifndef Q_MOC_RUN
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/cache.h>
 
#include <tf2_ros/message_filter.h>
 
#include <rviz/mesh_loader.h>
 
#include <OGRE/OgreManualObject.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreStringConverter.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreColourValue.h>
 
#endif
 
namespace rviz
{
// Forward declaration
class BoolProperty;
class ColorProperty;
class FloatProperty;
class IntProperty;
class RosTopicProperty;
class EnumProperty;
class StringProperty;
 
}  // End namespace rviz
 
namespace rviz_map_plugin
{
using std::shared_ptr;
using std::string;
using std::unique_ptr;
using std::vector;
 
// Forward declaration
class MeshVisual;
 
class MeshDisplay : public rviz::Display
{
  Q_OBJECT
 
public:
  MeshDisplay();
 
  ~MeshDisplay();
 
  void onEnable();
 
  void onDisable();
 
  void subscribe();
 
  void unsubscribe();
 
  void ignoreIncomingMessages();
 
  void setGeometry(shared_ptr<Geometry> geometry);
 
  void setVertexColors(vector<Color>& vertexColors);
 
  void clearVertexCosts();
 
  void addVertexCosts(std::string costlayer, vector<float>& vertexCosts);
 
  void setVertexNormals(vector<Normal>& vertexNormals);
 
  void setMaterials(vector<Material>& materials, vector<TexCoords>& texCoords);
 
  void addTexture(Texture& texture, uint32_t textureIndex);
 
  void setPose(Ogre::Vector3& position, Ogre::Quaternion& orientation);
 
private Q_SLOTS:
 
  void updateBufferSize();
 
  void updateMesh();
 
  void updateWireframe();
 
  void updateNormals();
 
  void updateNormalsColor();
 
  void updateNormalsSize();
 
  void updateVertexCosts();
 
  void updateVertexColorsTopic();
 
  void updateVertexCostsTopic();
 
  void updateTopic();
 
  void updateVertexColorService();
 
  void updateMaterialAndTextureServices();
 
private:
  void onInitialize();
 
  void initialServiceCall();
 
  void processMessage(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg);
 
  void incomingGeometry(const mesh_msgs::MeshGeometryStamped::ConstPtr& meshMsg);
 
  void incomingVertexColors(const mesh_msgs::MeshVertexColorsStamped::ConstPtr& colorsStamped);
 
  void incomingVertexCosts(const mesh_msgs::MeshVertexCostsStamped::ConstPtr& costsStamped);
 
  void requestVertexColors(std::string uuid);
 
  void requestMaterials(std::string uuid);
 
  void cacheVertexCosts(std::string layer, const std::vector<float>& costs);
 
  std::shared_ptr<MeshVisual> getLatestVisual();
 
  std::shared_ptr<MeshVisual> addNewVisual();
 
  bool m_ignoreMsgs;
 
  ros::ServiceClient m_vertexColorClient;
 
  ros::ServiceClient m_materialsClient;
 
  ros::ServiceClient m_textureClient;
 
  ros::ServiceClient m_uuidClient;
 
  ros::ServiceClient m_geometryClient;
 
  message_filters::Subscriber<mesh_msgs::MeshGeometryStamped> m_meshSubscriber;
 
  message_filters::Subscriber<mesh_msgs::MeshVertexColorsStamped> m_vertexColorsSubscriber;
 
  message_filters::Subscriber<mesh_msgs::MeshVertexCostsStamped> m_vertexCostsSubscriber;
 
  tf2_ros::MessageFilter<mesh_msgs::MeshGeometryStamped>* m_tfMeshFilter;
 
  tf2_ros::MessageFilter<mesh_msgs::MeshVertexColorsStamped>* m_tfVertexColorsFilter;
 
  tf2_ros::MessageFilter<mesh_msgs::MeshVertexCostsStamped>* m_tfVertexCostsFilter;
 
  message_filters::Cache<mesh_msgs::MeshGeometryStamped>* m_meshSynchronizer;
 
  message_filters::Cache<mesh_msgs::MeshVertexColorsStamped>* m_colorsSynchronizer;
 
  message_filters::Cache<mesh_msgs::MeshVertexCostsStamped>* m_costsSynchronizer;
 
  uint32_t m_messagesReceived;
 
  std::string m_lastUuid;
 
  std::queue<std::shared_ptr<MeshVisual>> m_visuals;
 
  rviz::RosTopicProperty* m_meshTopic;
 
  rviz::IntProperty* m_bufferSize;
 
  rviz::EnumProperty* m_displayType;
 
  rviz::ColorProperty* m_facesColor;
 
  rviz::FloatProperty* m_facesAlpha;
 
  rviz::RosTopicProperty* m_vertexColorsTopic;
 
  rviz::StringProperty* m_vertexColorServiceName;
 
  rviz::BoolProperty* m_showTexturedFacesOnly;
 
  rviz::StringProperty* m_materialServiceName;
 
  rviz::StringProperty* m_textureServiceName;
 
  rviz::EnumProperty* m_costColorType;
 
  rviz::RosTopicProperty* m_vertexCostsTopic;
 
  rviz::EnumProperty* m_selectVertexCostMap;
 
  rviz::BoolProperty* m_costUseCustomLimits;
 
  rviz::FloatProperty* m_costLowerLimit;
 
  rviz::FloatProperty* m_costUpperLimit;
 
  rviz::BoolProperty* m_showNormals;
 
  rviz::ColorProperty* m_normalsColor;
 
  rviz::FloatProperty* m_normalsAlpha;
 
  rviz::FloatProperty* m_scalingFactor;
 
  rviz::BoolProperty* m_showWireframe;
 
  rviz::ColorProperty* m_wireframeColor;
 
  rviz::FloatProperty* m_wireframeAlpha;
 
  std::map<std::string, std::vector<float>> m_costCache;
};
 
}  // end namespace rviz_map_plugin
 
#endif