From 5af50a9756496ae773722962a19bdd0d0d7102cd Mon Sep 17 00:00:00 2001 From: chisarie Date: Fri, 19 Oct 2018 10:39:08 +0200 Subject: [PATCH 01/36] added elevation layer package as a plugin of costmap_2d --- elevation_layer/.idea/codeStyles/Project.xml | 29 +++ .../.idea/elevation_layer_costmap_plugin.iml | 2 + elevation_layer/.idea/misc.xml | 7 + elevation_layer/.idea/modules.xml | 8 + elevation_layer/.idea/vcs.xml | 6 + elevation_layer/.idea/workspace.xml | 222 ++++++++++++++++++ elevation_layer/CMakeLists.txt | 75 ++++++ elevation_layer/README.md | 3 + elevation_layer/cfg/ElevationPlugin.cfg | 19 ++ elevation_layer/costmap_plugins.xml | 7 + elevation_layer/include/elevation_layer.h | 67 ++++++ elevation_layer/launch/elevation_params.yaml | 45 ++++ elevation_layer/package.xml | 31 +++ elevation_layer/src/elevation_layer.cpp | 169 +++++++++++++ 14 files changed, 690 insertions(+) create mode 100644 elevation_layer/.idea/codeStyles/Project.xml create mode 100644 elevation_layer/.idea/elevation_layer_costmap_plugin.iml create mode 100644 elevation_layer/.idea/misc.xml create mode 100644 elevation_layer/.idea/modules.xml create mode 100644 elevation_layer/.idea/vcs.xml create mode 100644 elevation_layer/.idea/workspace.xml create mode 100644 elevation_layer/CMakeLists.txt create mode 100644 elevation_layer/README.md create mode 100755 elevation_layer/cfg/ElevationPlugin.cfg create mode 100644 elevation_layer/costmap_plugins.xml create mode 100644 elevation_layer/include/elevation_layer.h create mode 100644 elevation_layer/launch/elevation_params.yaml create mode 100644 elevation_layer/package.xml create mode 100644 elevation_layer/src/elevation_layer.cpp diff --git a/elevation_layer/.idea/codeStyles/Project.xml b/elevation_layer/.idea/codeStyles/Project.xml new file mode 100644 index 00000000..30aa626c --- /dev/null +++ b/elevation_layer/.idea/codeStyles/Project.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/elevation_layer/.idea/elevation_layer_costmap_plugin.iml b/elevation_layer/.idea/elevation_layer_costmap_plugin.iml new file mode 100644 index 00000000..f08604bb --- /dev/null +++ b/elevation_layer/.idea/elevation_layer_costmap_plugin.iml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/elevation_layer/.idea/misc.xml b/elevation_layer/.idea/misc.xml new file mode 100644 index 00000000..8822db8f --- /dev/null +++ b/elevation_layer/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/elevation_layer/.idea/modules.xml b/elevation_layer/.idea/modules.xml new file mode 100644 index 00000000..bc02f116 --- /dev/null +++ b/elevation_layer/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/elevation_layer/.idea/vcs.xml b/elevation_layer/.idea/vcs.xml new file mode 100644 index 00000000..6c0b8635 --- /dev/null +++ b/elevation_layer/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/elevation_layer/.idea/workspace.xml b/elevation_layer/.idea/workspace.xml new file mode 100644 index 00000000..fa80b3cf --- /dev/null +++ b/elevation_layer/.idea/workspace.xml @@ -0,0 +1,222 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + DEFINITION_ORDER + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - 1539876559287 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file From 55ea64e5e8b86e8c2cc19ded5c89279ca8ff035e Mon Sep 17 00:00:00 2001 From: chisarie Date: Mon, 22 Oct 2018 12:53:31 +0200 Subject: [PATCH 03/36] implemented elevation layer for costmap_2d. It takes elevaton mapping as input. --- elevation_layer/costmap_plugins.xml | 2 +- elevation_layer/include/elevation_layer.h | 2 + elevation_layer/src/elevation_layer.cpp | 72 +++++++++++++---------- 3 files changed, 44 insertions(+), 32 deletions(-) diff --git a/elevation_layer/costmap_plugins.xml b/elevation_layer/costmap_plugins.xml index 2eb96b36..90624ab1 100644 --- a/elevation_layer/costmap_plugins.xml +++ b/elevation_layer/costmap_plugins.xml @@ -1,5 +1,5 @@ - + adds elevation map info to costmap_2d. diff --git a/elevation_layer/include/elevation_layer.h b/elevation_layer/include/elevation_layer.h index bb1c9908..18e16008 100644 --- a/elevation_layer/include/elevation_layer.h +++ b/elevation_layer/include/elevation_layer.h @@ -51,6 +51,7 @@ class ElevationLayer : public costmap_2d::CostmapLayer std::vector transformed_footprint_; bool rolling_window_; bool footprint_clearing_enabled_; + bool elevation_map_received_; void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y); @@ -59,6 +60,7 @@ class ElevationLayer : public costmap_2d::CostmapLayer grid_map::GridMap elevation_map_; ros::Subscriber elevation_subscriber_; double height_treshold_; + std::string elevation_topic_; }; } // namespace elevation_layer diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 985464a9..ca3f7a1f 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -30,19 +30,20 @@ namespace elevation_layer ElevationLayer::matchSize(); current_ = true; + elevation_map_received_ = false; global_frame_ = layered_costmap_->getGlobalFrameID(); // get our tf prefix ros::NodeHandle prefix_nh; const std::string tf_prefix = tf::getPrefixParam(prefix_nh); - std::string elevation_topic; // get the topics that we'll subscribe to from the parameter server - nh.param("elevation_topic", elevation_topic, std::string("")); - ROS_INFO(" Subscribed to Topics: %s", elevation_topic.c_str()); + nh.param("elevation_topic", elevation_topic_, std::string("")); + ROS_INFO(" Subscribed to Topics: %s", elevation_topic_.c_str()); nh.param("height_treshold", height_treshold_, 0.1); + ROS_INFO_STREAM("height_treshold" << height_treshold_); - elevation_subscriber_ = nh.subscribe(elevation_topic, 1, &ElevationLayer::elevationMapCallback, this); + elevation_subscriber_ = nh.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); dsrv_ = NULL; setupDynamicReconfigure(nh); } @@ -58,29 +59,19 @@ namespace elevation_layer { if (rolling_window_) updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); - if (!enabled_) + if (!enabled_ || !elevation_map_received_) return; useExtraBounds(min_x, min_y, max_x, max_y); - grid_map::Matrix& data = elevation_map_["elevation"]; //TODO: put layer name in config file for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { const grid_map::Index gridmap_index(*iterator); - int px = gridmap_index(0); - int py = gridmap_index(1); + grid_map::Position vertexPositionXY; + elevation_map_.getPosition(gridmap_index, vertexPositionXY); + double px = vertexPositionXY.x(); + double py = vertexPositionXY.y(); - // now we need to compute the map coordinates for the observation - unsigned int mx, my; - if (!worldToMap(px, py, mx, my)) { - ROS_DEBUG("Computing map coords failed"); - continue; - } - if ( data(px, py) > height_treshold_ ) // If point too high, label as obstacle - { - unsigned int costmap_index = getIndex(mx, my); - costmap_[costmap_index] = LETHAL_OBSTACLE; - touch(px, py, min_x, min_y, max_x, max_y); - } + touch(px, py, min_x, min_y, max_x, max_y); } updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); } @@ -99,9 +90,29 @@ namespace elevation_layer void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { - if (!enabled_) + if (!enabled_ || !elevation_map_received_) return; + + grid_map::Matrix& data = elevation_map_["elevation"]; //TODO: put layer name in config file + for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index gridmap_index(*iterator); + grid_map::Position vertexPositionXY; + elevation_map_.getPosition(gridmap_index, vertexPositionXY); + double px = vertexPositionXY.x(); + double py = vertexPositionXY.y(); + // now we need to compute the map coordinates for the observation + unsigned int mx, my; + if (!worldToMap(px, py, mx, my)) { + ROS_WARN("Computing map coords failed"); + continue; + } + if ( data(gridmap_index(0), gridmap_index(1)) > height_treshold_ ) // If point too high, label as obstacle + { + master_grid.setCost(mx, my, LETHAL_OBSTACLE); + } + } + if (footprint_clearing_enabled_) { setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE); @@ -126,6 +137,11 @@ namespace elevation_layer { ROS_WARN("Grid Map msg Conversion failed !"); } + if(!elevation_map_received_) + { + elevation_map_received_ = true; + ROS_INFO("map received !!!!!!!!!!!!!!!!!!!!!!!!!!!!!*****************"); + } } void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nh) @@ -152,18 +168,12 @@ namespace elevation_layer void ElevationLayer::activate() { // if we're stopped we need to re-subscribe to topics - for (unsigned int i = 0; i < elevation_subscribers_.size(); ++i) - { - if (elevation_subscribers_[i] != NULL) - elevation_subscribers_[i]->subscribe(); - } + ros::NodeHandle nh("~/" + name_); + elevation_subscriber_ = nh.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); + } void ElevationLayer::deactivate() { - for (unsigned int i = 0; i < elevation_subscribers_.size(); ++i) - { - if (elevation_subscribers_[i] != NULL) - elevation_subscribers_[i]->unsubscribe(); - } + elevation_subscriber_.shutdown(); } } \ No newline at end of file From fd906f5e04deac9e2553f315f6be46aa2bc38b6c Mon Sep 17 00:00:00 2001 From: chisarie Date: Mon, 22 Oct 2018 14:41:47 +0200 Subject: [PATCH 04/36] fixed namespace bug --- elevation_layer/include/elevation_layer.h | 6 +++--- elevation_layer/src/elevation_layer.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/elevation_layer/include/elevation_layer.h b/elevation_layer/include/elevation_layer.h index 18e16008..f7691c24 100644 --- a/elevation_layer/include/elevation_layer.h +++ b/elevation_layer/include/elevation_layer.h @@ -15,7 +15,7 @@ #include #include #include -#include +#include namespace elevation_layer @@ -43,7 +43,7 @@ class ElevationLayer : public costmap_2d::CostmapLayer protected: std::string global_frame_; ///< @brief The global frame for the costmap std::vector > elevation_subscribers_; ///< @brief Used for the observation message filters - dynamic_reconfigure::Server *dsrv_; + dynamic_reconfigure::Server *dsrv_; virtual void setupDynamicReconfigure(ros::NodeHandle& nh); @@ -56,7 +56,7 @@ class ElevationLayer : public costmap_2d::CostmapLayer double* max_x, double* max_y); private: - void reconfigureCB(costmap_2d::ElevationPluginConfig &config, uint32_t level); + void reconfigureCB(elevation_layer::ElevationPluginConfig &config, uint32_t level); grid_map::GridMap elevation_map_; ros::Subscriber elevation_subscriber_; double height_treshold_; diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index ca3f7a1f..cbd0f050 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -146,13 +146,13 @@ namespace elevation_layer void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nh) { - dsrv_ = new dynamic_reconfigure::Server(nh); - dynamic_reconfigure::Server::CallbackType cb = boost::bind( + dsrv_ = new dynamic_reconfigure::Server(nh); + dynamic_reconfigure::Server::CallbackType cb = boost::bind( &ElevationLayer::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); } - void ElevationLayer::reconfigureCB(costmap_2d::ElevationPluginConfig &config, uint32_t level) + void ElevationLayer::reconfigureCB(elevation_layer::ElevationPluginConfig &config, uint32_t level) { enabled_ = config.enabled; } From 2655dfd8b3e2adc8ccee88f984e3c97c1717dbde Mon Sep 17 00:00:00 2001 From: chisarie Date: Mon, 22 Oct 2018 18:10:34 +0200 Subject: [PATCH 05/36] working on including filter to find edges of elevation layer --- elevation_layer/include/elevation_layer.h | 12 ++++++- elevation_layer/launch/elevation_filters.yaml | 13 ++++++++ elevation_layer/src/elevation_layer.cpp | 31 +++++++++++++++++-- 3 files changed, 52 insertions(+), 4 deletions(-) create mode 100644 elevation_layer/launch/elevation_filters.yaml diff --git a/elevation_layer/include/elevation_layer.h b/elevation_layer/include/elevation_layer.h index f7691c24..1f123d74 100644 --- a/elevation_layer/include/elevation_layer.h +++ b/elevation_layer/include/elevation_layer.h @@ -11,6 +11,7 @@ #include #include #include "grid_map_ros/GridMapRosConverter.hpp" +#include #include #include @@ -61,7 +62,16 @@ class ElevationLayer : public costmap_2d::CostmapLayer ros::Subscriber elevation_subscriber_; double height_treshold_; std::string elevation_topic_; - }; + + //! Filter chain. + filters::FilterChain filterChain_; + + //! Filter chain parameters name. + std::string filter_chain_parameters_name_; + + bool filters_configuration_loaded_; + std::string layer_name_; + }; } // namespace elevation_layer diff --git a/elevation_layer/launch/elevation_filters.yaml b/elevation_layer/launch/elevation_filters.yaml new file mode 100644 index 00000000..4b8af751 --- /dev/null +++ b/elevation_layer/launch/elevation_filters.yaml @@ -0,0 +1,13 @@ +elevation_filters: + + # Edge detection on elevation layer with convolution filter as alternative to filter above. + - name: edge_detection + type: gridMapFilters/SlidingWindowMathExpressionFilter + params: + input_layer: elevation + output_layer: edges + expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation)' # Edge detection. + # expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation)' # Sharpen. + compute_empty_cells: false + edge_handling: empty # options: inside, crop, empty, mean + window_size: 3 # Make sure to make this compatible with the kernel matrix. \ No newline at end of file diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index cbd0f050..90dc31ce 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -31,6 +31,7 @@ namespace elevation_layer ElevationLayer::matchSize(); current_ = true; elevation_map_received_ = false; + filters_configuration_loaded_ = false; global_frame_ = layered_costmap_->getGlobalFrameID(); // get our tf prefix @@ -42,10 +43,22 @@ namespace elevation_layer ROS_INFO(" Subscribed to Topics: %s", elevation_topic_.c_str()); nh.param("height_treshold", height_treshold_, 0.1); ROS_INFO_STREAM("height_treshold" << height_treshold_); + if( !nh.param("filter_chain_parameters_name", filter_chain_parameters_name_, std::string("elevation_filters")) ) + { + ROS_WARN("did not find filter_chain_param_name, using default"); + } elevation_subscriber_ = nh.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); dsrv_ = NULL; setupDynamicReconfigure(nh); + + // Setup filter chain. + if (!filterChain_.configure(filter_chain_parameters_name_, nh)) { + ROS_WARN("Could not configure the filter chain!"); + } + else{ + filters_configuration_loaded_ = true; + } } ElevationLayer::~ElevationLayer() @@ -94,7 +107,7 @@ namespace elevation_layer return; - grid_map::Matrix& data = elevation_map_["elevation"]; //TODO: put layer name in config file + grid_map::Matrix& data = elevation_map_[layer_name_]; for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { const grid_map::Index gridmap_index(*iterator); grid_map::Position vertexPositionXY; @@ -133,14 +146,26 @@ namespace elevation_layer void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& elevation) { - if(!grid_map::GridMapRosConverter::fromMessage(*elevation, elevation_map_)) + grid_map::GridMap incoming_map; + grid_map::GridMap filtered_map; + if(!grid_map::GridMapRosConverter::fromMessage(*elevation, incoming_map)) { ROS_WARN("Grid Map msg Conversion failed !"); } if(!elevation_map_received_) { elevation_map_received_ = true; - ROS_INFO("map received !!!!!!!!!!!!!!!!!!!!!!!!!!!!!*****************"); + } + // Apply filter chain. + if (filters_configuration_loaded_ && filterChain_.update(incoming_map, filtered_map)) + { + elevation_map_ = filtered_map; + layer_name_ = "edges"; + } + else{ + ROS_WARN("Could not use the filter chain!"); + elevation_map_ = incoming_map; + layer_name_ = "edges"; } } From addee5cc126def58bebcb28fb28f5c110dea016d Mon Sep 17 00:00:00 2001 From: chisarie Date: Tue, 23 Oct 2018 13:36:52 +0200 Subject: [PATCH 06/36] adding grid_map_filters to the elevation layer to detect edges of the cost map --- elevation_layer/CMakeLists.txt | 2 + elevation_layer/include/elevation_layer.h | 6 +-- elevation_layer/launch/elevation_filters.yaml | 13 ------ elevation_layer/launch/elevation_params.yaml | 45 ------------------- elevation_layer/package.xml | 3 +- elevation_layer/src/elevation_layer.cpp | 30 +++++++++++-- 6 files changed, 31 insertions(+), 68 deletions(-) delete mode 100644 elevation_layer/launch/elevation_filters.yaml delete mode 100644 elevation_layer/launch/elevation_params.yaml diff --git a/elevation_layer/CMakeLists.txt b/elevation_layer/CMakeLists.txt index 0d51125b..55e7d8ef 100644 --- a/elevation_layer/CMakeLists.txt +++ b/elevation_layer/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS costmap_2d dynamic_reconfigure grid_map_ros + filters ) # add dynamic reconfigure configs @@ -30,6 +31,7 @@ catkin_package( costmap_2d dynamic_reconfigure grid_map_ros + filters ) diff --git a/elevation_layer/include/elevation_layer.h b/elevation_layer/include/elevation_layer.h index 1f123d74..56ccad1f 100644 --- a/elevation_layer/include/elevation_layer.h +++ b/elevation_layer/include/elevation_layer.h @@ -24,11 +24,7 @@ namespace elevation_layer class ElevationLayer : public costmap_2d::CostmapLayer { public: - ElevationLayer() - { - costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D. - } - + ElevationLayer(); virtual ~ElevationLayer(); virtual void onInitialize(); virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, diff --git a/elevation_layer/launch/elevation_filters.yaml b/elevation_layer/launch/elevation_filters.yaml deleted file mode 100644 index 4b8af751..00000000 --- a/elevation_layer/launch/elevation_filters.yaml +++ /dev/null @@ -1,13 +0,0 @@ -elevation_filters: - - # Edge detection on elevation layer with convolution filter as alternative to filter above. - - name: edge_detection - type: gridMapFilters/SlidingWindowMathExpressionFilter - params: - input_layer: elevation - output_layer: edges - expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation)' # Edge detection. - # expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation)' # Sharpen. - compute_empty_cells: false - edge_handling: empty # options: inside, crop, empty, mean - window_size: 3 # Make sure to make this compatible with the kernel matrix. \ No newline at end of file diff --git a/elevation_layer/launch/elevation_params.yaml b/elevation_layer/launch/elevation_params.yaml deleted file mode 100644 index 1cbec769..00000000 --- a/elevation_layer/launch/elevation_params.yaml +++ /dev/null @@ -1,45 +0,0 @@ -global_frame: /map -robot_base_frame: base_link -update_frequency: 5.0 -publish_frequency: 1.0 - -#set if you want the voxel map published -publish_voxel_map: true - -#set to true if you want to initialize the costmap from a static map -static_map: false - -#begin - COMMENT these lines if you set static_map to true -rolling_window: true -width: 6.0 -height: 6.0 -resolution: 0.025 -#end - COMMENT these lines if you set static_map to true - -#START VOXEL STUFF -map_type: voxel -origin_z: 0.0 -z_resolution: 0.2 -z_voxels: 10 -unknown_threshold: 10 -mark_threshold: 0 -#END VOXEL STUFF - -transform_tolerance: 0.3 -obstacle_range: 2.5 -max_obstacle_height: 2.0 -raytrace_range: 3.0 -footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] -#robot_radius: 0.46 -footprint_padding: 0.01 -inflation_radius: 0.55 -cost_scaling_factor: 10.0 -lethal_cost_threshold: 100 -observation_sources: base_scan -base_scan: {data_type: LaserScan, expected_update_rate: 0.4, - observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} - -#START ELEVATION STUFF -elevation_topic: /elevation_mapping/elevation_map -height_treshold: 0.25 # [meters] -#END ELEVATION STUFF \ No newline at end of file diff --git a/elevation_layer/package.xml b/elevation_layer/package.xml index 52abbb2b..3d7f60a8 100644 --- a/elevation_layer/package.xml +++ b/elevation_layer/package.xml @@ -18,13 +18,14 @@ costmap_2d dynamic_reconfigure grid_map_ros + filters roscpp tf costmap_2d dynamic_reconfigure grid_map_ros - + filters diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 90dc31ce..fab122d5 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -17,6 +17,11 @@ using costmap_2d::Observation; namespace elevation_layer { + ElevationLayer::ElevationLayer() : filterChain_("grid_map::GridMap") + { + costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D. + } + void ElevationLayer::onInitialize() { ros::NodeHandle nh("~/" + name_), g_nh; rolling_window_ = layered_costmap_->isRolling(); @@ -42,7 +47,7 @@ namespace elevation_layer nh.param("elevation_topic", elevation_topic_, std::string("")); ROS_INFO(" Subscribed to Topics: %s", elevation_topic_.c_str()); nh.param("height_treshold", height_treshold_, 0.1); - ROS_INFO_STREAM("height_treshold" << height_treshold_); + ROS_INFO_STREAM("height_treshold: " << height_treshold_); if( !nh.param("filter_chain_parameters_name", filter_chain_parameters_name_, std::string("elevation_filters")) ) { ROS_WARN("did not find filter_chain_param_name, using default"); @@ -53,7 +58,7 @@ namespace elevation_layer setupDynamicReconfigure(nh); // Setup filter chain. - if (!filterChain_.configure(filter_chain_parameters_name_, nh)) { + if (!filterChain_.configure(filter_chain_parameters_name_, nh)) { //TODO: add the yaml ROS_WARN("Could not configure the filter chain!"); } else{ @@ -107,7 +112,7 @@ namespace elevation_layer return; - grid_map::Matrix& data = elevation_map_[layer_name_]; + grid_map::Matrix& data = elevation_map_[layer_name_]; for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { const grid_map::Index gridmap_index(*iterator); grid_map::Position vertexPositionXY; @@ -160,12 +165,29 @@ namespace elevation_layer if (filters_configuration_loaded_ && filterChain_.update(incoming_map, filtered_map)) { elevation_map_ = filtered_map; + std::vector filtered_layer_names, incoming_layer_names; + + ROS_INFO("incoming_map:"); + incoming_layer_names = incoming_map.getLayers(); + for (int i = 0; i < incoming_layer_names.size(); ++i) + { + ROS_INFO_STREAM(incoming_layer_names[i]); + } + + ROS_INFO("filtered_map:"); + filtered_layer_names = filtered_map.getLayers(); + for (int i = 0; i < filtered_layer_names.size(); ++i) + { + ROS_INFO_STREAM(filtered_layer_names[i]); + } + + layer_name_ = "edges"; } else{ ROS_WARN("Could not use the filter chain!"); elevation_map_ = incoming_map; - layer_name_ = "edges"; + layer_name_ = "elevation"; } } From 105f9a566832daae956cc695d986a09c6a6d862b Mon Sep 17 00:00:00 2001 From: chisarie Date: Thu, 25 Oct 2018 18:05:44 +0200 Subject: [PATCH 07/36] elevation layer using filter done. Still one small bug with combination_method=0 --- elevation_layer/cfg/ElevationPlugin.cfg | 10 ---- elevation_layer/src/elevation_layer.cpp | 74 ++++++++++++------------- 2 files changed, 34 insertions(+), 50 deletions(-) diff --git a/elevation_layer/cfg/ElevationPlugin.cfg b/elevation_layer/cfg/ElevationPlugin.cfg index f02a2b0d..aeb8b08a 100755 --- a/elevation_layer/cfg/ElevationPlugin.cfg +++ b/elevation_layer/cfg/ElevationPlugin.cfg @@ -4,16 +4,6 @@ PACKAGE = "elevation_layer" from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t gen = ParameterGenerator() - gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) - -combo_enum = gen.enum([ gen.const("Overwrite", int_t, 0, "Overwrite values"), - gen.const("Maximum", int_t, 1, "Take the maximum of the values"), - gen.const("Nothing", int_t, 99, "Do nothing") - ], - "Method for combining layers enum") - -gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, edit_method=combo_enum) exit(gen.generate("elevation_layer", "elevation_layer", "ElevationPlugin")) diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index fab122d5..5639154d 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -26,13 +26,6 @@ namespace elevation_layer ros::NodeHandle nh("~/" + name_), g_nh; rolling_window_ = layered_costmap_->isRolling(); - bool track_unknown_space; - nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown()); - if (track_unknown_space) - default_value_ = NO_INFORMATION; - else - default_value_ = FREE_SPACE; - ElevationLayer::matchSize(); current_ = true; elevation_map_received_ = false; @@ -43,25 +36,43 @@ namespace elevation_layer ros::NodeHandle prefix_nh; const std::string tf_prefix = tf::getPrefixParam(prefix_nh); - // get the topics that we'll subscribe to from the parameter server - nh.param("elevation_topic", elevation_topic_, std::string("")); - ROS_INFO(" Subscribed to Topics: %s", elevation_topic_.c_str()); - nh.param("height_treshold", height_treshold_, 0.1); - ROS_INFO_STREAM("height_treshold: " << height_treshold_); + // get parameters from config file + if(!nh.param("elevation_topic", elevation_topic_, std::string(""))) + { + ROS_WARN("did not find elevation_topic, using default"); + } + if(!nh.param("height_treshold", height_treshold_, 0.12)) + { + ROS_WARN("did not find height_treshold, using default"); + } if( !nh.param("filter_chain_parameters_name", filter_chain_parameters_name_, std::string("elevation_filters")) ) { ROS_WARN("did not find filter_chain_param_name, using default"); } + if(!nh.param("footprint_clearing_enabled", footprint_clearing_enabled_, true)) + { + ROS_WARN("did not find footprint_clearing_enabled, using default"); + } + if(!nh.param("combination_method", combination_method_, 2)) + { + ROS_WARN("did not find combination_method, using default"); + } + bool track_unknown_space; + nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown()); + if (track_unknown_space) + default_value_ = NO_INFORMATION; + else + default_value_ = FREE_SPACE; + // Subscribe to topic elevation_subscriber_ = nh.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); dsrv_ = NULL; setupDynamicReconfigure(nh); // Setup filter chain. - if (!filterChain_.configure(filter_chain_parameters_name_, nh)) { //TODO: add the yaml + if (!filterChain_.configure(filter_chain_parameters_name_, nh)) { ROS_WARN("Could not configure the filter chain!"); - } - else{ + }else{ filters_configuration_loaded_ = true; } } @@ -77,11 +88,10 @@ namespace elevation_layer { if (rolling_window_) updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); - if (!enabled_ || !elevation_map_received_) + if (!(enabled_ && elevation_map_received_)) return; useExtraBounds(min_x, min_y, max_x, max_y); - for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { const grid_map::Index gridmap_index(*iterator); grid_map::Position vertexPositionXY; @@ -122,11 +132,11 @@ namespace elevation_layer // now we need to compute the map coordinates for the observation unsigned int mx, my; if (!worldToMap(px, py, mx, my)) { - ROS_WARN("Computing map coords failed"); continue; } if ( data(gridmap_index(0), gridmap_index(1)) > height_treshold_ ) // If point too high, label as obstacle { + ROS_INFO_STREAM("Found value: " << data(gridmap_index(0), gridmap_index(1))); master_grid.setCost(mx, my, LETHAL_OBSTACLE); } } @@ -139,7 +149,7 @@ namespace elevation_layer switch (combination_method_) { case 0: // Overwrite - updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); + updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); // TODO: This doesnt work, check why break; case 1: // Maximum updateWithMax(master_grid, min_i, min_j, max_i, max_j); @@ -157,31 +167,11 @@ namespace elevation_layer { ROS_WARN("Grid Map msg Conversion failed !"); } - if(!elevation_map_received_) - { - elevation_map_received_ = true; - } + incoming_map.convertToDefaultStartIndex(); // Apply filter chain. if (filters_configuration_loaded_ && filterChain_.update(incoming_map, filtered_map)) { elevation_map_ = filtered_map; - std::vector filtered_layer_names, incoming_layer_names; - - ROS_INFO("incoming_map:"); - incoming_layer_names = incoming_map.getLayers(); - for (int i = 0; i < incoming_layer_names.size(); ++i) - { - ROS_INFO_STREAM(incoming_layer_names[i]); - } - - ROS_INFO("filtered_map:"); - filtered_layer_names = filtered_map.getLayers(); - for (int i = 0; i < filtered_layer_names.size(); ++i) - { - ROS_INFO_STREAM(filtered_layer_names[i]); - } - - layer_name_ = "edges"; } else{ @@ -189,6 +179,10 @@ namespace elevation_layer elevation_map_ = incoming_map; layer_name_ = "elevation"; } + if(!elevation_map_received_) + { + elevation_map_received_ = true; + } } void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nh) From 4ed82cfcea0e56e177899fb0309be8386e8e006a Mon Sep 17 00:00:00 2001 From: chisarie Date: Fri, 26 Oct 2018 10:53:14 +0200 Subject: [PATCH 08/36] tried height treshold and filter treshold together, not good --- elevation_layer/include/elevation_layer.h | 7 ++++-- elevation_layer/src/elevation_layer.cpp | 27 +++++++++++++++++------ 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/elevation_layer/include/elevation_layer.h b/elevation_layer/include/elevation_layer.h index 56ccad1f..c3181fc6 100644 --- a/elevation_layer/include/elevation_layer.h +++ b/elevation_layer/include/elevation_layer.h @@ -57,6 +57,7 @@ class ElevationLayer : public costmap_2d::CostmapLayer grid_map::GridMap elevation_map_; ros::Subscriber elevation_subscriber_; double height_treshold_; + double edges_sharpness_treshold_; std::string elevation_topic_; //! Filter chain. @@ -66,8 +67,10 @@ class ElevationLayer : public costmap_2d::CostmapLayer std::string filter_chain_parameters_name_; bool filters_configuration_loaded_; - std::string layer_name_; - }; + bool filter_applied_; + std::string elevation_layer_name_; + std::string edges_layer_name_; +}; } // namespace elevation_layer diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 5639154d..9ed4c277 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -31,6 +31,9 @@ namespace elevation_layer elevation_map_received_ = false; filters_configuration_loaded_ = false; global_frame_ = layered_costmap_->getGlobalFrameID(); + elevation_layer_name_ = "elevation"; + edges_layer_name_ = "edges"; + filter_applied_ = false; // get our tf prefix ros::NodeHandle prefix_nh; @@ -57,6 +60,10 @@ namespace elevation_layer { ROS_WARN("did not find combination_method, using default"); } + if(!nh.param("edges_sharpness_treshold", edges_sharpness_treshold_, 0.12)) + { + ROS_WARN("did not find edges_sharpness_treshold, using default"); + } bool track_unknown_space; nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown()); if (track_unknown_space) @@ -121,8 +128,6 @@ namespace elevation_layer if (!enabled_ || !elevation_map_received_) return; - - grid_map::Matrix& data = elevation_map_[layer_name_]; for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { const grid_map::Index gridmap_index(*iterator); grid_map::Position vertexPositionXY; @@ -131,12 +136,20 @@ namespace elevation_layer double py = vertexPositionXY.y(); // now we need to compute the map coordinates for the observation unsigned int mx, my; - if (!worldToMap(px, py, mx, my)) { + if (!worldToMap(px, py, mx, my)) // if point outside of local costmap, ignore + { continue; } - if ( data(gridmap_index(0), gridmap_index(1)) > height_treshold_ ) // If point too high, label as obstacle + const grid_map::Matrix& elevation_data = elevation_map_[elevation_layer_name_]; + if ( elevation_data(gridmap_index(0), gridmap_index(1)) > height_treshold_ ) // If point too high, it could be an obstacle { - ROS_INFO_STREAM("Found value: " << data(gridmap_index(0), gridmap_index(1))); + if (filter_applied_) { + const grid_map::Matrix &edges_data = elevation_map_[edges_layer_name_]; + if (edges_data(gridmap_index(0), gridmap_index(1)) < edges_sharpness_treshold_) // if area not sharp, dont label as obstacle + { + continue; + } + } master_grid.setCost(mx, my, LETHAL_OBSTACLE); } } @@ -172,12 +185,12 @@ namespace elevation_layer if (filters_configuration_loaded_ && filterChain_.update(incoming_map, filtered_map)) { elevation_map_ = filtered_map; - layer_name_ = "edges"; + filter_applied_ = true; } else{ ROS_WARN("Could not use the filter chain!"); elevation_map_ = incoming_map; - layer_name_ = "elevation"; + filter_applied_ = false; } if(!elevation_map_received_) { From d505d15b6726d29ddbb8be7ae6c2f8c21738d32b Mon Sep 17 00:00:00 2001 From: chisarie Date: Fri, 26 Oct 2018 11:47:34 +0200 Subject: [PATCH 09/36] halfing the trshold if map filtered --- elevation_layer/src/elevation_layer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 9ed4c277..3bb5cbb6 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -186,6 +186,7 @@ namespace elevation_layer { elevation_map_ = filtered_map; filter_applied_ = true; + height_treshold_ /= 2; // Half the treshold since the highest sharpness is at midheigth of the obstacles } else{ ROS_WARN("Could not use the filter chain!"); From afbfd7d83f8ca18b73dd833da05213206d99a2f7 Mon Sep 17 00:00:00 2001 From: chisarie Date: Fri, 26 Oct 2018 14:08:24 +0200 Subject: [PATCH 10/36] putting the elevation data outside the for loop --- elevation_layer/src/elevation_layer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 3bb5cbb6..db76b43e 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -128,6 +128,7 @@ namespace elevation_layer if (!enabled_ || !elevation_map_received_) return; + const grid_map::Matrix& elevation_data = elevation_map_[elevation_layer_name_]; for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { const grid_map::Index gridmap_index(*iterator); grid_map::Position vertexPositionXY; @@ -140,7 +141,6 @@ namespace elevation_layer { continue; } - const grid_map::Matrix& elevation_data = elevation_map_[elevation_layer_name_]; if ( elevation_data(gridmap_index(0), gridmap_index(1)) > height_treshold_ ) // If point too high, it could be an obstacle { if (filter_applied_) { From 0a5a674ed07be870cf836fdb7c859c3fbd22147a Mon Sep 17 00:00:00 2001 From: chisarie Date: Mon, 29 Oct 2018 09:48:42 +0100 Subject: [PATCH 11/36] made filter thread safe --- elevation_layer/include/elevation_layer.h | 8 ++++++-- elevation_layer/src/elevation_layer.cpp | 14 +++++++++----- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/elevation_layer/include/elevation_layer.h b/elevation_layer/include/elevation_layer.h index c3181fc6..44a39dc0 100644 --- a/elevation_layer/include/elevation_layer.h +++ b/elevation_layer/include/elevation_layer.h @@ -5,6 +5,10 @@ #ifndef ELEVATION_LAYER_H #define ELEVATION_LAYER_H + +#include +#include + #include #include #include @@ -48,13 +52,14 @@ class ElevationLayer : public costmap_2d::CostmapLayer std::vector transformed_footprint_; bool rolling_window_; bool footprint_clearing_enabled_; - bool elevation_map_received_; + std::atomic_bool elevation_map_received_; void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y); private: void reconfigureCB(elevation_layer::ElevationPluginConfig &config, uint32_t level); grid_map::GridMap elevation_map_; + std::mutex elevation_map_mutex_; ros::Subscriber elevation_subscriber_; double height_treshold_; double edges_sharpness_treshold_; @@ -67,7 +72,6 @@ class ElevationLayer : public costmap_2d::CostmapLayer std::string filter_chain_parameters_name_; bool filters_configuration_loaded_; - bool filter_applied_; std::string elevation_layer_name_; std::string edges_layer_name_; }; diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index db76b43e..6db81ee1 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -33,7 +33,6 @@ namespace elevation_layer global_frame_ = layered_costmap_->getGlobalFrameID(); elevation_layer_name_ = "elevation"; edges_layer_name_ = "edges"; - filter_applied_ = false; // get our tf prefix ros::NodeHandle prefix_nh; @@ -93,6 +92,7 @@ namespace elevation_layer void ElevationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) { + std::lock_guard lock(elevation_map_mutex_); if (rolling_window_) updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); if (!(enabled_ && elevation_map_received_)) @@ -125,9 +125,10 @@ namespace elevation_layer void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { + std::lock_guard lock(elevation_map_mutex_); if (!enabled_ || !elevation_map_received_) return; - + const bool has_edges_layer = elevation_map_.exists(edges_layer_name_); const grid_map::Matrix& elevation_data = elevation_map_[elevation_layer_name_]; for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { const grid_map::Index gridmap_index(*iterator); @@ -143,7 +144,9 @@ namespace elevation_layer } if ( elevation_data(gridmap_index(0), gridmap_index(1)) > height_treshold_ ) // If point too high, it could be an obstacle { - if (filter_applied_) { + if (!has_edges_layer) { + ROS_WARN("No edges layer found !!"); + } else{ const grid_map::Matrix &edges_data = elevation_map_[edges_layer_name_]; if (edges_data(gridmap_index(0), gridmap_index(1)) < edges_sharpness_treshold_) // if area not sharp, dont label as obstacle { @@ -184,17 +187,18 @@ namespace elevation_layer // Apply filter chain. if (filters_configuration_loaded_ && filterChain_.update(incoming_map, filtered_map)) { + std::lock_guard lock(elevation_map_mutex_); elevation_map_ = filtered_map; - filter_applied_ = true; height_treshold_ /= 2; // Half the treshold since the highest sharpness is at midheigth of the obstacles } else{ + std::lock_guard lock(elevation_map_mutex_); ROS_WARN("Could not use the filter chain!"); elevation_map_ = incoming_map; - filter_applied_ = false; } if(!elevation_map_received_) { +// elevation_map_received_.store(true); elevation_map_received_ = true; } } From c8abffb8e48527862a476cefc4e07501fc6d2f02 Mon Sep 17 00:00:00 2001 From: chisarie Date: Tue, 30 Oct 2018 10:52:26 +0100 Subject: [PATCH 12/36] combination method and footprint clearing working --- elevation_layer/src/elevation_layer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 6db81ee1..01d8e1e4 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -153,7 +153,7 @@ namespace elevation_layer continue; } } - master_grid.setCost(mx, my, LETHAL_OBSTACLE); + setCost(mx, my, LETHAL_OBSTACLE); } } From 12d5302731b7c5cef371c8c822106d72edd70544 Mon Sep 17 00:00:00 2001 From: chisarie Date: Tue, 30 Oct 2018 13:38:21 +0100 Subject: [PATCH 13/36] label free space explicitly --- elevation_layer/src/elevation_layer.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 01d8e1e4..c40c85fb 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -150,11 +150,15 @@ namespace elevation_layer const grid_map::Matrix &edges_data = elevation_map_[edges_layer_name_]; if (edges_data(gridmap_index(0), gridmap_index(1)) < edges_sharpness_treshold_) // if area not sharp, dont label as obstacle { + setCost(mx, my, FREE_SPACE); continue; } } setCost(mx, my, LETHAL_OBSTACLE); } + else{ + setCost(mx, my, FREE_SPACE); + } } if (footprint_clearing_enabled_) @@ -165,7 +169,7 @@ namespace elevation_layer switch (combination_method_) { case 0: // Overwrite - updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); // TODO: This doesnt work, check why + updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); break; case 1: // Maximum updateWithMax(master_grid, min_i, min_j, max_i, max_j); From c05ebbe5eaf456ac7dfe6b0b6fea5676f01e7629 Mon Sep 17 00:00:00 2001 From: Gabriel Hottiger Date: Wed, 31 Oct 2018 15:36:25 +0100 Subject: [PATCH 14/36] add config file for elevation filters --- elevation_layer/config/elevation_filters.yaml | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 elevation_layer/config/elevation_filters.yaml diff --git a/elevation_layer/config/elevation_filters.yaml b/elevation_layer/config/elevation_filters.yaml new file mode 100644 index 00000000..347a2233 --- /dev/null +++ b/elevation_layer/config/elevation_filters.yaml @@ -0,0 +1,12 @@ +elevation_filters: + # Edge detection on elevation layer with convolution filter as alternative to filter above. + - name: edge_detection + type: gridMapFilters/SlidingWindowMathExpressionFilter + params: + input_layer: elevation + output_layer: edges + expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation)' # Edge detection. + # expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation)' # Sharpen. + compute_empty_cells: false + edge_handling: mean # options: inside, crop, empty, mean + window_size: 3 # Make sure to make this compatible with the kernel matrix. \ No newline at end of file From 68c83bb001b75a62f53bba3ef87f1b36605de867 Mon Sep 17 00:00:00 2001 From: chisarie Date: Mon, 5 Nov 2018 09:34:58 +0100 Subject: [PATCH 15/36] Updated README file --- elevation_layer/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/elevation_layer/README.md b/elevation_layer/README.md index 8117fd12..6feac62d 100644 --- a/elevation_layer/README.md +++ b/elevation_layer/README.md @@ -1,3 +1,3 @@ # Elevation layer -A plugin for [costmap_2d]](http://wiki.ros.org/costmap_2d). +A plugin for [costmap_2d]](http://wiki.ros.org/costmap_2d). It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d. From 88dec09dbe7b2c6d9edbcae14cc3ca1ec6b58648 Mon Sep 17 00:00:00 2001 From: chisarie Date: Tue, 6 Nov 2018 15:23:16 +0100 Subject: [PATCH 16/36] making changes from pull request feedback --- elevation_layer/CMakeLists.txt | 48 +- elevation_layer/include/elevation_layer.h | 82 ---- .../include/elevation_layer/elevation_layer.h | 76 ++++ elevation_layer/package.xml | 27 +- elevation_layer/src/elevation_layer.cpp | 419 ++++++++---------- 5 files changed, 310 insertions(+), 342 deletions(-) delete mode 100644 elevation_layer/include/elevation_layer.h create mode 100644 elevation_layer/include/elevation_layer/elevation_layer.h diff --git a/elevation_layer/CMakeLists.txt b/elevation_layer/CMakeLists.txt index 55e7d8ef..b6892caa 100644 --- a/elevation_layer/CMakeLists.txt +++ b/elevation_layer/CMakeLists.txt @@ -1,10 +1,13 @@ -# use c++11 -set(CMAKE_CXX_COMPILER_ARG1 -std=c++11) - cmake_minimum_required(VERSION 2.8.3) project(elevation_layer) -find_package(Boost REQUIRED COMPONENTS thread) +## Use C++11 +add_definitions(-std=c++11) +## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, +## enforcing cleaner code. +#add_definitions(-std=c++11 -Wall -Werror) + +## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS roscpp tf @@ -14,12 +17,23 @@ find_package(catkin REQUIRED COMPONENTS filters ) +# Find system libraries +find_package(Boost REQUIRED) + # add dynamic reconfigure configs generate_dynamic_reconfigure_options( cfg/ElevationPlugin.cfg ) -## declare catkin package +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include @@ -32,19 +46,26 @@ catkin_package( dynamic_reconfigure grid_map_ros filters + DEPENDS + Boost ) - - ## build ## -include_directories(include) +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations include_directories( - ${catkin_INCLUDE_DIRS} + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) ## declare a cpp library -add_library(elevation_layer +add_library(${PROJECT_NAME} src/elevation_layer.cpp ) @@ -52,15 +73,14 @@ add_library(elevation_layer ## cmake target dependencies of the executable/library # build config headers -add_dependencies(elevation_layer elevation_layer_gencfg) +add_dependencies(${PROJECT_NAME} elevation_layer_gencfg) ## libraries to link a library or executable target against -target_link_libraries(elevation_layer +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ) - ## install ## ## executables and/or libraries for installation @@ -71,7 +91,7 @@ install(TARGETS elevation_layer ) ## cpp-header files for installation -install(DIRECTORY include/ +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" ) diff --git a/elevation_layer/include/elevation_layer.h b/elevation_layer/include/elevation_layer.h deleted file mode 100644 index 44a39dc0..00000000 --- a/elevation_layer/include/elevation_layer.h +++ /dev/null @@ -1,82 +0,0 @@ -// -// Created by eugenio on 15.10.18. -// - -#ifndef ELEVATION_LAYER_H -#define ELEVATION_LAYER_H - - -#include -#include - -#include -#include -#include -#include -#include -#include "grid_map_ros/GridMapRosConverter.hpp" -#include - -#include -#include -#include -#include - - -namespace elevation_layer -{ -class ElevationLayer : public costmap_2d::CostmapLayer - { - public: - ElevationLayer(); - virtual ~ElevationLayer(); - 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 updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); - - virtual void activate(); - virtual void deactivate(); - virtual void reset(); - - void elevationMapCallback(const grid_map_msgs::GridMapConstPtr& occupancy_grid); - - protected: - std::string global_frame_; ///< @brief The global frame for the costmap - std::vector > elevation_subscribers_; ///< @brief Used for the observation message filters - dynamic_reconfigure::Server *dsrv_; - virtual void setupDynamicReconfigure(ros::NodeHandle& nh); - - - int combination_method_; - std::vector transformed_footprint_; - bool rolling_window_; - bool footprint_clearing_enabled_; - std::atomic_bool elevation_map_received_; - void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, - double* max_x, double* max_y); - - private: - void reconfigureCB(elevation_layer::ElevationPluginConfig &config, uint32_t level); - grid_map::GridMap elevation_map_; - std::mutex elevation_map_mutex_; - ros::Subscriber elevation_subscriber_; - double height_treshold_; - double edges_sharpness_treshold_; - std::string elevation_topic_; - - //! Filter chain. - filters::FilterChain filterChain_; - - //! Filter chain parameters name. - std::string filter_chain_parameters_name_; - - bool filters_configuration_loaded_; - std::string elevation_layer_name_; - std::string edges_layer_name_; -}; - -} // namespace elevation_layer - - -#endif //ELEVATION_LAYER_H diff --git a/elevation_layer/include/elevation_layer/elevation_layer.h b/elevation_layer/include/elevation_layer/elevation_layer.h new file mode 100644 index 00000000..f970a3cf --- /dev/null +++ b/elevation_layer/include/elevation_layer/elevation_layer.h @@ -0,0 +1,76 @@ +/* + * elevation_layer.h + * + * Created on: Nov 5, 2018 + * Author: Eugenio Chisari + * Institute: ANYbotics + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include "grid_map_ros/GridMapRosConverter.hpp" + +#include +#include +#include +#include + +namespace elevation_layer { +class ElevationLayer : public costmap_2d::CostmapLayer { + public: + ElevationLayer(); + ~ElevationLayer() override = default; + void onInitialize() override; + void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) override; + void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override; + + void activate() override; + void deactivate() override; + void reset() override; + + void elevationMapCallback(const grid_map_msgs::GridMapConstPtr& occupancy_grid); + + protected: + std::string global_frame_; ///< @brief The global frame for the costmap + std::vector > + elevation_subscribers_; ///< @brief Used for the observation message filters + dynamic_reconfigure::Server* dsrv_; + virtual void setupDynamicReconfigure(ros::NodeHandle& nh); + + int combination_method_; + std::vector transformed_footprint_; + bool rolling_window_; + bool footprint_clearing_enabled_; + std::atomic_bool elevation_map_received_; + void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y); + + private: + void reconfigureCB(elevation_layer::ElevationPluginConfig& config, uint32_t level); + grid_map::GridMap elevation_map_; + std::mutex elevation_map_mutex_; + ros::Subscriber elevation_subscriber_; + double height_threshold_; + double edges_sharpness_threshold_; + std::string elevation_topic_; + + //! Filter chain. + filters::FilterChain filterChain_; + + //! Filter chain parameters name. + std::string filter_chain_parameters_name_; + + bool filters_configuration_loaded_; + std::string elevation_layer_name_; + std::string edges_layer_name_; +}; + +} // namespace elevation_layer \ No newline at end of file diff --git a/elevation_layer/package.xml b/elevation_layer/package.xml index 3d7f60a8..6598eb21 100644 --- a/elevation_layer/package.xml +++ b/elevation_layer/package.xml @@ -1,31 +1,22 @@ - + elevation_layer - 0.0.2 - adds elevation mapping informations to costmap_2d - - + Eugenio Chisari Eugenio Chisari - BSD catkin - roscpp - tf - costmap_2d - dynamic_reconfigure - grid_map_ros - filters + roscpp + boost + tf + costmap_2d + dynamic_reconfigure + grid_map_ros + filters - roscpp - tf - costmap_2d - dynamic_reconfigure - grid_map_ros - filters diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index c40c85fb..b82db906 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -1,242 +1,205 @@ -// -// Created by eugenio on 16.10.18. -// - -#include +/* + * elevation_layer.cpp + * + * Created on: Nov 5, 2018 + * Author: Eugenio Chisari + * Institute: ANYbotics + */ + +#include #include PLUGINLIB_EXPORT_CLASS(elevation_layer::ElevationLayer, costmap_2d::Layer) -using costmap_2d::NO_INFORMATION; -using costmap_2d::LETHAL_OBSTACLE; using costmap_2d::FREE_SPACE; +using costmap_2d::LETHAL_OBSTACLE; +using costmap_2d::NO_INFORMATION; -using costmap_2d::ObservationBuffer; using costmap_2d::Observation; +using costmap_2d::ObservationBuffer; -namespace elevation_layer -{ - - ElevationLayer::ElevationLayer() : filterChain_("grid_map::GridMap") - { - costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D. - } - - void ElevationLayer::onInitialize() { - ros::NodeHandle nh("~/" + name_), g_nh; - rolling_window_ = layered_costmap_->isRolling(); - - ElevationLayer::matchSize(); - current_ = true; - elevation_map_received_ = false; - filters_configuration_loaded_ = false; - global_frame_ = layered_costmap_->getGlobalFrameID(); - elevation_layer_name_ = "elevation"; - edges_layer_name_ = "edges"; - - // get our tf prefix - ros::NodeHandle prefix_nh; - const std::string tf_prefix = tf::getPrefixParam(prefix_nh); - - // get parameters from config file - if(!nh.param("elevation_topic", elevation_topic_, std::string(""))) - { - ROS_WARN("did not find elevation_topic, using default"); - } - if(!nh.param("height_treshold", height_treshold_, 0.12)) - { - ROS_WARN("did not find height_treshold, using default"); - } - if( !nh.param("filter_chain_parameters_name", filter_chain_parameters_name_, std::string("elevation_filters")) ) - { - ROS_WARN("did not find filter_chain_param_name, using default"); - } - if(!nh.param("footprint_clearing_enabled", footprint_clearing_enabled_, true)) - { - ROS_WARN("did not find footprint_clearing_enabled, using default"); - } - if(!nh.param("combination_method", combination_method_, 2)) - { - ROS_WARN("did not find combination_method, using default"); - } - if(!nh.param("edges_sharpness_treshold", edges_sharpness_treshold_, 0.12)) - { - ROS_WARN("did not find edges_sharpness_treshold, using default"); - } - bool track_unknown_space; - nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown()); - if (track_unknown_space) - default_value_ = NO_INFORMATION; - else - default_value_ = FREE_SPACE; - - // Subscribe to topic - elevation_subscriber_ = nh.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); - dsrv_ = NULL; - setupDynamicReconfigure(nh); - - // Setup filter chain. - if (!filterChain_.configure(filter_chain_parameters_name_, nh)) { - ROS_WARN("Could not configure the filter chain!"); - }else{ - filters_configuration_loaded_ = true; - } - } - - ElevationLayer::~ElevationLayer() - { - if (dsrv_) - delete dsrv_; - } - - void ElevationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, - double* max_x, double* max_y) - { - std::lock_guard lock(elevation_map_mutex_); - if (rolling_window_) - updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); - if (!(enabled_ && elevation_map_received_)) - return; - useExtraBounds(min_x, min_y, max_x, max_y); - - for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { - const grid_map::Index gridmap_index(*iterator); - grid_map::Position vertexPositionXY; - elevation_map_.getPosition(gridmap_index, vertexPositionXY); - double px = vertexPositionXY.x(); - double py = vertexPositionXY.y(); - - touch(px, py, min_x, min_y, max_x, max_y); - } - updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); - } - - void ElevationLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, - double* max_x, double* max_y) - { - if (!footprint_clearing_enabled_) return; - costmap_2d::transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); - - for (unsigned int i = 0; i < transformed_footprint_.size(); i++) - { - touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); - } - } - - void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) +namespace elevation_layer { + +ElevationLayer::ElevationLayer() : filterChain_("grid_map::GridMap"), elevation_map_received_(false), filters_configuration_loaded_(false) { + costmap_ = nullptr; // this is the unsigned char* member of parent class Costmap2D. +} + +void ElevationLayer::onInitialize() { + ros::NodeHandle nh("~/" + name_), g_nh; + rolling_window_ = layered_costmap_->isRolling(); + + ElevationLayer::matchSize(); + current_ = true; + elevation_map_received_ = false; + filters_configuration_loaded_ = false; + global_frame_ = layered_costmap_->getGlobalFrameID(); + elevation_layer_name_ = "elevation"; + edges_layer_name_ = "edges"; + + // get our tf prefix + ros::NodeHandle prefix_nh; + const std::string tf_prefix = tf::getPrefixParam(prefix_nh); + + // get parameters from config file + if (!nh.param("elevation_topic", elevation_topic_, std::string(""))) { + ROS_WARN("did not find elevation_topic, using default"); + } + if (!nh.param("height_threshold", height_threshold_, 0.12)) { + ROS_WARN("did not find height_threshold, using default"); + } + if (!nh.param("filter_chain_parameters_name", filter_chain_parameters_name_, std::string("elevation_filters"))) { + ROS_WARN("did not find filter_chain_param_name, using default"); + } + if (!nh.param("footprint_clearing_enabled", footprint_clearing_enabled_, true)) { + ROS_WARN("did not find footprint_clearing_enabled, using default"); + } + if (!nh.param("combination_method", combination_method_, 2)) { + ROS_WARN("did not find combination_method, using default"); + } + if (!nh.param("edges_sharpness_threshold", edges_sharpness_threshold_, 0.12)) { + ROS_WARN("did not find edges_sharpness_threshold, using default"); + } + bool track_unknown_space; + nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown()); + default_value_ = track_unknown_space ? NO_INFORMATION : FREE_SPACE; + + // Subscribe to topic + elevation_subscriber_ = nh.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); + dsrv_ = nullptr; + setupDynamicReconfigure(nh); + + // Setup filter chain. + if (!filterChain_.configure(filter_chain_parameters_name_, nh)) { + ROS_WARN("Could not configure the filter chain!"); + } else { + filters_configuration_loaded_ = true; + } +} + +void ElevationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, + double* max_y) { + std::lock_guard lock(elevation_map_mutex_); + if (rolling_window_) updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); + if (!(enabled_ && elevation_map_received_)) return; + useExtraBounds(min_x, min_y, max_x, max_y); + + for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index gridmap_index(*iterator); + grid_map::Position vertexPositionXY; + elevation_map_.getPosition(gridmap_index, vertexPositionXY); + double px = vertexPositionXY.x(); + double py = vertexPositionXY.y(); + + touch(px, py, min_x, min_y, max_x, max_y); + } + updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void ElevationLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, + double* max_y) { + if (!footprint_clearing_enabled_) return; + costmap_2d::transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); + + for (auto& i : transformed_footprint_) { + touch(i.x, i.y, min_x, min_y, max_x, max_y); + } +} + +void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { + std::lock_guard lock(elevation_map_mutex_); + if (!enabled_ || !elevation_map_received_) return; + const bool has_edges_layer = elevation_map_.exists(edges_layer_name_); + if (!has_edges_layer) { + ROS_WARN_THROTTLE(0.2, "No edges layer found !!"); + } + const grid_map::Matrix& elevation_data = elevation_map_[elevation_layer_name_]; + for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index gridmap_index(*iterator); + grid_map::Position vertexPositionXY; + elevation_map_.getPosition(gridmap_index, vertexPositionXY); + double px = vertexPositionXY.x(); + double py = vertexPositionXY.y(); + // now we need to compute the map coordinates for the observation + unsigned int mx, my; + if (!worldToMap(px, py, mx, my)) // if point outside of local costmap, ignore { - std::lock_guard lock(elevation_map_mutex_); - if (!enabled_ || !elevation_map_received_) - return; - const bool has_edges_layer = elevation_map_.exists(edges_layer_name_); - const grid_map::Matrix& elevation_data = elevation_map_[elevation_layer_name_]; - for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { - const grid_map::Index gridmap_index(*iterator); - grid_map::Position vertexPositionXY; - elevation_map_.getPosition(gridmap_index, vertexPositionXY); - double px = vertexPositionXY.x(); - double py = vertexPositionXY.y(); - // now we need to compute the map coordinates for the observation - unsigned int mx, my; - if (!worldToMap(px, py, mx, my)) // if point outside of local costmap, ignore - { - continue; - } - if ( elevation_data(gridmap_index(0), gridmap_index(1)) > height_treshold_ ) // If point too high, it could be an obstacle - { - if (!has_edges_layer) { - ROS_WARN("No edges layer found !!"); - } else{ - const grid_map::Matrix &edges_data = elevation_map_[edges_layer_name_]; - if (edges_data(gridmap_index(0), gridmap_index(1)) < edges_sharpness_treshold_) // if area not sharp, dont label as obstacle - { - setCost(mx, my, FREE_SPACE); - continue; - } - } - setCost(mx, my, LETHAL_OBSTACLE); - } - else{ - setCost(mx, my, FREE_SPACE); - } - } - - if (footprint_clearing_enabled_) - { - setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE); - } - - switch (combination_method_) - { - case 0: // Overwrite - updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); - break; - case 1: // Maximum - updateWithMax(master_grid, min_i, min_j, max_i, max_j); - break; - default: // Nothing - break; - } + continue; } - - void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& elevation) + if (elevation_data(gridmap_index(0), gridmap_index(1)) > height_threshold_) // If point too high, it could be an obstacle { - grid_map::GridMap incoming_map; - grid_map::GridMap filtered_map; - if(!grid_map::GridMapRosConverter::fromMessage(*elevation, incoming_map)) - { - ROS_WARN("Grid Map msg Conversion failed !"); - } - incoming_map.convertToDefaultStartIndex(); - // Apply filter chain. - if (filters_configuration_loaded_ && filterChain_.update(incoming_map, filtered_map)) + if (has_edges_layer) { + const grid_map::Matrix& edges_data = elevation_map_[edges_layer_name_]; + if (edges_data(gridmap_index(0), gridmap_index(1)) < edges_sharpness_threshold_) // if area not sharp, dont label as obstacle { - std::lock_guard lock(elevation_map_mutex_); - elevation_map_ = filtered_map; - height_treshold_ /= 2; // Half the treshold since the highest sharpness is at midheigth of the obstacles - } - else{ - std::lock_guard lock(elevation_map_mutex_); - ROS_WARN("Could not use the filter chain!"); - elevation_map_ = incoming_map; + setCost(mx, my, FREE_SPACE); + continue; } - if(!elevation_map_received_) - { -// elevation_map_received_.store(true); - elevation_map_received_ = true; - } - } - - void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nh) - { - dsrv_ = new dynamic_reconfigure::Server(nh); - dynamic_reconfigure::Server::CallbackType cb = boost::bind( - &ElevationLayer::reconfigureCB, this, _1, _2); - dsrv_->setCallback(cb); - } - - void ElevationLayer::reconfigureCB(elevation_layer::ElevationPluginConfig &config, uint32_t level) - { - enabled_ = config.enabled; - } - - void ElevationLayer::reset() - { - deactivate(); - resetMaps(); - current_ = true; - activate(); - } - - void ElevationLayer::activate() - { - // if we're stopped we need to re-subscribe to topics - ros::NodeHandle nh("~/" + name_); - elevation_subscriber_ = nh.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); - - } - void ElevationLayer::deactivate() - { - elevation_subscriber_.shutdown(); + } + setCost(mx, my, LETHAL_OBSTACLE); + } else { + setCost(mx, my, FREE_SPACE); } -} \ No newline at end of file + } + + if (footprint_clearing_enabled_) { + setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE); + } + + switch (combination_method_) { + case 0: // Overwrite + updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); + break; + case 1: // Maximum + updateWithMax(master_grid, min_i, min_j, max_i, max_j); + break; + default: // Nothing + break; + } +} + +void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& elevation) { + grid_map::GridMap incoming_map; + grid_map::GridMap filtered_map; + if (!grid_map::GridMapRosConverter::fromMessage(*elevation, incoming_map)) { + ROS_WARN("Grid Map msg Conversion failed !"); + } + incoming_map.convertToDefaultStartIndex(); + // Apply filter chain. + if (filters_configuration_loaded_ && filterChain_.update(incoming_map, filtered_map)) { + std::lock_guard lock(elevation_map_mutex_); + elevation_map_ = filtered_map; + height_threshold_ /= 2; // Half the threshold since the highest sharpness is at midheigth of the obstacles + } else { + std::lock_guard lock(elevation_map_mutex_); + ROS_WARN("Could not use the filter chain!"); + elevation_map_ = incoming_map; + } + if (!elevation_map_received_) { + // elevation_map_received_.store(true); + elevation_map_received_ = true; + } +} + +void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nh) { + dsrv_ = new dynamic_reconfigure::Server(nh); + dynamic_reconfigure::Server::CallbackType cb = + boost::bind(&ElevationLayer::reconfigureCB, this, _1, _2); + dsrv_->setCallback(cb); +} + +void ElevationLayer::reconfigureCB(elevation_layer::ElevationPluginConfig& config, uint32_t level) { enabled_ = config.enabled; } + +void ElevationLayer::reset() { + deactivate(); + resetMaps(); + current_ = true; + activate(); +} + +void ElevationLayer::activate() { + // if we're stopped we need to re-subscribe to topics + ros::NodeHandle nh("~/" + name_); + elevation_subscriber_ = nh.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); +} +void ElevationLayer::deactivate() { elevation_subscriber_.shutdown(); } +} // namespace elevation_layer \ No newline at end of file From b2cfdea1a190c9bf3200efed884a6ecd37828289 Mon Sep 17 00:00:00 2001 From: chisarie Date: Tue, 6 Nov 2018 16:04:36 +0100 Subject: [PATCH 17/36] added elevation_filters.yaml in here --- elevation_layer/config/elevation_filters.yaml | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 elevation_layer/config/elevation_filters.yaml diff --git a/elevation_layer/config/elevation_filters.yaml b/elevation_layer/config/elevation_filters.yaml new file mode 100644 index 00000000..347a2233 --- /dev/null +++ b/elevation_layer/config/elevation_filters.yaml @@ -0,0 +1,12 @@ +elevation_filters: + # Edge detection on elevation layer with convolution filter as alternative to filter above. + - name: edge_detection + type: gridMapFilters/SlidingWindowMathExpressionFilter + params: + input_layer: elevation + output_layer: edges + expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation)' # Edge detection. + # expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation)' # Sharpen. + compute_empty_cells: false + edge_handling: mean # options: inside, crop, empty, mean + window_size: 3 # Make sure to make this compatible with the kernel matrix. \ No newline at end of file From a0387bc28ce6935be0ab48ab4f112c5f90bffa1f Mon Sep 17 00:00:00 2001 From: chisarie Date: Tue, 6 Nov 2018 17:17:17 +0100 Subject: [PATCH 18/36] added param_io, throttled some warnings and now dividing by 2.0 instead of 2 --- elevation_layer/CMakeLists.txt | 2 + .../include/elevation_layer/elevation_layer.h | 1 + elevation_layer/package.xml | 1 + elevation_layer/src/elevation_layer.cpp | 37 +++++++------------ 4 files changed, 18 insertions(+), 23 deletions(-) diff --git a/elevation_layer/CMakeLists.txt b/elevation_layer/CMakeLists.txt index b6892caa..9f4eeadb 100644 --- a/elevation_layer/CMakeLists.txt +++ b/elevation_layer/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure grid_map_ros filters + param_io ) # Find system libraries @@ -46,6 +47,7 @@ catkin_package( dynamic_reconfigure grid_map_ros filters + param_io DEPENDS Boost ) diff --git a/elevation_layer/include/elevation_layer/elevation_layer.h b/elevation_layer/include/elevation_layer/elevation_layer.h index f970a3cf..a5dcf6a4 100644 --- a/elevation_layer/include/elevation_layer/elevation_layer.h +++ b/elevation_layer/include/elevation_layer/elevation_layer.h @@ -18,6 +18,7 @@ #include #include #include "grid_map_ros/GridMapRosConverter.hpp" +#include #include #include diff --git a/elevation_layer/package.xml b/elevation_layer/package.xml index 6598eb21..e5c7e6cb 100644 --- a/elevation_layer/package.xml +++ b/elevation_layer/package.xml @@ -16,6 +16,7 @@ dynamic_reconfigure grid_map_ros filters + param_io diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index b82db906..ff4fe85d 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -41,26 +41,14 @@ void ElevationLayer::onInitialize() { const std::string tf_prefix = tf::getPrefixParam(prefix_nh); // get parameters from config file - if (!nh.param("elevation_topic", elevation_topic_, std::string(""))) { - ROS_WARN("did not find elevation_topic, using default"); - } - if (!nh.param("height_threshold", height_threshold_, 0.12)) { - ROS_WARN("did not find height_threshold, using default"); - } - if (!nh.param("filter_chain_parameters_name", filter_chain_parameters_name_, std::string("elevation_filters"))) { - ROS_WARN("did not find filter_chain_param_name, using default"); - } - if (!nh.param("footprint_clearing_enabled", footprint_clearing_enabled_, true)) { - ROS_WARN("did not find footprint_clearing_enabled, using default"); - } - if (!nh.param("combination_method", combination_method_, 2)) { - ROS_WARN("did not find combination_method, using default"); - } - if (!nh.param("edges_sharpness_threshold", edges_sharpness_threshold_, 0.12)) { - ROS_WARN("did not find edges_sharpness_threshold, using default"); - } - bool track_unknown_space; - nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown()); + param_io::getParam(nh, "elevation_topic", elevation_topic_); + param_io::getParam(nh,"height_threshold", height_threshold_); + param_io::getParam(nh,"filter_chain_parameters_name", filter_chain_parameters_name_); + param_io::getParam(nh,"footprint_clearing_enabled", footprint_clearing_enabled_); + param_io::getParam(nh,"combination_method", combination_method_); + param_io::getParam(nh,"edges_sharpness_threshold", edges_sharpness_threshold_); + bool track_unknown_space = layered_costmap_->isTrackingUnknown(); + param_io::getParam(nh,"track_unknown_space", track_unknown_space); default_value_ = track_unknown_space ? NO_INFORMATION : FREE_SPACE; // Subscribe to topic @@ -161,17 +149,20 @@ void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& grid_map::GridMap incoming_map; grid_map::GridMap filtered_map; if (!grid_map::GridMapRosConverter::fromMessage(*elevation, incoming_map)) { - ROS_WARN("Grid Map msg Conversion failed !"); + ROS_WARN_THROTTLE(0.2,"Grid Map msg Conversion failed !"); } incoming_map.convertToDefaultStartIndex(); + if (!(global_frame_ == incoming_map.getFrameId())) { + ROS_WARN_THROTTLE(0.2, "Incoming elevation_map frame different than expected! " ); + } // Apply filter chain. if (filters_configuration_loaded_ && filterChain_.update(incoming_map, filtered_map)) { std::lock_guard lock(elevation_map_mutex_); elevation_map_ = filtered_map; - height_threshold_ /= 2; // Half the threshold since the highest sharpness is at midheigth of the obstacles + height_threshold_ /= 2.0; // Half the threshold since the highest sharpness is at midheigth of the obstacles } else { std::lock_guard lock(elevation_map_mutex_); - ROS_WARN("Could not use the filter chain!"); + ROS_WARN_THROTTLE(0.2,"Could not use the filter chain!"); elevation_map_ = incoming_map; } if (!elevation_map_received_) { From f721df4080d000ca25144e472d0ca5e0c083e7aa Mon Sep 17 00:00:00 2001 From: chisarie Date: Tue, 6 Nov 2018 17:41:51 +0100 Subject: [PATCH 19/36] moved layer names in yaml file --- elevation_layer/src/elevation_layer.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index ff4fe85d..7a8e95f6 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -33,8 +33,6 @@ void ElevationLayer::onInitialize() { elevation_map_received_ = false; filters_configuration_loaded_ = false; global_frame_ = layered_costmap_->getGlobalFrameID(); - elevation_layer_name_ = "elevation"; - edges_layer_name_ = "edges"; // get our tf prefix ros::NodeHandle prefix_nh; @@ -42,13 +40,15 @@ void ElevationLayer::onInitialize() { // get parameters from config file param_io::getParam(nh, "elevation_topic", elevation_topic_); - param_io::getParam(nh,"height_threshold", height_threshold_); - param_io::getParam(nh,"filter_chain_parameters_name", filter_chain_parameters_name_); - param_io::getParam(nh,"footprint_clearing_enabled", footprint_clearing_enabled_); - param_io::getParam(nh,"combination_method", combination_method_); - param_io::getParam(nh,"edges_sharpness_threshold", edges_sharpness_threshold_); + param_io::getParam(nh, "height_threshold", height_threshold_); + param_io::getParam(nh, "filter_chain_parameters_name", filter_chain_parameters_name_); + param_io::getParam(nh, "elevation_layer_name", elevation_layer_name_); + param_io::getParam(nh, "edges_layer_name", edges_layer_name_); + param_io::getParam(nh, "footprint_clearing_enabled", footprint_clearing_enabled_); + param_io::getParam(nh, "combination_method", combination_method_); + param_io::getParam(nh, "edges_sharpness_threshold", edges_sharpness_threshold_); bool track_unknown_space = layered_costmap_->isTrackingUnknown(); - param_io::getParam(nh,"track_unknown_space", track_unknown_space); + param_io::getParam(nh, "track_unknown_space", track_unknown_space); default_value_ = track_unknown_space ? NO_INFORMATION : FREE_SPACE; // Subscribe to topic @@ -149,11 +149,11 @@ void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& grid_map::GridMap incoming_map; grid_map::GridMap filtered_map; if (!grid_map::GridMapRosConverter::fromMessage(*elevation, incoming_map)) { - ROS_WARN_THROTTLE(0.2,"Grid Map msg Conversion failed !"); + ROS_WARN_THROTTLE(0.2, "Grid Map msg Conversion failed !"); } incoming_map.convertToDefaultStartIndex(); if (!(global_frame_ == incoming_map.getFrameId())) { - ROS_WARN_THROTTLE(0.2, "Incoming elevation_map frame different than expected! " ); + ROS_WARN_THROTTLE(0.2, "Incoming elevation_map frame different than expected! "); } // Apply filter chain. if (filters_configuration_loaded_ && filterChain_.update(incoming_map, filtered_map)) { @@ -162,7 +162,7 @@ void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& height_threshold_ /= 2.0; // Half the threshold since the highest sharpness is at midheigth of the obstacles } else { std::lock_guard lock(elevation_map_mutex_); - ROS_WARN_THROTTLE(0.2,"Could not use the filter chain!"); + ROS_WARN_THROTTLE(0.2, "Could not use the filter chain!"); elevation_map_ = incoming_map; } if (!elevation_map_received_) { From cdc2d1abc5dac126faa95514977e8c5e215ed392 Mon Sep 17 00:00:00 2001 From: chisarie Date: Tue, 6 Nov 2018 17:58:50 +0100 Subject: [PATCH 20/36] added smart pointer --- elevation_layer/include/elevation_layer/elevation_layer.h | 2 +- elevation_layer/src/elevation_layer.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/elevation_layer/include/elevation_layer/elevation_layer.h b/elevation_layer/include/elevation_layer/elevation_layer.h index a5dcf6a4..bb649491 100644 --- a/elevation_layer/include/elevation_layer/elevation_layer.h +++ b/elevation_layer/include/elevation_layer/elevation_layer.h @@ -44,7 +44,7 @@ class ElevationLayer : public costmap_2d::CostmapLayer { std::string global_frame_; ///< @brief The global frame for the costmap std::vector > elevation_subscribers_; ///< @brief Used for the observation message filters - dynamic_reconfigure::Server* dsrv_; + std::unique_ptr< dynamic_reconfigure::Server > dsrv_; virtual void setupDynamicReconfigure(ros::NodeHandle& nh); int combination_method_; diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 7a8e95f6..abf4ee50 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -172,7 +172,7 @@ void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& } void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nh) { - dsrv_ = new dynamic_reconfigure::Server(nh); + dsrv_.reset(new dynamic_reconfigure::Server(nh)); dynamic_reconfigure::Server::CallbackType cb = boost::bind(&ElevationLayer::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); From 3c8ee6a59c4437da0b1f1e28e733b89395b0c6c6 Mon Sep 17 00:00:00 2001 From: chisarie Date: Wed, 7 Nov 2018 10:05:30 +0100 Subject: [PATCH 21/36] converted combination_method to enum --- .../include/elevation_layer/elevation_layer.h | 7 +++++-- elevation_layer/src/elevation_layer.cpp | 21 +++++++++++++++---- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/elevation_layer/include/elevation_layer/elevation_layer.h b/elevation_layer/include/elevation_layer/elevation_layer.h index bb649491..d1c87313 100644 --- a/elevation_layer/include/elevation_layer/elevation_layer.h +++ b/elevation_layer/include/elevation_layer/elevation_layer.h @@ -26,6 +26,10 @@ #include namespace elevation_layer { + +enum CombinationMethod {Overwrite, Maximum, Unknown}; +CombinationMethod convertCombinationMethod(const std::string& str); + class ElevationLayer : public costmap_2d::CostmapLayer { public: ElevationLayer(); @@ -46,8 +50,7 @@ class ElevationLayer : public costmap_2d::CostmapLayer { elevation_subscribers_; ///< @brief Used for the observation message filters std::unique_ptr< dynamic_reconfigure::Server > dsrv_; virtual void setupDynamicReconfigure(ros::NodeHandle& nh); - - int combination_method_; + CombinationMethod combination_method_; std::vector transformed_footprint_; bool rolling_window_; bool footprint_clearing_enabled_; diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index abf4ee50..da27f6ec 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -45,11 +45,13 @@ void ElevationLayer::onInitialize() { param_io::getParam(nh, "elevation_layer_name", elevation_layer_name_); param_io::getParam(nh, "edges_layer_name", edges_layer_name_); param_io::getParam(nh, "footprint_clearing_enabled", footprint_clearing_enabled_); - param_io::getParam(nh, "combination_method", combination_method_); param_io::getParam(nh, "edges_sharpness_threshold", edges_sharpness_threshold_); bool track_unknown_space = layered_costmap_->isTrackingUnknown(); param_io::getParam(nh, "track_unknown_space", track_unknown_space); default_value_ = track_unknown_space ? NO_INFORMATION : FREE_SPACE; + std::string combination_method; + param_io::getParam(nh, "combination_method", combination_method); + combination_method_ = convertCombinationMethod(combination_method); // Subscribe to topic elevation_subscriber_ = nh.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); @@ -134,17 +136,28 @@ void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, } switch (combination_method_) { - case 0: // Overwrite + case Overwrite: updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); break; - case 1: // Maximum + case Maximum: updateWithMax(master_grid, min_i, min_j, max_i, max_j); break; - default: // Nothing + default: // Do Nothing break; } } +CombinationMethod convertCombinationMethod(const std::string& str) { + if (str == "Overwrite") { + return Overwrite; + } else if (str == "Maximum") { + return Maximum; + } else { + ROS_WARN_THROTTLE(0.2, "Unknow combination method !"); + return Unknown; + } +} + void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& elevation) { grid_map::GridMap incoming_map; grid_map::GridMap filtered_map; From a15f192a9578845f6af04b6c5fa9ae9cda7b5fcc Mon Sep 17 00:00:00 2001 From: chisarie Date: Wed, 7 Nov 2018 10:29:26 +0100 Subject: [PATCH 22/36] set current_ to false if last received elevation map to old --- elevation_layer/include/elevation_layer/elevation_layer.h | 7 ++++--- elevation_layer/src/elevation_layer.cpp | 5 +++++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/elevation_layer/include/elevation_layer/elevation_layer.h b/elevation_layer/include/elevation_layer/elevation_layer.h index d1c87313..4f3e15b8 100644 --- a/elevation_layer/include/elevation_layer/elevation_layer.h +++ b/elevation_layer/include/elevation_layer/elevation_layer.h @@ -17,8 +17,8 @@ #include #include #include -#include "grid_map_ros/GridMapRosConverter.hpp" #include +#include "grid_map_ros/GridMapRosConverter.hpp" #include #include @@ -27,7 +27,7 @@ namespace elevation_layer { -enum CombinationMethod {Overwrite, Maximum, Unknown}; +enum CombinationMethod { Overwrite, Maximum, Unknown }; CombinationMethod convertCombinationMethod(const std::string& str); class ElevationLayer : public costmap_2d::CostmapLayer { @@ -48,7 +48,7 @@ class ElevationLayer : public costmap_2d::CostmapLayer { std::string global_frame_; ///< @brief The global frame for the costmap std::vector > elevation_subscribers_; ///< @brief Used for the observation message filters - std::unique_ptr< dynamic_reconfigure::Server > dsrv_; + std::unique_ptr > dsrv_; virtual void setupDynamicReconfigure(ros::NodeHandle& nh); CombinationMethod combination_method_; std::vector transformed_footprint_; @@ -62,6 +62,7 @@ class ElevationLayer : public costmap_2d::CostmapLayer { grid_map::GridMap elevation_map_; std::mutex elevation_map_mutex_; ros::Subscriber elevation_subscriber_; + ros::Time last_elevation_map_update_; double height_threshold_; double edges_sharpness_threshold_; std::string elevation_topic_; diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index da27f6ec..24c5d2c8 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -102,6 +102,10 @@ void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, if (!has_edges_layer) { ROS_WARN_THROTTLE(0.2, "No edges layer found !!"); } + ros::Duration time_since_elevation_map_received = ros::Time::now() - last_elevation_map_update_; + if (time_since_elevation_map_received > ros::Duration(5.0)) { + current_ = false; + } const grid_map::Matrix& elevation_data = elevation_map_[elevation_layer_name_]; for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { const grid_map::Index gridmap_index(*iterator); @@ -164,6 +168,7 @@ void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& if (!grid_map::GridMapRosConverter::fromMessage(*elevation, incoming_map)) { ROS_WARN_THROTTLE(0.2, "Grid Map msg Conversion failed !"); } + last_elevation_map_update_ = ros::Time::now(); incoming_map.convertToDefaultStartIndex(); if (!(global_frame_ == incoming_map.getFrameId())) { ROS_WARN_THROTTLE(0.2, "Incoming elevation_map frame different than expected! "); From 570622c49098083f7c62ff50ddd459d91e8517d3 Mon Sep 17 00:00:00 2001 From: chisarie Date: Wed, 7 Nov 2018 11:37:15 +0100 Subject: [PATCH 23/36] converted param_io back to standard Ros --- elevation_layer/CMakeLists.txt | 2 - .../include/elevation_layer/elevation_layer.h | 1 - elevation_layer/package.xml | 1 - elevation_layer/src/elevation_layer.cpp | 39 ++++++++++++++----- 4 files changed, 29 insertions(+), 14 deletions(-) diff --git a/elevation_layer/CMakeLists.txt b/elevation_layer/CMakeLists.txt index 9f4eeadb..b6892caa 100644 --- a/elevation_layer/CMakeLists.txt +++ b/elevation_layer/CMakeLists.txt @@ -15,7 +15,6 @@ find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure grid_map_ros filters - param_io ) # Find system libraries @@ -47,7 +46,6 @@ catkin_package( dynamic_reconfigure grid_map_ros filters - param_io DEPENDS Boost ) diff --git a/elevation_layer/include/elevation_layer/elevation_layer.h b/elevation_layer/include/elevation_layer/elevation_layer.h index 4f3e15b8..129d9182 100644 --- a/elevation_layer/include/elevation_layer/elevation_layer.h +++ b/elevation_layer/include/elevation_layer/elevation_layer.h @@ -17,7 +17,6 @@ #include #include #include -#include #include "grid_map_ros/GridMapRosConverter.hpp" #include diff --git a/elevation_layer/package.xml b/elevation_layer/package.xml index e5c7e6cb..6598eb21 100644 --- a/elevation_layer/package.xml +++ b/elevation_layer/package.xml @@ -16,7 +16,6 @@ dynamic_reconfigure grid_map_ros filters - param_io diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 24c5d2c8..0cc71917 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -25,7 +25,7 @@ ElevationLayer::ElevationLayer() : filterChain_("grid_map::GridMap"), elevation_ } void ElevationLayer::onInitialize() { - ros::NodeHandle nh("~/" + name_), g_nh; + ros::NodeHandle nh("~/" + name_); rolling_window_ = layered_costmap_->isRolling(); ElevationLayer::matchSize(); @@ -39,18 +39,36 @@ void ElevationLayer::onInitialize() { const std::string tf_prefix = tf::getPrefixParam(prefix_nh); // get parameters from config file - param_io::getParam(nh, "elevation_topic", elevation_topic_); - param_io::getParam(nh, "height_threshold", height_threshold_); - param_io::getParam(nh, "filter_chain_parameters_name", filter_chain_parameters_name_); - param_io::getParam(nh, "elevation_layer_name", elevation_layer_name_); - param_io::getParam(nh, "edges_layer_name", edges_layer_name_); - param_io::getParam(nh, "footprint_clearing_enabled", footprint_clearing_enabled_); - param_io::getParam(nh, "edges_sharpness_threshold", edges_sharpness_threshold_); + if (!nh.param("elevation_topic", elevation_topic_, std::string(""))) { + ROS_WARN("did not find elevation_topic, using default"); + } + if (!nh.param("height_threshold", height_threshold_, 0.0)) { + ROS_WARN("did not find height_treshold, using default"); + } + if (!nh.param("filter_chain_parameters_name", filter_chain_parameters_name_, std::string(""))) { + ROS_WARN("did not find filter_chain_param_name, using default"); + } + if (!nh.param("elevation_layer_name", elevation_layer_name_, std::string(""))) { + ROS_WARN("did not find elevation_layer_name, using default"); + } + if (!nh.param("edges_layer_name", edges_layer_name_, std::string(""))) { + ROS_WARN("did not find edges_layer_name, using default"); + } + if (!nh.param("footprint_clearing_enabled", footprint_clearing_enabled_, false)) { + ROS_WARN("did not find footprint_clearing_enabled, using default"); + } + if (!nh.param("edges_sharpness_threshold", edges_sharpness_threshold_, 0.0)) { + ROS_WARN("did not find edges_sharpness_treshold, using default"); + } bool track_unknown_space = layered_costmap_->isTrackingUnknown(); - param_io::getParam(nh, "track_unknown_space", track_unknown_space); + if (!nh.param("track_unknown_space", track_unknown_space, false)) { + ROS_WARN("did not find track_unknown_space, using default"); + } default_value_ = track_unknown_space ? NO_INFORMATION : FREE_SPACE; std::string combination_method; - param_io::getParam(nh, "combination_method", combination_method); + if (!nh.param("combination_method", combination_method, std::string(""))) { + ROS_WARN("did not find combination_method, using default"); + } combination_method_ = convertCombinationMethod(combination_method); // Subscribe to topic @@ -167,6 +185,7 @@ void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& grid_map::GridMap filtered_map; if (!grid_map::GridMapRosConverter::fromMessage(*elevation, incoming_map)) { ROS_WARN_THROTTLE(0.2, "Grid Map msg Conversion failed !"); + return; } last_elevation_map_update_ = ros::Time::now(); incoming_map.convertToDefaultStartIndex(); From d927d28bc0a2e039cb237e3c7118b1f12636ff81 Mon Sep 17 00:00:00 2001 From: chisarie Date: Wed, 7 Nov 2018 15:11:16 +0100 Subject: [PATCH 24/36] added doxygen comments to header file --- .../include/elevation_layer/elevation_layer.h | 115 ++++++++++++++++-- 1 file changed, 108 insertions(+), 7 deletions(-) diff --git a/elevation_layer/include/elevation_layer/elevation_layer.h b/elevation_layer/include/elevation_layer/elevation_layer.h index 129d9182..17e2f153 100644 --- a/elevation_layer/include/elevation_layer/elevation_layer.h +++ b/elevation_layer/include/elevation_layer/elevation_layer.h @@ -26,54 +26,155 @@ namespace elevation_layer { +/*! + * Method to update the cost of a portion of map + */ enum CombinationMethod { Overwrite, Maximum, Unknown }; + +/*! + * converts the string from the yaml file to the proper CombinationMethod enum + * @param str input string from yaml file + * @return CombinationMethod enum equivalent + */ CombinationMethod convertCombinationMethod(const std::string& str); +/*! + * Plug-in layer of a costmap_2d derived from elevation_map information + */ class ElevationLayer : public costmap_2d::CostmapLayer { public: + /** + * Constructor + */ ElevationLayer(); + + /** + * Destructor + */ ~ElevationLayer() override = default; + + /** + * Function called from parent class at initialization + */ void onInitialize() override; + + /** + * @brief This is called by the LayeredCostmap to poll this plugin as to how + * much of the costmap it needs to update. Each layer can increase + * the size of this bounds. + * + * For more details, see "Layered Costmaps for Context-Sensitive Navigation", + * by Lu et. Al, IROS 2014. + */ void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) override; + + /** + * @brief Actually update the underlying costmap, only within the bounds + * calculated during UpdateBounds(). + */ void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override; + /** @brief Restart publishers if they've been stopped. */ void activate() override; + + /** @brief Stop publishers. */ void deactivate() override; + void reset() override; + /** + * Callback to receive the grid_map msg from elevation_map + * @param occupancy_grid GridMap msg from elevation_map + */ void elevationMapCallback(const grid_map_msgs::GridMapConstPtr& occupancy_grid); protected: - std::string global_frame_; ///< @brief The global frame for the costmap - std::vector > - elevation_subscribers_; ///< @brief Used for the observation message filters - std::unique_ptr > dsrv_; + + /** + * set up the dynamic reconfigure + * @param nh + */ virtual void setupDynamicReconfigure(ros::NodeHandle& nh); + + /** + * clear obstacles inside the footprint of the robort if the flag footprint_clearing_enabled_ is true + * @param robot_x + * @param robot_y + * @param robot_yaw + * @param min_x + * @param min_y + * @param max_x + * @param max_y + */ + void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y); + + ///< @brief The global frame for the costmap + std::string global_frame_; + + ///< @brief Used for the observation message filters + std::vector > elevation_subscribers_; + + ///< @brief dynamic reconfigure server + std::unique_ptr > dsrv_; + + ///< @brief combination method to use to update the cost of a portion of map CombinationMethod combination_method_; + + ///< @brief polygon describing the form of the footprint of the robot std::vector transformed_footprint_; + + ///< @brief whether the local costmap should move together with the robot bool rolling_window_; + + ///< @brief whether to clean the obstacles inside the robot footprint bool footprint_clearing_enabled_; + + ///< @brief whether the elevation_map msg was received std::atomic_bool elevation_map_received_; - void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y); private: + + /** + * dynamic reconfiguration of the parameters + * @param config + * @param level + */ void reconfigureCB(elevation_layer::ElevationPluginConfig& config, uint32_t level); + + ///< @brief the elevation_map from which to take the information abut the environment (filtered or not) grid_map::GridMap elevation_map_; + + ///< @brief lock_guard mutex to make elevation_map setting thread safe std::mutex elevation_map_mutex_; + + ///< @brief Ros subscriber to grid_map msgs ros::Subscriber elevation_subscriber_; + + ///< @brief last time an elevation_map was received ros::Time last_elevation_map_update_; + + ///< @brief height threshold below which nothing is considered obstacle double height_threshold_; + + ///< @brief sharpness threshold above which an object is considered an obstacle double edges_sharpness_threshold_; + + ///< @brief topic_name of the elevation_map incoming msg std::string elevation_topic_; - //! Filter chain. + ///< @brief Filter chain used to filter the incoming elevation_map filters::FilterChain filterChain_; - //! Filter chain parameters name. + ///< @brief Filter chain parameters name to use std::string filter_chain_parameters_name_; + ///< @brief whether filters configuration parameters was found bool filters_configuration_loaded_; + + ///< @brief name of the layer of the incoming map to use std::string elevation_layer_name_; + + ///< @brief name to give to the filtered layer std::string edges_layer_name_; }; From eb2d3fd389bb739c87cd395e0a0a5068c50ed5a1 Mon Sep 17 00:00:00 2001 From: chisarie Date: Thu, 8 Nov 2018 10:01:05 +0100 Subject: [PATCH 25/36] added max_allowed_blind_time, taken from parameter server --- elevation_layer/include/elevation_layer/elevation_layer.h | 3 +++ elevation_layer/src/elevation_layer.cpp | 5 ++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/elevation_layer/include/elevation_layer/elevation_layer.h b/elevation_layer/include/elevation_layer/elevation_layer.h index 17e2f153..9ce56c4b 100644 --- a/elevation_layer/include/elevation_layer/elevation_layer.h +++ b/elevation_layer/include/elevation_layer/elevation_layer.h @@ -132,6 +132,9 @@ class ElevationLayer : public costmap_2d::CostmapLayer { ///< @brief whether the elevation_map msg was received std::atomic_bool elevation_map_received_; + ///< @brief after this time [seconds] without receiving any elevation_map, the robot will have to stop + double max_allowed_blind_time_; + private: /** diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 0cc71917..25cd731e 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -60,6 +60,9 @@ void ElevationLayer::onInitialize() { if (!nh.param("edges_sharpness_threshold", edges_sharpness_threshold_, 0.0)) { ROS_WARN("did not find edges_sharpness_treshold, using default"); } + if (!nh.param("max_allowed_blind_time", max_allowed_blind_time_, 0.0)) { + ROS_WARN("did not find max_allowed_blind_time, using default"); + } bool track_unknown_space = layered_costmap_->isTrackingUnknown(); if (!nh.param("track_unknown_space", track_unknown_space, false)) { ROS_WARN("did not find track_unknown_space, using default"); @@ -121,7 +124,7 @@ void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, ROS_WARN_THROTTLE(0.2, "No edges layer found !!"); } ros::Duration time_since_elevation_map_received = ros::Time::now() - last_elevation_map_update_; - if (time_since_elevation_map_received > ros::Duration(5.0)) { + if (time_since_elevation_map_received > ros::Duration(max_allowed_blind_time_)) { current_ = false; } const grid_map::Matrix& elevation_data = elevation_map_[elevation_layer_name_]; From 0e0a86a44b7d29250b26aacf415f15d3d38c00f2 Mon Sep 17 00:00:00 2001 From: chisarie Date: Mon, 12 Nov 2018 10:16:49 +0100 Subject: [PATCH 26/36] last cleaning ups, and also made combination_method case insensitive --- elevation_layer/CMakeLists.txt | 4 +- .../include/elevation_layer/elevation_layer.h | 2 + elevation_layer/src/elevation_layer.cpp | 72 ++++++++++++------- 3 files changed, 50 insertions(+), 28 deletions(-) diff --git a/elevation_layer/CMakeLists.txt b/elevation_layer/CMakeLists.txt index b6892caa..a130005b 100644 --- a/elevation_layer/CMakeLists.txt +++ b/elevation_layer/CMakeLists.txt @@ -38,7 +38,7 @@ catkin_package( INCLUDE_DIRS include LIBRARIES - elevation_layer + ${PROJECT_NAME} CATKIN_DEPENDS roscpp tf @@ -84,7 +84,7 @@ target_link_libraries(${PROJECT_NAME} ## install ## ## executables and/or libraries for installation -install(TARGETS elevation_layer +install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/elevation_layer/include/elevation_layer/elevation_layer.h b/elevation_layer/include/elevation_layer/elevation_layer.h index 9ce56c4b..882d0de6 100644 --- a/elevation_layer/include/elevation_layer/elevation_layer.h +++ b/elevation_layer/include/elevation_layer/elevation_layer.h @@ -10,6 +10,7 @@ #include #include +#include #include #include @@ -80,6 +81,7 @@ class ElevationLayer : public costmap_2d::CostmapLayer { /** @brief Stop publishers. */ void deactivate() override; + /** @brief Deactivate, reset the map and then reactivate*/ void reset() override; /** diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 25cd731e..023e2799 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -20,7 +20,8 @@ using costmap_2d::ObservationBuffer; namespace elevation_layer { -ElevationLayer::ElevationLayer() : filterChain_("grid_map::GridMap"), elevation_map_received_(false), filters_configuration_loaded_(false) { +ElevationLayer::ElevationLayer() + : filterChain_("grid_map::GridMap"), elevation_map_received_(false), filters_configuration_loaded_(false) { costmap_ = nullptr; // this is the unsigned char* member of parent class Costmap2D. } @@ -87,11 +88,20 @@ void ElevationLayer::onInitialize() { } } -void ElevationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, - double* max_y) { +void ElevationLayer::updateBounds(double robot_x, + double robot_y, + double robot_yaw, + double *min_x, + double *min_y, + double *max_x, + double *max_y) { std::lock_guard lock(elevation_map_mutex_); - if (rolling_window_) updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); - if (!(enabled_ && elevation_map_received_)) return; + if (rolling_window_) { + updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); + } + if (!(enabled_ && elevation_map_received_)) { + return; + } useExtraBounds(min_x, min_y, max_x, max_y); for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { @@ -106,19 +116,28 @@ void ElevationLayer::updateBounds(double robot_x, double robot_y, double robot_y updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); } -void ElevationLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, - double* max_y) { - if (!footprint_clearing_enabled_) return; +void ElevationLayer::updateFootprint(double robot_x, + double robot_y, + double robot_yaw, + double *min_x, + double *min_y, + double *max_x, + double *max_y) { + if (!footprint_clearing_enabled_) { + return; + } costmap_2d::transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); - for (auto& i : transformed_footprint_) { + for (auto &i : transformed_footprint_) { touch(i.x, i.y, min_x, min_y, max_x, max_y); } } -void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { +void ElevationLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) { std::lock_guard lock(elevation_map_mutex_); - if (!enabled_ || !elevation_map_received_) return; + if (!enabled_ || !elevation_map_received_) { + return; + } const bool has_edges_layer = elevation_map_.exists(edges_layer_name_); if (!has_edges_layer) { ROS_WARN_THROTTLE(0.2, "No edges layer found !!"); @@ -127,7 +146,7 @@ void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, if (time_since_elevation_map_received > ros::Duration(max_allowed_blind_time_)) { current_ = false; } - const grid_map::Matrix& elevation_data = elevation_map_[elevation_layer_name_]; + const grid_map::Matrix &elevation_data = elevation_map_[elevation_layer_name_]; for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { const grid_map::Index gridmap_index(*iterator); grid_map::Position vertexPositionXY; @@ -140,11 +159,13 @@ void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, { continue; } - if (elevation_data(gridmap_index(0), gridmap_index(1)) > height_threshold_) // If point too high, it could be an obstacle + if (elevation_data(gridmap_index(0), gridmap_index(1)) + > height_threshold_) // If point too high, it could be an obstacle { if (has_edges_layer) { - const grid_map::Matrix& edges_data = elevation_map_[edges_layer_name_]; - if (edges_data(gridmap_index(0), gridmap_index(1)) < edges_sharpness_threshold_) // if area not sharp, dont label as obstacle + const grid_map::Matrix &edges_data = elevation_map_[edges_layer_name_]; + if (edges_data(gridmap_index(0), gridmap_index(1)) + < edges_sharpness_threshold_) // if area not sharp, dont label as obstacle { setCost(mx, my, FREE_SPACE); continue; @@ -161,21 +182,19 @@ void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, } switch (combination_method_) { - case Overwrite: - updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); + case Overwrite:updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); break; - case Maximum: - updateWithMax(master_grid, min_i, min_j, max_i, max_j); + case Maximum:updateWithMax(master_grid, min_i, min_j, max_i, max_j); break; default: // Do Nothing break; } } -CombinationMethod convertCombinationMethod(const std::string& str) { - if (str == "Overwrite") { +CombinationMethod convertCombinationMethod(const std::string &str) { + if (boost::iequals(str, "Overwrite")) { // Case insensitive comparison return Overwrite; - } else if (str == "Maximum") { + } else if (boost::iequals(str, "Maximum")) { // Case insensitive comparison return Maximum; } else { ROS_WARN_THROTTLE(0.2, "Unknow combination method !"); @@ -183,7 +202,7 @@ CombinationMethod convertCombinationMethod(const std::string& str) { } } -void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& elevation) { +void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr &elevation) { grid_map::GridMap incoming_map; grid_map::GridMap filtered_map; if (!grid_map::GridMapRosConverter::fromMessage(*elevation, incoming_map)) { @@ -206,19 +225,20 @@ void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& elevation_map_ = incoming_map; } if (!elevation_map_received_) { - // elevation_map_received_.store(true); elevation_map_received_ = true; } } -void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nh) { +void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle &nh) { dsrv_.reset(new dynamic_reconfigure::Server(nh)); dynamic_reconfigure::Server::CallbackType cb = boost::bind(&ElevationLayer::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); } -void ElevationLayer::reconfigureCB(elevation_layer::ElevationPluginConfig& config, uint32_t level) { enabled_ = config.enabled; } +void ElevationLayer::reconfigureCB(elevation_layer::ElevationPluginConfig &config, uint32_t level) { + enabled_ = config.enabled; +} void ElevationLayer::reset() { deactivate(); From 437f3f679ad135381ad1cd6c91d2483b0be2507a Mon Sep 17 00:00:00 2001 From: chisarie Date: Mon, 12 Nov 2018 17:50:09 +0100 Subject: [PATCH 27/36] deleted useless nodehandles and made one as member of the class --- .../include/elevation_layer/elevation_layer.h | 2 + elevation_layer/src/elevation_layer.cpp | 39 ++++++++----------- 2 files changed, 19 insertions(+), 22 deletions(-) diff --git a/elevation_layer/include/elevation_layer/elevation_layer.h b/elevation_layer/include/elevation_layer/elevation_layer.h index 882d0de6..4fa191e7 100644 --- a/elevation_layer/include/elevation_layer/elevation_layer.h +++ b/elevation_layer/include/elevation_layer/elevation_layer.h @@ -146,6 +146,8 @@ class ElevationLayer : public costmap_2d::CostmapLayer { */ void reconfigureCB(elevation_layer::ElevationPluginConfig& config, uint32_t level); + ros::NodeHandle nh_; + ///< @brief the elevation_map from which to take the information abut the environment (filtered or not) grid_map::GridMap elevation_map_; diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 023e2799..0b0af930 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -26,7 +26,7 @@ ElevationLayer::ElevationLayer() } void ElevationLayer::onInitialize() { - ros::NodeHandle nh("~/" + name_); + nh_ = ros::NodeHandle("~/" + name_); rolling_window_ = layered_costmap_->isRolling(); ElevationLayer::matchSize(); @@ -35,53 +35,49 @@ void ElevationLayer::onInitialize() { filters_configuration_loaded_ = false; global_frame_ = layered_costmap_->getGlobalFrameID(); - // get our tf prefix - ros::NodeHandle prefix_nh; - const std::string tf_prefix = tf::getPrefixParam(prefix_nh); - // get parameters from config file - if (!nh.param("elevation_topic", elevation_topic_, std::string(""))) { + if (!nh_.param("elevation_topic", elevation_topic_, std::string(""))) { ROS_WARN("did not find elevation_topic, using default"); } - if (!nh.param("height_threshold", height_threshold_, 0.0)) { + if (!nh_.param("height_threshold", height_threshold_, 0.0)) { ROS_WARN("did not find height_treshold, using default"); } - if (!nh.param("filter_chain_parameters_name", filter_chain_parameters_name_, std::string(""))) { + if (!nh_.param("filter_chain_parameters_name", filter_chain_parameters_name_, std::string(""))) { ROS_WARN("did not find filter_chain_param_name, using default"); } - if (!nh.param("elevation_layer_name", elevation_layer_name_, std::string(""))) { + if (!nh_.param("elevation_layer_name", elevation_layer_name_, std::string(""))) { ROS_WARN("did not find elevation_layer_name, using default"); } - if (!nh.param("edges_layer_name", edges_layer_name_, std::string(""))) { + if (!nh_.param("edges_layer_name", edges_layer_name_, std::string(""))) { ROS_WARN("did not find edges_layer_name, using default"); } - if (!nh.param("footprint_clearing_enabled", footprint_clearing_enabled_, false)) { + if (!nh_.param("footprint_clearing_enabled", footprint_clearing_enabled_, false)) { ROS_WARN("did not find footprint_clearing_enabled, using default"); } - if (!nh.param("edges_sharpness_threshold", edges_sharpness_threshold_, 0.0)) { + if (!nh_.param("edges_sharpness_threshold", edges_sharpness_threshold_, 0.0)) { ROS_WARN("did not find edges_sharpness_treshold, using default"); } - if (!nh.param("max_allowed_blind_time", max_allowed_blind_time_, 0.0)) { + if (!nh_.param("max_allowed_blind_time", max_allowed_blind_time_, 0.0)) { ROS_WARN("did not find max_allowed_blind_time, using default"); } bool track_unknown_space = layered_costmap_->isTrackingUnknown(); - if (!nh.param("track_unknown_space", track_unknown_space, false)) { + if (!nh_.param("track_unknown_space", track_unknown_space, false)) { ROS_WARN("did not find track_unknown_space, using default"); } default_value_ = track_unknown_space ? NO_INFORMATION : FREE_SPACE; std::string combination_method; - if (!nh.param("combination_method", combination_method, std::string(""))) { + if (!nh_.param("combination_method", combination_method, std::string(""))) { ROS_WARN("did not find combination_method, using default"); } combination_method_ = convertCombinationMethod(combination_method); // Subscribe to topic - elevation_subscriber_ = nh.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); + elevation_subscriber_ = nh_.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); dsrv_ = nullptr; - setupDynamicReconfigure(nh); + setupDynamicReconfigure(nh_); // Setup filter chain. - if (!filterChain_.configure(filter_chain_parameters_name_, nh)) { + if (!filterChain_.configure(filter_chain_parameters_name_, nh_)) { ROS_WARN("Could not configure the filter chain!"); } else { filters_configuration_loaded_ = true; @@ -229,8 +225,8 @@ void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr & } } -void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle &nh) { - dsrv_.reset(new dynamic_reconfigure::Server(nh)); +void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle &nh_) { + dsrv_.reset(new dynamic_reconfigure::Server(nh_)); dynamic_reconfigure::Server::CallbackType cb = boost::bind(&ElevationLayer::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); @@ -249,8 +245,7 @@ void ElevationLayer::reset() { void ElevationLayer::activate() { // if we're stopped we need to re-subscribe to topics - ros::NodeHandle nh("~/" + name_); - elevation_subscriber_ = nh.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); + elevation_subscriber_ = nh_.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); } void ElevationLayer::deactivate() { elevation_subscriber_.shutdown(); } } // namespace elevation_layer \ No newline at end of file From 927731ad0dce96c35700a225eb692065f00540a5 Mon Sep 17 00:00:00 2001 From: chisarie Date: Tue, 4 Dec 2018 18:09:29 +0100 Subject: [PATCH 28/36] refactored variable names, include order and doxygen format --- .gitignore | 4 + elevation_layer/config/elevation_filters.yaml | 4 +- ...{elevation_layer.h => elevation_layer.hpp} | 130 ++++++++-------- elevation_layer/src/elevation_layer.cpp | 144 +++++++++--------- 4 files changed, 142 insertions(+), 140 deletions(-) rename elevation_layer/include/elevation_layer/{elevation_layer.h => elevation_layer.hpp} (55%) diff --git a/.gitignore b/.gitignore index 0a21979a..6fbd6f94 100644 --- a/.gitignore +++ b/.gitignore @@ -27,3 +27,7 @@ # Sublime *.sublime* + +# VSCode +.vscode +*/build/* diff --git a/elevation_layer/config/elevation_filters.yaml b/elevation_layer/config/elevation_filters.yaml index 347a2233..bbd49754 100644 --- a/elevation_layer/config/elevation_filters.yaml +++ b/elevation_layer/config/elevation_filters.yaml @@ -1,12 +1,10 @@ elevation_filters: - # Edge detection on elevation layer with convolution filter as alternative to filter above. - - name: edge_detection + - name: edge_detection # Edge detection on elevation layer with convolution filter. type: gridMapFilters/SlidingWindowMathExpressionFilter params: input_layer: elevation output_layer: edges expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation)' # Edge detection. - # expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation)' # Sharpen. compute_empty_cells: false edge_handling: mean # options: inside, crop, empty, mean window_size: 3 # Make sure to make this compatible with the kernel matrix. \ No newline at end of file diff --git a/elevation_layer/include/elevation_layer/elevation_layer.h b/elevation_layer/include/elevation_layer/elevation_layer.hpp similarity index 55% rename from elevation_layer/include/elevation_layer/elevation_layer.h rename to elevation_layer/include/elevation_layer/elevation_layer.hpp index 4fa191e7..306268ff 100644 --- a/elevation_layer/include/elevation_layer/elevation_layer.h +++ b/elevation_layer/include/elevation_layer/elevation_layer.hpp @@ -8,22 +8,27 @@ #pragma once +// C++ Libraries #include -#include #include +#include + +// Ros headers +#include +#include +#include +// Costmap2d headers #include +#include #include #include + +// Package related headers +#include "elevation_layer/ElevationPluginConfig.h" #include +#include #include -#include -#include "grid_map_ros/GridMapRosConverter.hpp" - -#include -#include -#include -#include namespace elevation_layer { @@ -44,22 +49,22 @@ CombinationMethod convertCombinationMethod(const std::string& str); */ class ElevationLayer : public costmap_2d::CostmapLayer { public: - /** + /*! * Constructor */ ElevationLayer(); - /** + /*! * Destructor */ ~ElevationLayer() override = default; - /** + /*! * Function called from parent class at initialization */ void onInitialize() override; - /** + /*! * @brief This is called by the LayeredCostmap to poll this plugin as to how * much of the costmap it needs to update. Each layer can increase * the size of this bounds. @@ -69,37 +74,36 @@ class ElevationLayer : public costmap_2d::CostmapLayer { */ void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) override; - /** + /*! * @brief Actually update the underlying costmap, only within the bounds * calculated during UpdateBounds(). */ void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override; - /** @brief Restart publishers if they've been stopped. */ + /*! @brief Restart publishers if they've been stopped. */ void activate() override; - /** @brief Stop publishers. */ + /*! @brief Stop publishers. */ void deactivate() override; - /** @brief Deactivate, reset the map and then reactivate*/ + /*! @brief Deactivate, reset the map and then reactivate*/ void reset() override; - /** + /*! * Callback to receive the grid_map msg from elevation_map * @param occupancy_grid GridMap msg from elevation_map */ void elevationMapCallback(const grid_map_msgs::GridMapConstPtr& occupancy_grid); protected: - - /** + /*! * set up the dynamic reconfigure * @param nh */ virtual void setupDynamicReconfigure(ros::NodeHandle& nh); - /** - * clear obstacles inside the footprint of the robort if the flag footprint_clearing_enabled_ is true + /*! + * clear obstacles inside the footprint of the robort if the flag footprintClearingEnabled_ is true * @param robot_x * @param robot_y * @param robot_yaw @@ -110,79 +114,75 @@ class ElevationLayer : public costmap_2d::CostmapLayer { */ void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y); - ///< @brief The global frame for the costmap - std::string global_frame_; + //! The global frame for the costmap + std::string globalFrame_; - ///< @brief Used for the observation message filters - std::vector > elevation_subscribers_; - - ///< @brief dynamic reconfigure server + //! dynamic reconfigure server std::unique_ptr > dsrv_; - ///< @brief combination method to use to update the cost of a portion of map - CombinationMethod combination_method_; + //! combination method to use to update the cost of a portion of map + CombinationMethod combinationMethod_; - ///< @brief polygon describing the form of the footprint of the robot - std::vector transformed_footprint_; + //! polygon describing the form of the footprint of the robot + std::vector transformedFootprint_; - ///< @brief whether the local costmap should move together with the robot - bool rolling_window_; + //! whether the local costmap should move together with the robot + bool rollingWindow_; - ///< @brief whether to clean the obstacles inside the robot footprint - bool footprint_clearing_enabled_; + //! whether to clean the obstacles inside the robot footprint + bool footprintClearingEnabled_; - ///< @brief whether the elevation_map msg was received - std::atomic_bool elevation_map_received_; + //! whether the elevation_map msg was received + std::atomic_bool elevationMapReceived_; - ///< @brief after this time [seconds] without receiving any elevation_map, the robot will have to stop - double max_allowed_blind_time_; + //! after this time [seconds] without receiving any elevation_map, the robot will have to stop + double maxAllowedBlindTime_; private: - - /** + /*! * dynamic reconfiguration of the parameters * @param config * @param level */ void reconfigureCB(elevation_layer::ElevationPluginConfig& config, uint32_t level); - ros::NodeHandle nh_; + ros::NodeHandle nodeHandle_; - ///< @brief the elevation_map from which to take the information abut the environment (filtered or not) - grid_map::GridMap elevation_map_; + //! the elevation_map from which to take the information abut the environment (filtered or not) + grid_map::GridMap elevationMap_; - ///< @brief lock_guard mutex to make elevation_map setting thread safe - std::mutex elevation_map_mutex_; + //! lock_guard mutex to make elevation_map setting thread safe + std::mutex elevationMapMutex_; - ///< @brief Ros subscriber to grid_map msgs - ros::Subscriber elevation_subscriber_; + //! Ros subscriber to grid_map msgs + ros::Subscriber elevationSubscriber_; - ///< @brief last time an elevation_map was received - ros::Time last_elevation_map_update_; + //! last time an elevation_map was received + ros::Time lastElevationMapUpdate_; - ///< @brief height threshold below which nothing is considered obstacle - double height_threshold_; + //! height threshold below which nothing is considered obstacle + double heightThreshold_; - ///< @brief sharpness threshold above which an object is considered an obstacle - double edges_sharpness_threshold_; + //! sharpness threshold above which an object is considered an obstacle + double edgesSharpnessThreshold_; - ///< @brief topic_name of the elevation_map incoming msg - std::string elevation_topic_; + //! topic_name of the elevation_map incoming msg + std::string elevationTopic_; - ///< @brief Filter chain used to filter the incoming elevation_map + //! Filter chain used to filter the incoming elevation_map filters::FilterChain filterChain_; - ///< @brief Filter chain parameters name to use - std::string filter_chain_parameters_name_; + //! Filter chain parameters name to use + std::string filterChainParametersName_; - ///< @brief whether filters configuration parameters was found - bool filters_configuration_loaded_; + //! whether filters configuration parameters was found + bool filtersConfigurationLoaded_; - ///< @brief name of the layer of the incoming map to use - std::string elevation_layer_name_; + //! name of the layer of the incoming map to use + std::string elevationLayerName_; - ///< @brief name to give to the filtered layer - std::string edges_layer_name_; + //! name to give to the filtered layer + std::string edgesLayerName_; }; } // namespace elevation_layer \ No newline at end of file diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_layer/src/elevation_layer.cpp index 0b0af930..75677f3e 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_layer/src/elevation_layer.cpp @@ -6,7 +6,7 @@ * Institute: ANYbotics */ -#include +#include "elevation_layer/elevation_layer.hpp" #include PLUGINLIB_EXPORT_CLASS(elevation_layer::ElevationLayer, costmap_2d::Layer) @@ -21,66 +21,66 @@ using costmap_2d::ObservationBuffer; namespace elevation_layer { ElevationLayer::ElevationLayer() - : filterChain_("grid_map::GridMap"), elevation_map_received_(false), filters_configuration_loaded_(false) { + : filterChain_("grid_map::GridMap"), elevationMapReceived_(false), filtersConfigurationLoaded_(false) { costmap_ = nullptr; // this is the unsigned char* member of parent class Costmap2D. } void ElevationLayer::onInitialize() { - nh_ = ros::NodeHandle("~/" + name_); - rolling_window_ = layered_costmap_->isRolling(); + nodeHandle_ = ros::NodeHandle("~/" + name_); + rollingWindow_ = layered_costmap_->isRolling(); ElevationLayer::matchSize(); current_ = true; - elevation_map_received_ = false; - filters_configuration_loaded_ = false; - global_frame_ = layered_costmap_->getGlobalFrameID(); + elevationMapReceived_ = false; + filtersConfigurationLoaded_ = false; + globalFrame_ = layered_costmap_->getGlobalFrameID(); // get parameters from config file - if (!nh_.param("elevation_topic", elevation_topic_, std::string(""))) { + if (!nodeHandle_.param("elevation_topic", elevationTopic_, std::string(""))) { ROS_WARN("did not find elevation_topic, using default"); } - if (!nh_.param("height_threshold", height_threshold_, 0.0)) { + if (!nodeHandle_.param("height_threshold", heightThreshold_, 0.0)) { ROS_WARN("did not find height_treshold, using default"); } - if (!nh_.param("filter_chain_parameters_name", filter_chain_parameters_name_, std::string(""))) { + if (!nodeHandle_.param("filter_chain_parameters_name", filterChainParametersName_, std::string(""))) { ROS_WARN("did not find filter_chain_param_name, using default"); } - if (!nh_.param("elevation_layer_name", elevation_layer_name_, std::string(""))) { + if (!nodeHandle_.param("elevation_layer_name", elevationLayerName_, std::string(""))) { ROS_WARN("did not find elevation_layer_name, using default"); } - if (!nh_.param("edges_layer_name", edges_layer_name_, std::string(""))) { + if (!nodeHandle_.param("edges_layer_name", edgesLayerName_, std::string(""))) { ROS_WARN("did not find edges_layer_name, using default"); } - if (!nh_.param("footprint_clearing_enabled", footprint_clearing_enabled_, false)) { + if (!nodeHandle_.param("footprint_clearing_enabled", footprintClearingEnabled_, false)) { ROS_WARN("did not find footprint_clearing_enabled, using default"); } - if (!nh_.param("edges_sharpness_threshold", edges_sharpness_threshold_, 0.0)) { + if (!nodeHandle_.param("edges_sharpness_threshold", edgesSharpnessThreshold_, 0.0)) { ROS_WARN("did not find edges_sharpness_treshold, using default"); } - if (!nh_.param("max_allowed_blind_time", max_allowed_blind_time_, 0.0)) { + if (!nodeHandle_.param("max_allowed_blind_time", maxAllowedBlindTime_, 0.0)) { ROS_WARN("did not find max_allowed_blind_time, using default"); } - bool track_unknown_space = layered_costmap_->isTrackingUnknown(); - if (!nh_.param("track_unknown_space", track_unknown_space, false)) { - ROS_WARN("did not find track_unknown_space, using default"); + bool trackUnknownSpace = layered_costmap_->isTrackingUnknown(); + if (!nodeHandle_.param("trackUnknownSpace", trackUnknownSpace, false)) { + ROS_WARN("did not find trackUnknownSpace, using default"); } - default_value_ = track_unknown_space ? NO_INFORMATION : FREE_SPACE; - std::string combination_method; - if (!nh_.param("combination_method", combination_method, std::string(""))) { + default_value_ = trackUnknownSpace ? NO_INFORMATION : FREE_SPACE; + std::string combinationMethod; + if (!nodeHandle_.param("combination_method", combinationMethod, std::string(""))) { ROS_WARN("did not find combination_method, using default"); } - combination_method_ = convertCombinationMethod(combination_method); + combinationMethod_ = convertCombinationMethod(combinationMethod); // Subscribe to topic - elevation_subscriber_ = nh_.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); + elevationSubscriber_ = nodeHandle_.subscribe(elevationTopic_, 1, &ElevationLayer::elevationMapCallback, this); dsrv_ = nullptr; - setupDynamicReconfigure(nh_); + setupDynamicReconfigure(nodeHandle_); // Setup filter chain. - if (!filterChain_.configure(filter_chain_parameters_name_, nh_)) { + if (!filterChain_.configure(filterChainParametersName_, nodeHandle_)) { ROS_WARN("Could not configure the filter chain!"); } else { - filters_configuration_loaded_ = true; + filtersConfigurationLoaded_ = true; } } @@ -91,19 +91,19 @@ void ElevationLayer::updateBounds(double robot_x, double *min_y, double *max_x, double *max_y) { - std::lock_guard lock(elevation_map_mutex_); - if (rolling_window_) { + std::lock_guard lock(elevationMapMutex_); + if (rollingWindow_) { updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); } - if (!(enabled_ && elevation_map_received_)) { + if (!(enabled_ && elevationMapReceived_)) { return; } useExtraBounds(min_x, min_y, max_x, max_y); - for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { - const grid_map::Index gridmap_index(*iterator); + for (grid_map::GridMapIterator iterator(elevationMap_); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index gridmapIndex(*iterator); grid_map::Position vertexPositionXY; - elevation_map_.getPosition(gridmap_index, vertexPositionXY); + elevationMap_.getPosition(gridmapIndex, vertexPositionXY); double px = vertexPositionXY.x(); double py = vertexPositionXY.y(); @@ -119,34 +119,34 @@ void ElevationLayer::updateFootprint(double robot_x, double *min_y, double *max_x, double *max_y) { - if (!footprint_clearing_enabled_) { + if (!footprintClearingEnabled_) { return; } - costmap_2d::transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); + costmap_2d::transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformedFootprint_); - for (auto &i : transformed_footprint_) { + for (auto &i : transformedFootprint_) { touch(i.x, i.y, min_x, min_y, max_x, max_y); } } void ElevationLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) { - std::lock_guard lock(elevation_map_mutex_); - if (!enabled_ || !elevation_map_received_) { + std::lock_guard lock(elevationMapMutex_); + if (!enabled_ || !elevationMapReceived_) { return; } - const bool has_edges_layer = elevation_map_.exists(edges_layer_name_); - if (!has_edges_layer) { + const bool hasEdgesLayer = elevationMap_.exists(edgesLayerName_); + if (!hasEdgesLayer) { ROS_WARN_THROTTLE(0.2, "No edges layer found !!"); } - ros::Duration time_since_elevation_map_received = ros::Time::now() - last_elevation_map_update_; - if (time_since_elevation_map_received > ros::Duration(max_allowed_blind_time_)) { + ros::Duration timeSinceElevationMapReceived = ros::Time::now() - lastElevationMapUpdate_; + if (timeSinceElevationMapReceived > ros::Duration(maxAllowedBlindTime_)) { current_ = false; } - const grid_map::Matrix &elevation_data = elevation_map_[elevation_layer_name_]; - for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { - const grid_map::Index gridmap_index(*iterator); + const grid_map::Matrix &elevationData = elevationMap_[elevationLayerName_]; + for (grid_map::GridMapIterator iterator(elevationMap_); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index gridmapIndex(*iterator); grid_map::Position vertexPositionXY; - elevation_map_.getPosition(gridmap_index, vertexPositionXY); + elevationMap_.getPosition(gridmapIndex, vertexPositionXY); double px = vertexPositionXY.x(); double py = vertexPositionXY.y(); // now we need to compute the map coordinates for the observation @@ -155,13 +155,13 @@ void ElevationLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, { continue; } - if (elevation_data(gridmap_index(0), gridmap_index(1)) - > height_threshold_) // If point too high, it could be an obstacle + if (elevationData(gridmapIndex(0), gridmapIndex(1)) + > heightThreshold_) // If point too high, it could be an obstacle { - if (has_edges_layer) { - const grid_map::Matrix &edges_data = elevation_map_[edges_layer_name_]; - if (edges_data(gridmap_index(0), gridmap_index(1)) - < edges_sharpness_threshold_) // if area not sharp, dont label as obstacle + if (hasEdgesLayer) { + const grid_map::Matrix &edgesData = elevationMap_[edgesLayerName_]; + if (edgesData(gridmapIndex(0), gridmapIndex(1)) + < edgesSharpnessThreshold_) // if area not sharp, dont label as obstacle { setCost(mx, my, FREE_SPACE); continue; @@ -173,11 +173,11 @@ void ElevationLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, } } - if (footprint_clearing_enabled_) { - setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE); + if (footprintClearingEnabled_) { + setConvexPolygonCost(transformedFootprint_, costmap_2d::FREE_SPACE); } - switch (combination_method_) { + switch (combinationMethod_) { case Overwrite:updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); break; case Maximum:updateWithMax(master_grid, min_i, min_j, max_i, max_j); @@ -199,34 +199,34 @@ CombinationMethod convertCombinationMethod(const std::string &str) { } void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr &elevation) { - grid_map::GridMap incoming_map; - grid_map::GridMap filtered_map; - if (!grid_map::GridMapRosConverter::fromMessage(*elevation, incoming_map)) { + grid_map::GridMap incomingMap; + grid_map::GridMap filteredMap; + if (!grid_map::GridMapRosConverter::fromMessage(*elevation, incomingMap)) { ROS_WARN_THROTTLE(0.2, "Grid Map msg Conversion failed !"); return; } - last_elevation_map_update_ = ros::Time::now(); - incoming_map.convertToDefaultStartIndex(); - if (!(global_frame_ == incoming_map.getFrameId())) { + lastElevationMapUpdate_ = ros::Time::now(); + incomingMap.convertToDefaultStartIndex(); + if (!(globalFrame_ == incomingMap.getFrameId())) { ROS_WARN_THROTTLE(0.2, "Incoming elevation_map frame different than expected! "); } // Apply filter chain. - if (filters_configuration_loaded_ && filterChain_.update(incoming_map, filtered_map)) { - std::lock_guard lock(elevation_map_mutex_); - elevation_map_ = filtered_map; - height_threshold_ /= 2.0; // Half the threshold since the highest sharpness is at midheigth of the obstacles + if (filtersConfigurationLoaded_ && filterChain_.update(incomingMap, filteredMap)) { + std::lock_guard lock(elevationMapMutex_); + elevationMap_ = filteredMap; + heightThreshold_ /= 2.0; // Half the threshold since the highest sharpness is at midheigth of the obstacles } else { - std::lock_guard lock(elevation_map_mutex_); + std::lock_guard lock(elevationMapMutex_); ROS_WARN_THROTTLE(0.2, "Could not use the filter chain!"); - elevation_map_ = incoming_map; + elevationMap_ = incomingMap; } - if (!elevation_map_received_) { - elevation_map_received_ = true; + if (!elevationMapReceived_) { + elevationMapReceived_ = true; } } -void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle &nh_) { - dsrv_.reset(new dynamic_reconfigure::Server(nh_)); +void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle &nodeHandle_) { + dsrv_.reset(new dynamic_reconfigure::Server(nodeHandle_)); dynamic_reconfigure::Server::CallbackType cb = boost::bind(&ElevationLayer::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); @@ -245,7 +245,7 @@ void ElevationLayer::reset() { void ElevationLayer::activate() { // if we're stopped we need to re-subscribe to topics - elevation_subscriber_ = nh_.subscribe(elevation_topic_, 1, &ElevationLayer::elevationMapCallback, this); + elevationSubscriber_ = nodeHandle_.subscribe(elevationTopic_, 1, &ElevationLayer::elevationMapCallback, this); } -void ElevationLayer::deactivate() { elevation_subscriber_.shutdown(); } +void ElevationLayer::deactivate() { elevationSubscriber_.shutdown(); } } // namespace elevation_layer \ No newline at end of file From 34c43c8bf23422b0022597fb3426c9f17a059213 Mon Sep 17 00:00:00 2001 From: chisarie Date: Wed, 5 Dec 2018 14:13:20 +0100 Subject: [PATCH 29/36] New software added to the main readme and removed comments in elevation_filters.yaml --- README.md | 5 +++++ elevation_layer/README.md | 3 --- elevation_layer/config/elevation_filters.yaml | 2 +- 3 files changed, 6 insertions(+), 4 deletions(-) delete mode 100644 elevation_layer/README.md diff --git a/README.md b/README.md index 32c3d13a..06152123 100644 --- a/README.md +++ b/README.md @@ -235,6 +235,11 @@ This is the main Robot-Centric Elevation Mapping node. It uses the distance sens The data for the sensor noise model. +## Costmap_2d Plug-in + +Elevation layer is a plugin for [costmap_2d] (http://wiki.ros.org/costmap_2d). Costmap_2d is part fo the ros navigation stack and it provides an implementation of a 2d costmap. Thanks to the elevation_layer plugin, the elevation mapping from this package can be used to build a costmap_2d to fit in the ros navigation stack. It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d. + +The layer applies two filters to the incoming data: a height filter and a sharpness filter. The behaviour can be adjusted tuning the relative parameters **`heightThreshold_`** and **`edgesSharpnessThreshold_`**. The former labels as obstacles all data above a certain height, while the latter labels as obstacles data above a certain edge sharpness, where a sharp edge is defined as a sudden change in height; this is used to avoid labeling slopes as obstacle. ## Bugs & Feature Requests diff --git a/elevation_layer/README.md b/elevation_layer/README.md deleted file mode 100644 index 6feac62d..00000000 --- a/elevation_layer/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# Elevation layer - -A plugin for [costmap_2d]](http://wiki.ros.org/costmap_2d). It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d. diff --git a/elevation_layer/config/elevation_filters.yaml b/elevation_layer/config/elevation_filters.yaml index bbd49754..2662181e 100644 --- a/elevation_layer/config/elevation_filters.yaml +++ b/elevation_layer/config/elevation_filters.yaml @@ -6,5 +6,5 @@ elevation_filters: output_layer: edges expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation)' # Edge detection. compute_empty_cells: false - edge_handling: mean # options: inside, crop, empty, mean + edge_handling: mean window_size: 3 # Make sure to make this compatible with the kernel matrix. \ No newline at end of file From cb4e2d632ecbc0c3b4567553b0a54a9769c9dd65 Mon Sep 17 00:00:00 2001 From: chisarie Date: Wed, 5 Dec 2018 14:24:09 +0100 Subject: [PATCH 30/36] minor readme.md chenges --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 06152123..266bd760 100644 --- a/README.md +++ b/README.md @@ -235,9 +235,9 @@ This is the main Robot-Centric Elevation Mapping node. It uses the distance sens The data for the sensor noise model. -## Costmap_2d Plug-in +## Costmap_2d plug-in -Elevation layer is a plugin for [costmap_2d] (http://wiki.ros.org/costmap_2d). Costmap_2d is part fo the ros navigation stack and it provides an implementation of a 2d costmap. Thanks to the elevation_layer plugin, the elevation mapping from this package can be used to build a costmap_2d to fit in the ros navigation stack. It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d. +**Elevation layer** is a plugin for Costmap_2d (http://wiki.ros.org/costmap_2d). Costmap_2d is part fo the ros navigation stack and it provides an implementation of a 2d costmap. Thanks to the elevation_layer plugin, the elevation mapping from this package can be used to build a costmap_2d to fit in the ros navigation stack. It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d. The layer applies two filters to the incoming data: a height filter and a sharpness filter. The behaviour can be adjusted tuning the relative parameters **`heightThreshold_`** and **`edgesSharpnessThreshold_`**. The former labels as obstacles all data above a certain height, while the latter labels as obstacles data above a certain edge sharpness, where a sharp edge is defined as a sudden change in height; this is used to avoid labeling slopes as obstacle. From 3b3dc7628b8ce3f169d296efd3d04718e7747c94 Mon Sep 17 00:00:00 2001 From: chisarie Date: Wed, 5 Dec 2018 17:21:02 +0100 Subject: [PATCH 31/36] elevation_layer renamed as elevation_maping_costmap_2d_plugin. Somehow doesn't build --- README.md | 2 +- elevation_layer/costmap_plugins.xml | 7 ------- .../CMakeLists.txt | 4 ++-- .../cfg/ElevationPlugin.cfg | 4 ++-- .../config/elevation_filters.yaml | 0 .../costmap_plugins.xml | 7 +++++++ .../elevation_layer.hpp | 10 +++++----- .../package.xml | 2 +- .../src/elevation_layer.cpp | 14 +++++++------- 9 files changed, 25 insertions(+), 25 deletions(-) delete mode 100644 elevation_layer/costmap_plugins.xml rename {elevation_layer => elevation_maping_costmap_2d_plugin}/CMakeLists.txt (94%) rename {elevation_layer => elevation_maping_costmap_2d_plugin}/cfg/ElevationPlugin.cfg (58%) rename {elevation_layer => elevation_maping_costmap_2d_plugin}/config/elevation_filters.yaml (100%) create mode 100644 elevation_maping_costmap_2d_plugin/costmap_plugins.xml rename {elevation_layer/include/elevation_layer => elevation_maping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin}/elevation_layer.hpp (92%) rename {elevation_layer => elevation_maping_costmap_2d_plugin}/package.xml (92%) rename {elevation_layer => elevation_maping_costmap_2d_plugin}/src/elevation_layer.cpp (93%) diff --git a/README.md b/README.md index 266bd760..f06580ce 100644 --- a/README.md +++ b/README.md @@ -237,7 +237,7 @@ This is the main Robot-Centric Elevation Mapping node. It uses the distance sens ## Costmap_2d plug-in -**Elevation layer** is a plugin for Costmap_2d (http://wiki.ros.org/costmap_2d). Costmap_2d is part fo the ros navigation stack and it provides an implementation of a 2d costmap. Thanks to the elevation_layer plugin, the elevation mapping from this package can be used to build a costmap_2d to fit in the ros navigation stack. It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d. +**elevation_mapping_costmap_2d_plugin** is a plugin for Costmap_2d (http://wiki.ros.org/costmap_2d). Costmap_2d is part fo the ros navigation stack and it provides an implementation of a 2d costmap. Thanks to the elevation_mapping_costmap_2d_plugin, the elevation mapping from this package can be used to build a costmap_2d to fit in the ros navigation stack. It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d. The layer applies two filters to the incoming data: a height filter and a sharpness filter. The behaviour can be adjusted tuning the relative parameters **`heightThreshold_`** and **`edgesSharpnessThreshold_`**. The former labels as obstacles all data above a certain height, while the latter labels as obstacles data above a certain edge sharpness, where a sharp edge is defined as a sudden change in height; this is used to avoid labeling slopes as obstacle. diff --git a/elevation_layer/costmap_plugins.xml b/elevation_layer/costmap_plugins.xml deleted file mode 100644 index 90624ab1..00000000 --- a/elevation_layer/costmap_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - adds elevation map info to costmap_2d. - - - diff --git a/elevation_layer/CMakeLists.txt b/elevation_maping_costmap_2d_plugin/CMakeLists.txt similarity index 94% rename from elevation_layer/CMakeLists.txt rename to elevation_maping_costmap_2d_plugin/CMakeLists.txt index a130005b..92b9df3d 100644 --- a/elevation_layer/CMakeLists.txt +++ b/elevation_maping_costmap_2d_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(elevation_layer) +project(elevation_mapping_costmap_2d_plugin) ## Use C++11 add_definitions(-std=c++11) @@ -73,7 +73,7 @@ add_library(${PROJECT_NAME} ## cmake target dependencies of the executable/library # build config headers -add_dependencies(${PROJECT_NAME} elevation_layer_gencfg) +add_dependencies(${PROJECT_NAME} elevation_mapping_costmap_2d_plugin_gencfg) ## libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME} diff --git a/elevation_layer/cfg/ElevationPlugin.cfg b/elevation_maping_costmap_2d_plugin/cfg/ElevationPlugin.cfg similarity index 58% rename from elevation_layer/cfg/ElevationPlugin.cfg rename to elevation_maping_costmap_2d_plugin/cfg/ElevationPlugin.cfg index aeb8b08a..e8817f93 100755 --- a/elevation_layer/cfg/ElevationPlugin.cfg +++ b/elevation_maping_costmap_2d_plugin/cfg/ElevationPlugin.cfg @@ -1,9 +1,9 @@ #!/usr/bin/env python -PACKAGE = "elevation_layer" +PACKAGE = "elevation_mapping_costmap_2d_plugin" from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t gen = ParameterGenerator() gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -exit(gen.generate("elevation_layer", "elevation_layer", "ElevationPlugin")) +exit(gen.generate("elevation_mapping_costmap_2d_plugin", "elevation_mapping_costmap_2d_plugin", "ElevationPlugin")) diff --git a/elevation_layer/config/elevation_filters.yaml b/elevation_maping_costmap_2d_plugin/config/elevation_filters.yaml similarity index 100% rename from elevation_layer/config/elevation_filters.yaml rename to elevation_maping_costmap_2d_plugin/config/elevation_filters.yaml diff --git a/elevation_maping_costmap_2d_plugin/costmap_plugins.xml b/elevation_maping_costmap_2d_plugin/costmap_plugins.xml new file mode 100644 index 00000000..2043afb4 --- /dev/null +++ b/elevation_maping_costmap_2d_plugin/costmap_plugins.xml @@ -0,0 +1,7 @@ + + + + adds elevation map info to costmap_2d. + + + diff --git a/elevation_layer/include/elevation_layer/elevation_layer.hpp b/elevation_maping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp similarity index 92% rename from elevation_layer/include/elevation_layer/elevation_layer.hpp rename to elevation_maping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp index 306268ff..a7aca4c8 100644 --- a/elevation_layer/include/elevation_layer/elevation_layer.hpp +++ b/elevation_maping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp @@ -25,12 +25,12 @@ #include // Package related headers -#include "elevation_layer/ElevationPluginConfig.h" +#include "elevation_mapping_costmap_2d_plugin/ElevationPluginConfig.h" #include #include #include -namespace elevation_layer { +namespace elevation_mapping_costmap_2d_plugin { /*! * Method to update the cost of a portion of map @@ -118,7 +118,7 @@ class ElevationLayer : public costmap_2d::CostmapLayer { std::string globalFrame_; //! dynamic reconfigure server - std::unique_ptr > dsrv_; + std::unique_ptr > dsrv_; //! combination method to use to update the cost of a portion of map CombinationMethod combinationMethod_; @@ -144,7 +144,7 @@ class ElevationLayer : public costmap_2d::CostmapLayer { * @param config * @param level */ - void reconfigureCB(elevation_layer::ElevationPluginConfig& config, uint32_t level); + void reconfigureCB(elevation_mapping_costmap_2d_plugin::ElevationPluginConfig& config, uint32_t level); ros::NodeHandle nodeHandle_; @@ -185,4 +185,4 @@ class ElevationLayer : public costmap_2d::CostmapLayer { std::string edgesLayerName_; }; -} // namespace elevation_layer \ No newline at end of file +} // namespace elevation_mapping_costmap_2d_plugin \ No newline at end of file diff --git a/elevation_layer/package.xml b/elevation_maping_costmap_2d_plugin/package.xml similarity index 92% rename from elevation_layer/package.xml rename to elevation_maping_costmap_2d_plugin/package.xml index 6598eb21..797f28d2 100644 --- a/elevation_layer/package.xml +++ b/elevation_maping_costmap_2d_plugin/package.xml @@ -1,6 +1,6 @@ - elevation_layer + elevation_mapping_costmap_2d_plugin 0.0.2 adds elevation mapping informations to costmap_2d Eugenio Chisari diff --git a/elevation_layer/src/elevation_layer.cpp b/elevation_maping_costmap_2d_plugin/src/elevation_layer.cpp similarity index 93% rename from elevation_layer/src/elevation_layer.cpp rename to elevation_maping_costmap_2d_plugin/src/elevation_layer.cpp index 75677f3e..7af3b349 100644 --- a/elevation_layer/src/elevation_layer.cpp +++ b/elevation_maping_costmap_2d_plugin/src/elevation_layer.cpp @@ -6,10 +6,10 @@ * Institute: ANYbotics */ -#include "elevation_layer/elevation_layer.hpp" +#include "elevation_mapping_costmap_2d_plugin/elevation_layer.hpp" #include -PLUGINLIB_EXPORT_CLASS(elevation_layer::ElevationLayer, costmap_2d::Layer) +PLUGINLIB_EXPORT_CLASS(elevation_mapping_costmap_2d_plugin::ElevationLayer, costmap_2d::Layer) using costmap_2d::FREE_SPACE; using costmap_2d::LETHAL_OBSTACLE; @@ -18,7 +18,7 @@ using costmap_2d::NO_INFORMATION; using costmap_2d::Observation; using costmap_2d::ObservationBuffer; -namespace elevation_layer { +namespace elevation_mapping_costmap_2d_plugin { ElevationLayer::ElevationLayer() : filterChain_("grid_map::GridMap"), elevationMapReceived_(false), filtersConfigurationLoaded_(false) { @@ -226,13 +226,13 @@ void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr & } void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle &nodeHandle_) { - dsrv_.reset(new dynamic_reconfigure::Server(nodeHandle_)); - dynamic_reconfigure::Server::CallbackType cb = + dsrv_.reset(new dynamic_reconfigure::Server(nodeHandle_)); + dynamic_reconfigure::Server::CallbackType cb = boost::bind(&ElevationLayer::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); } -void ElevationLayer::reconfigureCB(elevation_layer::ElevationPluginConfig &config, uint32_t level) { +void ElevationLayer::reconfigureCB(elevation_mapping_costmap_2d_plugin::ElevationPluginConfig &config, uint32_t level) { enabled_ = config.enabled; } @@ -248,4 +248,4 @@ void ElevationLayer::activate() { elevationSubscriber_ = nodeHandle_.subscribe(elevationTopic_, 1, &ElevationLayer::elevationMapCallback, this); } void ElevationLayer::deactivate() { elevationSubscriber_.shutdown(); } -} // namespace elevation_layer \ No newline at end of file +} // namespace elevation_mapping_costmap_2d_plugin \ No newline at end of file From 1833bfc41ee0880c4c94c78a94385e9a40e9c242 Mon Sep 17 00:00:00 2001 From: chisarie Date: Fri, 7 Dec 2018 09:47:28 +0100 Subject: [PATCH 32/36] fixed typo in folder --- .../CMakeLists.txt | 0 .../cfg/ElevationPlugin.cfg | 0 .../config/elevation_filters.yaml | 0 .../costmap_plugins.xml | 0 .../elevation_mapping_costmap_2d_plugin/elevation_layer.hpp | 0 .../package.xml | 0 .../src/elevation_layer.cpp | 2 +- 7 files changed, 1 insertion(+), 1 deletion(-) rename {elevation_maping_costmap_2d_plugin => elevation_mapping_costmap_2d_plugin}/CMakeLists.txt (100%) rename {elevation_maping_costmap_2d_plugin => elevation_mapping_costmap_2d_plugin}/cfg/ElevationPlugin.cfg (100%) rename {elevation_maping_costmap_2d_plugin => elevation_mapping_costmap_2d_plugin}/config/elevation_filters.yaml (100%) rename {elevation_maping_costmap_2d_plugin => elevation_mapping_costmap_2d_plugin}/costmap_plugins.xml (100%) rename {elevation_maping_costmap_2d_plugin => elevation_mapping_costmap_2d_plugin}/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp (100%) rename {elevation_maping_costmap_2d_plugin => elevation_mapping_costmap_2d_plugin}/package.xml (100%) rename {elevation_maping_costmap_2d_plugin => elevation_mapping_costmap_2d_plugin}/src/elevation_layer.cpp (99%) diff --git a/elevation_maping_costmap_2d_plugin/CMakeLists.txt b/elevation_mapping_costmap_2d_plugin/CMakeLists.txt similarity index 100% rename from elevation_maping_costmap_2d_plugin/CMakeLists.txt rename to elevation_mapping_costmap_2d_plugin/CMakeLists.txt diff --git a/elevation_maping_costmap_2d_plugin/cfg/ElevationPlugin.cfg b/elevation_mapping_costmap_2d_plugin/cfg/ElevationPlugin.cfg similarity index 100% rename from elevation_maping_costmap_2d_plugin/cfg/ElevationPlugin.cfg rename to elevation_mapping_costmap_2d_plugin/cfg/ElevationPlugin.cfg diff --git a/elevation_maping_costmap_2d_plugin/config/elevation_filters.yaml b/elevation_mapping_costmap_2d_plugin/config/elevation_filters.yaml similarity index 100% rename from elevation_maping_costmap_2d_plugin/config/elevation_filters.yaml rename to elevation_mapping_costmap_2d_plugin/config/elevation_filters.yaml diff --git a/elevation_maping_costmap_2d_plugin/costmap_plugins.xml b/elevation_mapping_costmap_2d_plugin/costmap_plugins.xml similarity index 100% rename from elevation_maping_costmap_2d_plugin/costmap_plugins.xml rename to elevation_mapping_costmap_2d_plugin/costmap_plugins.xml diff --git a/elevation_maping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp b/elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp similarity index 100% rename from elevation_maping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp rename to elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp diff --git a/elevation_maping_costmap_2d_plugin/package.xml b/elevation_mapping_costmap_2d_plugin/package.xml similarity index 100% rename from elevation_maping_costmap_2d_plugin/package.xml rename to elevation_mapping_costmap_2d_plugin/package.xml diff --git a/elevation_maping_costmap_2d_plugin/src/elevation_layer.cpp b/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp similarity index 99% rename from elevation_maping_costmap_2d_plugin/src/elevation_layer.cpp rename to elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp index 7af3b349..07b5c9ef 100644 --- a/elevation_maping_costmap_2d_plugin/src/elevation_layer.cpp +++ b/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp @@ -61,7 +61,7 @@ void ElevationLayer::onInitialize() { ROS_WARN("did not find max_allowed_blind_time, using default"); } bool trackUnknownSpace = layered_costmap_->isTrackingUnknown(); - if (!nodeHandle_.param("trackUnknownSpace", trackUnknownSpace, false)) { + if (!nodeHandle_.param("track_unknown_space", trackUnknownSpace, false)) { ROS_WARN("did not find trackUnknownSpace, using default"); } default_value_ = trackUnknownSpace ? NO_INFORMATION : FREE_SPACE; From 463fa920204fd7742cd162575de881ae448e353a Mon Sep 17 00:00:00 2001 From: chisarie Date: Fri, 7 Dec 2018 14:03:34 +0100 Subject: [PATCH 33/36] formatted code and made warnings throttles at 0.5 hz --- .../elevation_layer.hpp | 6 +- .../src/elevation_layer.cpp | 61 ++++++++----------- 2 files changed, 28 insertions(+), 39 deletions(-) diff --git a/elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp b/elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp index a7aca4c8..dc7f1425 100644 --- a/elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp +++ b/elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp @@ -14,9 +14,9 @@ #include // Ros headers -#include #include #include +#include // Costmap2d headers #include @@ -25,10 +25,10 @@ #include // Package related headers -#include "elevation_mapping_costmap_2d_plugin/ElevationPluginConfig.h" #include -#include #include +#include +#include "elevation_mapping_costmap_2d_plugin/ElevationPluginConfig.h" namespace elevation_mapping_costmap_2d_plugin { diff --git a/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp b/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp index 07b5c9ef..7911c0f2 100644 --- a/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp +++ b/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp @@ -20,8 +20,7 @@ using costmap_2d::ObservationBuffer; namespace elevation_mapping_costmap_2d_plugin { -ElevationLayer::ElevationLayer() - : filterChain_("grid_map::GridMap"), elevationMapReceived_(false), filtersConfigurationLoaded_(false) { +ElevationLayer::ElevationLayer() : filterChain_("grid_map::GridMap"), elevationMapReceived_(false), filtersConfigurationLoaded_(false) { costmap_ = nullptr; // this is the unsigned char* member of parent class Costmap2D. } @@ -84,13 +83,8 @@ void ElevationLayer::onInitialize() { } } -void ElevationLayer::updateBounds(double robot_x, - double robot_y, - double robot_yaw, - double *min_x, - double *min_y, - double *max_x, - double *max_y) { +void ElevationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, + double* max_y) { std::lock_guard lock(elevationMapMutex_); if (rollingWindow_) { updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); @@ -112,37 +106,32 @@ void ElevationLayer::updateBounds(double robot_x, updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); } -void ElevationLayer::updateFootprint(double robot_x, - double robot_y, - double robot_yaw, - double *min_x, - double *min_y, - double *max_x, - double *max_y) { +void ElevationLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, + double* max_y) { if (!footprintClearingEnabled_) { return; } costmap_2d::transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformedFootprint_); - for (auto &i : transformedFootprint_) { + for (auto& i : transformedFootprint_) { touch(i.x, i.y, min_x, min_y, max_x, max_y); } } -void ElevationLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) { +void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { std::lock_guard lock(elevationMapMutex_); if (!enabled_ || !elevationMapReceived_) { return; } const bool hasEdgesLayer = elevationMap_.exists(edgesLayerName_); if (!hasEdgesLayer) { - ROS_WARN_THROTTLE(0.2, "No edges layer found !!"); + ROS_WARN_STREAM_THROTTLE(0.5, edgesLayerName_ << " layer not found !!"); } ros::Duration timeSinceElevationMapReceived = ros::Time::now() - lastElevationMapUpdate_; if (timeSinceElevationMapReceived > ros::Duration(maxAllowedBlindTime_)) { current_ = false; } - const grid_map::Matrix &elevationData = elevationMap_[elevationLayerName_]; + const grid_map::Matrix& elevationData = elevationMap_[elevationLayerName_]; for (grid_map::GridMapIterator iterator(elevationMap_); !iterator.isPastEnd(); ++iterator) { const grid_map::Index gridmapIndex(*iterator); grid_map::Position vertexPositionXY; @@ -155,13 +144,11 @@ void ElevationLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, { continue; } - if (elevationData(gridmapIndex(0), gridmapIndex(1)) - > heightThreshold_) // If point too high, it could be an obstacle + if (elevationData(gridmapIndex(0), gridmapIndex(1)) > heightThreshold_) // If point too high, it could be an obstacle { if (hasEdgesLayer) { - const grid_map::Matrix &edgesData = elevationMap_[edgesLayerName_]; - if (edgesData(gridmapIndex(0), gridmapIndex(1)) - < edgesSharpnessThreshold_) // if area not sharp, dont label as obstacle + const grid_map::Matrix& edgesData = elevationMap_[edgesLayerName_]; + if (edgesData(gridmapIndex(0), gridmapIndex(1)) < edgesSharpnessThreshold_) // if area not sharp, dont label as obstacle { setCost(mx, my, FREE_SPACE); continue; @@ -178,37 +165,39 @@ void ElevationLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, } switch (combinationMethod_) { - case Overwrite:updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); + case Overwrite: + updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); break; - case Maximum:updateWithMax(master_grid, min_i, min_j, max_i, max_j); + case Maximum: + updateWithMax(master_grid, min_i, min_j, max_i, max_j); break; default: // Do Nothing break; } } -CombinationMethod convertCombinationMethod(const std::string &str) { - if (boost::iequals(str, "Overwrite")) { // Case insensitive comparison +CombinationMethod convertCombinationMethod(const std::string& str) { + if (boost::iequals(str, "Overwrite")) { // Case insensitive comparison return Overwrite; } else if (boost::iequals(str, "Maximum")) { // Case insensitive comparison return Maximum; } else { - ROS_WARN_THROTTLE(0.2, "Unknow combination method !"); + ROS_WARN_THROTTLE(0.5, "Unknow combination method !"); return Unknown; } } -void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr &elevation) { +void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& elevation) { grid_map::GridMap incomingMap; grid_map::GridMap filteredMap; if (!grid_map::GridMapRosConverter::fromMessage(*elevation, incomingMap)) { - ROS_WARN_THROTTLE(0.2, "Grid Map msg Conversion failed !"); + ROS_WARN_THROTTLE(0.5, "Grid Map msg Conversion failed !"); return; } lastElevationMapUpdate_ = ros::Time::now(); incomingMap.convertToDefaultStartIndex(); if (!(globalFrame_ == incomingMap.getFrameId())) { - ROS_WARN_THROTTLE(0.2, "Incoming elevation_map frame different than expected! "); + ROS_WARN_THROTTLE(0.5, "Incoming elevation_map frame different than expected! "); } // Apply filter chain. if (filtersConfigurationLoaded_ && filterChain_.update(incomingMap, filteredMap)) { @@ -217,7 +206,7 @@ void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr & heightThreshold_ /= 2.0; // Half the threshold since the highest sharpness is at midheigth of the obstacles } else { std::lock_guard lock(elevationMapMutex_); - ROS_WARN_THROTTLE(0.2, "Could not use the filter chain!"); + ROS_WARN_THROTTLE(0.5, "Could not use the filter chain!"); elevationMap_ = incomingMap; } if (!elevationMapReceived_) { @@ -225,14 +214,14 @@ void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr & } } -void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle &nodeHandle_) { +void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nodeHandle_) { dsrv_.reset(new dynamic_reconfigure::Server(nodeHandle_)); dynamic_reconfigure::Server::CallbackType cb = boost::bind(&ElevationLayer::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); } -void ElevationLayer::reconfigureCB(elevation_mapping_costmap_2d_plugin::ElevationPluginConfig &config, uint32_t level) { +void ElevationLayer::reconfigureCB(elevation_mapping_costmap_2d_plugin::ElevationPluginConfig& config, uint32_t level) { enabled_ = config.enabled; } From ae5817c9183d577a4c4b87f4e92eda423e7179f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A9ter=20Fankhauser?= Date: Mon, 11 Mar 2019 13:52:40 +0100 Subject: [PATCH 34/36] Update README.md --- README.md | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index f06580ce..52365013 100644 --- a/README.md +++ b/README.md @@ -91,9 +91,9 @@ Run the unit tests with In order to get the Robot-Centric Elevation Mapping to run with your robot, you will need to adapt a few parameters. It is the easiest if duplicate and adapt all the parameter files that you need to change from the `elevation_mapping_demos` package (e.g. the `simple_demo` example). These are specifically the parameter files in `config` and the launch file from the `launch` folder. -## Nodes +## Packages -### Node: elevation_mapping +### elevation_mapping This is the main Robot-Centric Elevation Mapping node. It uses the distance sensor measurements and the pose and covariance of the robot to generate an elevation map with variance estimates. @@ -235,11 +235,11 @@ This is the main Robot-Centric Elevation Mapping node. It uses the distance sens The data for the sensor noise model. -## Costmap_2d plug-in +### elevation_mapping_costmap_2d_plugin -**elevation_mapping_costmap_2d_plugin** is a plugin for Costmap_2d (http://wiki.ros.org/costmap_2d). Costmap_2d is part fo the ros navigation stack and it provides an implementation of a 2d costmap. Thanks to the elevation_mapping_costmap_2d_plugin, the elevation mapping from this package can be used to build a costmap_2d to fit in the ros navigation stack. It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d. +This is a plugin for [costmap_2d] and provides an implementation of a 2d costmap. With the `elevation_mapping_costmap_2d_plugin` package, the generated elevation map can be used to build a costmap_2d to work with the [ROS] [navigation] stack. It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d. -The layer applies two filters to the incoming data: a height filter and a sharpness filter. The behaviour can be adjusted tuning the relative parameters **`heightThreshold_`** and **`edgesSharpnessThreshold_`**. The former labels as obstacles all data above a certain height, while the latter labels as obstacles data above a certain edge sharpness, where a sharp edge is defined as a sudden change in height; this is used to avoid labeling slopes as obstacle. +The layer applies a height and a sharpness filter to the incoming data. The behaviour can be adjusted tuning the parameters `heightThreshold_` and `edgesSharpnessThreshold_`. The former labels as obstacles all data above a certain height, while the latter labels as obstacles data above a certain edge sharpness, where a sharp edge is defined as a sudden change in height; this is used to avoid labeling slopes as obstacle. ## Bugs & Feature Requests @@ -254,3 +254,5 @@ Please report bugs and request features using the [Issue Tracker](https://github [tf/tfMessage]: http://docs.ros.org/kinetic/api/tf/html/msg/tfMessage.html [std_srvs/Empty]: http://docs.ros.org/api/std_srvs/html/srv/Empty.html [grid_map_msg/GetGridMap]: https://github.com/anybotics/grid_map/blob/master/grid_map_msg/srv/GetGridMap.srv +[costmap_2d]: http://wiki.ros.org/costmap_2d +[navigation]: http://wiki.ros.org/navigation From 0c350f103576624a7d2f6b210fc118566a898d1e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?P=C3=A9ter=20Fankhauser?= Date: Mon, 11 Mar 2019 13:55:15 +0100 Subject: [PATCH 35/36] Update costmap_plugins.xml --- elevation_mapping_costmap_2d_plugin/costmap_plugins.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/elevation_mapping_costmap_2d_plugin/costmap_plugins.xml b/elevation_mapping_costmap_2d_plugin/costmap_plugins.xml index 2043afb4..95553082 100644 --- a/elevation_mapping_costmap_2d_plugin/costmap_plugins.xml +++ b/elevation_mapping_costmap_2d_plugin/costmap_plugins.xml @@ -1,7 +1,7 @@ - adds elevation map info to costmap_2d. + Adds elevation map info to costmap_2d. From c8d01e95d243474212e16c9dedc3c6c5b778f6a9 Mon Sep 17 00:00:00 2001 From: chisarie Date: Mon, 11 Mar 2019 16:07:15 +0100 Subject: [PATCH 36/36] All comments start with a capital letter and end with a period. --- .../elevation_layer.hpp | 86 ++++++++++--------- .../src/elevation_layer.cpp | 18 ++-- 2 files changed, 55 insertions(+), 49 deletions(-) diff --git a/elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp b/elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp index dc7f1425..4d5d2061 100644 --- a/elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp +++ b/elevation_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp @@ -8,23 +8,23 @@ #pragma once -// C++ Libraries +// C++ Libraries. #include #include #include -// Ros headers +// Ros headers. #include #include #include -// Costmap2d headers +// Costmap2d headers. #include #include #include #include -// Package related headers +// Package related headers. #include #include #include @@ -33,34 +33,34 @@ namespace elevation_mapping_costmap_2d_plugin { /*! - * Method to update the cost of a portion of map + * Method to update the cost of a portion of map. */ enum CombinationMethod { Overwrite, Maximum, Unknown }; /*! - * converts the string from the yaml file to the proper CombinationMethod enum - * @param str input string from yaml file - * @return CombinationMethod enum equivalent + * Converts the string from the yaml file to the proper CombinationMethod enum. + * @param str Input string from yaml file. + * @return CombinationMethod Enum equivalent. */ CombinationMethod convertCombinationMethod(const std::string& str); /*! - * Plug-in layer of a costmap_2d derived from elevation_map information + * Plug-in layer of a costmap_2d derived from elevation_map information. */ class ElevationLayer : public costmap_2d::CostmapLayer { public: /*! - * Constructor + * Constructor. */ ElevationLayer(); /*! - * Destructor + * Destructor. */ ~ElevationLayer() override = default; /*! - * Function called from parent class at initialization + * Function called from parent class at initialization. */ void onInitialize() override; @@ -80,30 +80,36 @@ class ElevationLayer : public costmap_2d::CostmapLayer { */ void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override; - /*! @brief Restart publishers if they've been stopped. */ + /*! + * @brief Restart publishers if they've been stopped. + */ void activate() override; - /*! @brief Stop publishers. */ + /*! + * @brief Stop publishers. + */ void deactivate() override; - /*! @brief Deactivate, reset the map and then reactivate*/ + /*! + * @brief Deactivate, reset the map and then reactivate + */ void reset() override; /*! - * Callback to receive the grid_map msg from elevation_map - * @param occupancy_grid GridMap msg from elevation_map + * Callback to receive the grid_map msg from elevation_map. + * @param occupancy_grid GridMap msg from elevation_map. */ void elevationMapCallback(const grid_map_msgs::GridMapConstPtr& occupancy_grid); protected: /*! - * set up the dynamic reconfigure + * Set up the dynamic reconfigure. * @param nh */ virtual void setupDynamicReconfigure(ros::NodeHandle& nh); /*! - * clear obstacles inside the footprint of the robort if the flag footprintClearingEnabled_ is true + * Clear obstacles inside the footprint of the robort if the flag footprintClearingEnabled_ is true. * @param robot_x * @param robot_y * @param robot_yaw @@ -114,33 +120,33 @@ class ElevationLayer : public costmap_2d::CostmapLayer { */ void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y); - //! The global frame for the costmap + //! The global frame for the costmap. std::string globalFrame_; - //! dynamic reconfigure server + //! Dynamic reconfigure server. std::unique_ptr > dsrv_; - //! combination method to use to update the cost of a portion of map + //! Combination method to use to update the cost of a portion of map. CombinationMethod combinationMethod_; - //! polygon describing the form of the footprint of the robot + //! Polygon describing the form of the footprint of the robot. std::vector transformedFootprint_; - //! whether the local costmap should move together with the robot + //! Whether the local costmap should move together with the robot. bool rollingWindow_; - //! whether to clean the obstacles inside the robot footprint + //! Whether to clean the obstacles inside the robot footprint. bool footprintClearingEnabled_; - //! whether the elevation_map msg was received + //! Whether the elevation_map msg was received. std::atomic_bool elevationMapReceived_; - //! after this time [seconds] without receiving any elevation_map, the robot will have to stop + //! After this time [seconds] without receiving any elevation_map, the robot will have to stop. double maxAllowedBlindTime_; private: /*! - * dynamic reconfiguration of the parameters + * Dynamic reconfiguration of the parameters. * @param config * @param level */ @@ -148,40 +154,40 @@ class ElevationLayer : public costmap_2d::CostmapLayer { ros::NodeHandle nodeHandle_; - //! the elevation_map from which to take the information abut the environment (filtered or not) + //! The elevation_map from which to take the information abut the environment (filtered or not). grid_map::GridMap elevationMap_; - //! lock_guard mutex to make elevation_map setting thread safe + //! lock_guard mutex to make elevation_map setting thread safe. std::mutex elevationMapMutex_; - //! Ros subscriber to grid_map msgs + //! Ros subscriber to grid_map msgs. ros::Subscriber elevationSubscriber_; - //! last time an elevation_map was received + //! Last time an elevation_map was received. ros::Time lastElevationMapUpdate_; - //! height threshold below which nothing is considered obstacle + //! Height threshold below which nothing is considered obstacle. double heightThreshold_; - //! sharpness threshold above which an object is considered an obstacle + //! Sharpness threshold above which an object is considered an obstacle. double edgesSharpnessThreshold_; - //! topic_name of the elevation_map incoming msg + //! Topic_name of the elevation_map incoming msg. std::string elevationTopic_; - //! Filter chain used to filter the incoming elevation_map + //! Filter chain used to filter the incoming elevation_map. filters::FilterChain filterChain_; - //! Filter chain parameters name to use + //! Filter chain parameters name to use. std::string filterChainParametersName_; - //! whether filters configuration parameters was found + //! Whether filters configuration parameters was found. bool filtersConfigurationLoaded_; - //! name of the layer of the incoming map to use + //! Name of the layer of the incoming map to use. std::string elevationLayerName_; - //! name to give to the filtered layer + //! Name to give to the filtered layer. std::string edgesLayerName_; }; diff --git a/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp b/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp index 7911c0f2..a04ff4d7 100644 --- a/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp +++ b/elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp @@ -70,7 +70,7 @@ void ElevationLayer::onInitialize() { } combinationMethod_ = convertCombinationMethod(combinationMethod); - // Subscribe to topic + // Subscribe to topic. elevationSubscriber_ = nodeHandle_.subscribe(elevationTopic_, 1, &ElevationLayer::elevationMapCallback, this); dsrv_ = nullptr; setupDynamicReconfigure(nodeHandle_); @@ -138,17 +138,17 @@ void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, elevationMap_.getPosition(gridmapIndex, vertexPositionXY); double px = vertexPositionXY.x(); double py = vertexPositionXY.y(); - // now we need to compute the map coordinates for the observation + // Now we need to compute the map coordinates for the observation. unsigned int mx, my; - if (!worldToMap(px, py, mx, my)) // if point outside of local costmap, ignore + if (!worldToMap(px, py, mx, my)) // If point outside of local costmap, ignore. { continue; } - if (elevationData(gridmapIndex(0), gridmapIndex(1)) > heightThreshold_) // If point too high, it could be an obstacle + if (elevationData(gridmapIndex(0), gridmapIndex(1)) > heightThreshold_) // If point too high, it could be an obstacle. { if (hasEdgesLayer) { const grid_map::Matrix& edgesData = elevationMap_[edgesLayerName_]; - if (edgesData(gridmapIndex(0), gridmapIndex(1)) < edgesSharpnessThreshold_) // if area not sharp, dont label as obstacle + if (edgesData(gridmapIndex(0), gridmapIndex(1)) < edgesSharpnessThreshold_) // If area not sharp, dont label as obstacle. { setCost(mx, my, FREE_SPACE); continue; @@ -171,15 +171,15 @@ void ElevationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, case Maximum: updateWithMax(master_grid, min_i, min_j, max_i, max_j); break; - default: // Do Nothing + default: // Do Nothing. break; } } CombinationMethod convertCombinationMethod(const std::string& str) { - if (boost::iequals(str, "Overwrite")) { // Case insensitive comparison + if (boost::iequals(str, "Overwrite")) { // Case insensitive comparison. return Overwrite; - } else if (boost::iequals(str, "Maximum")) { // Case insensitive comparison + } else if (boost::iequals(str, "Maximum")) { // Case insensitive comparison. return Maximum; } else { ROS_WARN_THROTTLE(0.5, "Unknow combination method !"); @@ -203,7 +203,7 @@ void ElevationLayer::elevationMapCallback(const grid_map_msgs::GridMapConstPtr& if (filtersConfigurationLoaded_ && filterChain_.update(incomingMap, filteredMap)) { std::lock_guard lock(elevationMapMutex_); elevationMap_ = filteredMap; - heightThreshold_ /= 2.0; // Half the threshold since the highest sharpness is at midheigth of the obstacles + heightThreshold_ /= 2.0; // Half the threshold since the highest sharpness is at midheigth of the obstacles. } else { std::lock_guard lock(elevationMapMutex_); ROS_WARN_THROTTLE(0.5, "Could not use the filter chain!");