Feature/filtered elevation layer #84

chisarie · 2018-11-05T08:37:29Z

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.

… as input.

… cost map

…on_method=0

hogabrie

Please have a look at our style guide.
Following the variable / function naming rules of costmap2d might make sense here, however make sure to use proper indents and formatting rules (s.a braces in short ifs). Also use doxygen style comments more consistently, look at the headers in elevation_mapping they are nicely documented.

hogabrie · 2018-11-05T08:45:36Z

elevation_layer/CMakeLists.txt

@@ -0,0 +1,77 @@
+# use c++11
+set(CMAKE_CXX_COMPILER_ARG1 -std=c++11)


Put this in the build section of the cmake and use
add_definitions(-std=c++11)

hogabrie · 2018-11-05T08:47:29Z

elevation_layer/CMakeLists.txt

+)
+
+## cpp-header files for installation
+install(DIRECTORY include/


I think this should be:
install(DIRECTORY include/${PROJECT_NAME}/

hogabrie · 2018-11-05T08:49:43Z

elevation_layer/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(elevation_layer)
+
+find_package(Boost REQUIRED COMPONENTS thread)


You never actually use boost in the cmake.
See here for a how-to.

hogabrie · 2018-11-05T08:50:38Z

elevation_layer/CMakeLists.txt

+)
+
+## declare a cpp library
+add_library(elevation_layer


FYI you can use ${PROJECT_NAME} whenever you have elevation_layer.

hogabrie · 2018-11-05T08:52:08Z

elevation_layer/include/elevation_layer.h

@@ -0,0 +1,82 @@
+//


Follow this packages file naming:
e.g.

/* * ElevationMapping.cpp * * Created on: Nov 12, 2013 * Author: Péter Fankhauser * Institute: ETH Zurich, ANYbotics */

hogabrie · 2018-11-05T09:05:04Z

elevation_layer/src/elevation_layer.cpp

+        }
+        bool track_unknown_space;
+        nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
+        if (track_unknown_space)


Use curly braces(styleguide)! Or shorthand default_value = track_unknown_space ? NO_INFORMATION : FREE_SPACE;

hogabrie · 2018-11-05T09:07:12Z

elevation_layer/src/elevation_layer.cpp

+        std::lock_guard<std::mutex> lock(elevation_map_mutex_);
+        if (rolling_window_)
+            updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
+        if (!(enabled_ && elevation_map_received_))


Makes more sense to do this before updating origin. Right?

I think you still want your local costmap to move around with the robot even though the elevation_mapping is not received. But actually is kind of the same

hogabrie · 2018-11-05T09:12:15Z

elevation_layer/src/elevation_layer.cpp

+        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++)


Use range based for:

for(auto&& point : transformed_footprint_) { touch(point.x, point.y, min_x, min_y, max_x, max_j); }

hogabrie · 2018-11-05T09:14:48Z

elevation_layer/src/elevation_layer.cpp

+            }
+            if ( elevation_data(gridmap_index(0), gridmap_index(1)) > height_treshold_ )  // If point too high, it could be an obstacle
+            {
+                if (!has_edges_layer) {


You could check this before even going into the for loop, since this is a property of the elevation_map. Also use a throttled message.

hogabrie · 2018-11-05T09:18:43Z

elevation_layer/src/elevation_layer.cpp

+
+        switch (combination_method_)
+        {
+            case 0:  // Overwrite


Use a string (that ideally maps to an enum) for combination_method_.

remod · 2018-11-05T08:47:39Z

elevation_layer/CMakeLists.txt

+include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)


you can have a single include_directories and list the elements

remod · 2018-11-05T08:48:50Z

elevation_layer/include/elevation_layer.h

+//
+
+#ifndef ELEVATION_LAYER_H
+#define ELEVATION_LAYER_H


You can use #pragma once, a more modern way of doing this.

remod · 2018-11-05T08:49:43Z

elevation_layer/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package>


I would use package version 2 here, so you only have <depend> instead of <build_depend> and <run_depend>

remod · 2018-11-05T08:50:34Z

elevation_layer/package.xml

+  <run_depend>costmap_2d</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>grid_map_ros</run_depend>
+  <run_depend>filters</run_depend>


boost is missing here (see https://github.com/ros/rosdistro/blob/master/rosdep/base.yaml and https://github.com/leggedrobotics/ros_best_practices/blob/master/ros_package_template/package.xml)

remod · 2018-11-05T08:52:40Z

elevation_layer/src/elevation_layer.cpp

+        const std::string tf_prefix = tf::getPrefixParam(prefix_nh);
+
+        // get parameters from config file
+        if(!nh.param("elevation_topic", elevation_topic_, std::string("")))


If you use param_io (in repo any_node), it will print warnings for you and you can shorten the code here.

I would not add this dependency in a package that could be used completely independently of our software stack.

remod · 2018-11-05T09:03:34Z

elevation_layer/package.xml

+  <!--  <author email="echisari@anybotics.com">Eugenio Chisari</author>  -->
+  <maintainer email="echisari@anybotics.com">Eugenio Chisari</maintainer>
+
+  <license>BSD</license>


Are we open sourcing this plugin?

I dont know

It's already open source, if we keep it in this repo 😉

remod · 2018-11-05T09:04:29Z

elevation_layer/include/elevation_layer.h

+    protected:
+        std::string global_frame_;  ///< @brief The global frame for the costmap
+        std::vector<boost::shared_ptr<message_filters::SubscriberBase> > elevation_subscribers_;  ///< @brief Used for the observation message filters
+        dynamic_reconfigure::Server<elevation_layer::ElevationPluginConfig> *dsrv_;


If you use a smart pointer here, you do not need to care about memory handling (calling delete)

remod · 2018-11-05T09:26:57Z

elevation_layer/src/elevation_layer.cpp

+
+using costmap_2d::NO_INFORMATION;
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::FREE_SPACE;


I think that we soon need more than binary traversability information. Imagine you have two paths to reach the goal, one path on flat terrain and one path with ramps/steps etc. Both paths are traversable, but one has higher costs. In these cases the planner will need get these costs through the map. Therefore we will need traversability information between 0 and 1.

Unrelated to that, during testing we saw that noise could result to untraversable cells in the costmap. In these cases the planner failed to find a path. In my opinion it would make sense to have a threshold below which untraversable cells are not considered to be an obstacle.

About the first point, yes its true, this one is the short term solution, in the future we will need a non-binary cost. About the second one, it is already implemented like this :)

remod · 2018-11-05T09:36:44Z

elevation_layer/src/elevation_layer.cpp

+            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 !!");


If this check fails, the warning will be printed many times. Can you do the check outside the for loop?

remod · 2018-11-05T09:37:33Z

elevation_layer/include/elevation_layer.h

+        virtual void setupDynamicReconfigure(ros::NodeHandle& nh);
+
+
+        int combination_method_;


You could use an enum or an enum class for this.

I don't think it is possible to get an enum type from a yaml file, is it?

I don't think so, but you could use a string that you transform to an enum. This makes it easier for the user to tweak the parameters of your layer (0, 1 or 2 is not very descriptive)

harmishhk · 2018-11-05T11:53:50Z

elevation_layer/src/elevation_layer.cpp

+        rolling_window_ = layered_costmap_->isRolling();
+
+        ElevationLayer::matchSize();
+        current_ = true;


Value of current_ is used by move_base to know if the layer is up-to-date (look here). Therefore it should be updated depending on elevation layer updates. For example if we have not received elevation map for certain period we should make this value to false so that move_base gives a warning and stops the robot.

harmishhk · 2018-11-05T11:57:55Z

elevation_layer/src/elevation_layer.cpp

+        filters_configuration_loaded_ = false;
+        global_frame_ = layered_costmap_->getGlobalFrameID();
+        elevation_layer_name_ = "elevation";
+        edges_layer_name_ = "edges";


Since both elevation_layer_name_ and edges_layer_name_ come from elevation mapping, shouldn't the names be configurable using parameters?

harmishhk · 2018-11-05T13:06:30Z

elevation_layer/src/elevation_layer.cpp

+        current_ = true;
+        elevation_map_received_ = false;
+        filters_configuration_loaded_ = false;
+        global_frame_ = layered_costmap_->getGlobalFrameID();


global_frame_ is never used in the code!

AFAIK, the elevation layer can only work if the costmap frame is set to odom (or same as the frame used by elevation mapping). The user should be warned if the frames in costmap and elevation layers are not matching.

harmishhk · 2018-11-05T13:27:20Z

elevation_layer/src/elevation_layer.cpp

+        {
+            std::lock_guard<std::mutex> lock(elevation_map_mutex_);
+            elevation_map_ = filtered_map;
+            height_treshold_ /= 2;      // Half the treshold since the highest sharpness is at midheigth of the obstacles


threshold (instead of treshold) is official spelling in English :)

…ad of 2

pfankhauser

Thanks, but this PR is not close to be merged. No documentation, wrong formatting, and some open issues from other reviewers. Please work on this before I take another look.

pfankhauser · 2018-12-04T13:15:26Z

elevation_layer/README.md

@@ -0,0 +1,3 @@
+# Elevation layer


Don't add README files for each package, but please document the new software in the main README.

pfankhauser · 2018-12-04T13:16:35Z

elevation_layer/config/elevation_filters.yaml

+     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.


Delete unused expressions.

pfankhauser · 2018-12-04T13:17:14Z

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.


This comment makes no sense because there's no filter above?

pfankhauser · 2018-12-04T13:17:49Z

elevation_layer/config/elevation_filters.yaml

+     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


The comments here are instructions for the grid map filter example, not fitting here.

pfankhauser · 2018-12-04T13:18:37Z

elevation_layer/include/elevation_layer/elevation_layer.h

+
+#pragma once
+
+#include <atomic>


Please group and order the includes correctly, see coding guidelines.

pfankhauser · 2018-12-04T13:18:48Z

elevation_layer/include/elevation_layer/elevation_layer.h

+#include <filters/filter_chain.h>
+#include <message_filters/subscriber.h>
+#include <ros/ros.h>
+#include "grid_map_ros/GridMapRosConverter.hpp"


Use correct < and "

pfankhauser · 2018-12-04T13:19:59Z

elevation_layer/include/elevation_layer/elevation_layer.h

+   */
+  void onInitialize() override;
+
+  /**


Everywhere: Use correct doxygen formatting!

pfankhauser · 2018-12-04T13:21:45Z

elevation_layer/src/elevation_layer.cpp

+}
+
+void ElevationLayer::onInitialize() {
+  nh_ = ros::NodeHandle("~/" + name_);


No abbreviations! See programming guidelines.

pfankhauser · 2018-12-04T13:22:14Z

elevation_layer/src/elevation_layer.cpp

+
+  ElevationLayer::matchSize();
+  current_ = true;
+  elevation_map_received_ = false;


Everywhere: Use correct formatting for variables.

pfankhauser · 2018-12-04T13:28:56Z

I don't think elevation_layer is a good name for this package. Since it's a plugin for costmap_2d, this should be somehow clear. What are the names of other costmap_2d plugins?

chisarie · 2018-12-04T15:30:55Z

I don't think elevation_layer is a good name for this package. Since it's a plugin for costmap_2d, this should be somehow clear. What are the names of other costmap_2d plugins?

The other names are inflation_layer, obstacle_layer, static_layer, voxel_layer.
https://github.com/ANYbotics/navigation_ros/tree/master/costmap_2d/plugins

…on_filters.yaml

…w doesn't build

…red_elevation_layer

chisarie · 2018-12-10T08:52:22Z

@pfankhauser Hi, addressed the comments you wrote, i hope i didn't miss anything.

pfankhauser · 2019-03-11T12:53:38Z

README.md

+
+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 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. 


heightThreshold_ is this an absolute height or relative to the neighbor cells?

So is this really sensible when the robot's position drifts over time?

We did't experience that. I mean, even if the robot drifts in map frame, the sensor still outputs proper depth values and elevation mapping is built on those, irrespective of the drift (right? I am not that sure anymore, i should doublecheck)

pfankhauser · 2019-03-11T12:54:12Z

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.

+### elevation_mapping_costmap_2d_plugin
+
+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.


Whats the message type of costmap_2d, OccupancyGrid? It would be good to state this.

pfankhauser · 2019-03-11T12:56:14Z

elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp

+  globalFrame_ = layered_costmap_->getGlobalFrameID();
+
+  // get parameters from config file
+  if (!nodeHandle_.param("elevation_topic", elevationTopic_, std::string(""))) {


All these parameters are not described in the README?

pfankhauser · 2019-03-11T12:57:15Z

...on_mapping_costmap_2d_plugin/include/elevation_mapping_costmap_2d_plugin/elevation_layer.hpp

+enum CombinationMethod { Overwrite, Maximum, Unknown };
+
+/*!
+ * converts the string from the yaml file to the proper CombinationMethod enum


All comments should start with a capital letter and end with a period, see coding guidelines.

chisarie · 2019-03-11T13:51:32Z

@harmishhk @hogabrie @pfankhauser Since we will not use costmap_2d, this is not needed anymore, right? Or Peter, do you want it merged anyway? Otherwise it can be closed.

harmishhk · 2019-03-11T14:41:57Z

We will not use costmap_2d in the future, but we are using at the moment, and I also think this could be useful for community if we merge. Moreover, the functionality of filtering at specific height will be needed also with grid_map based cost-map that we are going to use in the future.

pfankhauser · 2019-03-11T15:52:15Z

elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp

+}
+
+void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nodeHandle_) {
+  dsrv_.reset(new dynamic_reconfigure::Server<elevation_mapping_costmap_2d_plugin::ElevationPluginConfig>(nodeHandle_));


Please refrain from using abbreviations.

pfankhauser · 2019-03-11T15:52:27Z

elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp

+
+void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nodeHandle_) {
+  dsrv_.reset(new dynamic_reconfigure::Server<elevation_mapping_costmap_2d_plugin::ElevationPluginConfig>(nodeHandle_));
+  dynamic_reconfigure::Server<elevation_mapping_costmap_2d_plugin::ElevationPluginConfig>::CallbackType cb =


Same here, please refrain from using abbreviations.

pfankhauser · 2019-03-11T15:52:55Z

elevation_mapping_costmap_2d_plugin/src/elevation_layer.cpp

+void ElevationLayer::reset() {
+  deactivate();
+  resetMaps();
+  current_ = true;


Current what? Please use clearer variable names.

All those variables are derived from base classes in costmap_2d .....

maximilianwulf · 2019-04-23T06:43:08Z

@harmishhk and @pfankhauser what should we do about this PR?
Is @chisarie working on the changes?

pfankhauser · 2019-04-23T07:21:41Z

@harmishhk and @pfankhauser what should we do about this PR?
Is @chisarie working on the changes?

@chisarie is not working on this any more. There are some changes needed for this PR before we can merge it, as long as nobody needs it, I suggest to leave it in this branch for later reference.

chisarie and others added 15 commits October 19, 2018 10:39

added elevation layer package as a plugin of costmap_2d

5af50a9

deleted .idea files

b3d8ac0

implemented elevation layer for costmap_2d. It takes elevaton mapping…

55ea64e

… as input.

fixed namespace bug

fd906f5

working on including filter to find edges of elevation layer

2655dfd

adding grid_map_filters to the elevation layer to detect edges of the…

addee5c

… cost map

elevation layer using filter done. Still one small bug with combinati…

105f9a5

…on_method=0

tried height treshold and filter treshold together, not good

4ed82cf

halfing the trshold if map filtered

d505d15

putting the elevation data outside the for loop

afbfd7d

made filter thread safe

0a5a674

combination method and footprint clearing working

c8abffb

label free space explicitly

12d5302

add config file for elevation filters

c05ebbe

Updated README file

68c83bb

chisarie requested review from harmishhk, hogabrie, pfankhauser and remod November 5, 2018 08:37

hogabrie reviewed Nov 5, 2018

View reviewed changes

remod reviewed Nov 5, 2018

View reviewed changes

harmishhk reviewed Nov 5, 2018

View reviewed changes

chisarie added 5 commits November 6, 2018 15:23

making changes from pull request feedback

88dec09

added elevation_filters.yaml in here

b2cfdea

added param_io, throttled some warnings and now dividing by 2.0 inste…

a0387bc

…ad of 2

moved layer names in yaml file

f721df4

added smart pointer

cdc2d1a

remod approved these changes Nov 12, 2018

View reviewed changes

Merge branch 'feature/filtered_elevation_layer' into test/new_nav

9c7860b

pfankhauser reviewed Dec 4, 2018

View reviewed changes

pfankhauser self-requested a review December 4, 2018 13:26

chisarie added 7 commits December 4, 2018 18:09

refactored variable names, include order and doxygen format

927731a

New software added to the main readme and removed comments in elevati…

34c43c8

…on_filters.yaml

minor readme.md chenges

cb4e2d6

elevation_layer renamed as elevation_maping_costmap_2d_plugin. Someho…

3b3dc76

…w doesn't build

Merge remote-tracking branch 'origin/test/new_nav' into feature/filte…

a5b78aa

…red_elevation_layer

fixed typo in folder

1833bfc

formatted code and made warnings throttles at 0.5 hz

463fa92

Péter Fankhauser and others added 2 commits March 11, 2019 13:52

Update README.md

ae5817c

Update costmap_plugins.xml

0c350f1

pfankhauser requested changes Mar 11, 2019

View reviewed changes

All comments start with a capital letter and end with a period.

c8d01e9

pfankhauser reviewed Mar 11, 2019

View reviewed changes

chisarie closed this Mar 17, 2019

pfankhauser reopened this Apr 5, 2019

pfankhauser mentioned this pull request Apr 8, 2019

Any path planning ROS package compatible with this grid map? #29

Closed

hogabrie mentioned this pull request Oct 7, 2019

How would you use this with move_base? ANYbotics/grid_map#218

Open

usamex mentioned this pull request Dec 9, 2020

How can I get the costmap of elevation_map from the topic? #145

Closed

		@@ -0,0 +1,77 @@
		# use c++11
		set(CMAKE_CXX_COMPILER_ARG1 -std=c++11)

		virtual void setupDynamicReconfigure(ros::NodeHandle& nh);


		int combination_method_;

		@@ -0,0 +1,12 @@
		elevation_filters:
		# Edge detection on elevation layer with convolution filter as alternative to filter above.


		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 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.


		#pragma once

		#include <atomic>

Feature/filtered elevation layer #84

Are you sure you want to change the base?

Feature/filtered elevation layer #84

Conversation

chisarie commented Nov 5, 2018

hogabrie left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

pfankhauser left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

pfankhauser commented Dec 4, 2018

chisarie commented Dec 4, 2018 • edited Loading

chisarie commented Dec 10, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

chisarie commented Mar 11, 2019 • edited Loading

harmishhk commented Mar 11, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

maximilianwulf commented Apr 23, 2019

pfankhauser commented Apr 23, 2019

pfankhauser left a comment •

edited

Loading

chisarie commented Dec 4, 2018 •

edited

Loading

chisarie commented Mar 11, 2019 •

edited

Loading