From 2ec80f0fc0f4cf94259f391b0336c96687cfa443 Mon Sep 17 00:00:00 2001 From: Jorge Santos Date: Mon, 2 May 2016 09:01:31 +0200 Subject: [PATCH] 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 | 12 ++-- range_sensor_layer/src/range_sensor_layer.cpp | 65 ++++++++++++------- 4 files changed, 55 insertions(+), 26 deletions(-) diff --git a/range_sensor_layer/CMakeLists.txt b/range_sensor_layer/CMakeLists.txt index 7504ef0..cd6ce0d 100644 --- a/range_sensor_layer/CMakeLists.txt +++ b/range_sensor_layer/CMakeLists.txt @@ -1,6 +1,9 @@ cmake_minimum_required(VERSION 2.8.3) project(range_sensor_layer) +# Required to use unordered maps +add_definitions("-std=c++11") + find_package(catkin REQUIRED COMPONENTS angles roscpp diff --git a/range_sensor_layer/cfg/RangeSensorLayer.cfg b/range_sensor_layer/cfg/RangeSensorLayer.cfg index 8ec3a54..625dec5 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('max_angle', double_t, 0, 'Maximum angle (radians)', 12.5*3.14/180, 0, 3.1415) 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 ca2ed78..866f622 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,5 +1,8 @@ #ifndef RANGE_SENSOR_LAYER_H_ #define RANGE_SENSOR_LAYER_H_ + +#include + #include #include #include @@ -23,15 +26,15 @@ class RangeSensorLayer : public costmap_2d::CostmapLayer RangeSensorLayer(); virtual void onInitialize(); - virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, - double* max_y); + virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, + double* min_x, double* min_y, double* max_x, double* max_y); virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); virtual void reset(); 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); @@ -51,7 +54,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_; double max_angle_, phi_v_; std::string global_frame_; diff --git a/range_sensor_layer/src/range_sensor_layer.cpp b/range_sensor_layer/src/range_sensor_layer.cpp index d877a7a..02b5d62 100644 --- a/range_sensor_layer/src/range_sensor_layer.cpp +++ b/range_sensor_layer/src/range_sensor_layer.cpp @@ -34,6 +34,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")); @@ -92,7 +96,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()); } @@ -158,33 +164,50 @@ void RangeSensorLayer::reconfigureCB(range_sensor_layer::RangeSensorLayerConfig clear_threshold_ = config.clear_threshold; mark_threshold_ = config.mark_threshold; clear_on_max_reading_ = config.clear_on_max_reading; - + if(enabled_ != config.enabled) { 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() { - 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++) + std::unordered_map>::iterator buffer_it; + for(buffer_it = range_msgs_buffers_.begin(); buffer_it != range_msgs_buffers_.end(); buffer_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); + } } } @@ -242,11 +265,9 @@ void RangeSensorLayer::updateCostmap(sensor_msgs::Range& range_message, bool cle in.header.stamp = range_message.header.stamp; in.header.frame_id = range_message.header.frame_id; - if(!tf_->waitForTransform(global_frame_, in.header.frame_id, - in.header.stamp, ros::Duration(0.1)) ) { + if(!tf_->waitForTransform(global_frame_, in.header.frame_id, in.header.stamp, ros::Duration(0.1)) ) { ROS_ERROR_THROTTLE(1.0, "Range sensor layer can't transform from %s to %s at %f", - global_frame_.c_str(), in.header.frame_id.c_str(), - in.header.stamp.toSec()); + global_frame_.c_str(), in.header.frame_id.c_str(), in.header.stamp.toSec()); return; } @@ -340,13 +361,13 @@ void RangeSensorLayer::update_cell(double ox, double oy, double ot, double r, do //ROS_INFO("%f %f | %f %f = %f", dx, dy, theta, phi, sensor); //ROS_INFO("%f | %f %f | %f", prior, prob_occ, prob_not, new_prob); - unsigned char c = to_cost(new_prob); - setCost(x,y,c); + unsigned char c = to_cost(new_prob); + setCost(x,y,c); } } -void RangeSensorLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, - double* min_y, double* max_x, double* max_y) +void RangeSensorLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, + double* min_x, double* min_y, double* max_x, double* max_y) { if (layered_costmap_->isRolling()) updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);