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/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..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 @@ -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 @@ -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 processRangeMessageFunc_; boost::mutex range_message_mutex_; - std::list range_msgs_buffer_; + unsigned int 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 35c6bc6..83201c7 100644 --- a/range_sensor_layer/src/range_sensor_layer.cpp +++ b/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(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(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(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 range_msgs_buffer_copy; - + std::list range_msgs_to_update; range_message_mutex_.lock(); - range_msgs_buffer_copy = std::list(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::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