post fixes and cleanup

Author: amock

mesh_layers/src/inflation_layer.cpp

@@ -364,7 +364,7 @@ void InflationLayer::waveCostInflation(
   }
 
   // The LVR2 PMP wrapper
-  const auto mesh = std::dynamic_pointer_cast<lvr2::PMPMesh<mesh_map::Vector>>(map->mesh());
+  const std::shared_ptr<lvr2::PMPMesh<mesh_map::Vector> > mesh = map->mesh();
   if (nullptr == mesh)
   {
     RCLCPP_ERROR(get_logger(), "Failed to dynamic_cast mesh to lvr2::PMPMesh! Currently only the lvr2::PMPMesh backend is supported!");

mesh_map/include/mesh_map/abstract_layer.h

@@ -35,21 +35,20 @@
  *
  */
 
+#ifndef MESH_MAP__ABSTRACT_LAYER_H
+#define MESH_MAP__ABSTRACT_LAYER_H
+
 #include <functional>
 #include <memory>
 
 #include <boost/optional.hpp>
 #include <lvr2/io/AttributeMeshIOBase.hpp>
 #include <rclcpp/node.hpp>
 
-#ifndef MESH_MAP__ABSTRACT_LAYER_H
-#define MESH_MAP__ABSTRACT_LAYER_H
+#include <mesh_map/definitions.h>
+
 namespace mesh_map
 {
-class MeshMap;
-
-typedef lvr2::BaseVector<float> Vector;
-typedef lvr2::Normal<float> Normal;
 
 typedef std::function<void(const std::string&, const rclcpp::Time&, const std::set<lvr2::VertexHandle>&)> notify_func;
 
@@ -126,12 +125,12 @@ class AbstractLayer
    * @param barycentric_coords The thee barycentric coordinates.
    * @return The vector for the given barycentric coordinates with respect to the corresponding triangle. Default is an vertex with length 0.
    */
-  virtual lvr2::BaseVector<float> vectorAt(const std::array<lvr2::VertexHandle, 3>& vertices,
+  virtual Vector vectorAt(const std::array<lvr2::VertexHandle, 3>& vertices,
                                            const std::array<float, 3>& barycentric_coords)
   {
     (void) vertices;
     (void) barycentric_coords;
-    return lvr2::BaseVector<float>();
+    return Vector();
   }
 
   /**
@@ -140,7 +139,7 @@ class AbstractLayer
    * the vector field into the mesh map.
    * @return an optional vector map.
    */
-  virtual const boost::optional<lvr2::VertexMap<lvr2::BaseVector<float>>&> vectorMap()
+  virtual const boost::optional<lvr2::VertexMap<Vector>&> vectorMap()
   {
     return boost::none;
   }
@@ -149,10 +148,10 @@ class AbstractLayer
    * @brief Optional method if the layer computes vectors. Computes a vector for a given vertex handle
    * @return a vector for the given vertex. Default is an vertex with length 0.
    */
-  virtual lvr2::BaseVector<float> vectorAt(const lvr2::VertexHandle& vertex)
+  virtual Vector vectorAt(const lvr2::VertexHandle& vertex)
   {
     (void) vertex;
-    return lvr2::BaseVector<float>();
+    return Vector();
   }
 
   /**

mesh_map/include/mesh_map/definitions.h

@@ -0,0 +1,22 @@
+#ifndef MESH_MAP__DEFINITIONS_H
+#define MESH_MAP__DEFINITIONS_H
+
+#include <lvr2/geometry/BaseVector.hpp>
+#include <lvr2/geometry/Normal.hpp>
+#include <lvr2/geometry/PMPMesh.hpp>
+
+namespace mesh_map
+{
+
+using Normal = lvr2::Normal<float>;
+using Vector = lvr2::BaseVector<float>;
+
+class MeshMap;
+class AbstractLayer;
+class LayerManager;
+class NanoFlannMeshAdaptor;
+class LayerTimer;
+
+} // namespace mesh_map
+
+#endif // MESH_MAP__DEFINITIONS_H

mesh_map/include/mesh_map/layer_manager.h

@@ -35,7 +35,8 @@
  *
  */
 
-#pragma once
+#ifndef MESH_MAP__LAYER_MANAGER_H
+#define MESH_MAP__LAYER_MANAGER_H
 
 #include <boost/graph/adjacency_list.hpp>
 #include <pluginlib/class_loader.hpp>
@@ -44,13 +45,11 @@
 #include <lvr2/geometry/Handles.hpp>
 
 #include <mesh_map/abstract_layer.h>
+#include <mesh_map/definitions.h>
 
 namespace mesh_map
 {
 
-class AbstractLayer;
-class MeshMap;
-
 class LayerManager
 {
 public:
@@ -166,3 +165,5 @@ class LayerManager
 };
 
 } /* namespace mesh_map */
+
+#endif // MESH_MAP__LAYER_MANAGER_H

mesh_map/include/mesh_map/mesh_map.h

@@ -61,14 +61,16 @@
 #include <lvr2/algorithm/raycasting/RaycasterBase.hpp>
 #include <lvr2/geometry/BaseVector.hpp>
 #include <lvr2/io/AttributeMeshIOBase.hpp>
-#include <lvr2/geometry/BaseMesh.hpp>
+#include <lvr2/geometry/PMPMesh.hpp>
 
 #include "nanoflann.hpp"
 #include "nanoflann_mesh_adaptor.h"
 
 namespace mesh_map
 {
-class MeshMap : public std::enable_shared_from_this<MeshMap>
+
+class MeshMap 
+: public std::enable_shared_from_this<MeshMap>
 {
 public:
   inline static const std::string MESH_MAP_NAMESPACE = "mesh_map";
@@ -212,7 +214,7 @@ class MeshMap : public std::enable_shared_from_this<MeshMap>
    * @param barycentric_coords The barycentric coordinates of the query position.
    * @return An optional vector of the computed direction. It is valid if a vector has been computed successfully.
    */
-  boost::optional<Vector> directionAtPosition(const lvr2::VertexMap<lvr2::BaseVector<float>>& vector_map,
+  boost::optional<Vector> directionAtPosition(const lvr2::VertexMap<Vector>& vector_map,
                                               const std::array<lvr2::VertexHandle, 3>& vertices,
                                               const std::array<float, 3>& barycentric_coords);
 
@@ -269,7 +271,7 @@ class MeshMap : public std::enable_shared_from_this<MeshMap>
   /**
    * @brief Returns the stored mesh
    */
-  std::shared_ptr<lvr2::BaseMesh<Vector> > mesh()
+  std::shared_ptr<lvr2::PMPMesh<Vector> > mesh()
   {
     return mesh_ptr;
   }
@@ -399,7 +401,7 @@ class MeshMap : public std::enable_shared_from_this<MeshMap>
    * @param vector_map The vector field to publish
    * @param publish_face_vectors Enables to publish an additional vertex for the triangle's center.
    */
-  void publishVectorField(const std::string& name, const lvr2::DenseVertexMap<lvr2::BaseVector<float>>& vector_map,
+  void publishVectorField(const std::string& name, const lvr2::DenseVertexMap<Vector>& vector_map,
                           const bool publish_face_vectors = false);
 
   /**
@@ -410,7 +412,7 @@ class MeshMap : public std::enable_shared_from_this<MeshMap>
    * @param cost_function A cost function to compute costs inside a triangle
    * @param publish_face_vectors Enables to publish an additional vertex for the triangle's center.
    */
-  void publishVectorField(const std::string& name, const lvr2::DenseVertexMap<lvr2::BaseVector<float>>& vector_map,
+  void publishVectorField(const std::string& name, const lvr2::DenseVertexMap<Vector>& vector_map,
                           const lvr2::DenseVertexMap<float>& values,
                           const std::function<float(float)>& cost_function = {},
                           const bool publish_face_vectors = false);
@@ -453,8 +455,7 @@ class MeshMap : public std::enable_shared_from_this<MeshMap>
   //! However we could also implement a server connection here
   //! We might use the pluginlib for that
   std::shared_ptr<lvr2::AttributeMeshIOBase> mesh_io_ptr;
-  std::shared_ptr<lvr2::BaseMesh<Vector>> mesh_ptr;
-  std::string hem_impl_;
+  std::shared_ptr<lvr2::PMPMesh<Vector> >    mesh_ptr;
 
 private:
 

mesh_map/include/mesh_map/nanoflann_mesh_adaptor.h

@@ -38,16 +38,21 @@
 #ifndef MESH_MAP__NANOFLANN_MESH_ADAPTOR_H
 #define MESH_MAP__NANOFLANN_MESH_ADAPTOR_H
 
-#include <lvr2/geometry/BaseMesh.hpp>
+#include <lvr2/geometry/PMPMesh.hpp>
 #include "nanoflann.hpp"
+#include <mesh_map/util.h>
+
+namespace mesh_map
+{
 
-namespace mesh_map{
 struct NanoFlannMeshAdaptor
 {
-  const std::shared_ptr<lvr2::BaseMesh<lvr2::BaseVector<float>>> mesh;
+  const std::shared_ptr<lvr2::PMPMesh<Vector> > mesh;
 
   /// The constructor that sets the data set source
-  NanoFlannMeshAdaptor(const std::shared_ptr<lvr2::BaseMesh<lvr2::BaseVector<float>>> mesh) : mesh(mesh) { }
+  NanoFlannMeshAdaptor(const std::shared_ptr<lvr2::PMPMesh<Vector> > mesh) 
+  :mesh(mesh) 
+  { }
 
   inline lvr2::Index kdtree_get_point_count() const { return mesh->nextVertexIndex(); }
 
@@ -56,7 +61,7 @@ struct NanoFlannMeshAdaptor
     const lvr2::VertexHandle vH(idx);
     if(mesh->containsVertex(vH))
     {
-      const lvr2::BaseVector<float>& vertex = mesh->getVertexPosition(vH);
+      const Vector& vertex = mesh->getVertexPosition(vH);
       if (dim == 0) return vertex.x;
       else if (dim == 1) return vertex.y;
       else return vertex.z;
@@ -68,6 +73,7 @@ struct NanoFlannMeshAdaptor
   bool kdtree_get_bbox(BBOX& /*bb*/) const { return false; }
 
 }; // end of PointCloudAdaptor
-}
+
+} // namespace mesh_map
 
 #endif /* MESH_MAP__NANOFLANN_MESH_ADAPTOR_H */

mesh_map/include/mesh_map/util.h

@@ -49,16 +49,14 @@
 #include <tf2/LinearMath/Vector3.h>
 
 #include <lvr2/types/MeshBuffer.hpp>
+#include <lvr2/geometry/PMPMesh.hpp>
 #include <assimp/scene.h>
 
+#include <mesh_map/definitions.h>
+
 namespace mesh_map
 {
 
-//! use normals with datatype float
-typedef lvr2::Normal<float> Normal;
-
-//! use vectors with datatype folat
-typedef lvr2::BaseVector<float> Vector;
 
 
 lvr2::MeshBufferPtr extractMeshByName(const aiScene* ascene, std::string name);