Feat/obstacle height dynamic reconf

CMakeLists.txt

@@ -31,6 +31,7 @@ add_service_files(DIRECTORY msgs
 
 generate_dynamic_reconfigure_options(
   cfg/SpatioTemporalVoxelLayer.cfg
+  cfg/MeasurementBuffer.cfg
 )
 
 generate_messages(

cfg/MeasurementBuffer.cfg

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+PACKAGE = 'spatio_temporal_voxel_layer'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("min_obstacle_height", double_t, 0, "Minimum height of obstacles to detect", 0.0, 0.0, 3.0)
+gen.add("max_obstacle_height", double_t, 0, "Maximum height of obstacles to detect", 0.0, 0.0, 3.0)
+
+exit(gen.generate(PACKAGE, "buffer", "MeasurementBuffer"))

include/spatio_temporal_voxel_layer/measurement_buffer.hpp

@@ -41,6 +41,7 @@
 
 // measurement structs
 #include <spatio_temporal_voxel_layer/measurement_reading.h>
+#include <spatio_temporal_voxel_layer/MeasurementBufferConfig.h>
 // PCL
 #include <pcl_ros/transforms.h>
 #include <pcl/filters/voxel_grid.h>
@@ -53,6 +54,7 @@
 // ROS
 #include <ros/ros.h>
 #include <ros/time.h>
+#include <dynamic_reconfigure/server.h>
 // TF
 #include <tf2_ros/buffer.h>
 #include "message_filters/subscriber.h"
@@ -77,6 +79,8 @@ enum class Filters
 // conveniences for line lengths
 typedef std::list<observation::MeasurementReading>::iterator readings_iter;
 typedef sensor_msgs::PointCloud2::Ptr point_cloud_ptr;
+typedef spatio_temporal_voxel_layer::MeasurementBufferConfig dynamicReconfigureType;
+typedef dynamic_reconfigure::Server<dynamicReconfigureType> dynamicReconfigureServerType;
 
 // Measurement buffer
 class MeasurementBuffer
@@ -105,7 +109,8 @@ class MeasurementBuffer
                     const int& voxel_min_points,            \
                     const bool& enabled,                    \
                     const bool& clear_buffer_after_reading, \
-                    const ModelType& model_type);
+                    const ModelType& model_type,            \
+                    ros::NodeHandle& nh);
 
   ~MeasurementBuffer(void);
 
@@ -133,6 +138,11 @@ class MeasurementBuffer
   // Removing old observations from buffer
   void RemoveStaleObservations(void);
 
+  // Dynamic reconfigure
+  void DynamicReconfigureCallback(spatio_temporal_voxel_layer::MeasurementBufferConfig &config , uint32_t level);
+
+  ros::NodeHandle _node;
+  dynamicReconfigureServerType* _dynamic_reconfigure_server;
   tf2_ros::Buffer& _buffer;
   const ros::Duration _observation_keep_time, _expected_update_rate;
   boost::recursive_mutex _lock;

package.xml

@@ -1,10 +1,11 @@
 <?xml version="1.0"?>
 <package>
   <name>spatio_temporal_voxel_layer</name>
-  <version>1.4.5</version>
+  <version>1.5.0</version>
   <description>The spatio-temporal 3D obstacle costmap package</description>
 
   <maintainer email="[email protected]">Steve Macenski</maintainer>
+  <maintainer email="[email protected]">Roberto Carta</maintainer>
 
  <license>LGPL v2.1</license>
 

src/measurement_buffer.cpp

@@ -66,7 +66,8 @@ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name,          \
                                      const int& voxel_min_points,            \
                                      const bool& enabled,                    \
                                      const bool& clear_buffer_after_reading, \
-                                     const ModelType& model_type) :
+                                     const ModelType& model_type,            \
+                                     ros::NodeHandle& nh) :
 /*****************************************************************************/
     _buffer(tf), _observation_keep_time(observation_keep_time),
     _expected_update_rate(expected_update_rate),_last_updated(ros::Time::now()), 
@@ -79,14 +80,25 @@ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name,          \
     _marking(marking), _clearing(clearing), _voxel_size(voxel_size),
     _filter(filter), _voxel_min_points(voxel_min_points),
     _enabled(enabled), _clear_buffer_after_reading(clear_buffer_after_reading),
-    _model_type(model_type)
+    _model_type(model_type), _node(nh)
 {
+  // Dynamic reconfigure
+  _dynamic_reconfigure_server = new dynamicReconfigureServerType(_node);
+  dynamicReconfigureServerType::CallbackType f;
+  f = boost::bind(&MeasurementBuffer::DynamicReconfigureCallback, this, _1, _2);
+  _dynamic_reconfigure_server->setCallback(f);
+
 }
 
 /*****************************************************************************/
 MeasurementBuffer::~MeasurementBuffer(void)
 /*****************************************************************************/
 {
+  if(_dynamic_reconfigure_server)
+  {
+    delete _dynamic_reconfigure_server;
+    _dynamic_reconfigure_server = nullptr;
+  }
 }
 
 /*****************************************************************************/
@@ -237,6 +249,12 @@ void MeasurementBuffer::RemoveStaleObservations(void)
   }
 }
 
+void MeasurementBuffer::DynamicReconfigureCallback(spatio_temporal_voxel_layer::MeasurementBufferConfig &config , uint32_t level)
+{
+  _min_obstacle_height = config.min_obstacle_height;
+  _max_obstacle_height = config.max_obstacle_height;
+}
+
 /*****************************************************************************/
 void MeasurementBuffer::ResetAllMeasurements(void)
 /*****************************************************************************/

src/spatio_temporal_voxel_layer.cpp

@@ -81,7 +81,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
   nh.param("observation_sources", topics_string, std::string(""));
   // timeout in seconds for transforms
   nh.param("transform_tolerance", transform_tolerance, 0.2);
-  // whether to default on
+  // whether to default on 
   nh.param("enabled", _enabled, true);
   enabled_ = _enabled; // costmap_2d for some unexplicable reason uses globals
   // publish the voxel grid to visualize
@@ -230,7 +230,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
         transform_tolerance, min_z, max_z, vFOV, vFOVPadding, hFOV,       \
         decay_acceleration, marking, clearing, _voxel_size,               \
         filter, voxel_min_points, enabled, clear_after_reading,           \
-        model_type)));
+        model_type, source_node)));
 
     // Add buffer to marking observation buffers
     if (marking == true)