Feature dyn rec

elevation_mapping/CMakeLists.txt

@@ -24,6 +24,11 @@ find_package(catkin REQUIRED COMPONENTS
 find_package(Boost REQUIRED COMPONENTS system)
 find_package(Eigen3 REQUIRED)
 
+
+generate_dynamic_reconfigure_options(
+  cfg/ElevationMapPlugin.cfg
+)
+
 ###################################
 ## catkin specific configuration ##
 ###################################
@@ -61,6 +66,8 @@ catkin_package(
 ## Build ##
 ###########
 
+
+
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(

elevation_mapping/cfg/ElevationMapPlugin.cfg

@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+PACKAGE = "elevation_mapping"
+
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t
+
+gen = ParameterGenerator()
+gen.add("length_in_x",int_t, 0, "map length in x", 10,0,20)
+gen.add("length_in_y",int_t, 0, "map length in y", 10,0,20)
+gen.add("position_x",int_t, 0, "robot pose in x", 0,-10,10)
+gen.add("position_y",int_t, 0, "robot pose in y", 0,-10,10)
+gen.add("resolution",double_t, 0, "robot pose in y", 0.01,0.01,0.4)
+
+gen.add("min_variance",double_t, 0, "min_variance",0.0001,0.0,0.004)
+gen.add("max_variance",double_t, 0, "max_variance", 10000.0,1.0,100000.0)
+gen.add("mahalanobis_distance_threshold",double_t, 0, "mahalanobis_distance_threshold", 0.1,0.0,4.0)
+gen.add("multi_height_noise",double_t, 0, "multi_height_noise", 0.00002,0.0,0.4)
+
+
+exit(gen.generate("elevation_mapping", "elevation_mapping", "ElevationMapPlugin"))
+
+

elevation_mapping/include/elevation_mapping/ElevationMapping.hpp

@@ -38,6 +38,9 @@
 // Boost
 #include <boost/thread.hpp>
 
+// Dynamic Reconfigure
+#include <dynamic_reconfigure/server.h>
+#include <elevation_mapping/ElevationMapPluginConfig.h>
 
 namespace elevation_mapping {
 
@@ -121,6 +124,17 @@ class ElevationMapping
    */
   bool saveMap(grid_map_msgs::ProcessFile::Request& request, grid_map_msgs::ProcessFile::Response& response);
 
+ protected:
+   /**
+   * set up the dynamic reconfigure
+   * @param nh
+   */
+  virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
+
+
+  ///< @brief dynamic reconfigure server
+  std::unique_ptr<dynamic_reconfigure::Server<elevation_mapping::ElevationMapPluginConfig> > dsrv_;
+
  private:
 
   /*!
@@ -259,6 +273,10 @@ class ElevationMapping
 
   //! Becomes true when corresponding poses and point clouds can be found
   bool receivedFirstMatchingPointcloudAndPose_;
+
+  // Dynamic reconf 
+  void reconfigureCB(elevation_mapping::ElevationMapPluginConfig& config, uint32_t level);
+
 };
 
 } /* namespace */

elevation_mapping/src/ElevationMapping.cpp

@@ -201,6 +201,9 @@ bool ElevationMapping::readParameters()
   if (!sensorProcessor_->readParameters()) return false;
   if (!robotMotionMapUpdater_.readParameters()) return false;
 
+  dsrv_ = nullptr;
+  setupDynamicReconfigure(nodeHandle_);
+
   return true;
 }
 
@@ -494,4 +497,36 @@ void ElevationMapping::stopMapUpdateTimer()
   mapUpdateTimer_.stop();
 }
 
+  void ElevationMapping::setupDynamicReconfigure(ros::NodeHandle &nh_)
+  {
+    dsrv_.reset(new dynamic_reconfigure::Server<elevation_mapping::ElevationMapPluginConfig>(nh_));
+    dynamic_reconfigure::Server<elevation_mapping::ElevationMapPluginConfig>::CallbackType cb =
+        boost::bind(&ElevationMapping::reconfigureCB, this, _1, _2);
+    dsrv_->setCallback(cb);
+  }
+
+  void ElevationMapping::reconfigureCB(elevation_mapping::ElevationMapPluginConfig &config, uint32_t level)
+  {
+    // enabled_ = config.enabled;
+    // edges_sharpness_threshold_ = config.edge_sharpness;
+  grid_map::Length length;
+  grid_map::Position position;
+  double resolution;
+  length.x() = config.length_in_x;
+  length.y() = config.length_in_y;
+  position.x() = config.position_x;
+  position.y() = config.position_y;
+  resolution = config.resolution;
+
+  map_.setGeometry(length, resolution, position);
+
+  map_.minVariance_ = config.min_variance;
+  map_.maxVariance_ = config.max_variance;
+  map_.mahalanobisDistanceThreshold_ = config.mahalanobis_distance_threshold;
+  map_.multiHeightNoise_ = config.multi_height_noise;
+  map_.minHorizontalVariance_ = pow(resolution / 2.0, 2);
+
+  }
+
+
 } /* namespace */