Bugfix/range sensor buffer

.gitignore

@@ -1 +1,3 @@
 build/
+.idea
+cmake-build-debug

range_sensor_layer/CMakeLists.txt

@@ -2,6 +2,9 @@ cmake_minimum_required(VERSION 2.8.3)
 project(range_sensor_layer)
 set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror")
 
+# Required to use unordered maps
+add_definitions("-std=c++11")
+
 find_package(catkin REQUIRED COMPONENTS
     angles
     costmap_2d

range_sensor_layer/cfg/RangeSensorLayer.cfg

@@ -9,6 +9,7 @@ gen.add('enabled',               bool_t, 0, 'Whether to apply this plugin or not
 gen.add('phi',                 double_t, 0, 'Phi value', 1.2)
 gen.add('inflate_cone',        double_t, 0, 'Inflate the triangular area covered by the sensor (percentage)', 1, 0, 1)
 gen.add('no_readings_timeout', double_t, 0, 'No Readings Timeout', 0.0, 0.0)
+gen.add('ranges_buffer_size',     int_t, 0, 'Maximum number of ranges per topic to buffer until processing', 1, 1, 50)
 gen.add('clear_threshold',     double_t, 0, 'Probability below which cells are marked as free', 0.2, 0.0, 1.0)
 gen.add('mark_threshold',      double_t, 0, 'Probability above which cells are marked as occupied', 0.8, 0.0, 1.0)
 gen.add('clear_on_max_reading',  bool_t, 0, 'Clear on max reading', False)

range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h

@@ -1,17 +1,19 @@
 // Copyright 2018 David V. Lu!!
 #ifndef RANGE_SENSOR_LAYER_RANGE_SENSOR_LAYER_H_
 #define RANGE_SENSOR_LAYER_RANGE_SENSOR_LAYER_H_
+
+#include <list>
+#include <deque>
+#include <string>
+#include <vector>
+#include <unordered_map>
+
 #include <ros/ros.h>
 #include <costmap_2d/costmap_layer.h>
 #include <costmap_2d/layered_costmap.h>
 #include <sensor_msgs/Range.h>
 #include <range_sensor_layer/RangeSensorLayerConfig.h>
 #include <dynamic_reconfigure/server.h>
-#include <list>
-#include <map>
-#include <string>
-#include <utility>
-#include <vector>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 namespace range_sensor_layer
@@ -40,7 +42,7 @@ class RangeSensorLayer : public costmap_2d::CostmapLayer
 private:
   void reconfigureCB(range_sensor_layer::RangeSensorLayerConfig &config, uint32_t level);
 
-  void bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& range_message);
+  void bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& range_message, const std::string& topic);
   void processRangeMsg(sensor_msgs::Range& range_message);
   void processFixedRangeMsg(sensor_msgs::Range& range_message);
   void processVariableRangeMsg(sensor_msgs::Range& range_message);
@@ -68,7 +70,8 @@ class RangeSensorLayer : public costmap_2d::CostmapLayer
 
   boost::function<void(sensor_msgs::Range& range_message)> processRangeMessageFunc_;
   boost::mutex range_message_mutex_;
-  std::list<sensor_msgs::Range> range_msgs_buffer_;
+  unsigned int range_msgs_buffer_size_;
+  std::unordered_map<std::string, std::list<sensor_msgs::Range>> range_msgs_buffers_;
   std::map<std::pair<unsigned int, unsigned int>, double> marked_point_history_;
 
   double max_angle_, phi_v_;

range_sensor_layer/src/range_sensor_layer.cpp

@@ -44,10 +44,9 @@ void RangeSensorLayer::onInitialize()
   InputSensorType input_sensor_type = ALL;
   std::string sensor_type_name;
   nh.param("input_sensor_type", sensor_type_name, std::string("ALL"));
-
   nh.param("use_decay", use_decay_, false);
   nh.param("pixel_decay", pixel_decay_, 10.0);
-  nh.param("transform_tolerance_", transform_tolerance_, 0.3);
+  nh.param("transform_tolerance", transform_tolerance_, 0.3);
 
   boost::to_upper(sensor_type_name);
   ROS_INFO("%s: %s as input_sensor_type given", name_.c_str(), sensor_type_name.c_str());
@@ -103,7 +102,9 @@ void RangeSensorLayer::onInitialize()
           name_.c_str(), sensor_type_name.c_str());
       }
 
-      range_subs_.push_back(nh.subscribe(topic_name, 100, &RangeSensorLayer::bufferIncomingRangeMsg, this));
+      range_subs_.push_back(nh.subscribe<sensor_msgs::Range>(topic_name, 100,
+                                                             boost::bind(&RangeSensorLayer::bufferIncomingRangeMsg,
+                                                                         this, _1, topic_name)));
 
       ROS_INFO("RangeSensorLayer: subscribed to topic %s", range_subs_.back().getTopic().c_str());
     }
@@ -176,27 +177,41 @@ void RangeSensorLayer::reconfigureCB(range_sensor_layer::RangeSensorLayerConfig
     enabled_ = config.enabled;
     current_ = false;
   }
+
+  if (range_msgs_buffer_size_ > static_cast<size_t>(config.ranges_buffer_size))
+  {
+    boost::mutex::scoped_lock lock(range_message_mutex_);
+    for (auto& range_topic: range_msgs_buffers_)
+    {
+      while (range_topic.second.size() > static_cast<size_t>(config.ranges_buffer_size))
+        range_topic.second.pop_front();
+    }
+  }
+  range_msgs_buffer_size_ = config.ranges_buffer_size;
 }
 
-void RangeSensorLayer::bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& range_message)
+void RangeSensorLayer::bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& range_message,
+                                              const std::string& topic)
 {
   boost::mutex::scoped_lock lock(range_message_mutex_);
-  range_msgs_buffer_.push_back(*range_message);
+  range_msgs_buffers_[topic].push_back(*range_message);
+  if (range_msgs_buffers_[topic].size() > range_msgs_buffer_size_)
+    range_msgs_buffers_[topic].pop_front();
 }
 
 void RangeSensorLayer::updateCostmap()
 {
-  std::list<sensor_msgs::Range> range_msgs_buffer_copy;
-
+  std::list<sensor_msgs::Range> range_msgs_to_update;
   range_message_mutex_.lock();
-  range_msgs_buffer_copy = std::list<sensor_msgs::Range>(range_msgs_buffer_);
-  range_msgs_buffer_.clear();
+  for (auto& range_topic: range_msgs_buffers_)
+  {
+    range_msgs_to_update.splice(range_msgs_to_update.end(), range_topic.second);
+  }
   range_message_mutex_.unlock();
 
-  for (std::list<sensor_msgs::Range>::iterator range_msgs_it = range_msgs_buffer_copy.begin();
-       range_msgs_it != range_msgs_buffer_copy.end(); range_msgs_it++)
+  for (auto& range_msg : range_msgs_to_update)
   {
-    processRangeMessageFunc_(*range_msgs_it);
+    processRangeMessageFunc_(range_msg);
   }
 }
 
@@ -514,12 +529,14 @@ void RangeSensorLayer::reset()
 
 void RangeSensorLayer::deactivate()
 {
-  range_msgs_buffer_.clear();
+  boost::mutex::scoped_lock lock(range_message_mutex_);
+  range_msgs_buffers_.clear();
 }
 
 void RangeSensorLayer::activate()
 {
-  range_msgs_buffer_.clear();
+  boost::mutex::scoped_lock lock(range_message_mutex_);
+  range_msgs_buffers_.clear();
 }
 
 }  // namespace range_sensor_layer