The SobLayer offers a (likely) faster alternative to the commonly used costmap_2d::InflationLayer. It implements the distance transform algorithm described by Felzenszwalb and Huttenlocher [1].
If you're using ros-noetic, you can install the package via apt:
sudo apt install ros-noetic-sob-layer
If you're using an older ros-version or if you want to build the package from sources, follow the "normal" build process for catkin-based packages:
cd ~/catkin_ws/src
git clone https://github.com/dorezyuk/sob_layer.git
catkin build sob_layer --no-deps --cmake-args -DCMAKE_BUILD_TYPE=Release
You will get an additional performance boost by compiling with O3 optimization, which enables auto-vectorization:
catkin build sob_layer --no-deps --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-O3 -march=native"
The library offers also a benchmark-target.
The benchmarking should help you to figure out, if the
SobLayer offers a reasonable improvement over the costmap_2d::InflationLayer for your specific usecase/platform.
The benchmark-target requires the benchmark library.
If you want to build it, add -Dsob_layer_BENCHMARK=ON to your build command:
catkin build sob_layer --no-deps --cmake-args -DCMAKE_BUILD_TYPE=Release -Dsob_layer_BENCHMARK=ON
The library requires at least C++11. It has been tested on Ubuntu 18.04 and Ubuntu 20.04 running ros-melodic and ros-noetic, respectively. Following compilers are known to work
- GCC 7.5.0
- Clang 6.0.0
This library was benchmarked against the costmap_2d::InflationLayer. The benchmarks focus on two extreme scenarios.
In the first scenario we measure the performance of large, connected obstacles. We place a filled square in the middle of the map. The square is set to the size NxN. N ranges from 0 to 0.9 times the overall map's edge E (assuming the map is a square with the size ExE). An increasing N increases hence the "load-factor" (number of occupied cells).
In the second scenario we measure the performance of unconnected obstacles. On an flattened map, we mark every N-th cell as occupied. With decreasing values for N, more cells are marked as occupied - our "load-factor" increases. The used N-values are [101, 51, 41, 31, 21, 11, 6, 3, 2].
The image above shows the results. The results were obtained on a AMD Ryzen 5 PRO 4650U CPU. The library was compiled with GCC 7.5.0 and "-O3 -march=native" flags.
The y-axis indicates the cpu-time. The x-axis shows the "load-factor" (increasing from left to right). Both scenarios were run for two map-sizes: 100x100 and 1000x1000. The upper row shows the results for the smaller size, the lower for the larger.
The SobLayer follows mostly the configuration from costmap_2d::InflationLayer, easing the change for users.
# in your <common|local|global>_costmap.yaml
plugins:
# ...
-{name: inflation_layer, type: "sob_layer::SobLayer"}
inflation_layer:
# in meters. positive will suppress the automatic calculation from the
# footprint
inflation_radius: -1.
# in meters, as in costmap_2d::InflationLayer
inscribed_radius: 1.
# decay, as in costmap_2d::InflationLayer
cost_scaling_factor: 0
# same behavior as in every costmap_2d::Layer:
# calls to updateCosts and updateBounds will be skipped, if false
enabled: true
# If set to true, the costs with the value costmap_2d::NO_INFORMATION will
# be overwritten by any cost larger than costmap_2d::FREE_SPACE.
inflate_unknown: true
[1] Felzenszwalb, P., & Huttenlocher, D. (2012). Distance Transforms of Sampled Functions Theory of Computing, 8(19), 415–428.