From f22dac42b91e27255ad4781160fe3ac080474393 Mon Sep 17 00:00:00 2001 From: corot Date: Sun, 14 Jun 2015 16:51:14 +0200 Subject: [PATCH 1/9] Add eclipse project files --- range_sensor_layer/.cproject | 42 ++++++++++++++++++++++++++++++++++++ range_sensor_layer/.project | 27 +++++++++++++++++++++++ 2 files changed, 69 insertions(+) create mode 100644 range_sensor_layer/.cproject create mode 100644 range_sensor_layer/.project diff --git a/range_sensor_layer/.cproject b/range_sensor_layer/.cproject new file mode 100644 index 0000000..4876aab --- /dev/null +++ b/range_sensor_layer/.cproject @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/range_sensor_layer/.project b/range_sensor_layer/.project new file mode 100644 index 0000000..01346cf --- /dev/null +++ b/range_sensor_layer/.project @@ -0,0 +1,27 @@ + + + range_sensor_layer + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + + From 08dff098dc5615768967bbe37b8d60cefb526ef6 Mon Sep 17 00:00:00 2001 From: Jorge Santos Date: Mon, 2 May 2016 09:01:31 +0200 Subject: [PATCH 2/9] Issue #28: buffer range messages for each incoming topic and with a maximum, configurable, buffer size --- range_sensor_layer/CMakeLists.txt | 3 ++ range_sensor_layer/cfg/RangeSensorLayer.cfg | 1 + .../range_sensor_layer/range_sensor_layer.h | 2 +- range_sensor_layer/src/range_sensor_layer.cpp | 38 +++++++++++++++++-- 4 files changed, 39 insertions(+), 5 deletions(-) diff --git a/range_sensor_layer/CMakeLists.txt b/range_sensor_layer/CMakeLists.txt index f90a565..d56645a 100644 --- a/range_sensor_layer/CMakeLists.txt +++ b/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 diff --git a/range_sensor_layer/cfg/RangeSensorLayer.cfg b/range_sensor_layer/cfg/RangeSensorLayer.cfg index 58dcc96..82c4fdb 100755 --- a/range_sensor_layer/cfg/RangeSensorLayer.cfg +++ b/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) diff --git a/range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h b/range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h index 11db46d..bf44ed4 100644 --- a/range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h +++ b/range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h @@ -40,7 +40,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); diff --git a/range_sensor_layer/src/range_sensor_layer.cpp b/range_sensor_layer/src/range_sensor_layer.cpp index 35c6bc6..12e87ac 100644 --- a/range_sensor_layer/src/range_sensor_layer.cpp +++ b/range_sensor_layer/src/range_sensor_layer.cpp @@ -41,6 +41,10 @@ void RangeSensorLayer::onInitialize() nh.param("ns", topics_ns, std::string()); nh.param("topics", topic_names, topic_names); + int size; + nh.param("ranges_buffer_size", size, 1); + range_msgs_buffer_size_ = size; + InputSensorType input_sensor_type = ALL; std::string sensor_type_name; nh.param("input_sensor_type", sensor_type_name, std::string("ALL")); @@ -103,7 +107,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(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,12 +182,27 @@ void RangeSensorLayer::reconfigureCB(range_sensor_layer::RangeSensorLayerConfig enabled_ = config.enabled; current_ = false; } + + if (config.ranges_buffer_size < range_msgs_buffer_size_) + { + boost::mutex::scoped_lock lock(range_message_mutex_); + std::unordered_map>::iterator buffer_it; + for(buffer_it = range_msgs_buffers_.begin(); buffer_it != range_msgs_buffers_.end(); buffer_it++) + { + while (buffer_it->second.size() > config.ranges_buffer_size) + buffer_it->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() @@ -196,7 +217,16 @@ void RangeSensorLayer::updateCostmap() for (std::list::iterator range_msgs_it = range_msgs_buffer_copy.begin(); range_msgs_it != range_msgs_buffer_copy.end(); range_msgs_it++) { - processRangeMessageFunc_(*range_msgs_it); + range_message_mutex_.lock(); + std::list range_msgs_buffer_copy = std::list(buffer_it->second); + buffer_it->second.clear(); + range_message_mutex_.unlock(); + + for (std::list::iterator range_msgs_it = range_msgs_buffer_copy.begin(); + range_msgs_it != range_msgs_buffer_copy.end(); range_msgs_it++) + { + processRangeMessageFunc_(*range_msgs_it); + } } } From 77e2ea0d854074afccfdc289af72f6c6a2a772c5 Mon Sep 17 00:00:00 2001 From: corot Date: Thu, 12 Oct 2017 23:03:19 +0200 Subject: [PATCH 3/9] Use C++11 stuff and replace lists by deques --- range_sensor_layer/src/range_sensor_layer.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/range_sensor_layer/src/range_sensor_layer.cpp b/range_sensor_layer/src/range_sensor_layer.cpp index 12e87ac..8f76075 100644 --- a/range_sensor_layer/src/range_sensor_layer.cpp +++ b/range_sensor_layer/src/range_sensor_layer.cpp @@ -186,11 +186,10 @@ void RangeSensorLayer::reconfigureCB(range_sensor_layer::RangeSensorLayerConfig if (config.ranges_buffer_size < range_msgs_buffer_size_) { boost::mutex::scoped_lock lock(range_message_mutex_); - std::unordered_map>::iterator buffer_it; - for(buffer_it = range_msgs_buffers_.begin(); buffer_it != range_msgs_buffers_.end(); buffer_it++) + for(auto& range_topic: range_msgs_buffers_) { - while (buffer_it->second.size() > config.ranges_buffer_size) - buffer_it->second.pop_front(); + while (range_topic.second.size() > config.ranges_buffer_size) + range_topic.second.pop_front(); } } range_msgs_buffer_size_ = config.ranges_buffer_size; @@ -199,7 +198,6 @@ void RangeSensorLayer::reconfigureCB(range_sensor_layer::RangeSensorLayerConfig void RangeSensorLayer::bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& range_message, const std::string& topic) { - boost::mutex::scoped_lock lock(range_message_mutex_); range_msgs_buffers_[topic].push_back(*range_message); if (range_msgs_buffers_[topic].size() > range_msgs_buffer_size_) range_msgs_buffers_[topic].pop_front(); @@ -218,14 +216,13 @@ void RangeSensorLayer::updateCostmap() range_msgs_it != range_msgs_buffer_copy.end(); range_msgs_it++) { range_message_mutex_.lock(); - std::list range_msgs_buffer_copy = std::list(buffer_it->second); - buffer_it->second.clear(); + auto range_msgs_buffer_copy = range_topic.second; + range_topic.second.clear(); range_message_mutex_.unlock(); - for (std::list::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_buffer_copy) { - processRangeMessageFunc_(*range_msgs_it); + processRangeMessageFunc_(range_msg); } } } From 897e33413951b602592ace2ec5a626c88968bde3 Mon Sep 17 00:00:00 2001 From: corot Date: Tue, 7 Apr 2020 16:06:39 +0900 Subject: [PATCH 4/9] Lock range_message_mutex_ on adding new messages --- range_sensor_layer/src/range_sensor_layer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/range_sensor_layer/src/range_sensor_layer.cpp b/range_sensor_layer/src/range_sensor_layer.cpp index 8f76075..81066fe 100644 --- a/range_sensor_layer/src/range_sensor_layer.cpp +++ b/range_sensor_layer/src/range_sensor_layer.cpp @@ -198,6 +198,7 @@ void RangeSensorLayer::reconfigureCB(range_sensor_layer::RangeSensorLayerConfig void RangeSensorLayer::bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& range_message, const std::string& topic) { + boost::mutex::scoped_lock lock(range_message_mutex_); range_msgs_buffers_[topic].push_back(*range_message); if (range_msgs_buffers_[topic].size() > range_msgs_buffer_size_) range_msgs_buffers_[topic].pop_front(); From 79ed94ab5feb6f2214b4f246cae79fa8c7292bd8 Mon Sep 17 00:00:00 2001 From: corot Date: Tue, 7 Apr 2020 19:03:20 +0900 Subject: [PATCH 5/9] Fix compilation errors --- range_sensor_layer/src/range_sensor_layer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/range_sensor_layer/src/range_sensor_layer.cpp b/range_sensor_layer/src/range_sensor_layer.cpp index 81066fe..d6a34e9 100644 --- a/range_sensor_layer/src/range_sensor_layer.cpp +++ b/range_sensor_layer/src/range_sensor_layer.cpp @@ -183,12 +183,12 @@ void RangeSensorLayer::reconfigureCB(range_sensor_layer::RangeSensorLayerConfig current_ = false; } - if (config.ranges_buffer_size < range_msgs_buffer_size_) + if (range_msgs_buffer_size_ > static_cast(config.ranges_buffer_size)) { boost::mutex::scoped_lock lock(range_message_mutex_); for(auto& range_topic: range_msgs_buffers_) { - while (range_topic.second.size() > config.ranges_buffer_size) + while (range_topic.second.size() > static_cast(config.ranges_buffer_size)) range_topic.second.pop_front(); } } @@ -542,12 +542,12 @@ void RangeSensorLayer::reset() void RangeSensorLayer::deactivate() { - range_msgs_buffer_.clear(); + range_msgs_buffers_.clear(); } void RangeSensorLayer::activate() { - range_msgs_buffer_.clear(); + range_msgs_buffers_.clear(); } } // namespace range_sensor_layer From ec5ca981682cdbb765def6689971cda2396ffc6a Mon Sep 17 00:00:00 2001 From: corot Date: Tue, 25 May 2021 12:13:15 +0900 Subject: [PATCH 6/9] Fix memory crashed due to copying deques --- .../range_sensor_layer/range_sensor_layer.h | 15 +++++++++------ range_sensor_layer/src/range_sensor_layer.cpp | 16 +++------------- 2 files changed, 12 insertions(+), 19 deletions(-) diff --git a/range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h b/range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h index bf44ed4..be7e8f7 100644 --- a/range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h +++ b/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 +#include +#include +#include +#include + #include #include #include #include #include #include -#include -#include -#include -#include -#include #include namespace range_sensor_layer @@ -68,7 +70,8 @@ class RangeSensorLayer : public costmap_2d::CostmapLayer boost::function processRangeMessageFunc_; boost::mutex range_message_mutex_; - std::list range_msgs_buffer_; + size_t range_msgs_buffer_size_; + std::unordered_map> range_msgs_buffers_; std::map, double> marked_point_history_; double max_angle_, phi_v_; diff --git a/range_sensor_layer/src/range_sensor_layer.cpp b/range_sensor_layer/src/range_sensor_layer.cpp index d6a34e9..9d12b43 100644 --- a/range_sensor_layer/src/range_sensor_layer.cpp +++ b/range_sensor_layer/src/range_sensor_layer.cpp @@ -41,13 +41,10 @@ void RangeSensorLayer::onInitialize() nh.param("ns", topics_ns, std::string()); nh.param("topics", topic_names, topic_names); - int size; - nh.param("ranges_buffer_size", size, 1); - range_msgs_buffer_size_ = size; - InputSensorType input_sensor_type = ALL; std::string sensor_type_name; nh.param("input_sensor_type", sensor_type_name, std::string("ALL")); + nh.param("ranges_buffer_size", (int&)range_msgs_buffer_size_, 1); nh.param("use_decay", use_decay_, false); nh.param("pixel_decay", pixel_decay_, 10.0); @@ -207,17 +204,10 @@ void RangeSensorLayer::bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& void RangeSensorLayer::updateCostmap() { std::list range_msgs_buffer_copy; - - range_message_mutex_.lock(); - range_msgs_buffer_copy = std::list(range_msgs_buffer_); - range_msgs_buffer_.clear(); - range_message_mutex_.unlock(); - - for (std::list::iterator range_msgs_it = range_msgs_buffer_copy.begin(); - range_msgs_it != range_msgs_buffer_copy.end(); range_msgs_it++) + for (auto& range_topic: range_msgs_buffers_) { range_message_mutex_.lock(); - auto range_msgs_buffer_copy = range_topic.second; + range_msgs_buffer_copy = std::list(range_topic.second); range_topic.second.clear(); range_message_mutex_.unlock(); From 1f1f333df16db2fa1f7a530eef0cb1e1e8945023 Mon Sep 17 00:00:00 2001 From: corot Date: Tue, 25 May 2021 12:15:05 +0900 Subject: [PATCH 7/9] Add CLion dirs to gitignore and remove old eclipse files --- .gitignore | 2 ++ range_sensor_layer/.cproject | 42 ------------------------------------ range_sensor_layer/.project | 27 ----------------------- 3 files changed, 2 insertions(+), 69 deletions(-) delete mode 100644 range_sensor_layer/.cproject delete mode 100644 range_sensor_layer/.project diff --git a/.gitignore b/.gitignore index 567609b..a5268ce 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ build/ +.idea +cmake-build-debug diff --git a/range_sensor_layer/.cproject b/range_sensor_layer/.cproject deleted file mode 100644 index 4876aab..0000000 --- a/range_sensor_layer/.cproject +++ /dev/null @@ -1,42 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/range_sensor_layer/.project b/range_sensor_layer/.project deleted file mode 100644 index 01346cf..0000000 --- a/range_sensor_layer/.project +++ /dev/null @@ -1,27 +0,0 @@ - - - range_sensor_layer - - - - - - org.eclipse.cdt.managedbuilder.core.genmakebuilder - clean,full,incremental, - - - - - org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder - full,incremental, - - - - - - org.eclipse.cdt.core.cnature - org.eclipse.cdt.core.ccnature - org.eclipse.cdt.managedbuilder.core.managedBuildNature - org.eclipse.cdt.managedbuilder.core.ScannerConfigNature - - From 0bb333292cca385bbdd15c8964148eb3121224a7 Mon Sep 17 00:00:00 2001 From: corot Date: Tue, 25 May 2021 19:13:27 +0900 Subject: [PATCH 8/9] Do not read dynamically reconfigurable parameter ranges_buffer_size --- .../include/range_sensor_layer/range_sensor_layer.h | 2 +- range_sensor_layer/src/range_sensor_layer.cpp | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h b/range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h index be7e8f7..b8ccf12 100644 --- a/range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h +++ b/range_sensor_layer/include/range_sensor_layer/range_sensor_layer.h @@ -70,7 +70,7 @@ class RangeSensorLayer : public costmap_2d::CostmapLayer boost::function processRangeMessageFunc_; boost::mutex range_message_mutex_; - size_t range_msgs_buffer_size_; + unsigned int range_msgs_buffer_size_; std::unordered_map> range_msgs_buffers_; std::map, double> marked_point_history_; diff --git a/range_sensor_layer/src/range_sensor_layer.cpp b/range_sensor_layer/src/range_sensor_layer.cpp index 9d12b43..80f0adf 100644 --- a/range_sensor_layer/src/range_sensor_layer.cpp +++ b/range_sensor_layer/src/range_sensor_layer.cpp @@ -44,11 +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("ranges_buffer_size", (int&)range_msgs_buffer_size_, 1); - 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()); From 87bdee055c5cf8da97b7f8fc7af60a7a3c514ef9 Mon Sep 17 00:00:00 2001 From: corot Date: Thu, 10 Jun 2021 01:47:52 +0900 Subject: [PATCH 9/9] Lock range message mutex when reading and clearing buffers --- range_sensor_layer/src/range_sensor_layer.cpp | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/range_sensor_layer/src/range_sensor_layer.cpp b/range_sensor_layer/src/range_sensor_layer.cpp index 80f0adf..83201c7 100644 --- a/range_sensor_layer/src/range_sensor_layer.cpp +++ b/range_sensor_layer/src/range_sensor_layer.cpp @@ -181,7 +181,7 @@ void RangeSensorLayer::reconfigureCB(range_sensor_layer::RangeSensorLayerConfig if (range_msgs_buffer_size_ > static_cast(config.ranges_buffer_size)) { boost::mutex::scoped_lock lock(range_message_mutex_); - for(auto& range_topic: range_msgs_buffers_) + for (auto& range_topic: range_msgs_buffers_) { while (range_topic.second.size() > static_cast(config.ranges_buffer_size)) range_topic.second.pop_front(); @@ -201,18 +201,17 @@ void RangeSensorLayer::bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& void RangeSensorLayer::updateCostmap() { - std::list range_msgs_buffer_copy; + std::list range_msgs_to_update; + range_message_mutex_.lock(); for (auto& range_topic: range_msgs_buffers_) { - range_message_mutex_.lock(); - range_msgs_buffer_copy = std::list(range_topic.second); - range_topic.second.clear(); - range_message_mutex_.unlock(); + range_msgs_to_update.splice(range_msgs_to_update.end(), range_topic.second); + } + range_message_mutex_.unlock(); - for (auto& range_msg : range_msgs_buffer_copy) - { - processRangeMessageFunc_(range_msg); - } + for (auto& range_msg : range_msgs_to_update) + { + processRangeMessageFunc_(range_msg); } } @@ -530,11 +529,13 @@ void RangeSensorLayer::reset() void RangeSensorLayer::deactivate() { + boost::mutex::scoped_lock lock(range_message_mutex_); range_msgs_buffers_.clear(); } void RangeSensorLayer::activate() { + boost::mutex::scoped_lock lock(range_message_mutex_); range_msgs_buffers_.clear(); }