├── .gitignore ├── LICENSE ├── README.md ├── grid_map ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── grid_map_core ├── CHANGELOG.rst ├── CMakeLists.txt ├── cmake │ └── grid_map_core-extras.cmake ├── doc │ ├── grid_map_conventions.pdf │ ├── grid_map_conventions.png │ ├── grid_map_layers.pdf │ ├── grid_map_layers.png │ ├── interpolationGaussWorld.gif │ ├── interpolationSineWorld.gif │ ├── iterators │ │ ├── circle_iterator.gif │ │ ├── circle_iterator_preview.gif │ │ ├── ellipse_iterator.gif │ │ ├── ellipse_iterator_preview.gif │ │ ├── grid_map_iterator.gif │ │ ├── grid_map_iterator_preview.gif │ │ ├── line_iterator.gif │ │ ├── line_iterator_preview.gif │ │ ├── polygon_iterator.gif │ │ ├── polygon_iterator_preview.gif │ │ ├── spiral_iterator.gif │ │ ├── spiral_iterator_preview.gif │ │ ├── submap_iterator.gif │ │ └── submap_iterator_preview.gif │ ├── move_method.gif │ └── setposition_method.gif ├── include │ └── grid_map_core │ │ ├── BufferRegion.hpp │ │ ├── CubicInterpolation.hpp │ │ ├── GridMap.hpp │ │ ├── GridMapMath.hpp │ │ ├── Polygon.hpp │ │ ├── SubmapGeometry.hpp │ │ ├── TypeDefs.hpp │ │ ├── eigen_plugins │ │ ├── DenseBasePlugin.hpp │ │ ├── Functors.hpp │ │ └── FunctorsPlugin.hpp │ │ ├── grid_map_core.hpp │ │ ├── gtest_eigen.hpp │ │ ├── iterators │ │ ├── CircleIterator.hpp │ │ ├── EllipseIterator.hpp │ │ ├── GridMapIterator.hpp │ │ ├── LineIterator.hpp │ │ ├── PolygonIterator.hpp │ │ ├── SlidingWindowIterator.hpp │ │ ├── SpiralIterator.hpp │ │ ├── SubmapIterator.hpp │ │ └── iterators.hpp │ │ └── utils │ │ └── testing.hpp ├── package.xml ├── src │ ├── BufferRegion.cpp │ ├── CubicInterpolation.cpp │ ├── GridMap.cpp │ ├── GridMapMath.cpp │ ├── Polygon.cpp │ ├── SubmapGeometry.cpp │ └── iterators │ │ ├── CircleIterator.cpp │ │ ├── EllipseIterator.cpp │ │ ├── GridMapIterator.cpp │ │ ├── LineIterator.cpp │ │ ├── PolygonIterator.cpp │ │ ├── SlidingWindowIterator.cpp │ │ ├── SpiralIterator.cpp │ │ └── SubmapIterator.cpp └── test │ ├── CubicConvolutionInterpolationTest.cpp │ ├── CubicInterpolationTest.cpp │ ├── EigenPluginsTest.cpp │ ├── EllipseIteratorTest.cpp │ ├── GridMapIteratorTest.cpp │ ├── GridMapMathTest.cpp │ ├── GridMapTest.cpp │ ├── LineIteratorTest.cpp │ ├── PolygonIteratorTest.cpp │ ├── PolygonTest.cpp │ ├── SlidingWindowIteratorTest.cpp │ ├── SpiralIteratorTest.cpp │ ├── SubmapIteratorTest.cpp │ ├── test_grid_map_core.cpp │ ├── test_helpers.cpp │ └── test_helpers.hpp ├── grid_map_costmap_2d ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── grid_map_costmap_2d │ │ ├── Costmap2DConverter.hpp │ │ └── grid_map_costmap_2d.hpp ├── package.xml ├── rostest │ ├── CMakeLists.txt │ ├── README.md │ └── costmap_2d_ros │ │ ├── costmap_2d_ros.cpp │ │ ├── costmap_2d_ros.hpp │ │ └── costmap_2d_ros.test └── test │ ├── Costmap2DConverterTest.cpp │ └── test_grid_map_costmap_2d.cpp ├── grid_map_cv ├── CHANGELOG.rst ├── CMakeLists.txt ├── filter_plugins.xml ├── include │ └── grid_map_cv │ │ ├── GridMapCvConverter.hpp │ │ ├── GridMapCvProcessing.hpp │ │ ├── InpaintFilter.hpp │ │ └── grid_map_cv.hpp ├── package.xml ├── src │ ├── GridMapCvProcessing.cpp │ └── InpaintFilter.cpp └── test │ ├── GridMapCvProcessingTest.cpp │ ├── GridMapCvTest.cpp │ └── test_grid_map_cv.cpp ├── grid_map_demos ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ ├── filters_demo.yaml │ ├── filters_demo_filter_chain.yaml │ ├── grid_map_loader_demo.yaml │ ├── image_to_gridmap_demo.yaml │ ├── interpolation_demo.yaml │ ├── iterators_demo.yaml │ ├── move_demo.yaml │ ├── normal_filter_comparison.yaml │ ├── normal_filter_comparison_visualization.yaml │ ├── octomap_to_gridmap_demo.yaml │ ├── opencv_demo.yaml │ ├── resolution_change_demo.yaml │ ├── sdf_demo.yaml │ ├── simple_demo.yaml │ └── tutorial_demo.yaml ├── data │ ├── eth_logo.png │ ├── freiburg1_360.bt │ ├── grid_map.bag │ ├── heightmap_generated.png │ ├── linearGradient_horizontal.png │ ├── linearGradient_oblique_and_flat.png │ ├── linearGradient_oblique_only.png │ ├── linearGradient_vertical.png │ ├── radialGradient_circle.png │ ├── radialGradient_ellipse.png │ ├── radialGradient_horizontal.png │ └── terrain.png ├── doc │ ├── filters_demo.gif │ ├── filters_demo_preview.gif │ ├── image_to_grid_map_demo_result.png │ └── opencv_demo_result.gif ├── include │ └── grid_map_demos │ │ ├── FiltersDemo.hpp │ │ ├── GridmapToImageDemo.hpp │ │ ├── ImageToGridmapDemo.hpp │ │ ├── InterpolationDemo.hpp │ │ ├── IteratorsDemo.hpp │ │ ├── OctomapToGridmapDemo.hpp │ │ └── SdfDemo.hpp ├── launch │ ├── filters_demo.launch │ ├── grid_map_loader_demo.launch │ ├── image_to_gridmap_demo.launch │ ├── interpolation_demo.launch │ ├── iterators_demo.launch │ ├── move_demo.launch │ ├── normal_filter_comparison.launch │ ├── octomap_to_gridmap_demo.launch │ ├── opencv_demo.launch │ ├── resolution_change_demo.launch │ ├── sdf_demo.launch │ ├── simple_demo.launch │ └── tutorial_demo.launch ├── package.xml ├── rviz │ ├── filters_demo.rviz │ ├── grid_map_demo.rviz │ ├── grid_map_iterators_demo.rviz │ ├── interpolation_demo.rviz │ ├── normal_filter_comparison_demo.rviz │ ├── octomap_to_gridmap_demo.rviz │ └── sdf_demo.rviz ├── scripts │ └── image_publisher.py ├── src │ ├── FiltersDemo.cpp │ ├── GridmapToImageDemo.cpp │ ├── ImageToGridmapDemo.cpp │ ├── InterpolationDemo.cpp │ ├── IteratorsDemo.cpp │ ├── OctomapToGridmapDemo.cpp │ ├── SdfDemo.cpp │ ├── filters_demo_node.cpp │ ├── grid_map_to_image_demo_node.cpp │ ├── image_to_gridmap_demo_node.cpp │ ├── interpolation_demo_node.cpp │ ├── iterator_benchmark.cpp │ ├── iterators_demo_node.cpp │ ├── move_demo_node.cpp │ ├── normal_filter_comparison_node.cpp │ ├── octomap_to_gridmap_demo_node.cpp │ ├── opencv_demo_node.cpp │ ├── resolution_change_demo_node.cpp │ ├── sdf_demo_node.cpp │ ├── simple_demo_node.cpp │ └── tutorial_demo_node.cpp └── test │ └── empty_test.cpp ├── grid_map_filters ├── CHANGELOG.rst ├── CMakeLists.txt ├── filter_plugins.xml ├── include │ ├── EigenLab │ │ ├── EigenLab.h │ │ └── LICENSE │ └── grid_map_filters │ │ ├── BufferNormalizerFilter.hpp │ │ ├── ColorBlendingFilter.hpp │ │ ├── ColorFillFilter.hpp │ │ ├── ColorMapFilter.hpp │ │ ├── CurvatureFilter.hpp │ │ ├── DeletionFilter.hpp │ │ ├── DuplicationFilter.hpp │ │ ├── LightIntensityFilter.hpp │ │ ├── MathExpressionFilter.hpp │ │ ├── MeanInRadiusFilter.hpp │ │ ├── MedianFillFilter.hpp │ │ ├── MinInRadiusFilter.hpp │ │ ├── MockFilter.hpp │ │ ├── NormalColorMapFilter.hpp │ │ ├── NormalVectorsFilter.hpp │ │ ├── SetBasicLayersFilter.hpp │ │ ├── SlidingWindowMathExpressionFilter.hpp │ │ └── ThresholdFilter.hpp ├── package.xml ├── src │ ├── BufferNormalizerFilter.cpp │ ├── ColorBlendingFilter.cpp │ ├── ColorFillFilter.cpp │ ├── ColorMapFilter.cpp │ ├── CurvatureFilter.cpp │ ├── DeletionFilter.cpp │ ├── DuplicationFilter.cpp │ ├── LightIntensityFilter.cpp │ ├── MathExpressionFilter.cpp │ ├── MeanInRadiusFilter.cpp │ ├── MedianFillFilter.cpp │ ├── MinInRadiusFilter.cpp │ ├── MockFilter.cpp │ ├── NormalColorMapFilter.cpp │ ├── NormalVectorsFilter.cpp │ ├── SetBasicLayersFilter.cpp │ ├── SlidingWindowMathExpressionFilter.cpp │ ├── ThresholdFilter.cpp │ └── plugins.cpp └── test │ ├── median_fill_filter_test.cpp │ ├── mock_filter_test.cpp │ ├── test_grid_map_filters.cpp │ └── threshold_filter_test.cpp ├── grid_map_loader ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── grid_map_loader │ │ └── GridMapLoader.hpp ├── package.xml ├── src │ ├── GridMapLoader.cpp │ └── grid_map_loader_node.cpp └── test │ ├── empty_test.cpp │ └── test_grid_map_loader.cpp ├── grid_map_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── msg │ ├── GridMap.msg │ └── GridMapInfo.msg ├── package.xml └── srv │ ├── GetGridMap.srv │ ├── GetGridMapInfo.srv │ ├── ProcessFile.srv │ └── SetGridMap.srv ├── grid_map_octomap ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── grid_map_octomap │ │ ├── GridMapOctomapConverter.hpp │ │ └── grid_map_octomap.hpp ├── package.xml ├── src │ └── GridMapOctomapConverter.cpp └── test │ ├── OctomapConverterTest.cpp │ └── test_grid_map_octomap.cpp ├── grid_map_pcl ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── config │ └── parameters.yaml ├── data │ └── input_cloud.pcd ├── doc │ ├── forest.png │ ├── indoor.png │ └── outdoor.png ├── include │ └── grid_map_pcl │ │ ├── GridMapPclConverter.hpp │ │ ├── GridMapPclLoader.hpp │ │ ├── PclLoaderParameters.hpp │ │ ├── PointcloudProcessor.hpp │ │ ├── grid_map_pcl.hpp │ │ └── helpers.hpp ├── launch │ └── grid_map_pcl_loader_node.launch ├── package.xml ├── rviz │ └── grid_map_vis.rviz ├── src │ ├── GridMapPclConverter.cpp │ ├── GridMapPclLoader.cpp │ ├── PclLoaderParameters.cpp │ ├── PointcloudProcessor.cpp │ ├── grid_map_pcl_loader_node.cpp │ ├── helpers.cpp │ └── pointcloud_publisher_node.cpp └── test │ ├── GridMapPclLoaderTest.cpp │ ├── HelpersTest.cpp │ ├── PointcloudCreator.cpp │ ├── PointcloudCreator.hpp │ ├── PointcloudProcessorTest.cpp │ ├── test_data │ ├── parameters.yaml │ └── plane_noisy.pcd │ ├── test_grid_map_pcl.cpp │ ├── test_helpers.cpp │ └── test_helpers.hpp ├── grid_map_ros ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── grid_map_ros │ │ ├── GridMapMsgHelpers.hpp │ │ ├── GridMapRosConverter.hpp │ │ ├── PolygonRosConverter.hpp │ │ ├── grid_map_ros.hpp │ │ └── message_traits.hpp ├── package.xml ├── src │ ├── GridMapMsgHelpers.cpp │ ├── GridMapRosConverter.cpp │ └── PolygonRosConverter.cpp └── test │ ├── GridMapRosTest.cpp │ └── test_grid_map_ros.cpp ├── grid_map_rviz_plugin ├── CHANGELOG.rst ├── CMakeLists.txt ├── doc │ ├── grid_map_rviz_plugin.png │ └── grid_map_rviz_plugin_example.png ├── icons │ └── classes │ │ └── GridMap.png ├── include │ └── grid_map_rviz_plugin │ │ ├── GridMapColorMaps.hpp │ │ ├── GridMapDisplay.hpp │ │ ├── GridMapVisual.hpp │ │ └── modified │ │ ├── frame_manager.h │ │ └── message_filter_display.h ├── package.xml ├── plugin_description.xml ├── src │ ├── GridMapColorMaps.cpp │ ├── GridMapDisplay.cpp │ └── GridMapVisual.cpp └── test │ ├── empty_test.cpp │ └── test_grid_map_rviz_plugin.cpp ├── grid_map_sdf ├── CHANGELOG.rst ├── CMakeLists.txt ├── doc │ └── anymal_sdf_demo.gif ├── include │ └── grid_map_sdf │ │ ├── DistanceDerivatives.hpp │ │ ├── Gridmap3dLookup.hpp │ │ ├── PixelBorderDistance.hpp │ │ ├── SignedDistance2d.hpp │ │ ├── SignedDistanceField.hpp │ │ └── Utils.hpp ├── package.xml ├── src │ ├── SignedDistance2d.cpp │ └── SignedDistanceField.cpp └── test │ ├── include │ └── naiveSignedDistance.hpp │ ├── test3dLookup.cpp │ ├── testDerivatives.cpp │ ├── testPixelBorderDistance.cpp │ ├── testSignedDistance2d.cpp │ ├── testSignedDistance3d.cpp │ └── test_grid_map_sdf.cpp └── grid_map_visualization ├── CHANGELOG.rst ├── CMakeLists.txt ├── doc ├── grid_cells.jpg ├── grid_cells_preview.jpg ├── occupancy_grid.jpg ├── occupancy_grid_preview.jpg ├── point_cloud.jpg ├── point_cloud_preview.jpg ├── vectors.jpg └── vectors_preview.jpg ├── include └── grid_map_visualization │ ├── GridMapVisualization.hpp │ ├── GridMapVisualizationHelpers.hpp │ └── visualizations │ ├── FlatPointCloudVisualization.hpp │ ├── GridCellsVisualization.hpp │ ├── MapRegionVisualization.hpp │ ├── OccupancyGridVisualization.hpp │ ├── PointCloudVisualization.hpp │ ├── VectorVisualization.hpp │ ├── VisualizationBase.hpp │ └── VisualizationFactory.hpp ├── package.xml ├── src ├── GridMapVisualization.cpp ├── GridMapVisualizationHelpers.cpp ├── grid_map_visualization_node.cpp └── visualizations │ ├── FlatPointCloudVisualization.cpp │ ├── GridCellsVisualization.cpp │ ├── MapRegionVisualization.cpp │ ├── OccupancyGridVisualization.cpp │ ├── PointCloudVisualization.cpp │ ├── VectorVisualization.cpp │ ├── VisualizationBase.cpp │ └── VisualizationFactory.cpp └── test ├── empty_test.cpp └── test_grid_map_visualization.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | 30 | # Eclipse 31 | .cproject 32 | .project 33 | .settings 34 | *.pydevproject 35 | 36 | # OS X 37 | .DS_Store 38 | 39 | grid_map_pcl/data/* 40 | !grid_map_pcl/data/input_cloud.pcd 41 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2019, ANYbotics AG. 2 | 3 | Redistribution and use in source and binary forms, with or without 4 | modification, are permitted provided that the following conditions 5 | are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | 10 | 2. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the 13 | distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived 17 | from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /grid_map/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package grid_map 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.7.0 (2022-03-17) 6 | ------------------ 7 | 8 | 1.6.4 (2020-12-04) 9 | ------------------ 10 | 11 | 1.6.2 (2019-10-14) 12 | ------------------ 13 | 14 | 1.6.1 (2019-02-27) 15 | ------------------ 16 | * Updated author e-mail address. 17 | * Contributors: Peter Fankhauser 18 | 19 | 1.6.0 (2017-11-24) 20 | ------------------ 21 | 22 | 1.5.2 (2017-07-25) 23 | ------------------ 24 | 25 | 1.5.1 (2017-07-25) 26 | ------------------ 27 | 28 | 1.5.0 (2017-07-18) 29 | ------------------ 30 | 31 | 1.4.2 (2017-01-24) 32 | ------------------ 33 | 34 | 1.4.1 (2016-10-23) 35 | ------------------ 36 | * Added new grid_map_pcl package. 37 | * Contributors: Peter Fankhauser, Dominic Jud 38 | 39 | 1.4.0 (2016-08-22) 40 | ------------------ 41 | * Added new package grid_map_rviz_plugin. 42 | * Contributors: Peter Fankhauser 43 | 44 | 1.3.3 (2016-05-10) 45 | ------------------ 46 | * Release for ROS Kinetic. 47 | * Contributors: Peter Fankhauser 48 | 49 | 1.3.2 (2016-05-10) 50 | ------------------ 51 | 52 | 1.3.1 (2016-05-10) 53 | ------------------ 54 | 55 | 1.3.0 (2016-04-26) 56 | ------------------ 57 | * Separated OpenCV to grid map conversions to grid_map_cv package. The new methods 58 | are more generalized, faster, and can be used without ROS message types. 59 | * Contributors: Peter Fankhauser 60 | 61 | 1.2.0 (2016-03-03) 62 | ------------------ 63 | * Added new package grid_map as metapackage (`#34 `_). 64 | -------------------------------------------------------------------------------- /grid_map/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.1) 2 | project(grid_map) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /grid_map/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map 4 | 1.7.18 5 | Meta-package for the universal grid map library. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/grid_map 10 | http://github.com/anybotics/grid_map/issues 11 | Péter Fankhauser 12 | catkin 13 | grid_map_core 14 | grid_map_ros 15 | grid_map_cv 16 | grid_map_msgs 17 | grid_map_filters 18 | grid_map_visualization 19 | grid_map_rviz_plugin 20 | grid_map_loader 21 | grid_map_demos 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /grid_map_core/cmake/grid_map_core-extras.cmake: -------------------------------------------------------------------------------- 1 | set(EIGEN_FUNCTORS_PLUGIN_PATH "grid_map_core/eigen_plugins/FunctorsPlugin.hpp") 2 | if (EIGEN_FUNCTORS_PLUGIN) 3 | if (NOT EIGEN_FUNCTORS_PLUGIN STREQUAL EIGEN_FUNCTORS_PLUGIN_PATH) 4 | MESSAGE(FATAL_ERROR "EIGEN_FUNCTORS_PLUGIN already defined!") 5 | endif () 6 | else (EIGEN_FUNCTORS_PLUGIN) 7 | add_definitions(-DEIGEN_FUNCTORS_PLUGIN=\"${EIGEN_FUNCTORS_PLUGIN_PATH}\") 8 | endif (EIGEN_FUNCTORS_PLUGIN) 9 | 10 | set(EIGEN_DENSEBASE_PLUGIN_PATH "grid_map_core/eigen_plugins/DenseBasePlugin.hpp") 11 | if (EIGEN_DENSEBASE_PLUGIN) 12 | if (NOT EIGEN_DENSEBASE_PLUGIN STREQUAL EIGEN_DENSEBASE_PLUGIN_PATH) 13 | MESSAGE(FATAL_ERROR "EIGEN_DENSEBASE_PLUGIN already defined!") 14 | endif () 15 | else (EIGEN_DENSEBASE_PLUGIN) 16 | add_definitions(-DEIGEN_DENSEBASE_PLUGIN=\"${EIGEN_DENSEBASE_PLUGIN_PATH}\") 17 | endif (EIGEN_DENSEBASE_PLUGIN) -------------------------------------------------------------------------------- /grid_map_core/doc/grid_map_conventions.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/grid_map_conventions.pdf -------------------------------------------------------------------------------- /grid_map_core/doc/grid_map_conventions.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/grid_map_conventions.png -------------------------------------------------------------------------------- /grid_map_core/doc/grid_map_layers.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/grid_map_layers.pdf -------------------------------------------------------------------------------- /grid_map_core/doc/grid_map_layers.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/grid_map_layers.png -------------------------------------------------------------------------------- /grid_map_core/doc/interpolationGaussWorld.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/interpolationGaussWorld.gif -------------------------------------------------------------------------------- /grid_map_core/doc/interpolationSineWorld.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/interpolationSineWorld.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/circle_iterator.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/circle_iterator.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/circle_iterator_preview.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/circle_iterator_preview.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/ellipse_iterator.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/ellipse_iterator.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/ellipse_iterator_preview.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/ellipse_iterator_preview.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/grid_map_iterator.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/grid_map_iterator.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/grid_map_iterator_preview.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/grid_map_iterator_preview.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/line_iterator.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/line_iterator.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/line_iterator_preview.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/line_iterator_preview.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/polygon_iterator.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/polygon_iterator.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/polygon_iterator_preview.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/polygon_iterator_preview.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/spiral_iterator.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/spiral_iterator.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/spiral_iterator_preview.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/spiral_iterator_preview.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/submap_iterator.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/submap_iterator.gif -------------------------------------------------------------------------------- /grid_map_core/doc/iterators/submap_iterator_preview.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/iterators/submap_iterator_preview.gif -------------------------------------------------------------------------------- /grid_map_core/doc/move_method.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/move_method.gif -------------------------------------------------------------------------------- /grid_map_core/doc/setposition_method.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_core/doc/setposition_method.gif -------------------------------------------------------------------------------- /grid_map_core/include/grid_map_core/BufferRegion.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BufferRegion.hpp 3 | * 4 | * Created on: Aug 19, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "grid_map_core/TypeDefs.hpp" 12 | 13 | namespace grid_map { 14 | 15 | /*! 16 | * This class holds information about a rectangular region 17 | * of cells of the circular buffer. 18 | */ 19 | class BufferRegion 20 | { 21 | public: 22 | 23 | /*! 24 | * The definition of the buffer region positions. 25 | */ 26 | enum class Quadrant 27 | { 28 | Undefined, 29 | TopLeft, 30 | TopRight, 31 | BottomLeft, 32 | BottomRight 33 | }; 34 | 35 | constexpr static unsigned int nQuadrants = 4; 36 | 37 | BufferRegion(); 38 | BufferRegion(Index startIndex, Size size, BufferRegion::Quadrant quadrant); 39 | virtual ~BufferRegion() = default; 40 | 41 | const Index& getStartIndex() const; 42 | void setStartIndex(const Index& startIndex); 43 | const Size& getSize() const; 44 | void setSize(const Size& size); 45 | BufferRegion::Quadrant getQuadrant() const; 46 | void setQuadrant(BufferRegion::Quadrant type); 47 | 48 | private: 49 | 50 | //! Start index (typically top-left) of the buffer region. 51 | Index startIndex_; 52 | 53 | //! Size of the buffer region. 54 | Size size_; 55 | 56 | //! Quadrant type of the buffer region. 57 | Quadrant quadrant_; 58 | 59 | public: 60 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 61 | }; 62 | 63 | } /* namespace grid_map */ 64 | -------------------------------------------------------------------------------- /grid_map_core/include/grid_map_core/SubmapGeometry.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SubmapGeometry.hpp 3 | * 4 | * Created on: Aug 18, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | namespace grid_map { 14 | 15 | class GridMap; 16 | 17 | /*! 18 | * This class holds information about the geometry of submap 19 | * region of a grid map. Note that, this class does NOT hold 20 | * the any data of the grid map. 21 | */ 22 | class SubmapGeometry 23 | { 24 | public: 25 | 26 | /*! 27 | * Constructor. Note that the requested position and length 28 | * of the submap is adapted to fit the geometry of the parent 29 | * grid map. 30 | * @param[in] gridMap the parent grid map containing the submap. 31 | * @param[in] position the requested submap position (center). 32 | * @param[in] length the requested submap length. 33 | * @param[out] isSuccess true if successful, false otherwise. 34 | */ 35 | SubmapGeometry(const GridMap& gridMap, const Position& position, const Length& length, 36 | bool& isSuccess); 37 | 38 | virtual ~SubmapGeometry() = default; 39 | 40 | const GridMap& getGridMap() const; 41 | const Length& getLength() const; 42 | const Position& getPosition() const; 43 | const Index& getRequestedIndexInSubmap() const; 44 | const Size& getSize() const; 45 | double getResolution() const; 46 | const Index& getStartIndex() const; 47 | 48 | private: 49 | 50 | //! Parent grid map of the submap. 51 | const GridMap& gridMap_; 52 | 53 | //! Start index (typically top left) index of the submap. 54 | Index startIndex_; 55 | 56 | //! Size of the submap. 57 | Size size_; 58 | 59 | //! Position (center) of the submap. 60 | Position position_; 61 | 62 | //! Length of the submap. 63 | Length length_; 64 | 65 | //! Index in the submap that corresponds to the requested 66 | //! position of the submap. 67 | Index requestedIndexInSubmap_; 68 | 69 | public: 70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 71 | }; 72 | 73 | } /* namespace grid_map */ 74 | -------------------------------------------------------------------------------- /grid_map_core/include/grid_map_core/TypeDefs.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TypeDefs.hpp 3 | * 4 | * Created on: March 18, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | // Eigen 10 | #pragma once 11 | 12 | #include 13 | 14 | namespace grid_map { 15 | 16 | using Matrix = Eigen::MatrixXf; 17 | using DataType = Matrix::Scalar; 18 | using Position = Eigen::Vector2d; 19 | using Vector = Eigen::Vector2d; 20 | using Position3 = Eigen::Vector3d; 21 | using Vector3 = Eigen::Vector3d; 22 | using Index = Eigen::Array2i; 23 | using Size = Eigen::Array2i; 24 | using Length = Eigen::Array2d; 25 | using Time = uint64_t; 26 | 27 | /* 28 | * Interpolations are ordered in the order 29 | * of increasing accuracy and computational complexity. 30 | * INTER_NEAREST - fastest, but least accurate, 31 | * INTER_CUBIC - slowest, but the most accurate. 32 | * see: 33 | * https://en.wikipedia.org/wiki/Bicubic_interpolation 34 | * https://web.archive.org/web/20051024202307/http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm 35 | * for more info. Cubic convolution algorithm is also known as piecewise cubic 36 | * interpolation and in general does not guarantee continuous 37 | * first derivatives. 38 | */ 39 | enum class InterpolationMethods{ 40 | INTER_NEAREST, // nearest neighbor interpolation 41 | INTER_LINEAR, // bilinear interpolation 42 | INTER_CUBIC_CONVOLUTION, //piecewise bicubic interpolation using convolution algorithm 43 | INTER_CUBIC // standard bicubic interpolation 44 | }; 45 | 46 | } // namespace grid_map 47 | 48 | -------------------------------------------------------------------------------- /grid_map_core/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | Scalar numberOfFinites() const 4 | { 5 | if (SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) { 6 | return Scalar(0); 7 | } 8 | return Scalar((derived().array() == derived().array()).count()); 9 | } 10 | 11 | Scalar sumOfFinites() const 12 | { 13 | if (SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) { 14 | return Scalar(0); 15 | } 16 | return Scalar(this->redux(Eigen::internal::scalar_sum_of_finites_op())); 17 | } 18 | 19 | Scalar meanOfFinites() const 20 | { 21 | return Scalar(this->redux(Eigen::internal::scalar_sum_of_finites_op())) / this->numberOfFinites(); 22 | } 23 | 24 | Scalar minCoeffOfFinites() const 25 | { 26 | return Scalar(this->redux(Eigen::internal::scalar_min_of_finites_op())); 27 | } 28 | 29 | Scalar maxCoeffOfFinites() const 30 | { 31 | return Scalar(this->redux(Eigen::internal::scalar_max_of_finites_op())); 32 | } 33 | -------------------------------------------------------------------------------- /grid_map_core/include/grid_map_core/eigen_plugins/Functors.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Functors.hpp 3 | * 4 | * Created on: Nov 23, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | namespace grid_map { 12 | 13 | template 14 | struct Clamp 15 | { 16 | Clamp(const Scalar& min, const Scalar& max) 17 | : min_(min), 18 | max_(max) 19 | { 20 | } 21 | Scalar operator()(const Scalar& x) const 22 | { 23 | return x < min_ ? min_ : (x > max_ ? max_ : x); 24 | } 25 | Scalar min_, max_; 26 | }; 27 | 28 | } // namespace grid_map 29 | -------------------------------------------------------------------------------- /grid_map_core/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | template struct scalar_sum_of_finites_op { 4 | EIGEN_EMPTY_STRUCT_CTOR(scalar_sum_of_finites_op) 5 | EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { 6 | using std::isfinite; 7 | if (isfinite(a) && isfinite(b)) return a + b; 8 | if (isfinite(a)) return a; 9 | if (isfinite(b)) return b; 10 | return a + b; 11 | } 12 | }; 13 | template 14 | struct functor_traits > { 15 | enum { 16 | Cost = 2 * NumTraits::ReadCost + NumTraits::AddCost, 17 | PacketAccess = false 18 | }; 19 | }; 20 | 21 | template 22 | struct scalar_min_of_finites_op { 23 | EIGEN_EMPTY_STRUCT_CTOR(scalar_min_of_finites_op) 24 | EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { 25 | using std::min; 26 | using std::isfinite; 27 | if (isfinite(a) && isfinite(b)) return (min)(a, b); 28 | if (isfinite(a)) return a; 29 | if (isfinite(b)) return b; 30 | return (min)(a, b); 31 | } 32 | }; 33 | template 34 | struct functor_traits > { 35 | enum { 36 | Cost = NumTraits::AddCost, 37 | PacketAccess = false 38 | }; 39 | }; 40 | 41 | template 42 | struct scalar_max_of_finites_op { 43 | EIGEN_EMPTY_STRUCT_CTOR(scalar_max_of_finites_op) 44 | EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { 45 | using std::max; 46 | using std::isfinite; 47 | if (isfinite(a) && isfinite(b)) return (max)(a, b); 48 | if (isfinite(a)) return a; 49 | if (isfinite(b)) return b; 50 | return (max)(a, b); 51 | } 52 | }; 53 | template 54 | struct functor_traits > { 55 | enum { 56 | Cost = NumTraits::AddCost, 57 | PacketAccess = false 58 | }; 59 | }; 60 | -------------------------------------------------------------------------------- /grid_map_core/include/grid_map_core/grid_map_core.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * grid_map_core.hpp 3 | * 4 | * Created on: Jul 14, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "grid_map_core/TypeDefs.hpp" 12 | #include "grid_map_core/GridMap.hpp" 13 | #include "grid_map_core/SubmapGeometry.hpp" 14 | #include "grid_map_core/GridMapMath.hpp" 15 | #include "grid_map_core/BufferRegion.hpp" 16 | #include "grid_map_core/Polygon.hpp" 17 | #include "grid_map_core/iterators/iterators.hpp" 18 | #include "grid_map_core/eigen_plugins/Functors.hpp" 19 | -------------------------------------------------------------------------------- /grid_map_core/include/grid_map_core/iterators/iterators.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * iterators.hpp 3 | * 4 | * Created on: Sep 22, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "grid_map_core/iterators/GridMapIterator.hpp" 12 | #include "grid_map_core/iterators/SubmapIterator.hpp" 13 | #include "grid_map_core/iterators/CircleIterator.hpp" 14 | #include "grid_map_core/iterators/EllipseIterator.hpp" 15 | #include "grid_map_core/iterators/SpiralIterator.hpp" 16 | #include "grid_map_core/iterators/LineIterator.hpp" 17 | #include "grid_map_core/iterators/PolygonIterator.hpp" 18 | #include "grid_map_core/iterators/SlidingWindowIterator.hpp" 19 | -------------------------------------------------------------------------------- /grid_map_core/include/grid_map_core/utils/testing.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copied from ANYbotics/eigen_utils, to avoid testing dependency. 3 | */ 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | #define ASSERT_MATRICES_EQ_WITH_NAN(first, second) assertMatrixesEqualWithNan((first), #first, (second), #second, __LINE__) 11 | static void assertMatrixesEqualWithNan(Eigen::Ref first, std::string firstName, 12 | Eigen::Ref second, std::string secondName, int line) { 13 | ASSERT_EQ(first.rows(), second.rows()); 14 | ASSERT_EQ(first.cols(), second.cols()); 15 | 16 | bool matricesAreEqual = true; 17 | for (Eigen::Index row = 0; row < first.rows() && matricesAreEqual; ++row) { 18 | for (Eigen::Index col = 0; col < first.cols() && matricesAreEqual; ++col) { 19 | bool ifRealThenValid = first.block<1, 1>(row, col).isApprox(second.block<1, 1>(row, col)); 20 | bool bothNaN = std::isnan(first(row, col)) && std::isnan(second(row, col)); 21 | if (ifRealThenValid || bothNaN) { 22 | continue; 23 | } else { 24 | matricesAreEqual = false; 25 | } 26 | } 27 | } 28 | 29 | Eigen::IOFormat compactFormat(2, 0, ",", "\n", "[", "]"); 30 | ASSERT_TRUE(matricesAreEqual) // NO LINT 31 | << "L. " << std::to_string(line) << ": Matrices are not equal" // NO LINT 32 | << "\n" // NO LINT 33 | << firstName << "\n" // NO LINT 34 | << first.format(compactFormat) << "\n" // NO LINT 35 | << secondName << "\n" // NO LINT 36 | << second.format(compactFormat) << "\n"; // NO LINT 37 | } 38 | -------------------------------------------------------------------------------- /grid_map_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_core 4 | 1.7.18 5 | Universal grid map library to manage two-dimensional grid maps with multiple data layers. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/grid_map 10 | http://github.com/anybotics/grid_map/issues 11 | Péter Fankhauser 12 | catkin 13 | 14 | eigen 15 | 16 | gtest 17 | 18 | -------------------------------------------------------------------------------- /grid_map_core/src/BufferRegion.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BufferRegion.cpp 3 | * 4 | * Created on: Aug 19, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | #include 9 | 10 | namespace grid_map { 11 | 12 | BufferRegion::BufferRegion() : startIndex_(Index::Zero()), 13 | size_(Size::Zero()), 14 | quadrant_(BufferRegion::Quadrant::Undefined) 15 | { 16 | } 17 | 18 | BufferRegion::BufferRegion(Index index, Size size, BufferRegion::Quadrant quadrant) : startIndex_(std::move(index)), 19 | size_(std::move(size)), 20 | quadrant_(std::move(quadrant)) 21 | { 22 | } 23 | 24 | const Index& BufferRegion::getStartIndex() const 25 | { 26 | return startIndex_; 27 | } 28 | 29 | void BufferRegion::setStartIndex(const Index& startIndex) 30 | { 31 | startIndex_ = startIndex; 32 | } 33 | 34 | const Size& BufferRegion::getSize() const 35 | { 36 | return size_; 37 | } 38 | 39 | void BufferRegion::setSize(const Size& size) 40 | { 41 | size_ = size; 42 | } 43 | 44 | BufferRegion::Quadrant BufferRegion::getQuadrant() const 45 | { 46 | return quadrant_; 47 | } 48 | 49 | void BufferRegion::setQuadrant(BufferRegion::Quadrant type) 50 | { 51 | quadrant_ = type; 52 | } 53 | 54 | } /* namespace grid_map */ 55 | 56 | 57 | -------------------------------------------------------------------------------- /grid_map_core/src/SubmapGeometry.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SubmapGeometry.cpp 3 | * 4 | * Created on: Aug 18, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | namespace grid_map { 13 | 14 | SubmapGeometry::SubmapGeometry(const GridMap& gridMap, const Position& position, 15 | const Length& length, bool& isSuccess) 16 | : gridMap_(gridMap) 17 | { 18 | isSuccess = getSubmapInformation(startIndex_, size_, position_, length_, 19 | requestedIndexInSubmap_, position, length, gridMap_.getLength(), 20 | gridMap_.getPosition(), gridMap_.getResolution(), 21 | gridMap_.getSize(), gridMap_.getStartIndex()); 22 | } 23 | 24 | const GridMap& SubmapGeometry::getGridMap() const 25 | { 26 | return gridMap_; 27 | } 28 | 29 | const Length& SubmapGeometry::getLength() const 30 | { 31 | return length_; 32 | } 33 | 34 | const Position& SubmapGeometry::getPosition() const 35 | { 36 | return position_; 37 | } 38 | 39 | const Index& SubmapGeometry::getRequestedIndexInSubmap() const 40 | { 41 | return requestedIndexInSubmap_; 42 | } 43 | 44 | const Size& SubmapGeometry::getSize() const 45 | { 46 | return size_; 47 | } 48 | 49 | double SubmapGeometry::getResolution() const 50 | { 51 | return gridMap_.getResolution(); 52 | } 53 | 54 | const Index& SubmapGeometry::getStartIndex() const 55 | { 56 | return startIndex_; 57 | } 58 | 59 | } /* namespace grid_map */ 60 | -------------------------------------------------------------------------------- /grid_map_core/src/iterators/GridMapIterator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GridMapIterator.cpp 3 | * 4 | * Created on: Sep 22, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_core/iterators/GridMapIterator.hpp" 10 | #include "grid_map_core/GridMapMath.hpp" 11 | 12 | namespace grid_map { 13 | 14 | GridMapIterator::GridMapIterator(const grid_map::GridMap& gridMap) 15 | { 16 | size_ = gridMap.getSize(); 17 | startIndex_ = gridMap.getStartIndex(); 18 | linearSize_ = size_.prod(); 19 | linearIndex_ = 0; 20 | isPastEnd_ = false; 21 | } 22 | 23 | GridMapIterator::GridMapIterator(const GridMapIterator* other) 24 | { 25 | size_ = other->size_; 26 | startIndex_ = other->startIndex_; 27 | linearSize_ = other->linearSize_; 28 | linearIndex_ = other->linearIndex_; 29 | isPastEnd_ = other->isPastEnd_; 30 | } 31 | 32 | bool GridMapIterator::operator !=(const GridMapIterator& other) const 33 | { 34 | return linearIndex_ != other.linearIndex_; 35 | } 36 | 37 | Index GridMapIterator::operator *() const 38 | { 39 | return getIndexFromLinearIndex(linearIndex_, size_); 40 | } 41 | 42 | const size_t& GridMapIterator::getLinearIndex() const 43 | { 44 | return linearIndex_; 45 | } 46 | 47 | Index GridMapIterator::getUnwrappedIndex() const 48 | { 49 | return getIndexFromBufferIndex(*(*this), size_, startIndex_); 50 | } 51 | 52 | GridMapIterator& GridMapIterator::operator ++() 53 | { 54 | size_t newIndex = linearIndex_ + 1; 55 | if (newIndex < linearSize_) { 56 | linearIndex_ = newIndex; 57 | } else { 58 | isPastEnd_ = true; 59 | } 60 | return *this; 61 | } 62 | 63 | GridMapIterator GridMapIterator::end() const 64 | { 65 | GridMapIterator res(this); 66 | res.linearIndex_ = linearSize_ - 1; 67 | return res; 68 | } 69 | 70 | bool GridMapIterator::isPastEnd() const 71 | { 72 | return isPastEnd_; 73 | } 74 | 75 | } /* namespace grid_map */ 76 | -------------------------------------------------------------------------------- /grid_map_core/test/EllipseIteratorTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * EllipseIteratorTest.cpp 3 | * 4 | * Created on: Dec 2, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_core/iterators/EllipseIterator.hpp" 10 | #include "grid_map_core/GridMap.hpp" 11 | 12 | // gtest 13 | #include 14 | 15 | // Vector 16 | #include 17 | 18 | using grid_map::GridMap; 19 | using grid_map::Length; 20 | using grid_map::Position; 21 | using grid_map::EllipseIterator; 22 | 23 | TEST(EllipseIterator, OneCellWideEllipse) 24 | { 25 | GridMap map( { "types" }); 26 | map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); 27 | 28 | EllipseIterator iterator(map, Position(0.0, 0.0), Length(8.0, 1.0)); 29 | 30 | EXPECT_FALSE(iterator.isPastEnd()); 31 | EXPECT_EQ(0, (*iterator)(0)); 32 | EXPECT_EQ(2, (*iterator)(1)); 33 | 34 | ++iterator; 35 | EXPECT_FALSE(iterator.isPastEnd()); 36 | EXPECT_EQ(1, (*iterator)(0)); 37 | EXPECT_EQ(2, (*iterator)(1)); 38 | 39 | ++iterator; 40 | EXPECT_FALSE(iterator.isPastEnd()); 41 | EXPECT_EQ(2, (*iterator)(0)); 42 | EXPECT_EQ(2, (*iterator)(1)); 43 | 44 | ++iterator; 45 | ++iterator; 46 | ++iterator; 47 | ++iterator; 48 | ++iterator; 49 | EXPECT_FALSE(iterator.isPastEnd()); 50 | EXPECT_EQ(7, (*iterator)(0)); 51 | EXPECT_EQ(2, (*iterator)(1)); 52 | 53 | ++iterator; 54 | EXPECT_TRUE(iterator.isPastEnd()); 55 | } 56 | -------------------------------------------------------------------------------- /grid_map_core/test/GridMapIteratorTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GridMapDataIterator.cpp 3 | * 4 | * Created on: Feb 16, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_core/iterators/GridMapIterator.hpp" 10 | #include "grid_map_core/GridMap.hpp" 11 | 12 | // gtest 13 | #include 14 | 15 | // Vector 16 | #include 17 | 18 | using grid_map::GridMap; 19 | using grid_map::Length; 20 | using grid_map::Position; 21 | using grid_map::GridMapIterator; 22 | 23 | TEST(GridMapIterator, Simple) 24 | { 25 | GridMap map; 26 | map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) 27 | map.add("layer", 0.0); 28 | GridMapIterator iterator(map); 29 | 30 | unsigned int i = 0; 31 | for (; !iterator.isPastEnd(); ++iterator, ++i) { 32 | map.at("layer", *iterator) = 1.0; 33 | EXPECT_FALSE(iterator.isPastEnd()); 34 | } 35 | 36 | EXPECT_EQ(40, i); 37 | EXPECT_TRUE(iterator.isPastEnd()); 38 | EXPECT_TRUE((map["layer"].array() == 1.0f).all()); 39 | } 40 | 41 | TEST(GridMapIterator, LinearIndex) 42 | { 43 | GridMap map; 44 | map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) 45 | map.add("layer", 0.0); 46 | GridMapIterator iterator(map); 47 | 48 | auto& data = map["layer"]; 49 | unsigned int i = 0; 50 | for (; !iterator.isPastEnd(); ++iterator, ++i) { 51 | data(static_cast(iterator.getLinearIndex())) = 1.0; 52 | EXPECT_EQ(i, iterator.getLinearIndex()); 53 | EXPECT_FALSE(iterator.isPastEnd()); 54 | } 55 | 56 | EXPECT_EQ(40, i); 57 | EXPECT_TRUE(iterator.isPastEnd()); 58 | EXPECT_TRUE((map["layer"].array() == 1.0f).all()); 59 | } 60 | -------------------------------------------------------------------------------- /grid_map_core/test/SpiralIteratorTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SpiralIteratorTest.cpp 3 | * 4 | * Created on: Jul 26, 2017 5 | * Author: Benjamin Scholz 6 | * Institute: University of Hamburg, TAMS 7 | */ 8 | 9 | #include "grid_map_core/iterators/SpiralIterator.hpp" 10 | #include "grid_map_core/GridMap.hpp" 11 | 12 | // gtest 13 | #include 14 | 15 | // Vector 16 | #include 17 | 18 | using grid_map::GridMap; 19 | using grid_map::Length; 20 | using grid_map::Position; 21 | using grid_map::SpiralIterator; 22 | 23 | TEST(SpiralIterator, CenterOutOfMap) 24 | { 25 | GridMap map( { "types" }); 26 | map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); 27 | Position center(8.0, 0.0); 28 | double radius = 5.0; 29 | 30 | SpiralIterator iterator(map, center, radius); 31 | 32 | Position iterator_position; 33 | map.getPosition(*iterator, iterator_position); 34 | 35 | EXPECT_TRUE(map.isInside(iterator_position)); 36 | } 37 | -------------------------------------------------------------------------------- /grid_map_core/test/test_grid_map_core.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_grid_map.cpp 3 | * 4 | * Created on: Feb 10, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | // gtest 10 | #include 11 | 12 | // Run all the tests that were declared with TEST() 13 | int main(int argc, char **argv) 14 | { 15 | testing::InitGoogleTest(&argc, argv); 16 | srand((int)time(nullptr)); 17 | return RUN_ALL_TESTS(); 18 | } 19 | -------------------------------------------------------------------------------- /grid_map_costmap_2d/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package grid_map_costmap_2d 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.7.0 (2022-03-17) 6 | ------------------ 7 | 8 | 1.6.4 (2020-12-04) 9 | ------------------ 10 | 11 | 1.6.2 (2019-10-14) 12 | ------------------ 13 | 14 | 1.6.1 (2019-02-27) 15 | ------------------ 16 | * Merge pull request `#202 `_ from ANYbotics/fix/tf_for_indigo_and_kinetic 17 | Fix/tf for indigo and kinetic 18 | * [grid_map_costmap_2d] Fixed test on melodic. 19 | * [grid_map_costmap_2d] Tests using tf instead of tf2 for indigo/kinetic. 20 | * Refactoring. 21 | * Initializing ROSCostmap with tf buffer. 22 | * typos fixed. 23 | * fixed typo in initializer list. 24 | * renamed member variable of costmap according to styleguide. 25 | * fixed backwards compatibility for costmap_2d. 26 | * Updated host changes. 27 | * Updated author e-mail address. 28 | * Contributors: Marco Sütterlin, Peter Fankhauser, Péter Fankhauser, Remo Diethelm, Samuel Bachmann 29 | 30 | 1.6.0 (2017-11-24) 31 | ------------------ 32 | 33 | 1.5.2 (2017-07-25) 34 | ------------------ 35 | 36 | 1.5.1 (2017-07-25) 37 | ------------------ 38 | 39 | 1.5.0 (2017-07-18) 40 | ------------------ 41 | * Move costmap_2d conversion into separate package. 42 | * Contributors: Peter Fankhauser 43 | -------------------------------------------------------------------------------- /grid_map_costmap_2d/include/grid_map_costmap_2d/grid_map_costmap_2d.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * grid_map.hpp 3 | * 4 | * Created on: Jul 14, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | -------------------------------------------------------------------------------- /grid_map_costmap_2d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_costmap_2d 4 | 1.7.18 5 | Interface for grid maps to the costmap_2d format. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/grid_map 10 | http://github.com/anybotics/grid_map/issues 11 | Péter Fankhauser 12 | catkin 13 | 14 | grid_map_core 15 | costmap_2d 16 | tf 17 | 18 | gtest 19 | 20 | -------------------------------------------------------------------------------- /grid_map_costmap_2d/rostest/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################## 2 | # Unit Tests 3 | # 4 | # Only run when CATKIN_ENABLE_TESTING is true. 5 | ############################################################################## 6 | 7 | find_package(rostest REQUIRED) 8 | 9 | ############################################################################## 10 | # Macro 11 | ############################################################################## 12 | 13 | macro(${PROJECT_NAME}_add_rostest test_name) 14 | add_rostest_gtest(test_${test_name} ${test_name}/${test_name}.test ${test_name}/${test_name}.cpp) 15 | if (TARGET test_${test_name}) 16 | add_dependencies(test_${test_name} ${catkin_EXPORTED_TARGETS}) 17 | target_link_libraries(test_${test_name} ${catkin_LIBRARIES}) 18 | endif() 19 | endmacro() 20 | 21 | ############################################################################## 22 | # Tests 23 | ############################################################################## 24 | 25 | grid_map_costmap_2d_add_rostest(costmap_2d_ros) 26 | -------------------------------------------------------------------------------- /grid_map_costmap_2d/rostest/README.md: -------------------------------------------------------------------------------- 1 | 2 | I can never remember how to run them... 3 | 4 | * http://wiki.ros.org/rostest/Commandline 5 | 6 | ``` 7 | rostest --text mytest.test 8 | ``` 9 | 10 | -------------------------------------------------------------------------------- /grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /grid_map_costmap_2d/test/test_grid_map_costmap_2d.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_grid_map.cpp 3 | * 4 | * Created on: Feb 10, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | // gtest 10 | #include 11 | 12 | // Run all the tests that were declared with TEST() 13 | int main(int argc, char **argv) 14 | { 15 | testing::InitGoogleTest(&argc, argv); 16 | srand((int)time(0)); 17 | return RUN_ALL_TESTS(); 18 | } 19 | -------------------------------------------------------------------------------- /grid_map_cv/filter_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Use OpenCV to inpaint/fill holes in a layer. 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /grid_map_cv/include/grid_map_cv/InpaintFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * InpaintFilter.hpp 3 | * 4 | * Created on: May 6, 2017 5 | * Author: Tanja Baumann, Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | //OpenCV 14 | #include "grid_map_cv/grid_map_cv.hpp" 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | namespace grid_map { 21 | 22 | /*! 23 | * Uses OpenCV function to inpaint/fill holes in the input layer. 24 | */ 25 | template 26 | class InpaintFilter : public filters::FilterBase { 27 | 28 | public: 29 | /*! 30 | * Constructor 31 | */ 32 | InpaintFilter(); 33 | 34 | /*! 35 | * Destructor. 36 | */ 37 | virtual ~InpaintFilter(); 38 | 39 | /*! 40 | * Configures the filter from parameters on the Parameter Server 41 | */ 42 | virtual bool configure(); 43 | 44 | /*! 45 | * Adds a new output layer to the map. 46 | * Uses the OpenCV function inpaint holes in the input layer. 47 | * Saves to filled map in the outputlayer. 48 | * @param mapIn grid map containing input layer 49 | * @param mapOut grid map containing mapIn and inpainted input layer. 50 | */ 51 | virtual bool update(const T& mapIn, T& mapOut); 52 | 53 | private: 54 | 55 | //! Inpainting radius. 56 | double radius_; 57 | 58 | //! Input layer name. 59 | std::string inputLayer_; 60 | 61 | //! Output layer name. 62 | std::string outputLayer_; 63 | }; 64 | 65 | } /* namespace */ 66 | -------------------------------------------------------------------------------- /grid_map_cv/include/grid_map_cv/grid_map_cv.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * grid_map_cv.hpp 3 | * 4 | * Created on: Apr 14, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | -------------------------------------------------------------------------------- /grid_map_cv/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_cv 4 | 1.7.18 5 | Conversions between grid maps and OpenCV images. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/grid_map 10 | http://github.com/anybotics/grid_map/issues 11 | Péter Fankhauser 12 | Magnus Gärtner 13 | catkin 14 | 15 | grid_map_core 16 | cv_bridge 17 | filters 18 | libopencv-dev 19 | 20 | gtest 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /grid_map_cv/test/test_grid_map_cv.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_grid_map_cv.cpp 3 | * 4 | * Created on: Apr 14, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | // gtest 10 | #include 11 | 12 | // Run all the tests that were declared with TEST() 13 | int main(int argc, char **argv) 14 | { 15 | testing::InitGoogleTest(&argc, argv); 16 | srand((int)time(0)); 17 | return RUN_ALL_TESTS(); 18 | } 19 | -------------------------------------------------------------------------------- /grid_map_demos/config/filters_demo.yaml: -------------------------------------------------------------------------------- 1 | image_to_gridmap_demo: 2 | image_topic: "/image_publisher/image" 3 | resolution: 0.02 4 | map_frame_id: "map" 5 | min_height: -0.5 6 | max_height: 1.0 7 | 8 | grid_map_visualization: 9 | grid_map_topic: /grid_map_filter_demo/filtered_map 10 | grid_map_visualizations: 11 | - name: surface_normals 12 | type: vectors 13 | params: 14 | layer_prefix: normal_vectors_ 15 | position_layer: elevation 16 | scale: 0.06 17 | line_width: 0.005 18 | color: 15600153 # red 19 | - name: traversability_grid 20 | type: occupancy_grid 21 | params: 22 | layer: traversability 23 | data_min: 0.0 24 | data_max: 1.0 25 | -------------------------------------------------------------------------------- /grid_map_demos/config/grid_map_loader_demo.yaml: -------------------------------------------------------------------------------- 1 | grid_map_topic: /grid_map_loader_demo/grid_map 2 | grid_map_visualizations: 3 | - name: elevation_points 4 | type: point_cloud 5 | params: 6 | layer: elevation 7 | 8 | -------------------------------------------------------------------------------- /grid_map_demos/config/image_to_gridmap_demo.yaml: -------------------------------------------------------------------------------- 1 | image_to_gridmap_demo: 2 | image_topic: "/image_publisher/image" 3 | resolution: 0.03 4 | map_frame_id: "map" 5 | min_height: -0.1 6 | max_height: 0.1 7 | 8 | grid_map_visualization: 9 | grid_map_topic: /image_to_gridmap_demo/grid_map 10 | grid_map_visualizations: 11 | - name: elevation_points 12 | type: point_cloud 13 | params: 14 | layer: elevation 15 | - name: elevation_grid 16 | type: occupancy_grid 17 | params: 18 | layer: elevation 19 | data_min: -0.2 20 | data_max: 0.2 21 | - name: flat_grid 22 | type: flat_point_cloud 23 | params: 24 | height: 0.0 25 | -------------------------------------------------------------------------------- /grid_map_demos/config/interpolation_demo.yaml: -------------------------------------------------------------------------------- 1 | 2 | world: Sine # options are: Sine, Poly, Gauss, Tanh 3 | 4 | interpolation_type: Nearest # options are: Nearest, Linear, Cubic_convolution, Cubic 5 | 6 | world_size: 7 | length: 5.0 8 | width: 5.0 9 | 10 | groundtruth_resolution: 0.02 # resolution in which ground truth is displayed in rviz 11 | 12 | interpolation: 13 | data_resolution: 0.1 # resolution of the data points for the interpolating algorithm 14 | # this number should be higher than groundtruth_resolution 15 | # if you want to see any effect 16 | 17 | interpolated_resolution: 0.02 # resolution requested for the interpolated map 18 | # this number should be lower than data_resolution 19 | # if you want to see any interpolation effect 20 | -------------------------------------------------------------------------------- /grid_map_demos/config/iterators_demo.yaml: -------------------------------------------------------------------------------- 1 | grid_map_topic: /grid_map_iterators_demo/grid_map 2 | grid_map_visualizations: 3 | - name: occupancy_grid 4 | type: occupancy_grid 5 | params: 6 | layer: type 7 | data_min: 0.0 8 | data_max: 1.0 9 | -------------------------------------------------------------------------------- /grid_map_demos/config/move_demo.yaml: -------------------------------------------------------------------------------- 1 | grid_map_topic: /grid_map_move_demo/grid_map 2 | grid_map_visualizations: 3 | - name: occupancy_grid 4 | type: occupancy_grid 5 | params: 6 | layer: layer 7 | data_min: 0.0 8 | data_max: 1.0 9 | -------------------------------------------------------------------------------- /grid_map_demos/config/normal_filter_comparison.yaml: -------------------------------------------------------------------------------- 1 | grid_map_filters: 2 | filter_chain: 3 | # Compute surface normals. 4 | - 5 | name: surface_normals_area 6 | type: gridMapFilters/NormalVectorsFilter 7 | params: 8 | input_layer: elevation_filtered 9 | algorithm: area 10 | output_layers_prefix: normal_area_ 11 | radius: 0.05 12 | normal_vector_positive_axis: z 13 | parallelization_enabled: true 14 | thread_number: 4 15 | # thread_number: -1 doesn't limit the number of threads to be used 16 | 17 | - 18 | name: surface_normals_raster 19 | type: gridMapFilters/NormalVectorsFilter 20 | params: 21 | input_layer: elevation_filtered 22 | algorithm: raster 23 | output_layers_prefix: normal_raster_ 24 | normal_vector_positive_axis: z 25 | parallelization_enabled: true 26 | thread_number: 4 -------------------------------------------------------------------------------- /grid_map_demos/config/normal_filter_comparison_visualization.yaml: -------------------------------------------------------------------------------- 1 | grid_map_topic: /normal_filter_comparison_demo/grid_map 2 | 3 | grid_map_visualizations: 4 | 5 | - name: elevation_points 6 | type: point_cloud 7 | params: 8 | layer: elevation 9 | 10 | - name: noisy_points 11 | type: point_cloud 12 | params: 13 | layer: elevation_noisy 14 | 15 | - name: filtered_points 16 | type: point_cloud 17 | params: 18 | layer: elevation_filtered 19 | 20 | - name: map_region 21 | type: map_region 22 | params: 23 | color: 3289650 24 | line_width: 0.003 25 | 26 | - name: elevation_grid 27 | type: occupancy_grid 28 | params: 29 | layer: elevation 30 | data_min: 0.08 31 | data_max: -0.16 32 | 33 | - name: error_grid 34 | type: occupancy_grid 35 | params: 36 | layer: error 37 | data_min: -0.15 38 | data_max: 0.15 39 | 40 | - name: surface_normals_analytic 41 | type: vectors 42 | params: 43 | layer_prefix: normal_analytic_ 44 | position_layer: elevation 45 | scale: 0.06 46 | line_width: 0.005 47 | color: 15600153 # red 48 | 49 | - name: surface_normals_raster 50 | type: vectors 51 | params: 52 | layer_prefix: normal_raster_ 53 | position_layer: elevation 54 | scale: 0.06 55 | line_width: 0.005 56 | color: 255 # blue 57 | 58 | - name: surface_normals_area 59 | type: vectors 60 | params: 61 | layer_prefix: normal_area_ 62 | position_layer: elevation 63 | scale: 0.06 64 | line_width: 0.005 65 | color: 65280 # green 66 | 67 | - name: elevation_cells 68 | type: grid_cells 69 | params: 70 | layer: elevation 71 | lower_threshold: -0.08 72 | upper_threshold: 0.0 73 | -------------------------------------------------------------------------------- /grid_map_demos/config/octomap_to_gridmap_demo.yaml: -------------------------------------------------------------------------------- 1 | octomap_to_gridmap_demo: 2 | octomap_service_topic: "/octomap_binary" 3 | min_z: -2.0 4 | max_z: 2.0 5 | 6 | grid_map_visualization: 7 | grid_map_topic: /octomap_to_gridmap_demo/grid_map 8 | grid_map_visualizations: 9 | - name: elevation_points 10 | type: point_cloud 11 | params: 12 | layer: elevation 13 | - name: elevation_grid 14 | type: occupancy_grid 15 | params: 16 | layer: elevation 17 | data_min: -2.0 18 | data_max: 2.0 19 | - name: map_region 20 | type: map_region 21 | params: 22 | color: 3289650 23 | line_width: 0.1 24 | -------------------------------------------------------------------------------- /grid_map_demos/config/opencv_demo.yaml: -------------------------------------------------------------------------------- 1 | grid_map_topic: /grid_map_opencv_demo/grid_map 2 | grid_map_visualizations: 3 | - name: elevation_points 4 | type: point_cloud 5 | params: 6 | layer: elevation -------------------------------------------------------------------------------- /grid_map_demos/config/resolution_change_demo.yaml: -------------------------------------------------------------------------------- 1 | grid_map_topic: /grid_map_resolution_change_demo/grid_map 2 | grid_map_visualizations: 3 | - name: occupancy_grid 4 | type: occupancy_grid 5 | params: 6 | layer: elevation 7 | data_min: 0.0 8 | data_max: 0.3 -------------------------------------------------------------------------------- /grid_map_demos/config/sdf_demo.yaml: -------------------------------------------------------------------------------- 1 | grid_map_topic: /image_to_gridmap_demo/grid_map 2 | pointcloud_topic: /grid_map_sdf_demo/signed_distance_field 3 | elevation_layer: "elevation" 4 | image_to_gridmap_demo: 5 | image_topic: "/image_publisher/image" 6 | resolution: 0.03 7 | map_frame_id: "map" 8 | min_height: -0.3 9 | max_height: 0.4 10 | grid_map_visualization: 11 | grid_map_topic: /image_to_gridmap_demo/grid_map 12 | grid_map_visualizations: 13 | - name: elevation_points 14 | type: point_cloud 15 | params: 16 | layer: elevation -------------------------------------------------------------------------------- /grid_map_demos/config/simple_demo.yaml: -------------------------------------------------------------------------------- 1 | grid_map_topic: /grid_map_simple_demo/grid_map 2 | grid_map_visualizations: 3 | - name: elevation_points 4 | type: point_cloud 5 | params: 6 | layer: elevation 7 | - name: elevation_grid 8 | type: occupancy_grid 9 | params: 10 | layer: elevation 11 | data_min: 0.08 12 | data_max: -0.16 13 | -------------------------------------------------------------------------------- /grid_map_demos/config/tutorial_demo.yaml: -------------------------------------------------------------------------------- 1 | grid_map_topic: /grid_map_tutorial_demo/grid_map 2 | 3 | grid_map_visualizations: 4 | 5 | - name: elevation_points 6 | type: point_cloud 7 | params: 8 | layer: elevation 9 | 10 | - name: noisy_points 11 | type: point_cloud 12 | params: 13 | layer: elevation_noisy 14 | 15 | - name: filtered_points 16 | type: point_cloud 17 | params: 18 | layer: elevation_filtered 19 | 20 | - name: map_region 21 | type: map_region 22 | params: 23 | color: 3289650 24 | line_width: 0.003 25 | 26 | - name: elevation_grid 27 | type: occupancy_grid 28 | params: 29 | layer: elevation 30 | data_min: 0.08 31 | data_max: -0.16 32 | 33 | - name: error_grid 34 | type: occupancy_grid 35 | params: 36 | layer: error 37 | data_min: -0.15 38 | data_max: 0.15 39 | 40 | - name: surface_normals 41 | type: vectors 42 | params: 43 | layer_prefix: normal_ 44 | position_layer: elevation 45 | scale: 0.06 46 | line_width: 0.005 47 | color: 15600153 # red 48 | 49 | - name: elevation_cells 50 | type: grid_cells 51 | params: 52 | layer: elevation 53 | lower_threshold: -0.08 54 | upper_threshold: 0.0 55 | -------------------------------------------------------------------------------- /grid_map_demos/data/eth_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/data/eth_logo.png -------------------------------------------------------------------------------- /grid_map_demos/data/freiburg1_360.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/data/freiburg1_360.bt -------------------------------------------------------------------------------- /grid_map_demos/data/grid_map.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/data/grid_map.bag -------------------------------------------------------------------------------- /grid_map_demos/data/heightmap_generated.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/data/heightmap_generated.png -------------------------------------------------------------------------------- /grid_map_demos/data/linearGradient_horizontal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/data/linearGradient_horizontal.png -------------------------------------------------------------------------------- /grid_map_demos/data/linearGradient_oblique_and_flat.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/data/linearGradient_oblique_and_flat.png -------------------------------------------------------------------------------- /grid_map_demos/data/linearGradient_oblique_only.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/data/linearGradient_oblique_only.png -------------------------------------------------------------------------------- /grid_map_demos/data/linearGradient_vertical.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/data/linearGradient_vertical.png -------------------------------------------------------------------------------- /grid_map_demos/data/radialGradient_circle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/data/radialGradient_circle.png -------------------------------------------------------------------------------- /grid_map_demos/data/radialGradient_ellipse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/data/radialGradient_ellipse.png -------------------------------------------------------------------------------- /grid_map_demos/data/radialGradient_horizontal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/data/radialGradient_horizontal.png -------------------------------------------------------------------------------- /grid_map_demos/data/terrain.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/data/terrain.png -------------------------------------------------------------------------------- /grid_map_demos/doc/filters_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/doc/filters_demo.gif -------------------------------------------------------------------------------- /grid_map_demos/doc/filters_demo_preview.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/doc/filters_demo_preview.gif -------------------------------------------------------------------------------- /grid_map_demos/doc/image_to_grid_map_demo_result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/doc/image_to_grid_map_demo_result.png -------------------------------------------------------------------------------- /grid_map_demos/doc/opencv_demo_result.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_demos/doc/opencv_demo_result.gif -------------------------------------------------------------------------------- /grid_map_demos/include/grid_map_demos/FiltersDemo.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FiltersDemo.hpp 3 | * 4 | * Created on: Aug 16, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | * 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | 14 | #pragma GCC diagnostic push 15 | #pragma GCC diagnostic ignored "-Wpedantic" 16 | #pragma GCC diagnostic ignored "-Wformat" 17 | #include 18 | #pragma GCC diagnostic pop 19 | #include 20 | #include 21 | 22 | namespace grid_map_demos { 23 | 24 | /*! 25 | * Applies a chain of grid map filters to a topic and 26 | * republishes the resulting grid map. 27 | */ 28 | class FiltersDemo 29 | { 30 | public: 31 | 32 | /*! 33 | * Constructor. 34 | * @param nodeHandle the ROS node handle. 35 | * @param success signalizes if filter is configured ok or not. 36 | */ 37 | FiltersDemo(ros::NodeHandle& nodeHandle, bool& success); 38 | 39 | /*! 40 | * Destructor. 41 | */ 42 | virtual ~FiltersDemo(); 43 | 44 | /*! 45 | * Reads and verifies the ROS parameters. 46 | * @return true if successful. 47 | */ 48 | bool readParameters(); 49 | 50 | /*! 51 | * Callback method for the incoming grid map message. 52 | * @param message the incoming message. 53 | */ 54 | void callback(const grid_map_msgs::GridMap& message); 55 | 56 | private: 57 | 58 | //! ROS nodehandle. 59 | ros::NodeHandle& nodeHandle_; 60 | 61 | //! Name of the input grid map topic. 62 | std::string inputTopic_; 63 | 64 | //! Name of the output grid map topic. 65 | std::string outputTopic_; 66 | 67 | //! Grid map subscriber 68 | ros::Subscriber subscriber_; 69 | 70 | //! Grid map publisher. 71 | ros::Publisher publisher_; 72 | 73 | //! Filter chain. 74 | filters::FilterChain filterChain_; 75 | 76 | //! Filter chain parameters name. 77 | std::string filterChainParametersName_; 78 | }; 79 | 80 | } /* namespace */ -------------------------------------------------------------------------------- /grid_map_demos/include/grid_map_demos/GridmapToImageDemo.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GridMapToImageDemo.hpp 3 | * 4 | * Created on: Oktober 19, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | * 8 | */ 9 | 10 | #pragma once 11 | 12 | // ROS 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | namespace grid_map_demos { 21 | 22 | /*! 23 | * Saves an ElevationMapping layer as image. 24 | */ 25 | class GridMapToImageDemo { 26 | public: 27 | /*! 28 | * Constructor. 29 | * @param nodeHandle the ROS node handle. 30 | */ 31 | GridMapToImageDemo(ros::NodeHandle& nodeHandle); 32 | 33 | /*! 34 | * Destructor. 35 | */ 36 | virtual ~GridMapToImageDemo(); 37 | 38 | private: 39 | /*! 40 | * @brief Reads and verifies the ROS parameters. 41 | */ 42 | void readParameters(); 43 | 44 | /** 45 | * @brief The callback receiving the grid map. 46 | * It will convert the elevation layer into a png image and save it to the specified location. Afterwards the node will terminate. 47 | * @param msg the recieved grid map to save to a file. 48 | */ 49 | void gridMapCallback(const grid_map_msgs::GridMap& msg); 50 | 51 | //! ROS nodehandle. 52 | ros::NodeHandle& nodeHandle_; 53 | 54 | //! GridMap subscriber 55 | ros::Subscriber gridMapSubscriber_; 56 | 57 | //! Name of the grid map topic. 58 | std::string gridMapTopic_; 59 | 60 | //! Path where to store the image. 61 | std::string filePath_; 62 | }; 63 | 64 | } // namespace grid_map_demos 65 | -------------------------------------------------------------------------------- /grid_map_demos/include/grid_map_demos/ImageToGridmapDemo.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ImageToGridmapDemo.hpp 3 | * 4 | * Created on: May 4, 2015 5 | * Author: Martin Wermelinger 6 | * Institute: ETH Zurich, ANYbotics 7 | * 8 | */ 9 | 10 | #pragma once 11 | 12 | // ROS 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include 19 | 20 | namespace grid_map_demos { 21 | 22 | /*! 23 | * Loads an image and saves it as layer 'elevation' of a grid map. 24 | * The grid map is published and can be viewed in Rviz. 25 | */ 26 | class ImageToGridmapDemo 27 | { 28 | public: 29 | 30 | /*! 31 | * Constructor. 32 | * @param nodeHandle the ROS node handle. 33 | */ 34 | ImageToGridmapDemo(ros::NodeHandle& nodeHandle); 35 | 36 | /*! 37 | * Destructor. 38 | */ 39 | virtual ~ImageToGridmapDemo(); 40 | 41 | /*! 42 | * Reads and verifies the ROS parameters. 43 | * @return true if successful. 44 | */ 45 | bool readParameters(); 46 | 47 | void imageCallback(const sensor_msgs::Image& msg); 48 | 49 | private: 50 | 51 | //! ROS nodehandle. 52 | ros::NodeHandle& nodeHandle_; 53 | 54 | //! Grid map publisher. 55 | ros::Publisher gridMapPublisher_; 56 | 57 | //! Grid map data. 58 | grid_map::GridMap map_; 59 | 60 | //! Image subscriber 61 | ros::Subscriber imageSubscriber_; 62 | 63 | //! Name of the grid map topic. 64 | std::string imageTopic_; 65 | 66 | //! Length of the grid map in x direction. 67 | double mapLengthX_; 68 | 69 | //! Resolution of the grid map. 70 | double resolution_; 71 | 72 | //! Range of the height values. 73 | double minHeight_; 74 | double maxHeight_; 75 | 76 | //! Frame id of the grid map. 77 | std::string mapFrameId_; 78 | 79 | bool mapInitialized_; 80 | }; 81 | 82 | } /* namespace */ 83 | -------------------------------------------------------------------------------- /grid_map_demos/include/grid_map_demos/IteratorsDemo.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * IteratorsDemo.hpp 3 | * 4 | * Created on: Nov 4, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | * 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | 14 | // ROS 15 | #include 16 | 17 | namespace grid_map_demos { 18 | 19 | /*! 20 | * Visualizes a grid map by publishing different topics that can be viewed in Rviz. 21 | */ 22 | class IteratorsDemo 23 | { 24 | public: 25 | 26 | /*! 27 | * Constructor. 28 | * @param nodeHandle the ROS node handle. 29 | */ 30 | IteratorsDemo(ros::NodeHandle& nodeHandle); 31 | 32 | /*! 33 | * Destructor. 34 | */ 35 | virtual ~IteratorsDemo(); 36 | 37 | /*! 38 | * Demos. 39 | */ 40 | void demoGridMapIterator(); 41 | void demoSubmapIterator(); 42 | void demoCircleIterator(); 43 | void demoEllipseIterator(); 44 | void demoSpiralIterator(); 45 | void demoLineIterator(); 46 | void demoPolygonIterator(const bool prepareForOtherDemos = false); 47 | void demoSlidingWindowIterator(); 48 | 49 | /*! 50 | * Publish the grid map to ROS. 51 | */ 52 | void publish(); 53 | 54 | private: 55 | 56 | //! ROS nodehandle. 57 | ros::NodeHandle& nodeHandle_; 58 | 59 | //! Grid map publisher. 60 | ros::Publisher gridMapPublisher_; 61 | 62 | //! Polygon publisher. 63 | ros::Publisher polygonPublisher_; 64 | 65 | //! Grid map data. 66 | grid_map::GridMap map_; 67 | }; 68 | 69 | } /* namespace */ 70 | -------------------------------------------------------------------------------- /grid_map_demos/include/grid_map_demos/OctomapToGridmapDemo.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OctomapToGridmapDemo.hpp 3 | * 4 | * Created on: May 03, 2017 5 | * Author: Jeff Delmerico 6 | * Institute: University of Zürich, Robotics and Perception Group 7 | */ 8 | 9 | #pragma once 10 | 11 | // ROS 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | namespace grid_map_demos { 19 | 20 | /*! 21 | * Receives a volumetric OctoMap and converts it to a grid map with an elevation layer. 22 | * The grid map is published and can be viewed in Rviz. 23 | */ 24 | class OctomapToGridmapDemo 25 | { 26 | public: 27 | 28 | /*! 29 | * Constructor. 30 | * @param nodeHandle the ROS node handle. 31 | */ 32 | OctomapToGridmapDemo(ros::NodeHandle& nodeHandle); 33 | 34 | /*! 35 | * Destructor. 36 | */ 37 | virtual ~OctomapToGridmapDemo(); 38 | 39 | /*! 40 | * Reads and verifies the ROS parameters. 41 | * @return true if successful. 42 | */ 43 | bool readParameters(); 44 | 45 | void convertAndPublishMap(); 46 | 47 | private: 48 | 49 | //! ROS nodehandle. 50 | ros::NodeHandle& nodeHandle_; 51 | 52 | //! Grid map publisher. 53 | ros::Publisher gridMapPublisher_; 54 | 55 | //! Octomap publisher. 56 | ros::Publisher octomapPublisher_; 57 | 58 | //! Grid map data. 59 | grid_map::GridMap map_; 60 | 61 | //! Name of the grid map topic. 62 | std::string octomapServiceTopic_; 63 | 64 | //! Octomap service client 65 | ros::ServiceClient client_; 66 | 67 | //! Bounding box of octomap to convert. 68 | float minX_; 69 | float maxX_; 70 | float minY_; 71 | float maxY_; 72 | float minZ_; 73 | float maxZ_; 74 | }; 75 | 76 | } /* namespace */ 77 | -------------------------------------------------------------------------------- /grid_map_demos/include/grid_map_demos/SdfDemo.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SdfDemo.hpp 3 | * 4 | * Created on: May 3, 2022 5 | * Author: Ruben Grandia 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | // ROS 14 | #include 15 | 16 | // gridmap 17 | #include 18 | #include 19 | 20 | namespace grid_map_demos { 21 | 22 | /** 23 | * Subscribes to a gridmap, converts it to a signed distance field, and publishes the signed distance field as a pointcloud. 24 | */ 25 | class SdfDemo { 26 | public: 27 | 28 | /** 29 | * Constructor. 30 | * @param nodeHandle : the ROS node handle. 31 | * @param mapTopic : grid map topic to subscribe to 32 | * @param elevationLayer : name of the elevation layer 33 | * @param pointcloudTopic : point cloud topic to publish the result to 34 | */ 35 | SdfDemo(ros::NodeHandle& nodeHandle, const std::string& mapTopic, std::string elevationLayer, const std::string& pointcloudTopic); 36 | 37 | private: 38 | void callback(const grid_map_msgs::GridMap& message); 39 | 40 | //! Grid map subscriber. 41 | ros::Subscriber gridmapSubscriber_; 42 | 43 | //! Pointcloud publisher. 44 | ros::Publisher pointcloudPublisher_; 45 | 46 | //! Free space publisher. 47 | ros::Publisher freespacePublisher_; 48 | 49 | //! Occupied space publisher. 50 | ros::Publisher occupiedPublisher_; 51 | 52 | //! Elevation layer in the received grid map 53 | std::string elevationLayer_; 54 | }; 55 | 56 | } // namespace grid_map_demos -------------------------------------------------------------------------------- /grid_map_demos/launch/filters_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /grid_map_demos/launch/grid_map_loader_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /grid_map_demos/launch/image_to_gridmap_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /grid_map_demos/launch/interpolation_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 9 | 10 | 11 | 12 | 14 | 15 | -------------------------------------------------------------------------------- /grid_map_demos/launch/iterators_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /grid_map_demos/launch/move_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /grid_map_demos/launch/normal_filter_comparison.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /grid_map_demos/launch/octomap_to_gridmap_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /grid_map_demos/launch/opencv_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /grid_map_demos/launch/resolution_change_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /grid_map_demos/launch/sdf_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /grid_map_demos/launch/simple_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /grid_map_demos/launch/tutorial_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /grid_map_demos/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_demos 4 | 1.7.18 5 | Demo nodes to demonstrate the usage of the grid map library. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/grid_map 10 | http://github.com/anybotics/grid_map/issues 11 | Péter Fankhauser 12 | catkin 13 | 14 | roscpp 15 | grid_map_core 16 | grid_map_cv 17 | grid_map_filters 18 | grid_map_loader 19 | grid_map_msgs 20 | grid_map_octomap 21 | grid_map_ros 22 | grid_map_rviz_plugin 23 | grid_map_sdf 24 | grid_map_visualization 25 | geometry_msgs 26 | sensor_msgs 27 | cv_bridge 28 | octomap_msgs 29 | 30 | gtest 31 | 32 | -------------------------------------------------------------------------------- /grid_map_demos/scripts/image_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # simple script to publish a image from a file. 3 | import rospy 4 | import rospkg 5 | import time 6 | import cv2 7 | import sensor_msgs.msg 8 | 9 | def callback(self): 10 | """ Convert a image to a ROS compatible message 11 | (sensor_msgs.Image). 12 | """ 13 | global publisher, imagePath 14 | 15 | img = cv2.imread(imagePath, cv2.IMREAD_UNCHANGED) 16 | 17 | # print img.shape 18 | # print img.size 19 | # print img.dtype.itemsize 20 | 21 | rosimage = sensor_msgs.msg.Image() 22 | if img.dtype.itemsize == 2: 23 | if len(img.shape) == 3: 24 | if img.shape[2] == 3: 25 | rosimage.encoding = 'bgr16' 26 | if img.shape[2] == 4: 27 | rosimage.encoding = 'bgra16' 28 | else: 29 | rosimage.encoding = 'mono16' 30 | if img.dtype.itemsize == 1: 31 | if len(img.shape) == 3: 32 | if img.shape[2] == 3: 33 | rosimage.encoding = 'bgr8' 34 | if img.shape[2] == 4: 35 | rosimage.encoding = 'bgra8' 36 | else: 37 | rosimage.encoding = 'mono8' 38 | # print "Encoding: ", rosimage.encoding 39 | 40 | rosimage.width = img.shape[1] 41 | rosimage.height = img.shape[0] 42 | rosimage.step = img.strides[0] 43 | rosimage.data = img.tostring() 44 | rosimage.header.stamp = rospy.Time.now() 45 | rosimage.header.frame_id = 'map' 46 | 47 | publisher.publish(rosimage) 48 | 49 | 50 | #Main function initializes node and subscribers and starts the ROS loop 51 | def main_program(): 52 | global publisher, imagePath 53 | rospack = rospkg.RosPack() 54 | rospy.init_node('image_publisher') 55 | imagePath = rospy.get_param('~image_path') 56 | topicName = rospy.get_param('~topic') 57 | publisher = rospy.Publisher(topicName, sensor_msgs.msg.Image, queue_size=10) 58 | rospy.Timer(rospy.Duration(2), callback) 59 | rospy.spin() 60 | 61 | if __name__ == '__main__': 62 | try: 63 | main_program() 64 | except rospy.ROSInterruptException: pass 65 | -------------------------------------------------------------------------------- /grid_map_demos/src/FiltersDemo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FiltersDemo.cpp 3 | * 4 | * Created on: Aug 16, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | * 8 | */ 9 | 10 | #include "grid_map_demos/FiltersDemo.hpp" 11 | 12 | using namespace grid_map; 13 | 14 | namespace grid_map_demos { 15 | 16 | FiltersDemo::FiltersDemo(ros::NodeHandle& nodeHandle, bool& success) 17 | : nodeHandle_(nodeHandle), 18 | filterChain_("grid_map::GridMap") 19 | { 20 | if (!readParameters()) { 21 | success = false; 22 | return; 23 | } 24 | 25 | subscriber_ = nodeHandle_.subscribe(inputTopic_, 1, &FiltersDemo::callback, this); 26 | publisher_ = nodeHandle_.advertise(outputTopic_, 1, true); 27 | 28 | // Setup filter chain. 29 | if (!filterChain_.configure(filterChainParametersName_, nodeHandle)) { 30 | ROS_ERROR("Could not configure the filter chain!"); 31 | success = false; 32 | return; 33 | } 34 | 35 | success = true; 36 | } 37 | 38 | FiltersDemo::~FiltersDemo() 39 | { 40 | } 41 | 42 | bool FiltersDemo::readParameters() 43 | { 44 | if (!nodeHandle_.getParam("input_topic", inputTopic_)) { 45 | ROS_ERROR("Could not read parameter `input_topic`."); 46 | return false; 47 | } 48 | nodeHandle_.param("output_topic", outputTopic_, std::string("output")); 49 | nodeHandle_.param("filter_chain_parameter_name", filterChainParametersName_, std::string("grid_map_filters")); 50 | return true; 51 | } 52 | 53 | void FiltersDemo::callback(const grid_map_msgs::GridMap& message) 54 | { 55 | // Convert message to map. 56 | GridMap inputMap; 57 | GridMapRosConverter::fromMessage(message, inputMap); 58 | 59 | // Apply filter chain. 60 | grid_map::GridMap outputMap; 61 | if (!filterChain_.update(inputMap, outputMap)) { 62 | ROS_ERROR("Could not update the grid map filter chain!"); 63 | return; 64 | } 65 | 66 | ROS_INFO("PUBLISH"); 67 | // Publish filtered output grid map. 68 | grid_map_msgs::GridMap outputMessage; 69 | GridMapRosConverter::toMessage(outputMap, outputMessage); 70 | publisher_.publish(outputMessage); 71 | } 72 | 73 | } /* namespace */ 74 | -------------------------------------------------------------------------------- /grid_map_demos/src/GridmapToImageDemo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GridMapToImageDemo.cpp 3 | * 4 | * Created on: October 19, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_demos/GridmapToImageDemo.hpp" 10 | 11 | #include 12 | #include 13 | 14 | namespace grid_map_demos { 15 | 16 | GridMapToImageDemo::GridMapToImageDemo(ros::NodeHandle& nodeHandle) 17 | : nodeHandle_(nodeHandle) 18 | { 19 | readParameters(); 20 | gridMapSubscriber_ = nodeHandle_.subscribe(gridMapTopic_, 1, &GridMapToImageDemo::gridMapCallback, this); 21 | ROS_ERROR("Subscribed to %s", nodeHandle_.resolveName(gridMapTopic_).c_str()); 22 | } 23 | 24 | GridMapToImageDemo::~GridMapToImageDemo()=default; 25 | 26 | void GridMapToImageDemo::readParameters() 27 | { 28 | nodeHandle_.param("grid_map_topic", gridMapTopic_, std::string("/grid_map")); 29 | nodeHandle_.param("file", filePath_, std::string("~/.ros/grid_map.png")); 30 | } 31 | 32 | void GridMapToImageDemo::gridMapCallback(const grid_map_msgs::GridMap& msg) 33 | { 34 | ROS_INFO("Saving map received from: %s to file %s.", nodeHandle_.resolveName(gridMapTopic_).c_str(), filePath_.c_str()); 35 | grid_map::GridMap map; 36 | cv_bridge::CvImage image; 37 | grid_map::GridMapRosConverter::fromMessage(msg, map, {"elevation"}); 38 | grid_map::GridMapRosConverter::toCvImage(map,"elevation", sensor_msgs::image_encodings::MONO8, image); 39 | bool success = cv::imwrite(filePath_.c_str(),image.image, {cv::IMWRITE_PNG_STRATEGY_DEFAULT}); 40 | ROS_INFO("Success writing image: %s", success?"true":"false"); 41 | ros::shutdown(); 42 | } 43 | 44 | } /* namespace */ 45 | -------------------------------------------------------------------------------- /grid_map_demos/src/ImageToGridmapDemo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ImageToGridmapDemo.cpp 3 | * 4 | * Created on: May 4, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_demos/ImageToGridmapDemo.hpp" 10 | 11 | namespace grid_map_demos { 12 | 13 | ImageToGridmapDemo::ImageToGridmapDemo(ros::NodeHandle& nodeHandle) 14 | : nodeHandle_(nodeHandle), 15 | map_(grid_map::GridMap({"elevation"})), 16 | mapInitialized_(false) 17 | { 18 | readParameters(); 19 | map_.setBasicLayers({"elevation"}); 20 | imageSubscriber_ = nodeHandle_.subscribe(imageTopic_, 1, &ImageToGridmapDemo::imageCallback, this); 21 | gridMapPublisher_ = nodeHandle_.advertise("grid_map", 1, true); 22 | } 23 | 24 | ImageToGridmapDemo::~ImageToGridmapDemo() 25 | { 26 | } 27 | 28 | bool ImageToGridmapDemo::readParameters() 29 | { 30 | nodeHandle_.param("image_topic", imageTopic_, std::string("/image")); 31 | nodeHandle_.param("resolution", resolution_, 0.03); 32 | nodeHandle_.param("min_height", minHeight_, 0.0); 33 | nodeHandle_.param("max_height", maxHeight_, 1.0); 34 | return true; 35 | } 36 | 37 | void ImageToGridmapDemo::imageCallback(const sensor_msgs::Image& msg) 38 | { 39 | if (!mapInitialized_) { 40 | grid_map::GridMapRosConverter::initializeFromImage(msg, resolution_, map_); 41 | ROS_INFO("Initialized map with size %f x %f m (%i x %i cells).", map_.getLength().x(), 42 | map_.getLength().y(), map_.getSize()(0), map_.getSize()(1)); 43 | mapInitialized_ = true; 44 | } 45 | grid_map::GridMapRosConverter::addLayerFromImage(msg, "elevation", map_, minHeight_, maxHeight_); 46 | grid_map::GridMapRosConverter::addColorLayerFromImage(msg, "color", map_); 47 | 48 | // Publish as grid map. 49 | grid_map_msgs::GridMap mapMessage; 50 | grid_map::GridMapRosConverter::toMessage(map_, mapMessage); 51 | gridMapPublisher_.publish(mapMessage); 52 | } 53 | 54 | } /* namespace */ 55 | -------------------------------------------------------------------------------- /grid_map_demos/src/grid_map_to_image_demo_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * grid_map_to_image_demo.cpp 3 | * 4 | * Created on: October 19, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_demos/GridmapToImageDemo.hpp" 10 | 11 | #include 12 | 13 | /* 14 | * Usage: 15 | * $ rosrun grid_map_demos grid_map_to_image_demo _grid_map_topic:=/grid_map _file:=/home/$USER/Desktop/grid_map_image.png 16 | */ 17 | int main(int argc, char** argv) 18 | { 19 | // Initialize node and publisher. 20 | ros::init(argc, argv, "grid_map_to_image_demo"); 21 | ros::NodeHandle nh("~"); 22 | grid_map_demos::GridMapToImageDemo gridMapToImageDemo(nh); 23 | 24 | ros::spin(); 25 | return EXIT_SUCCESS; 26 | } 27 | -------------------------------------------------------------------------------- /grid_map_demos/src/image_to_gridmap_demo_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * image_to_gridmap_demo_node.cpp 3 | * 4 | * Created on: May 04, 2015 5 | * Author: Martin Wermelinger 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include 10 | #include "grid_map_demos/ImageToGridmapDemo.hpp" 11 | 12 | int main(int argc, char** argv) 13 | { 14 | // Initialize node and publisher. 15 | ros::init(argc, argv, "image_to_gridmap_demo"); 16 | ros::NodeHandle nh("~"); 17 | grid_map_demos::ImageToGridmapDemo imageToGridmapDemo(nh); 18 | 19 | ros::spin(); 20 | return 0; 21 | } 22 | -------------------------------------------------------------------------------- /grid_map_demos/src/interpolation_demo_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * interpolation_demo_node.cpp 3 | * 4 | * Created on: Mar 16, 2020 5 | * Author: jelavice 6 | */ 7 | 8 | #include "grid_map_demos/InterpolationDemo.hpp" 9 | #include 10 | 11 | int main(int argc, char** argv) 12 | { 13 | ros::init(argc, argv, "grid_map_interpolation_demo"); 14 | ros::NodeHandle nodeHandle("~"); 15 | grid_map_demos::InterpolationDemo interpolationDemo(&nodeHandle); 16 | ros::spin(); 17 | return 0; 18 | } 19 | -------------------------------------------------------------------------------- /grid_map_demos/src/iterators_demo_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * grid_map_iterators_demo_node.cpp 3 | * 4 | * Created on: Nov 4, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include 10 | #include "grid_map_demos/IteratorsDemo.hpp" 11 | 12 | int main(int argc, char** argv) 13 | { 14 | ros::init(argc, argv, "grid_map_iterators_demo"); 15 | 16 | ros::NodeHandle nodeHandle("~"); 17 | grid_map_demos::IteratorsDemo iteratorsDemo(nodeHandle); 18 | 19 | ros::requestShutdown(); 20 | return 0; 21 | } 22 | -------------------------------------------------------------------------------- /grid_map_demos/src/octomap_to_gridmap_demo_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * octomap_to_gridmap_demo_node.cpp 3 | * 4 | * Created on: May 03, 2017 5 | * Author: Jeff Delmerico 6 | * Institute: University of Zürich, Robotics and Perception Group 7 | */ 8 | 9 | #include 10 | #include "grid_map_demos/OctomapToGridmapDemo.hpp" 11 | 12 | int main(int argc, char** argv) 13 | { 14 | // Initialize node and publisher. 15 | ros::init(argc, argv, "octomap_to_gridmap_demo"); 16 | ros::NodeHandle nh("~"); 17 | grid_map_demos::OctomapToGridmapDemo octomapToGridmapDemo(nh); 18 | ros::Duration(2.0).sleep(); 19 | 20 | ros::Rate r(0.1); // 1 hz 21 | while (ros::ok()) 22 | { 23 | octomapToGridmapDemo.convertAndPublishMap(); 24 | ros::spinOnce(); 25 | r.sleep(); 26 | } 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /grid_map_demos/src/sdf_demo_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * sdf_demo_node.cpp 3 | * 4 | * Created on: May 3, 2022 5 | * Author: Ruben Grandia 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include "grid_map_demos/SdfDemo.hpp" 14 | 15 | int main(int argc, char** argv) 16 | { 17 | ros::init(argc, argv, "grid_map_sdf_demo"); 18 | ros::NodeHandle nodeHandle(""); 19 | 20 | std::string elevationLayer; 21 | nodeHandle.getParam("elevation_layer", elevationLayer); 22 | 23 | std::string mapTopic; 24 | nodeHandle.getParam("grid_map_topic", mapTopic); 25 | 26 | std::string pointcloudTopic; 27 | nodeHandle.getParam("pointcloud_topic", pointcloudTopic); 28 | 29 | grid_map_demos::SdfDemo sdfDemo(nodeHandle, mapTopic, elevationLayer, pointcloudTopic); 30 | 31 | ros::spin(); 32 | return 0; 33 | } -------------------------------------------------------------------------------- /grid_map_demos/src/simple_demo_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace grid_map; 7 | 8 | int main(int argc, char** argv) 9 | { 10 | // Initialize node and publisher. 11 | ros::init(argc, argv, "grid_map_simple_demo"); 12 | ros::NodeHandle nh("~"); 13 | ros::Publisher publisher = nh.advertise("grid_map", 1, true); 14 | 15 | // Create grid map. 16 | GridMap map({"elevation"}); 17 | map.setFrameId("map"); 18 | map.setGeometry(Length(1.2, 2.0), 0.03); 19 | ROS_INFO("Created map with size %f x %f m (%i x %i cells).", 20 | map.getLength().x(), map.getLength().y(), 21 | map.getSize()(0), map.getSize()(1)); 22 | 23 | // Work with grid map in a loop. 24 | ros::Rate rate(30.0); 25 | while (nh.ok()) { 26 | 27 | // Add data to grid map. 28 | ros::Time time = ros::Time::now(); 29 | for (GridMapIterator it(map); !it.isPastEnd(); ++it) { 30 | Position position; 31 | map.getPosition(*it, position); 32 | map.at("elevation", *it) = -0.04 + 0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()) * position.x(); 33 | } 34 | 35 | // Publish grid map. 36 | map.setTimestamp(time.toNSec()); 37 | grid_map_msgs::GridMap message; 38 | GridMapRosConverter::toMessage(map, message); 39 | publisher.publish(message); 40 | ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", message.info.header.stamp.toSec()); 41 | 42 | // Wait for next cycle. 43 | rate.sleep(); 44 | } 45 | 46 | return 0; 47 | } 48 | -------------------------------------------------------------------------------- /grid_map_demos/test/empty_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | TEST(Placeholder, emptyTest) { // NOLINT 4 | ASSERT_TRUE(true); 5 | } 6 | -------------------------------------------------------------------------------- /grid_map_filters/include/EigenLab/LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 Dr. Marcel Paz Goldschen-Ohm 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/BufferNormalizerFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BufferNormalizerFilter.hpp 3 | * 4 | * Created on: Aug 18, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace grid_map { 17 | 18 | /*! 19 | * Normalizes the buffer of a map such that it has default (zero) start index. 20 | */ 21 | class BufferNormalizerFilter : public filters::FilterBase { 22 | public: 23 | /*! 24 | * Constructor 25 | */ 26 | BufferNormalizerFilter(); 27 | 28 | /*! 29 | * Destructor. 30 | */ 31 | ~BufferNormalizerFilter() override; 32 | 33 | /*! 34 | * Configures the filter from parameters on the parameter server. 35 | */ 36 | bool configure() override; 37 | 38 | /*! 39 | * Normalizes the buffer of a map. 40 | * @param mapIn the input map before normalization. 41 | * @param mapOut the normalized map. 42 | */ 43 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 44 | }; 45 | 46 | } // namespace grid_map 47 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/ColorBlendingFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ColorBlendingFilter.hpp 3 | * 4 | * Created on: Sep 14, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace grid_map { 18 | 19 | /*! 20 | * Blend two color layers. 21 | */ 22 | class ColorBlendingFilter : public filters::FilterBase { 23 | public: 24 | /*! 25 | * Constructor 26 | */ 27 | ColorBlendingFilter(); 28 | 29 | /*! 30 | * Destructor. 31 | */ 32 | ~ColorBlendingFilter() override; 33 | 34 | /*! 35 | * Configures the filter. 36 | */ 37 | bool configure() override; 38 | 39 | /*! 40 | * Compute a new color layer based on blending two color layers. 41 | * @param mapIn grid map containing the two color layers. 42 | * @param mapOut grid map containing mapIn and the blended color layer. 43 | */ 44 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 45 | 46 | private: 47 | enum class BlendModes { Normal, HardLight, SoftLight }; 48 | 49 | //! Input layers. 50 | std::string backgroundLayer_, foregroundLayer_; 51 | 52 | //! Opacity of foreground layer. 53 | double opacity_; 54 | 55 | //! Blend mode. 56 | BlendModes blendMode_; 57 | 58 | //! Output layer name. 59 | std::string outputLayer_; 60 | }; 61 | 62 | } // namespace grid_map 63 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/ColorFillFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ColorFillFilter.hpp 3 | * 4 | * Created on: Sep 14, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace grid_map { 17 | 18 | /*! 19 | * Creates a new color layer. 20 | */ 21 | class ColorFillFilter : public filters::FilterBase { 22 | public: 23 | /*! 24 | * Constructor 25 | */ 26 | ColorFillFilter(); 27 | 28 | /*! 29 | * Destructor. 30 | */ 31 | ~ColorFillFilter() override; 32 | 33 | /*! 34 | * Configures the filter. 35 | */ 36 | bool configure() override; 37 | 38 | /*! 39 | * Adds a new color layer. 40 | * @param mapIn grid map to add the new layer. 41 | * @param mapOut grid map the grid map with the new color layer. 42 | */ 43 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 44 | 45 | private: 46 | //! Color. 47 | double r_, g_, b_; 48 | 49 | //! Mask layer name. 50 | std::string maskLayer_; 51 | 52 | //! Output layer name. 53 | std::string outputLayer_; 54 | }; 55 | 56 | } // namespace grid_map 57 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/ColorMapFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ColorMapFilter.hpp 3 | * 4 | * Created on: Sep 23, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace grid_map { 19 | 20 | /*! 21 | * Creates a new color layer with the color mapped between min. and max. value. 22 | */ 23 | class ColorMapFilter : public filters::FilterBase { 24 | public: 25 | /*! 26 | * Constructor 27 | */ 28 | ColorMapFilter(); 29 | 30 | /*! 31 | * Destructor. 32 | */ 33 | ~ColorMapFilter() override; 34 | 35 | /*! 36 | * Configures the filter. 37 | */ 38 | bool configure() override; 39 | 40 | /*! 41 | * Adds a new color layer. 42 | * @param mapIn grid map to add the new layer. 43 | * @param mapOut grid map the grid map with the new color layer. 44 | */ 45 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 46 | 47 | private: 48 | //! Min./max. colors. 49 | Eigen::Vector3f minColor_, maxColor_; 50 | 51 | //! Min./max. values. 52 | double min_, max_; 53 | 54 | //! Input layer name. 55 | std::string inputLayer_; 56 | 57 | //! Output layer name. 58 | std::string outputLayer_; 59 | }; 60 | 61 | } // namespace grid_map 62 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/CurvatureFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * CurvatureFilter.hpp 3 | * 4 | * Created on: Sep 23, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace grid_map { 18 | 19 | /*! 20 | * Compute the curvature (second derivative) of a layer in the map. 21 | */ 22 | class CurvatureFilter : public filters::FilterBase { 23 | public: 24 | /*! 25 | * Constructor 26 | */ 27 | CurvatureFilter(); 28 | 29 | /*! 30 | * Destructor. 31 | */ 32 | ~CurvatureFilter() override; 33 | 34 | /*! 35 | * Configures the filter from parameters on the Parameter Server 36 | */ 37 | bool configure() override; 38 | 39 | /*! 40 | * Compute the curvature of a layer in a map and 41 | * saves it as additional grid map layer. 42 | * @param mapIn grid map containing the layer for which the curvature is computed for. 43 | * @param mapOut grid map containing mapIn and the new layer for the curvature. 44 | */ 45 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 46 | 47 | private: 48 | //! Input layer name. 49 | std::string inputLayer_; 50 | 51 | //! Output layer name. 52 | std::string outputLayer_; 53 | }; 54 | 55 | } // namespace grid_map 56 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/DeletionFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * DeletionFilter.hpp 3 | * 4 | * Created on: Mar 19, 2015 5 | * Author: Martin Wermelinger, Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace grid_map { 18 | 19 | /*! 20 | * Deletion filter class deletes layers of a grid map. 21 | */ 22 | class DeletionFilter : public filters::FilterBase { 23 | public: 24 | /*! 25 | * Constructor 26 | */ 27 | DeletionFilter(); 28 | 29 | /*! 30 | * Destructor. 31 | */ 32 | ~DeletionFilter() override; 33 | 34 | /*! 35 | * Configures the filter from parameters on the parameter server. 36 | */ 37 | bool configure() override; 38 | 39 | /*! 40 | * Deletes the specified layers of a grid map. 41 | * @param mapIn gridMap with the different layers. 42 | * @param mapOut gridMap without the deleted layers. 43 | */ 44 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 45 | 46 | private: 47 | //! List of layers that should be deleted. 48 | std::vector layers_; 49 | }; 50 | 51 | } // namespace grid_map 52 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/DuplicationFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * DuplicationFilter.hpp 3 | * 4 | * Created on: Aug 18, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace grid_map { 17 | 18 | /*! 19 | * Duplication filter class duplicates a layer of a grid map. 20 | */ 21 | class DuplicationFilter : public filters::FilterBase { 22 | public: 23 | /*! 24 | * Constructor 25 | */ 26 | DuplicationFilter(); 27 | 28 | /*! 29 | * Destructor. 30 | */ 31 | ~DuplicationFilter() override; 32 | 33 | /*! 34 | * Configures the filter from parameters on the parameter server. 35 | */ 36 | bool configure() override; 37 | 38 | /*! 39 | * Duplicates the specified layers of a grid map. 40 | * @param mapIn with the layer to duplicate. 41 | * @param mapOut with the layer duplicated. 42 | */ 43 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 44 | 45 | private: 46 | //! Name of the layer that is duplicated. 47 | std::string inputLayer_; 48 | 49 | //! Name of the new layer. 50 | std::string outputLayer_; 51 | }; 52 | 53 | } // namespace grid_map 54 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/LightIntensityFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LightIntensityFilter.hpp 3 | * 4 | * Created on: Sep 23, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace grid_map { 18 | 19 | /*! 20 | * Compute the diffuse lighting of a surface as new black and white color layer. 21 | */ 22 | class LightIntensityFilter : public filters::FilterBase { 23 | public: 24 | /*! 25 | * Constructor 26 | */ 27 | LightIntensityFilter(); 28 | 29 | /*! 30 | * Destructor. 31 | */ 32 | ~LightIntensityFilter() override; 33 | 34 | /*! 35 | * Configures the filter from parameters on the Parameter Server 36 | */ 37 | bool configure() override; 38 | 39 | /*! 40 | * Compute the diffuse lighting layer. 41 | * @param mapIn grid map containing the layers of the normal vectors. 42 | * @param mapOut grid map containing mapIn and the black and white lighting color layer. 43 | */ 44 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 45 | 46 | private: 47 | //! Input layers prefix. 48 | std::string inputLayersPrefix_; 49 | 50 | //! Output layer name. 51 | std::string outputLayer_; 52 | 53 | //! Light direction. 54 | Eigen::Vector3f lightDirection_; 55 | }; 56 | 57 | } // namespace grid_map 58 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/MathExpressionFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MathExpressionFilter.hpp 3 | * 4 | * Created on: Aug 16, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include "EigenLab/EigenLab.h" 13 | 14 | #include 15 | #include 16 | 17 | namespace grid_map { 18 | 19 | /*! 20 | * Parses and evaluates a mathematical matrix expression with layers of a grid map. 21 | */ 22 | class MathExpressionFilter : public filters::FilterBase { 23 | public: 24 | /*! 25 | * Constructor 26 | */ 27 | MathExpressionFilter(); 28 | 29 | /*! 30 | * Destructor. 31 | */ 32 | ~MathExpressionFilter() override; 33 | 34 | /*! 35 | * Configures the filter from parameters on the parameter server. 36 | */ 37 | bool configure() override; 38 | 39 | /*! 40 | * Takes the minimum out of different layers of a grid map. 41 | * @param mapIn gridMap with the different layers to take the min. 42 | * @param mapOut gridMap with an additional layer containing the sum. 43 | */ 44 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 45 | 46 | private: 47 | //! EigenLab parser. 48 | EigenLab::Parser parser_; 49 | 50 | //! Expression to parse. 51 | std::string expression_; 52 | 53 | //! Output layer name. 54 | std::string outputLayer_; 55 | }; 56 | 57 | } // namespace grid_map 58 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/MeanInRadiusFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MeanInRadiusFilter.hpp 3 | * 4 | * Created on: May 3, 2017 5 | * Author: Tanja Baumann, Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace grid_map { 17 | 18 | /*! 19 | * Filter class to find the mean of the values inside a radius. 20 | */ 21 | class MeanInRadiusFilter : public filters::FilterBase { 22 | public: 23 | /*! 24 | * Constructor 25 | */ 26 | MeanInRadiusFilter(); 27 | 28 | /*! 29 | * Destructor. 30 | */ 31 | ~MeanInRadiusFilter() override; 32 | 33 | /*! 34 | * Configures the filter from parameters on the Parameter Server 35 | */ 36 | bool configure() override; 37 | 38 | /*! 39 | * Computes for each value in the input layer the mean of all values in a radius around it 40 | * Saves this mean in an additional output layer. 41 | * @param mapIn grid map containing the input layer. 42 | * @param mapOut grid map containing the layers of the input map and the new layer. 43 | */ 44 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 45 | 46 | private: 47 | //! Radius to take the mean from. 48 | double radius_; 49 | 50 | //! Input layer name. 51 | std::string inputLayer_; 52 | 53 | //! Output layer name. 54 | std::string outputLayer_; 55 | }; 56 | 57 | } // namespace grid_map 58 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/MinInRadiusFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MinInRadiusFilter.hpp 3 | * 4 | * Created on: May 3, 2017 5 | * Author: Tanja Baumann, Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace grid_map { 17 | 18 | /*! 19 | * Filter class to compute the minimal value inside a radius. 20 | */ 21 | class MinInRadiusFilter : public filters::FilterBase { 22 | public: 23 | /*! 24 | * Constructor. 25 | */ 26 | MinInRadiusFilter(); 27 | 28 | /*! 29 | * Destructor. 30 | */ 31 | ~MinInRadiusFilter() override; 32 | 33 | /*! 34 | * Configures the filter from parameters on the Parameter Server 35 | */ 36 | bool configure() override; 37 | 38 | /*! 39 | * Computes for each value in the input layer the minimum of all values in a radius around it. 40 | * Saves this minimal value in an additional output layer. 41 | * @param mapIn grid map containing the input layer. 42 | * @param mapOut grid map containing the original layers and the new layer with the minimal values. 43 | */ 44 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 45 | 46 | private: 47 | //! Radius to take the minimum in. 48 | double radius_; 49 | 50 | //! Input layer name. 51 | std::string inputLayer_; 52 | 53 | //! Output layer name. 54 | std::string outputLayer_; 55 | }; 56 | 57 | } // namespace grid_map 58 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/MockFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MockFilter.hpp 3 | * 4 | * Created on: Sep 24, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace grid_map { 17 | 18 | /*! 19 | * Duplication filter class duplicates a layer of a grid map. 20 | */ 21 | class MockFilter : public filters::FilterBase { 22 | public: 23 | /*! 24 | * Constructor 25 | */ 26 | MockFilter(); 27 | 28 | /*! 29 | * Destructor. 30 | */ 31 | ~MockFilter() override; 32 | 33 | /*! 34 | * Configures the filter from parameters on the parameter server. 35 | */ 36 | bool configure() override; 37 | 38 | /*! 39 | * Copies the input to the output. The time for the update is specified by processingTime_. Optionally the update is logged. 40 | * @param mapIn Input. 41 | * @param mapOut Output. 42 | */ 43 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 44 | 45 | private: 46 | //! Flag indicating wheter to also log on update. 47 | bool printName_{false}; 48 | 49 | //! The time [ms] that the update function takes. 50 | uint processingTime_{0}; 51 | }; 52 | 53 | } // namespace grid_map 54 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/NormalColorMapFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * NormalColorMapFilter.hpp 3 | * 4 | * Created on: Aug 22, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace grid_map { 18 | 19 | /*! 20 | * Compute a new color layer based on normal vectors layers. 21 | */ 22 | class NormalColorMapFilter : public filters::FilterBase { 23 | public: 24 | /*! 25 | * Constructor 26 | */ 27 | NormalColorMapFilter(); 28 | 29 | /*! 30 | * Destructor. 31 | */ 32 | ~NormalColorMapFilter() override; 33 | 34 | /*! 35 | * Configures the filter from parameters on the Parameter Server 36 | */ 37 | bool configure() override; 38 | 39 | /*! 40 | * Compute a new color layer based on normal vectors layers. 41 | * @param mapIn grid map containing the layers of the normal vectors. 42 | * @param mapOut grid map containing mapIn and the new color layer. 43 | */ 44 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 45 | 46 | private: 47 | //! Input layers prefix. 48 | std::string inputLayersPrefix_; 49 | 50 | //! Output layer name. 51 | std::string outputLayer_; 52 | }; 53 | 54 | } // namespace grid_map 55 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/SetBasicLayersFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SetBasicLayersFilters.hpp 3 | * 4 | * Created on: Aug 16, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace grid_map { 18 | 19 | /*! 20 | * Set specified layers of a grid map as basic layers. 21 | */ 22 | class SetBasicLayersFilter : public filters::FilterBase { 23 | public: 24 | /*! 25 | * Constructor 26 | */ 27 | SetBasicLayersFilter(); 28 | 29 | /*! 30 | * Destructor. 31 | */ 32 | ~SetBasicLayersFilter() override; 33 | 34 | /*! 35 | * Configures the filter from parameters on the parameter server. 36 | */ 37 | bool configure() override; 38 | 39 | /*! 40 | * Set the specified layers as basic layers. 41 | * @param mapIn input grid map. 42 | * @param mapOut output grid map with basic layers set. 43 | */ 44 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 45 | 46 | private: 47 | //! List of layers that should be set as basic layers. 48 | std::vector layers_; 49 | }; 50 | 51 | } // namespace grid_map 52 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/SlidingWindowMathExpressionFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SlidingWindowMathExpressionFilter.hpp 3 | * 4 | * Created on: Aug 18, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include "EigenLab/EigenLab.h" 18 | 19 | namespace grid_map { 20 | 21 | /*! 22 | * Parse and evaluate a mathematical matrix expression within a sliding window on a layer of a grid map. 23 | */ 24 | class SlidingWindowMathExpressionFilter : public filters::FilterBase { 25 | public: 26 | /*! 27 | * Constructor 28 | */ 29 | SlidingWindowMathExpressionFilter(); 30 | 31 | /*! 32 | * Destructor. 33 | */ 34 | ~SlidingWindowMathExpressionFilter() override; 35 | 36 | /*! 37 | * Configures the filter from parameters on the parameter server. 38 | */ 39 | bool configure() override; 40 | 41 | /*! 42 | * Takes the minimum out of different layers of a grid map. 43 | * @param mapIn gridMap with the different layers to take the min. 44 | * @param mapOut gridMap with an additional layer containing the sum. 45 | */ 46 | bool update(const GridMap& mapIn, GridMap& mapOut) override; 47 | 48 | private: 49 | //! Input layer name. 50 | std::string inputLayer_; 51 | 52 | //! Output layer name. 53 | std::string outputLayer_; 54 | 55 | //! EigenLab parser. 56 | EigenLab::Parser parser_; 57 | 58 | //! Expression to parse. 59 | std::string expression_; 60 | 61 | //! Window size. 62 | int windowSize_; 63 | 64 | //! If window length (instead of window size) should be used. 65 | bool useWindowLength_; 66 | 67 | //! Window length. 68 | double windowLength_; 69 | 70 | //! If empty cells should be computed as well. 71 | bool isComputeEmptyCells_; 72 | 73 | //! Edge handling method. 74 | SlidingWindowIterator::EdgeHandling edgeHandling_; 75 | }; 76 | 77 | } // namespace grid_map 78 | -------------------------------------------------------------------------------- /grid_map_filters/include/grid_map_filters/ThresholdFilter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ThresholdFilter.hpp 3 | * 4 | * Created on: Mar 16, 2015 5 | * Author: Martin Wermelinger, Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace grid_map { 18 | 19 | /*! 20 | * Threshold filter class to set values below/above a threshold to a 21 | * specified value. 22 | */ 23 | class ThresholdFilter : public filters::FilterBase { 24 | public: 25 | /*! 26 | * Constructor 27 | */ 28 | ThresholdFilter(); 29 | 30 | /*! 31 | * Destructor. 32 | */ 33 | virtual ~ThresholdFilter(); 34 | 35 | /*! 36 | * Configures the filter from parameters on the parameter server. 37 | */ 38 | virtual bool configure(); 39 | 40 | /*! 41 | * Uses either an upper or lower threshold. If the threshold is exceeded 42 | * the cell value is set to the predefined value setTo_. 43 | * @param mapIn GridMap with the different layers to apply a threshold. 44 | * @param mapOut GridMap with the threshold applied to the layers. 45 | */ 46 | virtual bool update(const GridMap& mapIn, GridMap& mapOut); 47 | 48 | private: 49 | //! Layer the threshold will be evaluated. 50 | std::string conditionLayer_; 51 | 52 | //! Layer the threshold should be applied to. 53 | std::string outputLayer_; 54 | 55 | //! Lower Threshold 56 | double lowerThreshold_; 57 | 58 | //! Upper Threshold 59 | double upperThreshold_; 60 | 61 | //! Booleans to decide which threshold should be used. 62 | bool useLowerThreshold_, useUpperThreshold_; 63 | 64 | //! If threshold triggered set to this value 65 | double setTo_; 66 | }; 67 | 68 | } // namespace grid_map 69 | -------------------------------------------------------------------------------- /grid_map_filters/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_filters 4 | 1.7.18 5 | Processing grid maps as a sequence of ROS filters. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/grid_map 10 | http://github.com/anybotics/grid_map/issues 11 | Péter Fankhauser 12 | Martin Wermelinger 13 | catkin 14 | 15 | filters 16 | grid_map_core 17 | grid_map_ros 18 | grid_map_msgs 19 | tbb 20 | libopencv-dev 21 | 22 | gtest 23 | google-mock 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /grid_map_filters/src/BufferNormalizerFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * BufferNormalizerFilter.cpp 3 | * 4 | * Created on: Aug 18, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_filters/BufferNormalizerFilter.hpp" 10 | 11 | #include 12 | 13 | using namespace filters; 14 | 15 | namespace grid_map { 16 | 17 | BufferNormalizerFilter::BufferNormalizerFilter() = default; 18 | 19 | BufferNormalizerFilter::~BufferNormalizerFilter() = default; 20 | 21 | bool BufferNormalizerFilter::configure() { 22 | return true; 23 | } 24 | 25 | bool BufferNormalizerFilter::update(const GridMap& mapIn, GridMap& mapOut) { 26 | mapOut = mapIn; 27 | mapOut.convertToDefaultStartIndex(); 28 | 29 | return true; 30 | } 31 | 32 | } // namespace grid_map 33 | -------------------------------------------------------------------------------- /grid_map_filters/src/DeletionFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * DeletionFilter.cpp 3 | * 4 | * Created on: Mar 19, 2015 5 | * Author: Martin Wermelinger, Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_filters/DeletionFilter.hpp" 10 | 11 | #include 12 | 13 | using namespace filters; 14 | 15 | namespace grid_map { 16 | 17 | DeletionFilter::DeletionFilter() = default; 18 | 19 | DeletionFilter::~DeletionFilter() = default; 20 | 21 | bool DeletionFilter::configure() { 22 | // Load Parameters 23 | if (!FilterBase::getParam(std::string("layers"), layers_)) { 24 | ROS_ERROR("DeletionFilter did not find parameter 'layers'."); 25 | return false; 26 | } 27 | 28 | return true; 29 | } 30 | 31 | bool DeletionFilter::update(const GridMap& mapIn, GridMap& mapOut) { 32 | mapOut = mapIn; 33 | 34 | for (const auto& layer : layers_) { 35 | // Check if layer exists. 36 | if (!mapOut.exists(layer)) { 37 | ROS_ERROR("Check your deletion layers! Type %s does not exist.", layer.c_str()); 38 | continue; 39 | } 40 | 41 | if (!mapOut.erase(layer)) { 42 | ROS_ERROR("Could not remove type %s.", layer.c_str()); 43 | } 44 | } 45 | 46 | return true; 47 | } 48 | 49 | } // namespace grid_map 50 | -------------------------------------------------------------------------------- /grid_map_filters/src/DuplicationFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * DuplicationFilter.cpp 3 | * 4 | * Created on: Aug 18, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_filters/DuplicationFilter.hpp" 10 | 11 | #include 12 | 13 | using namespace filters; 14 | 15 | namespace grid_map { 16 | 17 | DuplicationFilter::DuplicationFilter() = default; 18 | 19 | DuplicationFilter::~DuplicationFilter() = default; 20 | 21 | bool DuplicationFilter::configure() { 22 | if (!FilterBase::getParam(std::string("input_layer"), inputLayer_)) { 23 | ROS_ERROR("DuplicationFilter did not find parameter 'input_layer'."); 24 | return false; 25 | } 26 | 27 | if (!FilterBase::getParam(std::string("output_layer"), outputLayer_)) { 28 | ROS_ERROR("DuplicationFilter did not find parameter 'output_layer'."); 29 | return false; 30 | } 31 | 32 | return true; 33 | } 34 | 35 | bool DuplicationFilter::update(const GridMap& mapIn, GridMap& mapOut) { 36 | mapOut = mapIn; 37 | mapOut.add(outputLayer_, mapIn[inputLayer_]); 38 | return true; 39 | } 40 | 41 | } // namespace grid_map 42 | -------------------------------------------------------------------------------- /grid_map_filters/src/MathExpressionFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MathExpressionFilter.cpp 3 | * 4 | * Created on: Aug 16, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_filters/MathExpressionFilter.hpp" 10 | 11 | #include 12 | 13 | using namespace filters; 14 | 15 | namespace grid_map { 16 | 17 | MathExpressionFilter::MathExpressionFilter() = default; 18 | 19 | MathExpressionFilter::~MathExpressionFilter() = default; 20 | 21 | bool MathExpressionFilter::configure() { 22 | if (!FilterBase::getParam(std::string("expression"), expression_)) { 23 | ROS_ERROR("MathExpressionFilter did not find parameter 'expression'."); 24 | return false; 25 | } 26 | 27 | if (!FilterBase::getParam(std::string("output_layer"), outputLayer_)) { 28 | ROS_ERROR("MathExpressionFilter did not find parameter 'output_layer'."); 29 | return false; 30 | } 31 | 32 | // TODO Can we make caching work with changing shared variable? 33 | // parser_.setCacheExpressions(true); 34 | return true; 35 | } 36 | 37 | bool MathExpressionFilter::update(const GridMap& mapIn, GridMap& mapOut) { 38 | mapOut = mapIn; 39 | for (const auto& layer : mapOut.getLayers()) { 40 | parser_.var(layer).setShared(mapOut[layer]); 41 | } 42 | EigenLab::Value result(parser_.eval(expression_)); 43 | mapOut.add(outputLayer_, result.matrix()); 44 | return true; 45 | } 46 | 47 | } // namespace grid_map 48 | -------------------------------------------------------------------------------- /grid_map_filters/src/MockFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MockFilter.cpp 3 | * 4 | * Created on: Sep 24, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_filters/MockFilter.hpp" 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | using namespace filters; 17 | 18 | namespace grid_map { 19 | 20 | MockFilter::MockFilter() = default; 21 | 22 | MockFilter::~MockFilter() = default; 23 | 24 | bool MockFilter::configure() { 25 | if (!FilterBase::getParam(std::string("processing_time"), processingTime_)) { 26 | ROS_ERROR("MockFilter did not find parameter 'processing_time'."); 27 | return false; 28 | } 29 | 30 | if (!FilterBase::getParam(std::string("print_name"), printName_)) { 31 | ROS_INFO("MockFilter did not find parameter 'print_name'. Not printing the name. "); 32 | printName_ = false; 33 | } 34 | 35 | return true; 36 | } 37 | 38 | bool MockFilter::update(const GridMap& mapIn, GridMap& mapOut) { 39 | mapOut = mapIn; 40 | if (printName_) { 41 | ROS_INFO_STREAM(this->getName() << ": update()"); 42 | } 43 | std::this_thread::sleep_for(std::chrono::milliseconds(processingTime_)); 44 | return true; 45 | } 46 | 47 | } // namespace grid_map 48 | -------------------------------------------------------------------------------- /grid_map_filters/src/NormalColorMapFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * NormalColorMapFilter.cpp 3 | * 4 | * Created on: Aug 22, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_filters/NormalColorMapFilter.hpp" 10 | 11 | #include 12 | 13 | #include 14 | 15 | using namespace filters; 16 | 17 | namespace grid_map { 18 | 19 | NormalColorMapFilter::NormalColorMapFilter() = default; 20 | 21 | NormalColorMapFilter::~NormalColorMapFilter() = default; 22 | 23 | bool NormalColorMapFilter::configure() { 24 | if (!FilterBase::getParam(std::string("input_layers_prefix"), inputLayersPrefix_)) { 25 | ROS_ERROR("Normal color map filter did not find parameter `input_layers_prefix`."); 26 | return false; 27 | } 28 | ROS_DEBUG("Normal color map filter input layers prefix is = %s.", inputLayersPrefix_.c_str()); 29 | 30 | if (!FilterBase::getParam(std::string("output_layer"), outputLayer_)) { 31 | ROS_ERROR("Normal color map filter did not find parameter `output_layer`."); 32 | return false; 33 | } 34 | ROS_DEBUG("Normal color map filter output_layer = %s.", outputLayer_.c_str()); 35 | return true; 36 | } 37 | 38 | bool NormalColorMapFilter::update(const GridMap& mapIn, GridMap& mapOut) { 39 | const auto& normalX = mapIn[inputLayersPrefix_ + "x"]; 40 | const auto& normalY = mapIn[inputLayersPrefix_ + "y"]; 41 | const auto& normalZ = mapIn[inputLayersPrefix_ + "z"]; 42 | 43 | mapOut = mapIn; 44 | mapOut.add(outputLayer_); 45 | auto& color = mapOut[outputLayer_]; 46 | 47 | // X: -1 to +1 : Red: 0 to 255 48 | // Y: -1 to +1 : Green: 0 to 255 49 | // Z: 0 to 1 : Blue: 128 to 255 50 | 51 | // For each cell in map. 52 | for (Eigen::Index i = 0; i < color.size(); ++i) { 53 | const Eigen::Vector3f colorVector((normalX(i) + 1.0) / 2.0, (normalY(i) + 1.0) / 2.0, (normalZ(i) / 2.0) + 0.5); 54 | colorVectorToValue(colorVector, color(i)); 55 | } 56 | 57 | return true; 58 | } 59 | 60 | } // namespace grid_map 61 | -------------------------------------------------------------------------------- /grid_map_filters/src/SetBasicLayersFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SetBasicLayersFilters.cpp 3 | * 4 | * Created on: Aug 16, 2017 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_filters/SetBasicLayersFilter.hpp" 10 | 11 | #include 12 | 13 | using namespace filters; 14 | 15 | namespace grid_map { 16 | 17 | SetBasicLayersFilter::SetBasicLayersFilter() = default; 18 | 19 | SetBasicLayersFilter::~SetBasicLayersFilter() = default; 20 | 21 | bool SetBasicLayersFilter::configure() { 22 | if (!FilterBase::getParam(std::string("layers"), layers_)) { 23 | ROS_ERROR("SetBasicLayersFilters did not find parameter 'layers'."); 24 | return false; 25 | } 26 | 27 | return true; 28 | } 29 | 30 | bool SetBasicLayersFilter::update(const GridMap& mapIn, GridMap& mapOut) { 31 | mapOut = mapIn; 32 | std::vector layersChecked; 33 | 34 | for (const auto& layer : layers_) { 35 | if (!mapOut.exists(layer)) { 36 | ROS_WARN("Layer `%s` does not exist and is not set as basic layer.", layer.c_str()); 37 | continue; 38 | } 39 | layersChecked.push_back(layer); 40 | } 41 | 42 | mapOut.setBasicLayers(layersChecked); 43 | return true; 44 | } 45 | 46 | } // namespace grid_map 47 | -------------------------------------------------------------------------------- /grid_map_filters/test/mock_filter_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "grid_map_filters/MockFilter.hpp" 4 | 5 | using namespace grid_map; 6 | using namespace ::testing; 7 | 8 | TEST(MockFilter, ConstructFilterTest) { // NOLINT 9 | MockFilter mockFilter{}; 10 | SUCCEED(); 11 | } -------------------------------------------------------------------------------- /grid_map_filters/test/test_grid_map_filters.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | using ::testing::InitGoogleTest; 5 | 6 | int main(int argc, char** argv) { 7 | InitGoogleTest(&argc, argv); 8 | 9 | return RUN_ALL_TESTS(); 10 | } 11 | -------------------------------------------------------------------------------- /grid_map_loader/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package grid_map_loader 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.7.0 (2022-03-17) 6 | ------------------ 7 | 8 | 1.6.4 (2020-12-04) 9 | ------------------ 10 | 11 | 1.6.2 (2019-10-14) 12 | ------------------ 13 | 14 | 1.6.1 (2019-02-27) 15 | ------------------ 16 | * Updated host changes. 17 | * Updated author e-mail address. 18 | * Contributors: Peter Fankhauser, Péter Fankhauser 19 | 20 | 1.6.0 (2017-11-24) 21 | ------------------ 22 | 23 | 1.5.2 (2017-07-25) 24 | ------------------ 25 | 26 | 1.5.1 (2017-07-25) 27 | ------------------ 28 | 29 | 1.5.0 (2017-07-18) 30 | ------------------ 31 | 32 | 1.4.2 (2017-01-24) 33 | ------------------ 34 | 35 | 1.4.1 (2016-10-23) 36 | ------------------ 37 | 38 | 1.4.0 (2016-08-22) 39 | ------------------ 40 | 41 | 1.3.3 (2016-05-10) 42 | ------------------ 43 | * Release for ROS Kinetic. 44 | * Contributors: Peter Fankhauser 45 | 46 | 1.3.2 (2016-05-10) 47 | ------------------ 48 | 49 | 1.3.1 (2016-05-10) 50 | ------------------ 51 | 52 | 1.3.0 (2016-04-26) 53 | ------------------ 54 | 55 | 1.2.0 (2016-03-03) 56 | ------------------ 57 | * Changed the package name from `grid_map` to `grid_map_ros` and made `grid_map` a metapackage (`#34 `_). 58 | * Contributors: Peter Fankhauser 59 | 60 | 1.1.3 (2016-01-11) 61 | ------------------ 62 | 63 | 1.1.2 (2016-01-11) 64 | ------------------ 65 | * Should fix errors on build server regarding Eigen3 and visualization_msgs dependencies. 66 | 67 | 1.1.1 (2016-01-11) 68 | ------------------ 69 | * Changes to CMakeLists.txt to enable compatibility with Ubuntu Saucy. 70 | 71 | 1.1.0 (2016-01-08) 72 | ------------------- 73 | * added installation instructions in CMakeLists 74 | * general improvements and bugfixes 75 | 76 | 1.0.0 (2015-11-20) 77 | ------------------- 78 | * release for Springer ROS Book Chapter 79 | -------------------------------------------------------------------------------- /grid_map_loader/include/grid_map_loader/GridMapLoader.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GridMapLoader.hpp 3 | * 4 | * Created on: Aug 24, 2015 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | * 8 | */ 9 | 10 | #pragma once 11 | 12 | // ROS 13 | #include 14 | 15 | // Grid map 16 | #include 17 | 18 | // STD 19 | #include 20 | 21 | namespace grid_map_loader { 22 | 23 | /*! 24 | * Loads and publishes a grid map from a bag file. 25 | */ 26 | class GridMapLoader 27 | { 28 | public: 29 | 30 | /*! 31 | * Constructor. 32 | * @param nodeHandle the ROS node handle. 33 | */ 34 | GridMapLoader(ros::NodeHandle nodeHandle); 35 | 36 | /*! 37 | * Destructor. 38 | */ 39 | virtual ~GridMapLoader(); 40 | 41 | /*! 42 | * Reads and verifies the ROS parameters. 43 | * @return true if successful. 44 | */ 45 | bool readParameters(); 46 | 47 | /*! 48 | * Loads the grid map from the bag file. 49 | * @return true if successful, false otherwise. 50 | */ 51 | bool load(); 52 | 53 | /*! 54 | * Publishes the grid map. 55 | */ 56 | void publish(); 57 | 58 | private: 59 | 60 | //! ROS nodehandle. 61 | ros::NodeHandle nodeHandle_; 62 | 63 | //! Grid map publisher. 64 | ros::Publisher publisher_; 65 | 66 | //! Grid map data. 67 | grid_map::GridMap map_; 68 | 69 | //! Path the ROS bag to be published. 70 | std::string filePath_; 71 | 72 | //! Topic name of the grid map in the ROS bag. 73 | std::string bagTopic_; 74 | 75 | //! Topic name of the grid map to be loaded. 76 | std::string publishTopic_; 77 | 78 | //! Duration to publish the grid map. 79 | ros::Duration duration_; 80 | }; 81 | 82 | } /* namespace */ 83 | -------------------------------------------------------------------------------- /grid_map_loader/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_loader 4 | 1.7.18 5 | Loading and publishing grid maps from bag files. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/grid_map 10 | http://github.com/anybotics/grid_map/issues 11 | Péter Fankhauser 12 | catkin 13 | 14 | roscpp 15 | grid_map_ros 16 | grid_map_msgs 17 | 18 | gtest 19 | 20 | -------------------------------------------------------------------------------- /grid_map_loader/src/GridMapLoader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GridMapLoader.cpp 3 | * 4 | * Created on: Aug 24, 2015 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | using namespace grid_map; 14 | 15 | namespace grid_map_loader { 16 | 17 | GridMapLoader::GridMapLoader(ros::NodeHandle nodeHandle) 18 | : nodeHandle_(nodeHandle) 19 | { 20 | readParameters(); 21 | publisher_ = nodeHandle_.advertise(publishTopic_, 1, true); 22 | if (!load()) return; 23 | publish(); 24 | ros::requestShutdown(); 25 | } 26 | 27 | GridMapLoader::~GridMapLoader() 28 | { 29 | } 30 | 31 | bool GridMapLoader::readParameters() 32 | { 33 | nodeHandle_.param("bag_topic", bagTopic_, std::string("/grid_map")); 34 | nodeHandle_.param("publish_topic", publishTopic_, bagTopic_); 35 | nodeHandle_.param("file_path", filePath_, std::string()); 36 | double durationInSec; 37 | nodeHandle_.param("duration", durationInSec, 5.0); 38 | duration_.fromSec(durationInSec); 39 | return true; 40 | } 41 | 42 | bool GridMapLoader::load() 43 | { 44 | ROS_INFO_STREAM("Loading grid map from path " << filePath_ << "."); 45 | return GridMapRosConverter::loadFromBag(filePath_, bagTopic_, map_); 46 | } 47 | 48 | void GridMapLoader::publish() 49 | { 50 | grid_map_msgs::GridMap message; 51 | grid_map::GridMapRosConverter::toMessage(map_, message); 52 | publisher_.publish(message); 53 | duration_.sleep(); 54 | } 55 | 56 | } /* namespace */ 57 | -------------------------------------------------------------------------------- /grid_map_loader/src/grid_map_loader_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * grid_map_loader_node.cpp 3 | * 4 | * Created on: Aug 24, 2015 5 | * Author: Peter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | int main(int argc, char** argv) 13 | { 14 | ros::init(argc, argv, "grid_map_loader"); 15 | ros::NodeHandle nodeHandle("~"); 16 | grid_map_loader::GridMapLoader gridMapLoader(nodeHandle); 17 | ros::waitForShutdown(); 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /grid_map_loader/test/empty_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | TEST(Placeholder, emptyTest) { // NOLINT 4 | ASSERT_TRUE(true); 5 | } 6 | -------------------------------------------------------------------------------- /grid_map_loader/test/test_grid_map_loader.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | using ::testing::InitGoogleTest; 5 | 6 | int main(int argc, char** argv) { 7 | InitGoogleTest(&argc, argv); 8 | 9 | return RUN_ALL_TESTS(); 10 | } 11 | -------------------------------------------------------------------------------- /grid_map_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package grid_map_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.7.0 (2022-03-17) 6 | ------------------ 7 | 8 | 1.6.4 (2020-12-04) 9 | ------------------ 10 | 11 | 1.6.2 (2019-10-14) 12 | ------------------ 13 | 14 | 1.6.1 (2019-02-27) 15 | ------------------ 16 | * Merge remote-tracking branch 'origin/master' into fix/melodic 17 | * Merge pull request `#198 `_ from ANYbotics/feature/extend_process_file_srv 18 | Extend ProcessFile.srv to include topic_name field. 19 | * Restore single topic_name field, as discussed with Peter. 20 | * Update ProcessFile.srv 21 | * Extend ProcessFile.srv to include topic_name field. 22 | * Updated author e-mail address. 23 | * Contributors: Marco Tranzatto, MarcoSuetterlin, Peter Fankhauser, Péter Fankhauser 24 | 25 | 1.6.0 (2017-11-24) 26 | ------------------ 27 | 28 | 1.5.2 (2017-07-25) 29 | ------------------ 30 | 31 | 1.5.1 (2017-07-25) 32 | ------------------ 33 | 34 | 1.5.0 (2017-07-18) 35 | ------------------ 36 | 37 | 1.4.2 (2017-01-24) 38 | ------------------ 39 | 40 | 1.4.1 (2016-10-23) 41 | ------------------ 42 | 43 | 1.4.0 (2016-08-22) 44 | ------------------ 45 | 46 | 1.3.3 (2016-05-10) 47 | ------------------ 48 | * Release for ROS Kinetic. 49 | * Contributors: Peter Fankhauser 50 | 51 | 1.3.2 (2016-05-10) 52 | ------------------ 53 | 54 | 1.3.1 (2016-05-10) 55 | ------------------ 56 | 57 | 1.3.0 (2016-04-26) 58 | ------------------ 59 | 60 | 1.2.0 (2016-03-03) 61 | ------------------ 62 | * [grid_map_msgs] package exports 63 | * Contributors: Daniel Stonier 64 | 65 | 1.1.3 (2016-01-11) 66 | ------------------ 67 | 68 | 1.1.2 (2016-01-11) 69 | ------------------ 70 | * Should fix errors on build server regarding Eigen3 and visualization_msgs dependencies. 71 | 72 | 1.1.1 (2016-01-11) 73 | ------------------ 74 | 75 | 1.1.0 (2016-01-08) 76 | ------------------- 77 | * added new srv definition for processing of grid map files 78 | * general improvements and bugfixes 79 | 80 | 1.0.0 (2015-11-20) 81 | ------------------- 82 | * release for Springer ROS Book Chapter 83 | -------------------------------------------------------------------------------- /grid_map_msgs/msg/GridMap.msg: -------------------------------------------------------------------------------- 1 | # Grid map header 2 | GridMapInfo info 3 | 4 | # Grid map layer names. 5 | string[] layers 6 | 7 | # Grid map basic layer names (optional). The basic layers 8 | # determine which layers from `layers` need to be valid 9 | # in order for a cell of the grid map to be valid. 10 | string[] basic_layers 11 | 12 | # Grid map data. 13 | std_msgs/Float32MultiArray[] data 14 | 15 | # Row start index (default 0). 16 | uint16 outer_start_index 17 | 18 | # Column start index (default 0). 19 | uint16 inner_start_index 20 | -------------------------------------------------------------------------------- /grid_map_msgs/msg/GridMapInfo.msg: -------------------------------------------------------------------------------- 1 | # Header (time and frame) 2 | std_msgs/Header header 3 | 4 | # Resolution of the grid [m/cell]. 5 | float64 resolution 6 | 7 | # Length in x-direction [m]. 8 | float64 length_x 9 | 10 | # Length in y-direction [m]. 11 | float64 length_y 12 | 13 | # Pose of the grid map center in the frame defined in `header` [m]. 14 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /grid_map_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_msgs 4 | 1.7.18 5 | Definition of the multi-layered grid map message type. 6 | 7 | BSD 8 | Péter Fankhauser 9 | Maximilian Wulf 10 | Yoshua Nava 11 | http://github.com/anybotics/grid_map 12 | http://github.com/anybotics/grid_map/issues 13 | 14 | catkin 15 | ament_cmake 16 | rosidl_default_generators 17 | 18 | message_generation 19 | 20 | std_msgs 21 | geometry_msgs 22 | 23 | message_runtime 24 | rosidl_default_runtime 25 | 26 | rosidl_interface_packages 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /grid_map_msgs/srv/GetGridMap.srv: -------------------------------------------------------------------------------- 1 | # Frame id of the submap position request. 2 | string frame_id 3 | 4 | # Requested submap position in x-direction [m]. 5 | float64 position_x 6 | 7 | # Requested submap position in y-direction [m]. 8 | float64 position_y 9 | 10 | # Requested submap length in x-direction [m]. 11 | float64 length_x 12 | 13 | # Requested submap width in y-direction [m]. 14 | float64 length_y 15 | 16 | # Requested layers. If empty, get all layers. 17 | string[] layers 18 | 19 | --- 20 | 21 | # Submap 22 | grid_map_msgs/GridMap map 23 | -------------------------------------------------------------------------------- /grid_map_msgs/srv/GetGridMapInfo.srv: -------------------------------------------------------------------------------- 1 | --- 2 | 3 | # Grid map info 4 | grid_map_msgs/GridMapInfo info 5 | -------------------------------------------------------------------------------- /grid_map_msgs/srv/ProcessFile.srv: -------------------------------------------------------------------------------- 1 | # Absolute file path. 2 | string file_path 3 | 4 | # For ROS bags: topic name that should be processed (optional). 5 | string topic_name 6 | 7 | --- 8 | 9 | # True if file processing was successful. 10 | bool success 11 | -------------------------------------------------------------------------------- /grid_map_msgs/srv/SetGridMap.srv: -------------------------------------------------------------------------------- 1 | # map 2 | grid_map_msgs/GridMap map 3 | 4 | --- 5 | -------------------------------------------------------------------------------- /grid_map_octomap/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package grid_map_octomap 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.7.0 (2022-03-17) 6 | ------------------ 7 | 8 | 1.6.4 (2020-12-04) 9 | ------------------ 10 | 11 | 1.6.2 (2019-10-14) 12 | ------------------ 13 | 14 | 1.6.1 (2019-02-27) 15 | ------------------ 16 | * Updated author e-mail address. 17 | * Contributors: Peter Fankhauser 18 | 19 | 1.6.0 (2017-11-24) 20 | ------------------ 21 | 22 | 1.5.2 (2017-07-25) 23 | ------------------ 24 | 25 | 1.5.1 (2017-07-25) 26 | ------------------ 27 | 28 | 1.5.0 (2017-07-18) 29 | ------------------ 30 | * Added new package for octomap to grid map conversion. 31 | * Contributors: Jeff Delmerico, Peter Fankhauser 32 | -------------------------------------------------------------------------------- /grid_map_octomap/include/grid_map_octomap/GridMapOctomapConverter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GridMapOctomapConverter.hpp 3 | * 4 | * Created on: May 1, 2017 5 | * Author: Jeff Delmerico, Peter Fankhauser 6 | * Institute: University of Zürich, Robotics and Perception Group 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | // Octomap 14 | #include 15 | 16 | // STD 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace grid_map { 23 | 24 | /*! 25 | * Conversions between grid maps and Octomap types. 26 | */ 27 | class GridMapOctomapConverter 28 | { 29 | public: 30 | /*! 31 | * Default constructor. 32 | */ 33 | GridMapOctomapConverter(); 34 | 35 | /*! 36 | * Destructor. 37 | */ 38 | virtual ~GridMapOctomapConverter(); 39 | 40 | /*! 41 | * Converts an Octomap to a grid map in the same coordinate frame, with a 42 | * cell resolution equal to the leaf voxel size in the Octomap. Only creates 43 | * a layer for elevation. 44 | * This changes the geometry of the grid map and deletes all layer contents. 45 | * Note: Bounding box coordinates are not checked for sanity - if you provide 46 | * values outside of the gridmap, undefined behavior may result. 47 | * @param[in] octomap the octomap. 48 | * @param[in] layer the layer that is filled with the octomap data. 49 | * @param[out] gridMap the grid map to be initialized. 50 | * @param[in] minPoint (optional) minimum coordinate for bounding box. 51 | * @param[in] maxPoint (optional) maximum coordinate for bounding box. 52 | * @return true if successful, false otherwise. 53 | */ 54 | static bool fromOctomap(const octomap::OcTree& octomap, 55 | const std::string& layer, 56 | grid_map::GridMap& gridMap, 57 | const grid_map::Position3* minPoint = nullptr, 58 | const grid_map::Position3* maxPoint = nullptr); 59 | 60 | }; 61 | 62 | } /* namespace */ 63 | -------------------------------------------------------------------------------- /grid_map_octomap/include/grid_map_octomap/grid_map_octomap.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * grid_map_octomap.hpp 3 | * 4 | * Created on: May 1, 2017 5 | * Author: Jeff Delmerico 6 | * Institute: University of Zürich, Robotics and Perception Group 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | -------------------------------------------------------------------------------- /grid_map_octomap/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_octomap 4 | 1.7.18 5 | Conversions between grid maps and OctoMap types. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/grid_map 10 | http://github.com/anybotics/grid_map/issues 11 | Jeff Delmerico 12 | Péter Fankhauser 13 | catkin 14 | 15 | grid_map_core 16 | octomap 17 | 18 | gtest 19 | 20 | -------------------------------------------------------------------------------- /grid_map_octomap/test/test_grid_map_octomap.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_grid_map_octomap.cpp 3 | * 4 | * Created on: May 1, 2017 5 | * Author: Jeff Delmerico 6 | * Institute: University of Zürich, Robotics and Perception Group 7 | */ 8 | 9 | // gtest 10 | #include 11 | 12 | // Run all the tests that were declared with TEST() 13 | int main(int argc, char **argv) 14 | { 15 | testing::InitGoogleTest(&argc, argv); 16 | srand((int)time(0)); 17 | return RUN_ALL_TESTS(); 18 | } 19 | -------------------------------------------------------------------------------- /grid_map_pcl/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package grid_map_pcl 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.7.0 (2022-03-17) 6 | ------------------ 7 | 8 | 1.6.4 (2020-12-04) 9 | ------------------ 10 | 11 | 1.6.2 (2019-10-14) 12 | ------------------ 13 | 14 | 1.6.1 (2019-02-27) 15 | ------------------ 16 | * Updated host changes. 17 | * Updated author e-mail address. 18 | * Contributors: Peter Fankhauser, Péter Fankhauser 19 | 20 | 1.6.0 (2017-11-24) 21 | ------------------ 22 | 23 | 1.5.2 (2017-07-25) 24 | ------------------ 25 | 26 | 1.5.1 (2017-07-25) 27 | ------------------ 28 | 29 | 1.5.0 (2017-07-18) 30 | ------------------ 31 | * Improved efficiency of mesh conversion. 32 | * Contributors: Alex Millane 33 | 34 | 1.4.2 (2017-01-24) 35 | ------------------ 36 | * Addressing C++ compiler warnings. 37 | * Contributors: Peter Fankhauser 38 | 39 | 1.4.1 (2016-10-23) 40 | ------------------ 41 | * Added new grid_map_pcl package to convert from PCL mesh to grid map. 42 | * Contributors: Dominic Jud 43 | -------------------------------------------------------------------------------- /grid_map_pcl/config/parameters.yaml: -------------------------------------------------------------------------------- 1 | pcl_grid_map_extraction: 2 | num_processing_threads: 4 3 | cloud_transform: 4 | translation: 5 | x: 0.0 6 | y: 0.0 7 | z: 0.0 8 | rotation: #intrinsic rotation X-Y-Z (r-p-y)sequence 9 | r: 0.0 10 | p: 0.0 11 | y: 0.0 12 | cluster_extraction: 13 | cluster_tolerance: 0.05 14 | min_num_points: 4 15 | max_num_points: 1000000 16 | use_max_height_as_cell_elevation: false 17 | outlier_removal: 18 | is_remove_outliers: false 19 | mean_K: 10 20 | stddev_threshold: 1.0 21 | downsampling: 22 | is_downsample_cloud: false 23 | voxel_size: 24 | x: 0.02 25 | y: 0.02 26 | z: 0.02 27 | grid_map: 28 | min_num_points_per_cell: 4 29 | max_num_points_per_cell: 100000 30 | resolution: 0.1 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /grid_map_pcl/data/input_cloud.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_pcl/data/input_cloud.pcd -------------------------------------------------------------------------------- /grid_map_pcl/doc/forest.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_pcl/doc/forest.png -------------------------------------------------------------------------------- /grid_map_pcl/doc/indoor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_pcl/doc/indoor.png -------------------------------------------------------------------------------- /grid_map_pcl/doc/outdoor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_pcl/doc/outdoor.png -------------------------------------------------------------------------------- /grid_map_pcl/include/grid_map_pcl/GridMapPclConverter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GridMapPclConverter.hpp 3 | * 4 | * Created on: Apr 14, 2016 5 | * Author: Dominic Jud 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | // PCL 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | // STD 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace grid_map { 27 | 28 | /*! 29 | * Conversions between grid maps and PCL types. 30 | */ 31 | class GridMapPclConverter { 32 | public: 33 | /*! 34 | * Default constructor. 35 | */ 36 | GridMapPclConverter() = default; 37 | 38 | /*! 39 | * Destructor. 40 | */ 41 | virtual ~GridMapPclConverter() = default; 42 | 43 | /*! 44 | * Initializes the geometry of a grid map from a polygon mesh. This changes 45 | * the geometry of the map and deletes all contents of the layers! 46 | * @param[in] mesh the mesh. 47 | * @param[in] resolution the desired resolution of the grid map [m/cell]. 48 | * @param[out] gridMap the grid map to be initialized. 49 | * @return true if successful, false otherwise. 50 | */ 51 | static bool initializeFromPolygonMesh(const pcl::PolygonMesh& mesh, const double resolution, grid_map::GridMap& gridMap); 52 | 53 | /*! 54 | * Adds a layer with data from a polygon mesh. The mesh is ray traced from 55 | * above (negative z-Direction). 56 | * @param[in] mesh the mesh to be added. It can only consist of triangles! 57 | * @param[in] layer the layer that is filled with the mesh data. 58 | * @param[out] gridMap the grid map to be populated. 59 | * @return true if successful, false otherwise. 60 | */ 61 | static bool addLayerFromPolygonMesh(const pcl::PolygonMesh& mesh, const std::string& layer, grid_map::GridMap& gridMap); 62 | 63 | private: 64 | static bool rayTriangleIntersect(const Eigen::Vector3f& point, const Eigen::Vector3f& ray, const Eigen::Matrix3f& triangleVertices, 65 | Eigen::Vector3f& intersectionPoint); 66 | }; 67 | 68 | } /* namespace grid_map */ 69 | -------------------------------------------------------------------------------- /grid_map_pcl/include/grid_map_pcl/grid_map_pcl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * grid_map_pcl.hpp 3 | * 4 | * Created on: Oct 19, 2016 5 | * Author: Dominic Jud 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | -------------------------------------------------------------------------------- /grid_map_pcl/include/grid_map_pcl/helpers.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * helpers.hpp 3 | * 4 | * Created on: Nov 20, 2019 5 | * Author: Edo Jelavic 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace grid_map { 17 | 18 | class GridMapPclLoader; 19 | class GridMap; 20 | 21 | namespace grid_map_pcl { 22 | 23 | void setVerbosityLevelToDebugIfFlagSet(const ros::NodeHandle& nh); 24 | 25 | std::string getParameterPath(const ros::NodeHandle& nh); 26 | 27 | std::string getOutputBagPath(const ros::NodeHandle& nh); 28 | 29 | std::string getPcdFilePath(const ros::NodeHandle& nh); 30 | 31 | std::string getMapFrame(const ros::NodeHandle& nh); 32 | 33 | std::string getMapRosbagTopic(const ros::NodeHandle& nh); 34 | 35 | std::string getMapLayerName(const ros::NodeHandle& nh); 36 | 37 | void saveGridMap(const grid_map::GridMap& gridMap, const ros::NodeHandle& nh, const std::string& mapTopic); 38 | 39 | inline void printTimeElapsedToRosInfoStream(const std::chrono::system_clock::time_point& start, const std::string& prefix); 40 | 41 | void processPointcloud(grid_map::GridMapPclLoader* gridMapPclLoader, const ros::NodeHandle& nh); 42 | 43 | using Point = ::pcl::PointXYZ; 44 | using Pointcloud = ::pcl::PointCloud; 45 | enum class XYZ : int { X, Y, Z }; 46 | 47 | Eigen::Affine3f getRigidBodyTransform(const Eigen::Vector3d& translation, const Eigen::Vector3d& intrinsicRpy); 48 | Eigen::Matrix3f getRotationMatrix(double angle, XYZ axis); 49 | 50 | // processing point clouds 51 | Eigen::Vector3d calculateMeanOfPointPositions(Pointcloud::ConstPtr inputCloud); 52 | Pointcloud::Ptr transformCloud(Pointcloud::ConstPtr inputCloud, const Eigen::Affine3f& transformMatrix); 53 | Pointcloud::Ptr loadPointcloudFromPcd(const std::string& filename); 54 | 55 | } /* namespace grid_map_pcl*/ 56 | 57 | } /* namespace grid_map */ 58 | -------------------------------------------------------------------------------- /grid_map_pcl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_pcl 4 | 1.7.18 5 | Conversions between grid maps and Point Cloud Library (PCL) types. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/grid_map 10 | http://github.com/anybotics/grid_map/issues 11 | Dominic Jud 12 | Edo Jelavic 13 | 14 | catkin 15 | 16 | 17 | 18 | grid_map_core 19 | grid_map_msgs 20 | grid_map_ros 21 | libpcl-all-dev 22 | pcl_ros 23 | roscpp 24 | yaml-cpp 25 | 26 | rviz 27 | 28 | 29 | gtest 30 | 31 | -------------------------------------------------------------------------------- /grid_map_pcl/src/grid_map_pcl_loader_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * grid_map_pcl_loader_node.cpp 3 | * 4 | * Created on: Aug 26, 2019 5 | * Author: Edo Jelavic 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include "grid_map_pcl/GridMapPclLoader.hpp" 15 | #include "grid_map_pcl/helpers.hpp" 16 | 17 | namespace gm = ::grid_map::grid_map_pcl; 18 | 19 | int main(int argc, char** argv) { 20 | ros::init(argc, argv, "grid_map_pcl_loader_node"); 21 | ros::NodeHandle nh("~"); 22 | gm::setVerbosityLevelToDebugIfFlagSet(nh); 23 | 24 | ros::Publisher gridMapPub; 25 | gridMapPub = nh.advertise("grid_map_from_raw_pointcloud", 1, true); 26 | 27 | grid_map::GridMapPclLoader gridMapPclLoader; 28 | const std::string pathToCloud = gm::getPcdFilePath(nh); 29 | gridMapPclLoader.loadParameters(gm::getParameterPath(nh)); 30 | gridMapPclLoader.loadCloudFromPcdFile(pathToCloud); 31 | 32 | gm::processPointcloud(&gridMapPclLoader, nh); 33 | 34 | grid_map::GridMap gridMap = gridMapPclLoader.getGridMap(); 35 | gridMap.setFrameId(gm::getMapFrame(nh)); 36 | 37 | gm::saveGridMap(gridMap, nh, gm::getMapRosbagTopic(nh)); 38 | 39 | // publish grid map 40 | grid_map_msgs::GridMap msg; 41 | grid_map::GridMapRosConverter::toMessage(gridMap, msg); 42 | gridMapPub.publish(msg); 43 | 44 | // run 45 | ros::spin(); 46 | return EXIT_SUCCESS; 47 | } 48 | -------------------------------------------------------------------------------- /grid_map_pcl/src/pointcloud_publisher_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * pointcloud_publisher_node.cpp 3 | * 4 | * Created on: Aug 19, 2021 5 | * Author: Edo Jelavic 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include "grid_map_pcl/helpers.hpp" 13 | 14 | namespace gm = ::grid_map::grid_map_pcl; 15 | using Point = ::pcl::PointXYZ; 16 | using PointCloud = ::pcl::PointCloud; 17 | 18 | void publishCloud(const std::string& filename, const ros::Publisher& pub, const std::string& frame) { 19 | PointCloud::Ptr cloud(new pcl::PointCloud); 20 | cloud = gm::loadPointcloudFromPcd(filename); 21 | cloud->header.frame_id = frame; 22 | sensor_msgs::PointCloud2 msg; 23 | pcl::toROSMsg(*cloud, msg); 24 | ROS_INFO_STREAM("Publishing loaded cloud, number of points: " << cloud->points.size()); 25 | msg.header.stamp = ros::Time::now(); 26 | pub.publish(msg); 27 | } 28 | 29 | int main(int argc, char** argv) { 30 | ros::init(argc, argv, "point_cloud_pub_node"); 31 | ros::NodeHandle nh("~"); 32 | 33 | const std::string pathToCloud = gm::getPcdFilePath(nh); 34 | const std::string cloudFrame = nh.param("cloud_frame", ""); 35 | // publish cloud 36 | ros::Publisher cloudPub = nh.advertise("raw_pointcloud", 1, true); 37 | publishCloud(pathToCloud, cloudPub, cloudFrame); 38 | 39 | // run 40 | ros::spin(); 41 | return EXIT_SUCCESS; 42 | } 43 | -------------------------------------------------------------------------------- /grid_map_pcl/test/HelpersTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * HelpersTest.cpp 3 | * 4 | * Created on: Nov 21, 2019 5 | * Author: Edo Jelavic 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include "grid_map_pcl/helpers.hpp" 14 | 15 | #include "PointcloudCreator.hpp" 16 | #include "test_helpers.hpp" 17 | 18 | namespace grid_map { 19 | namespace grid_map_pcl_test { 20 | 21 | TEST(HelpersTest, MeanPositionTest) { 22 | grid_map_pcl_test::setVerbosityLevel(ros::console::levels::Warn); 23 | const auto seed = rand(); 24 | grid_map_pcl_test::rndGenerator.seed(seed); 25 | 26 | const unsigned int numTests = 10; 27 | for (unsigned int i = 0; i < numTests; ++i) { 28 | double mean, stdDev; 29 | auto cloud = grid_map_pcl_test::PointcloudCreator::createBlobOfPoints(&mean, &stdDev); 30 | auto meanOfAllPoints = grid_map_pcl::calculateMeanOfPointPositions(cloud); 31 | 32 | EXPECT_NEAR(meanOfAllPoints.x(), mean, 3 * stdDev); 33 | EXPECT_NEAR(meanOfAllPoints.y(), mean, 3 * stdDev); 34 | EXPECT_NEAR(meanOfAllPoints.z(), mean, 3 * stdDev); 35 | } 36 | 37 | if (::testing::Test::HasFailure()) { 38 | std::cout << "\n Test MeanPositionTest failed with seed: " << seed << std::endl; 39 | } 40 | } 41 | 42 | } /*namespace grid_map_pcl_test */ 43 | } // namespace grid_map 44 | -------------------------------------------------------------------------------- /grid_map_pcl/test/PointcloudProcessorTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PointcloudProcessorTest.cpp 3 | * 4 | * Created on: Nov 21, 2019 5 | * Author: Edo Jelavic 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include "grid_map_pcl/PointcloudProcessor.hpp" 14 | 15 | #include "PointcloudCreator.hpp" 16 | #include "test_helpers.hpp" 17 | 18 | namespace grid_map { 19 | namespace grid_map_pcl_test { 20 | 21 | TEST(PointcloudProcessorTest, ExtractClusters) { 22 | grid_map_pcl_test::setVerbosityLevel(ros::console::levels::Warn); 23 | const auto seed = rand(); 24 | grid_map_pcl_test::rndGenerator.seed(seed); 25 | 26 | const unsigned int numTests = 10; 27 | for (unsigned int i = 0; i < numTests; ++i) { 28 | double minZ, stdDevZ; 29 | int nBlobs; 30 | auto cloud = grid_map_pcl_test::PointcloudCreator::createNBlobsAboveEachOther(&minZ, &stdDevZ, &nBlobs); 31 | grid_map::grid_map_pcl::PointcloudProcessor pointcloudProcessor; 32 | pointcloudProcessor.loadParameters(grid_map_pcl_test::getConfigFilePath()); 33 | auto clusters = pointcloudProcessor.extractClusterCloudsFromPointcloud(cloud); 34 | EXPECT_EQ(clusters.size(), nBlobs); 35 | } 36 | 37 | if (::testing::Test::HasFailure()) { 38 | std::cout << "\n Test ExtractClusters failed with seed: " << seed << std::endl; 39 | } 40 | } 41 | 42 | } /*namespace grid_map_pcl_test */ 43 | } // namespace grid_map 44 | -------------------------------------------------------------------------------- /grid_map_pcl/test/test_data/parameters.yaml: -------------------------------------------------------------------------------- 1 | pcl_grid_map_extraction: 2 | num_processing_threads: 4 3 | cloud_transform: 4 | translation: 5 | x: 0.0 6 | y: 0.0 7 | z: 0.0 8 | rotation: #intrinsic rotation X-Y-Z (r-p-y)sequence 9 | r: 0.0 10 | p: 0.0 11 | y: 0.0 12 | cluster_extraction: 13 | cluster_tolerance: 0.05 14 | min_num_points: 4 15 | max_num_points: 1000000 16 | use_max_height_as_cell_elevation: false 17 | outlier_removal: 18 | is_remove_outliers: true 19 | mean_K: 10 20 | stddev_threshold: 1.0 21 | downsampling: 22 | is_downsample_cloud: true 23 | voxel_size: 24 | x: 0.02 25 | y: 0.02 26 | z: 0.02 27 | grid_map: 28 | min_num_points_per_cell: 4 29 | max_num_points_per_cell: 100000 30 | resolution: 0.1 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /grid_map_pcl/test/test_data/plane_noisy.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_pcl/test/test_data/plane_noisy.pcd -------------------------------------------------------------------------------- /grid_map_pcl/test/test_grid_map_pcl.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_grid_map_pcl.cpp 3 | * 4 | * Created on: Nov 4, 2019 5 | * Author: Edo Jelavic 6 | * Institute: ETH Zurich, Robotic Systems Lab 7 | */ 8 | 9 | // gtest 10 | #include 11 | #include 12 | 13 | int argc; 14 | char** argv; 15 | 16 | // Run all the tests that were declared with TEST() 17 | int main(int _argc, char** _argv) { 18 | ros::Time::init(); 19 | argc = _argc; 20 | argv = _argv; 21 | testing::InitGoogleTest(&_argc, _argv); 22 | srand((int)time(nullptr)); 23 | return RUN_ALL_TESTS(); 24 | } 25 | -------------------------------------------------------------------------------- /grid_map_ros/include/grid_map_ros/grid_map_ros.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * grid_map.hpp 3 | * 4 | * Created on: Jul 14, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | -------------------------------------------------------------------------------- /grid_map_ros/include/grid_map_ros/message_traits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace ros { 7 | namespace message_traits { 8 | 9 | template<> 10 | struct HasHeader : public TrueType {}; 11 | 12 | template <> 13 | struct Header>::type> 14 | { 15 | static std_msgs::Header* pointer(grid_map_msgs::GridMap& m) 16 | { 17 | return &m.info.header; 18 | } 19 | 20 | static std_msgs::Header const* pointer(const grid_map_msgs::GridMap& m) 21 | { 22 | return &m.info.header; 23 | } 24 | }; 25 | 26 | template<> 27 | struct FrameId>::type> 28 | { 29 | static std::string* pointer(grid_map_msgs::GridMap& m) 30 | { 31 | return &m.info.header.frame_id; 32 | } 33 | 34 | static std::string const* pointer(const grid_map_msgs::GridMap& m) 35 | { 36 | return &m.info.header.frame_id; 37 | } 38 | 39 | static std::string value(const grid_map_msgs::GridMap& m) 40 | { 41 | return m.info.header.frame_id; 42 | } 43 | }; 44 | 45 | template<> 46 | struct TimeStamp>::type> 47 | { 48 | static ros::Time* pointer(grid_map_msgs::GridMap& m) 49 | { 50 | return &m.info.header.stamp; 51 | } 52 | 53 | static ros::Time const* pointer(const grid_map_msgs::GridMap& m) 54 | { 55 | return &m.info.header.stamp; 56 | } 57 | 58 | static ros::Time value(const grid_map_msgs::GridMap& m) 59 | { 60 | return m.info.header.stamp; 61 | } 62 | }; 63 | 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /grid_map_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_ros 4 | 1.7.18 5 | ROS interface for the grid map library to manage two-dimensional grid maps with multiple data layers. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/grid_map 10 | http://github.com/anybotics/grid_map/issues 11 | Péter Fankhauser 12 | catkin 13 | 14 | roscpp 15 | grid_map_core 16 | grid_map_msgs 17 | grid_map_cv 18 | grid_map_sdf 19 | sensor_msgs 20 | nav_msgs 21 | std_msgs 22 | geometry_msgs 23 | cv_bridge 24 | rosbag 25 | tf 26 | visualization_msgs 27 | 28 | gtest 29 | 30 | -------------------------------------------------------------------------------- /grid_map_ros/src/GridMapMsgHelpers.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GridMapMsgHelpers.hpp 3 | * 4 | * Created on: Sep 8, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_ros/GridMapMsgHelpers.hpp" 10 | 11 | // Boost 12 | #include 13 | 14 | namespace grid_map { 15 | 16 | int nDimensions() 17 | { 18 | return 2; 19 | } 20 | 21 | std::map storageIndexNames = boost::assign::map_list_of 22 | (StorageIndices::Column, "column_index") 23 | (StorageIndices::Row, "row_index"); 24 | 25 | } /* namespace */ 26 | -------------------------------------------------------------------------------- /grid_map_ros/test/test_grid_map_ros.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_grid_map.cpp 3 | * 4 | * Created on: Feb 10, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | // gtest 10 | #include 11 | 12 | // Run all the tests that were declared with TEST() 13 | int main(int argc, char **argv) 14 | { 15 | testing::InitGoogleTest(&argc, argv); 16 | srand((int)time(0)); 17 | return RUN_ALL_TESTS(); 18 | } 19 | -------------------------------------------------------------------------------- /grid_map_rviz_plugin/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package grid_map_rviz_plugin 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.7.0 (2022-03-17) 6 | ------------------ 7 | 8 | 1.6.4 (2020-12-04) 9 | ------------------ 10 | 11 | 1.6.2 (2019-10-14) 12 | ------------------ 13 | 14 | 1.6.1 (2019-02-27) 15 | ------------------ 16 | * Made FrameManager and MessageFilterDisplay ROS version dependent. 17 | * Update to tf2 MessageFilters 18 | * Updated host changes. 19 | * Updated author e-mail address. 20 | * Contributors: Marco Sütterlin, Peter Fankhauser, Péter Fankhauser 21 | 22 | 1.6.0 (2017-11-24) 23 | ------------------ 24 | * Added option to rviz plugin to hide color faces. 25 | * Fixed bug for RViz plugin. 26 | * Fixing cpp-check warnings and errors. 27 | * Contributors: Péter Fankhauser 28 | 29 | 1.5.2 (2017-07-25) 30 | ------------------ 31 | * Fixes error in grid_map_rviz_plugin when rendering lines. 32 | * Contributors: Peter Fankhauser 33 | 34 | 1.5.1 (2017-07-25) 35 | ------------------ 36 | 37 | 1.5.0 (2017-07-18) 38 | ------------------ 39 | * Improved visualization to support triangles in all directions. 40 | * Fixes transparency issue for grid maps in RViz. (`#68 `_). 41 | * Fixed package XML tag. 42 | * Contributors: Perry Franklin, Stefan Fabian, Peter Fankhauser 43 | 44 | 1.4.2 (2017-01-24) 45 | ------------------ 46 | * Cleanup thanks to message traits. 47 | * Contributors: Peter Fankhauser 48 | 49 | 1.4.1 (2016-10-23) 50 | ------------------ 51 | * Added functionality to display color from grid map layer. 52 | * Added better handling of basic layers in Grid Map RViz plugin. 53 | * Added functionality to invert rainbow colors in RViz plugin. 54 | * Contributors: Philipp Kruesi, Péter Fankhauser 55 | 56 | 1.4.0 (2016-08-22) 57 | ------------------ 58 | * Added new package grid_map_rviz_plugin to visualize grid map layers as 3d surfaces. 59 | * Updated documentation. 60 | * Contributors: Péter Fankhauser, Philipp Kruesi 61 | -------------------------------------------------------------------------------- /grid_map_rviz_plugin/doc/grid_map_rviz_plugin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_rviz_plugin/doc/grid_map_rviz_plugin.png -------------------------------------------------------------------------------- /grid_map_rviz_plugin/doc/grid_map_rviz_plugin_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_rviz_plugin/doc/grid_map_rviz_plugin_example.png -------------------------------------------------------------------------------- /grid_map_rviz_plugin/icons/classes/GridMap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_rviz_plugin/icons/classes/GridMap.png -------------------------------------------------------------------------------- /grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapColorMaps.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GridMapColorMaps.hpp 3 | * 4 | * Created on: Apr 27, 2021 5 | * Author: Matias Mattamala 6 | * Institute: University of Oxford 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace grid_map_rviz_plugin { 18 | 19 | extern std::map>> const colorMap; 20 | 21 | inline Ogre::ColourValue getColorMap(float intensity, const std::vector>& ctable) { 22 | intensity = std::min(intensity, 1.0f); 23 | intensity = std::max(intensity, 0.0f); 24 | 25 | int idx = int(floor(intensity*255)); 26 | idx = std::min(idx, 255); 27 | idx = std::max(idx, 0); 28 | 29 | // Get color from table 30 | std::vector const& rgb = ctable.at(idx); 31 | 32 | Ogre::ColourValue color; 33 | color.r = rgb[0]; 34 | color.g = rgb[1]; 35 | color.b = rgb[2]; 36 | 37 | return color; 38 | } 39 | 40 | inline std::vector getColorMapNames() { 41 | std::vector types; 42 | types.reserve(colorMap.size()); 43 | 44 | for(auto const& pair : colorMap) { 45 | types.push_back(pair.first); 46 | } 47 | 48 | return types; 49 | } 50 | 51 | } // end namespace grid_map_rviz_plugin -------------------------------------------------------------------------------- /grid_map_rviz_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_rviz_plugin 4 | 1.7.18 5 | RViz plugin for displaying grid map messages. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | Philipp Krüsi 10 | Péter Fankhauser 11 | 12 | catkin 13 | 14 | qtbase5-dev 15 | rviz 16 | grid_map_ros 17 | grid_map_msgs 18 | libqt5-core 19 | libqt5-gui 20 | libqt5-widgets 21 | rviz 22 | grid_map_ros 23 | grid_map_msgs 24 | 25 | gtest 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /grid_map_rviz_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | Displays grid_map_msgs/GridMap messages. 6 | grid_map_msgs/GridMap 7 | 8 | 9 | -------------------------------------------------------------------------------- /grid_map_rviz_plugin/test/empty_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | TEST(Placeholder, emptyTest) { // NOLINT 4 | ASSERT_TRUE(true); 5 | } 6 | -------------------------------------------------------------------------------- /grid_map_rviz_plugin/test/test_grid_map_rviz_plugin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_grid_map.cpp 3 | * 4 | * Created on: Feb 10, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | // gtest 10 | #include 11 | 12 | // Run all the tests that were declared with TEST() 13 | int main(int argc, char **argv) 14 | { 15 | testing::InitGoogleTest(&argc, argv); 16 | srand((int)time(0)); 17 | return RUN_ALL_TESTS(); 18 | } 19 | -------------------------------------------------------------------------------- /grid_map_sdf/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package grid_map_sdf 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.7.0 (2022-03-17) 6 | ------------------ 7 | * Release of new implementation to convert grid maps to signed distance fields. 8 | * Contributors: Ruben Grandia 9 | 10 | 1.6.4 (2020-12-04) 11 | ------------------ 12 | 13 | 1.6.2 (2019-10-14) 14 | ------------------ 15 | 16 | 1.6.1 (2019-02-27) 17 | ------------------ 18 | * Updated host changes. 19 | * Updated author e-mail address. 20 | * Contributors: Peter Fankhauser, Péter Fankhauser 21 | 22 | 1.6.0 (2017-11-24) 23 | ------------------ 24 | * Release of new implementation to convert grid maps to signed distance fields. 25 | * Contributors: Takahiro Miki, Péter Fankhauser 26 | 27 | 1.5.2 (2017-07-25 19:54) 28 | ------------------------ 29 | 30 | 1.5.1 (2017-07-25 11:25) 31 | ------------------------ 32 | 33 | 1.5.0 (2017-07-18) 34 | ------------------ 35 | 36 | 1.4.2 (2017-01-24) 37 | ------------------ 38 | 39 | 1.4.1 (2016-10-23) 40 | ------------------ 41 | 42 | 1.4.0 (2016-08-22) 43 | ------------------ 44 | 45 | 1.3.3 (2016-05-10 20:35) 46 | ------------------------ 47 | 48 | 1.3.1 (2016-05-10 16:41) 49 | ------------------------ 50 | 51 | 1.3.0 (2016-04-26) 52 | ------------------ 53 | 54 | 1.2.0 (2016-03-03) 55 | ------------------ 56 | 57 | 1.1.3 (2016-01-11 17:55:26 +0100) 58 | --------------------------------- 59 | 60 | 1.1.2 (2016-01-11 16:38) 61 | ------------------------ 62 | 63 | 1.1.1 (2016-01-11 13:51) 64 | ------------------------ 65 | 66 | 1.1.0 (2016-01-08) 67 | ------------------ 68 | -------------------------------------------------------------------------------- /grid_map_sdf/doc/anymal_sdf_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_sdf/doc/anymal_sdf_demo.gif -------------------------------------------------------------------------------- /grid_map_sdf/include/grid_map_sdf/DistanceDerivatives.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * DistanceDerivatives.h 3 | * 4 | * Created on: Aug 10, 2020 5 | * Author: Ruben Grandia 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | namespace grid_map { 14 | namespace signed_distance_field { 15 | 16 | /** 17 | * Takes the columnwise central difference of a matrix. Uses single sided difference at the boundaries. 18 | * diff = (col(i+1) - col(i-1)) / (2 * resolution) 19 | * 20 | * @param data [in] : data to take the difference of. 21 | * @param centralDifference [out] : matrix to store the result in. 22 | * @param resolution [in] : scaling of the distance between two columns. 23 | */ 24 | inline void columnwiseCentralDifference(const Matrix& data, Matrix& centralDifference, float resolution) { 25 | assert(data.cols() >= 2); // Minimum size to take finite differences. 26 | 27 | const Eigen::Index m{data.cols()}; 28 | const float resInv{1.0F / resolution}; 29 | const float doubleResInv{1.0F / (2.0F * resolution)}; 30 | 31 | // First column 32 | centralDifference.col(0) = resInv * (data.col(1) - data.col(0)); 33 | 34 | // All the middle columns 35 | for (Eigen::Index i = 1; i + 1 < m; ++i) { 36 | centralDifference.col(i) = doubleResInv * (data.col(i + 1) - data.col(i - 1)); 37 | } 38 | 39 | // Last column 40 | centralDifference.col(m - 1) = resInv * (data.col(m - 1) - data.col(m - 2)); 41 | } 42 | 43 | /** 44 | * Takes the finite difference between layers 45 | * result = (data_{k+1} - data{k}) / resolution 46 | */ 47 | inline void layerFiniteDifference(const Matrix& data_k, const Matrix& data_kp1, Matrix& result, float resolution) { 48 | const float resInv{1.0F / resolution}; 49 | result = resInv * (data_kp1 - data_k); 50 | } 51 | 52 | /** 53 | * Takes the central difference between layers 54 | * result = (data_{k+1} - data{k-1}) / (2.0 * resolution) 55 | */ 56 | inline void layerCentralDifference(const Matrix& data_km1, const Matrix& data_kp1, Matrix& result, float resolution) { 57 | const float doubleResInv{1.0F / (2.0F * resolution)}; 58 | result = doubleResInv * (data_kp1 - data_km1); 59 | } 60 | 61 | } // namespace signed_distance_field 62 | } // namespace grid_map -------------------------------------------------------------------------------- /grid_map_sdf/include/grid_map_sdf/Utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Utils.h 3 | * 4 | * Created on: Jul 10, 2020 5 | * Author: Ruben Grandia 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #pragma once 10 | 11 | namespace grid_map { 12 | namespace signed_distance_field { 13 | 14 | // Check existence of inf 15 | static_assert(std::numeric_limits::has_infinity, "float does not support infinity"); 16 | 17 | //! Distance value that is considered infinite. 18 | constexpr float INF = std::numeric_limits::infinity(); 19 | 20 | } // namespace signed_distance_field 21 | } // namespace grid_map -------------------------------------------------------------------------------- /grid_map_sdf/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_sdf 4 | 1.7.18 5 | Generates signed distance fields from grid maps. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/grid_map 10 | http://github.com/anybotics/grid_map/issues 11 | Takahiro Miki 12 | Péter Fankhauser 13 | catkin 14 | 15 | grid_map_core 16 | 17 | gtest 18 | 19 | -------------------------------------------------------------------------------- /grid_map_sdf/test/test3dLookup.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test3dLookup.cpp 3 | * 4 | * Created on: Aug 18, 2020 5 | * Author: Ruben Grandia 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #include 10 | 11 | #include "grid_map_sdf/Gridmap3dLookup.hpp" 12 | 13 | using namespace grid_map; 14 | using namespace signed_distance_field; 15 | using size_t_3d = Gridmap3dLookup::size_t_3d; 16 | 17 | TEST(testGridmap3dLookup, nearestNode) { 18 | const size_t_3d gridsize{8, 9, 10}; 19 | const Position3 gridOrigin{-0.1, -0.2, -0.4}; 20 | const double resolution = 0.1; 21 | 22 | Gridmap3dLookup gridmap3DLookup(gridsize, gridOrigin, resolution); 23 | 24 | // Retrieve origin 25 | const size_t_3d originNode = gridmap3DLookup.nearestNode(gridOrigin); 26 | ASSERT_EQ(originNode.x, 0); 27 | ASSERT_EQ(originNode.y, 0); 28 | ASSERT_EQ(originNode.z, 0); 29 | 30 | // test underflow 31 | const size_t_3d originNodeProjected = gridmap3DLookup.nearestNode(gridOrigin + Position3{1e20, 1e20, -1e20}); 32 | ASSERT_EQ(originNodeProjected.x, 0); 33 | ASSERT_EQ(originNodeProjected.y, 0); 34 | ASSERT_EQ(originNodeProjected.z, 0); 35 | 36 | // test overflow 37 | const size_t_3d maxNodeProjected = gridmap3DLookup.nearestNode(gridOrigin + Position3{-1e20, -1e20, +1e20}); 38 | ASSERT_EQ(maxNodeProjected.x, gridsize.x - 1); 39 | ASSERT_EQ(maxNodeProjected.y, gridsize.y - 1); 40 | ASSERT_EQ(maxNodeProjected.z, gridsize.z - 1); 41 | 42 | // Nearest neighbour 43 | const size_t_3d nodeIndex{3, 4, 5}; 44 | const Position3 nodePosition = gridmap3DLookup.nodePosition(nodeIndex); 45 | const size_t_3d closestNodeIndex = gridmap3DLookup.nearestNode(nodePosition + 0.49 * Position3::Constant(resolution)); 46 | ASSERT_EQ(closestNodeIndex.x, nodeIndex.x); 47 | ASSERT_EQ(closestNodeIndex.y, nodeIndex.y); 48 | ASSERT_EQ(closestNodeIndex.z, nodeIndex.z); 49 | } 50 | 51 | TEST(testGridmap3dLookup, linearIndex) { 52 | const size_t_3d gridsize{8, 9, 10}; 53 | const Position3 gridOrigin{-0.1, -0.2, -0.4}; 54 | const double resolution = 0.1; 55 | 56 | Gridmap3dLookup gridmap3DLookup(gridsize, gridOrigin, resolution); 57 | ASSERT_EQ(gridmap3DLookup.linearIndex({0, 0, 0}), 0); 58 | ASSERT_EQ(gridmap3DLookup.linearIndex({gridsize.x - 1, gridsize.y - 1, gridsize.z - 1}), gridmap3DLookup.linearSize() - 1); 59 | } -------------------------------------------------------------------------------- /grid_map_sdf/test/testDerivatives.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * testDerivatives.cpp 3 | * 4 | * Created on: Aug 10, 2020 5 | * Author: Ruben Grandia 6 | * Institute: ETH Zurich 7 | */ 8 | 9 | #include 10 | 11 | #include "grid_map_sdf/DistanceDerivatives.hpp" 12 | 13 | using namespace grid_map; 14 | using namespace signed_distance_field; 15 | 16 | TEST(testDerivatives, columnwise) { 17 | Matrix data(2, 3); 18 | data << 1.0, 2.0, 4.0, 2.0, 4.0, 6.0; 19 | 20 | float resolution{0.1F}; 21 | float doubleResolution{2.0F * resolution}; 22 | 23 | Matrix manualDifference(2, 3); 24 | manualDifference << 1.0 / resolution, 3.0 / doubleResolution, 2.0 / resolution, 2.0 / resolution, 4.0 / doubleResolution, 25 | 2.0 / resolution; 26 | 27 | Matrix computedDifference = Matrix::Zero(data.rows(), data.cols()); 28 | columnwiseCentralDifference(data, computedDifference, resolution); 29 | 30 | ASSERT_TRUE(manualDifference.isApprox(computedDifference)); 31 | } 32 | -------------------------------------------------------------------------------- /grid_map_sdf/test/test_grid_map_sdf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_grid_map_sdf.cpp 3 | * 4 | * Created on: Aug 28, 2017 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | // gtest 10 | #include 11 | 12 | // Run all the tests that were declared with TEST() 13 | int main(int argc, char **argv) 14 | { 15 | testing::InitGoogleTest(&argc, argv); 16 | srand((int)time(0)); 17 | return RUN_ALL_TESTS(); 18 | } 19 | -------------------------------------------------------------------------------- /grid_map_visualization/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package grid_map_visualization 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.7.0 (2022-03-17) 6 | ------------------ 7 | 8 | 1.6.4 (2020-12-04) 9 | ------------------ 10 | 11 | 1.6.2 (2019-10-14) 12 | ------------------ 13 | 14 | 1.6.1 (2019-02-27) 15 | ------------------ 16 | * Updated host changes. 17 | * Updated author e-mail address. 18 | * Contributors: Peter Fankhauser, Péter Fankhauser 19 | 20 | 1.6.0 (2017-11-24) 21 | ------------------ 22 | 23 | 1.5.2 (2017-07-25) 24 | ------------------ 25 | 26 | 1.5.1 (2017-07-25) 27 | ------------------ 28 | 29 | 1.5.0 (2017-07-18) 30 | ------------------ 31 | 32 | 1.4.2 (2017-01-24) 33 | ------------------ 34 | 35 | 1.4.1 (2016-10-23) 36 | ------------------ 37 | 38 | 1.4.0 (2016-08-22) 39 | ------------------ 40 | 41 | 1.3.3 (2016-05-10) 42 | ------------------ 43 | * Release for ROS Kinetic. 44 | * Contributors: Peter Fankhauser 45 | 46 | 1.3.2 (2016-05-10) 47 | ------------------ 48 | 49 | 1.3.1 (2016-05-10) 50 | ------------------ 51 | 52 | 1.3.0 (2016-04-26) 53 | ------------------ 54 | * Added new visualizer for flat point clouds and updated documentation and demos. 55 | * Contributors: Peter Fankhauser, Daniel Stonier 56 | 57 | 1.2.0 (2016-03-03) 58 | ------------------ 59 | * Changed the package name from `grid_map` to `grid_map_ros` and made `grid_map` a metapackage (`#34 `_). 60 | * Contributors: Peter Fankhauser 61 | 62 | 1.1.3 (2016-01-11) 63 | ------------------ 64 | * Removed unnecessary include of filter_base. 65 | 66 | 1.1.2 (2016-01-11) 67 | ------------------ 68 | * Should fix errors on build server regarding Eigen3 and visualization_msgs dependencies. 69 | 70 | 1.1.1 (2016-01-11) 71 | ------------------ 72 | * Changes to CMakeLists.txt to enable compatibility with Ubuntu Saucy. 73 | 74 | 1.1.0 (2016-01-08) 75 | ------------------- 76 | * added installation instructions in CMakeLists 77 | * general improvements and bugfixes 78 | 79 | 1.0.0 (2015-11-20) 80 | ------------------- 81 | * release for Springer ROS Book Chapter 82 | -------------------------------------------------------------------------------- /grid_map_visualization/doc/grid_cells.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_visualization/doc/grid_cells.jpg -------------------------------------------------------------------------------- /grid_map_visualization/doc/grid_cells_preview.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_visualization/doc/grid_cells_preview.jpg -------------------------------------------------------------------------------- /grid_map_visualization/doc/occupancy_grid.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_visualization/doc/occupancy_grid.jpg -------------------------------------------------------------------------------- /grid_map_visualization/doc/occupancy_grid_preview.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_visualization/doc/occupancy_grid_preview.jpg -------------------------------------------------------------------------------- /grid_map_visualization/doc/point_cloud.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_visualization/doc/point_cloud.jpg -------------------------------------------------------------------------------- /grid_map_visualization/doc/point_cloud_preview.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_visualization/doc/point_cloud_preview.jpg -------------------------------------------------------------------------------- /grid_map_visualization/doc/vectors.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_visualization/doc/vectors.jpg -------------------------------------------------------------------------------- /grid_map_visualization/doc/vectors_preview.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ANYbotics/grid_map/73a5b61fc80c09be8a5744997b3ee3e6902a98fb/grid_map_visualization/doc/vectors_preview.jpg -------------------------------------------------------------------------------- /grid_map_visualization/include/grid_map_visualization/visualizations/FlatPointCloudVisualization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FlatPointCloudVisualization.hpp 3 | * 4 | * Created on: Mar 9, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | // ROS 15 | #include 16 | 17 | namespace grid_map_visualization { 18 | 19 | /*! 20 | * Visualization of the grid map as a flat point cloud. 21 | */ 22 | class FlatPointCloudVisualization : public VisualizationBase 23 | { 24 | public: 25 | 26 | /*! 27 | * Constructor. 28 | * @param nodeHandle the ROS node handle. 29 | * @param name the name of the visualization. 30 | */ 31 | FlatPointCloudVisualization(ros::NodeHandle& nodeHandle, const std::string& name); 32 | 33 | /*! 34 | * Destructor. 35 | */ 36 | virtual ~FlatPointCloudVisualization(); 37 | 38 | /*! 39 | * Read parameters from ROS. 40 | * @param config the parameters as XML. 41 | * @return true if successful. 42 | */ 43 | bool readParameters(XmlRpc::XmlRpcValue& config); 44 | 45 | /*! 46 | * Initialization. 47 | */ 48 | bool initialize(); 49 | 50 | /*! 51 | * Generates the visualization. 52 | * @param map the grid map to visualize. 53 | * @return true if successful. 54 | */ 55 | bool visualize(const grid_map::GridMap& map); 56 | 57 | private: 58 | //! Type that is transformed to points. 59 | std::string layer_; 60 | 61 | //! Height of the z-coordinate at which the flat point cloud is visualized. 62 | double height_; 63 | }; 64 | 65 | } /* namespace */ 66 | -------------------------------------------------------------------------------- /grid_map_visualization/include/grid_map_visualization/visualizations/GridCellsVisualization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * GridCellsVisualization.hpp 3 | * 4 | * Created on: Mar 28, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | // ROS 15 | #include 16 | 17 | namespace grid_map_visualization { 18 | 19 | class GridCellsVisualization : public VisualizationBase 20 | { 21 | public: 22 | 23 | /*! 24 | * Constructor. 25 | * @param nodeHandle the ROS node handle. 26 | * @param name the name of the visualization. 27 | */ 28 | GridCellsVisualization(ros::NodeHandle& nodeHandle, const std::string& name); 29 | 30 | /*! 31 | * Destructor. 32 | */ 33 | virtual ~GridCellsVisualization(); 34 | 35 | /*! 36 | * Read parameters from ROS. 37 | * @param config the parameters as XML. 38 | * @return true if successful. 39 | */ 40 | bool readParameters(XmlRpc::XmlRpcValue& config); 41 | 42 | /*! 43 | * Initialization. 44 | */ 45 | bool initialize(); 46 | 47 | /*! 48 | * Generates the visualization. 49 | * @param map the grid map to visualize. 50 | * @return true if successful. 51 | */ 52 | bool visualize(const grid_map::GridMap& map); 53 | 54 | private: 55 | 56 | //! Type that is transformed to the occupancy grid. 57 | std::string layer_; 58 | 59 | //! Values that are between lower and upper threshold are shown. 60 | float lowerThreshold_, upperThreshold_; 61 | }; 62 | 63 | } /* namespace */ 64 | -------------------------------------------------------------------------------- /grid_map_visualization/include/grid_map_visualization/visualizations/MapRegionVisualization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MapRegionVisualization.hpp 3 | * 4 | * Created on: Jun 18, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | // ROS 15 | #include 16 | #include 17 | #include 18 | 19 | namespace grid_map_visualization { 20 | 21 | /*! 22 | * Visualization of the region of the grid map as border line. 23 | */ 24 | class MapRegionVisualization : public VisualizationBase 25 | { 26 | public: 27 | 28 | /*! 29 | * Constructor. 30 | * @param nodeHandle the ROS node handle. 31 | * @param name the name of the visualization. 32 | */ 33 | MapRegionVisualization(ros::NodeHandle& nodeHandle, const std::string& name); 34 | 35 | /*! 36 | * Destructor. 37 | */ 38 | virtual ~MapRegionVisualization(); 39 | 40 | /*! 41 | * Read parameters from ROS. 42 | * @param config the parameters as XML. 43 | * @return true if successful. 44 | */ 45 | bool readParameters(XmlRpc::XmlRpcValue& config); 46 | 47 | /*! 48 | * Initialization. 49 | */ 50 | bool initialize(); 51 | 52 | /*! 53 | * Generates the visualization. 54 | * @param map the grid map to visualize. 55 | * @return true if successful. 56 | */ 57 | bool visualize(const grid_map::GridMap& map); 58 | 59 | private: 60 | 61 | //! Marker to be published. 62 | visualization_msgs::Marker marker_; 63 | 64 | //! Number of vertices of the map region visualization. 65 | const unsigned int nVertices_; 66 | 67 | //! Color of the map region visualization. 68 | std_msgs::ColorRGBA color_; 69 | 70 | //! Line width of the map region marker [m]. 71 | double lineWidth_; 72 | 73 | }; 74 | 75 | } /* namespace */ 76 | -------------------------------------------------------------------------------- /grid_map_visualization/include/grid_map_visualization/visualizations/OccupancyGridVisualization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PointCloudOccupancyGrid.hpp 3 | * 4 | * Created on: Nov 3, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | // ROS 15 | #include 16 | 17 | namespace grid_map_visualization { 18 | 19 | class OccupancyGridVisualization : public VisualizationBase 20 | { 21 | public: 22 | 23 | /*! 24 | * Constructor. 25 | * @param nodeHandle the ROS node handle. 26 | * @param name the name of the visualization. 27 | */ 28 | OccupancyGridVisualization(ros::NodeHandle& nodeHandle, const std::string& name); 29 | 30 | /*! 31 | * Destructor. 32 | */ 33 | virtual ~OccupancyGridVisualization(); 34 | 35 | /*! 36 | * Read parameters from ROS. 37 | * @param config the parameters as XML. 38 | * @return true if successful. 39 | */ 40 | bool readParameters(XmlRpc::XmlRpcValue& config); 41 | 42 | /*! 43 | * Initialization. 44 | */ 45 | bool initialize(); 46 | 47 | /*! 48 | * Generates the visualization. 49 | * @param map the grid map to visualize. 50 | * @return true if successful. 51 | */ 52 | bool visualize(const grid_map::GridMap& map); 53 | 54 | private: 55 | 56 | //! Type that is transformed to the occupancy grid. 57 | std::string layer_; 58 | 59 | //! Minimum and maximum value of the grid map data (used to normalize the cell data in [min, max]). 60 | float dataMin_, dataMax_; 61 | }; 62 | 63 | } /* namespace */ 64 | -------------------------------------------------------------------------------- /grid_map_visualization/include/grid_map_visualization/visualizations/PointCloudVisualization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PointCloudVisualization.hpp 3 | * 4 | * Created on: Sep 11, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | // ROS 15 | #include 16 | 17 | namespace grid_map_visualization { 18 | 19 | /*! 20 | * Visualization of the grid map as a point cloud. 21 | */ 22 | class PointCloudVisualization : public VisualizationBase 23 | { 24 | public: 25 | 26 | /*! 27 | * Constructor. 28 | * @param nodeHandle the ROS node handle. 29 | * @param name the name of the visualization. 30 | */ 31 | PointCloudVisualization(ros::NodeHandle& nodeHandle, const std::string& name); 32 | 33 | /*! 34 | * Destructor. 35 | */ 36 | virtual ~PointCloudVisualization(); 37 | 38 | /*! 39 | * Read parameters from ROS. 40 | * @param config the parameters as XML. 41 | * @return true if successful. 42 | */ 43 | bool readParameters(XmlRpc::XmlRpcValue& config); 44 | 45 | /*! 46 | * Initialization. 47 | */ 48 | bool initialize(); 49 | 50 | /*! 51 | * Generates the visualization. 52 | * @param map the grid map to visualize. 53 | * @return true if successful. 54 | */ 55 | bool visualize(const grid_map::GridMap& map); 56 | 57 | private: 58 | //! Type that is transformed to points. 59 | std::string layer_; 60 | }; 61 | 62 | } /* namespace */ 63 | -------------------------------------------------------------------------------- /grid_map_visualization/include/grid_map_visualization/visualizations/VectorVisualization.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VectorVisualization.hpp 3 | * 4 | * Created on: Sep 16, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | // ROS 15 | #include 16 | #include 17 | #include 18 | 19 | // STD 20 | #include 21 | 22 | namespace grid_map_visualization { 23 | 24 | /*! 25 | * Visualization a combination of three layers of the grid map as a vector field. 26 | */ 27 | class VectorVisualization : public VisualizationBase 28 | { 29 | public: 30 | 31 | /*! 32 | * Constructor. 33 | * @param nodeHandle the ROS node handle. 34 | * @param name the name of the visualization. 35 | */ 36 | VectorVisualization(ros::NodeHandle& nodeHandle, const std::string& name); 37 | 38 | /*! 39 | * Destructor. 40 | */ 41 | virtual ~VectorVisualization(); 42 | 43 | /*! 44 | * Read parameters from ROS. 45 | * @param config the parameters as XML. 46 | * @return true if successful. 47 | */ 48 | bool readParameters(XmlRpc::XmlRpcValue& config); 49 | 50 | /*! 51 | * Initialization. 52 | */ 53 | bool initialize(); 54 | 55 | /*! 56 | * Generates the visualization. 57 | * @param map the grid map to visualize. 58 | * @return true if successful. 59 | */ 60 | bool visualize(const grid_map::GridMap& map); 61 | 62 | private: 63 | 64 | //! Marker to be published. 65 | visualization_msgs::Marker marker_; 66 | 67 | //! Types that are transformed to vectors. 68 | std::vector types_; 69 | 70 | //! Type that is the position of the vectors. 71 | std::string positionLayer_; 72 | 73 | //! Scaling of the vectors. 74 | double scale_; 75 | 76 | //! Width of the line markers [m]. 77 | double lineWidth_; 78 | 79 | //! Color of the vectors. 80 | std_msgs::ColorRGBA color_; 81 | }; 82 | 83 | } /* namespace */ 84 | -------------------------------------------------------------------------------- /grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationFactory.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VisualizationFactory.hpp 3 | * 4 | * Created on: Mar 20, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace grid_map_visualization { 17 | 18 | class VisualizationFactory 19 | { 20 | public: 21 | VisualizationFactory(ros::NodeHandle& nodeHandle); 22 | virtual ~VisualizationFactory(); 23 | 24 | bool isValidType(const std::string& type); 25 | std::shared_ptr getInstance(const std::string& type, const std::string& name); 26 | 27 | private: 28 | ros::NodeHandle& nodeHandle_; 29 | std::vector types_; 30 | }; 31 | 32 | } /* namespace */ 33 | -------------------------------------------------------------------------------- /grid_map_visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_map_visualization 4 | 1.7.18 5 | Configurable tool to visualize grid maps in RViz. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/grid_map 10 | http://github.com/anybotics/grid_map/issues 11 | Péter Fankhauser 12 | catkin 13 | 14 | roscpp 15 | grid_map_core 16 | grid_map_ros 17 | grid_map_msgs 18 | tf 19 | visualization_msgs 20 | sensor_msgs 21 | nav_msgs 22 | 23 | gtest 24 | 25 | -------------------------------------------------------------------------------- /grid_map_visualization/src/grid_map_visualization_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * grid_map_visualization_node.cpp 3 | * 4 | * Created on: Oct 3, 2013 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include 10 | #include "grid_map_visualization/GridMapVisualization.hpp" 11 | 12 | int main(int argc, char** argv) 13 | { 14 | ros::init(argc, argv, "grid_map_visualization"); 15 | 16 | ros::NodeHandle nodeHandle("~"); 17 | 18 | grid_map_visualization::GridMapVisualization gridMapVisualization(nodeHandle, "grid_map_visualizations"); 19 | 20 | ros::spin(); 21 | return 0; 22 | } 23 | -------------------------------------------------------------------------------- /grid_map_visualization/src/visualizations/FlatPointCloudVisualization.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * FlatPointCloudVisualization.cpp 3 | * 4 | * Created on: Mar 9, 2016 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace grid_map_visualization { 15 | 16 | FlatPointCloudVisualization::FlatPointCloudVisualization(ros::NodeHandle& nodeHandle, const std::string& name) 17 | : VisualizationBase(nodeHandle, name), 18 | height_(0.0) 19 | { 20 | } 21 | 22 | FlatPointCloudVisualization::~FlatPointCloudVisualization() 23 | { 24 | } 25 | 26 | bool FlatPointCloudVisualization::readParameters(XmlRpc::XmlRpcValue& config) 27 | { 28 | VisualizationBase::readParameters(config); 29 | 30 | height_ = 0.0; 31 | if (!getParam("height", height_)) { 32 | ROS_INFO("FlatPointCloudVisualization with name '%s' did not find a 'height' parameter. Using default.", name_.c_str()); 33 | } 34 | 35 | return true; 36 | } 37 | 38 | bool FlatPointCloudVisualization::initialize() 39 | { 40 | publisher_ = nodeHandle_.advertise(name_, 1, true); 41 | return true; 42 | } 43 | 44 | bool FlatPointCloudVisualization::visualize(const grid_map::GridMap& map) 45 | { 46 | if (!isActive()) return true; 47 | sensor_msgs::PointCloud2 pointCloud; 48 | 49 | grid_map::GridMap mapCopy(map); 50 | mapCopy.add("flat", height_); 51 | grid_map::GridMapRosConverter::toPointCloud(mapCopy, "flat", pointCloud); 52 | 53 | publisher_.publish(pointCloud); 54 | return true; 55 | } 56 | 57 | } /* namespace */ 58 | -------------------------------------------------------------------------------- /grid_map_visualization/src/visualizations/OccupancyGridVisualization.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OccupancyGridVisualization.cpp 3 | * 4 | * Created on: Nov 3, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "grid_map_visualization/visualizations/OccupancyGridVisualization.hpp" 10 | #include 11 | #include 12 | 13 | namespace grid_map_visualization { 14 | 15 | OccupancyGridVisualization::OccupancyGridVisualization(ros::NodeHandle& nodeHandle, const std::string& name) 16 | : VisualizationBase(nodeHandle, name), 17 | dataMin_(0.0), 18 | dataMax_(1.0) 19 | { 20 | } 21 | 22 | OccupancyGridVisualization::~OccupancyGridVisualization() 23 | { 24 | } 25 | 26 | bool OccupancyGridVisualization::readParameters(XmlRpc::XmlRpcValue& config) 27 | { 28 | VisualizationBase::readParameters(config); 29 | 30 | if (!getParam("layer", layer_)) { 31 | ROS_ERROR("OccupancyGridVisualization with name '%s' did not find a 'layer' parameter.", name_.c_str()); 32 | return false; 33 | } 34 | 35 | if (!getParam("data_min", dataMin_)) { 36 | ROS_ERROR("OccupancyGridVisualization with name '%s' did not find a 'data_min' parameter.", name_.c_str()); 37 | return false; 38 | } 39 | 40 | if (!getParam("data_max", dataMax_)) { 41 | ROS_ERROR("OccupancyGridVisualization with name '%s' did not find a 'data_max' parameter.", name_.c_str()); 42 | return false; 43 | } 44 | 45 | return true; 46 | } 47 | 48 | bool OccupancyGridVisualization::initialize() 49 | { 50 | publisher_ = nodeHandle_.advertise(name_, 1, true); 51 | return true; 52 | } 53 | 54 | bool OccupancyGridVisualization::visualize(const grid_map::GridMap& map) 55 | { 56 | if (!isActive()) return true; 57 | if (!map.exists(layer_)) { 58 | ROS_WARN_STREAM("OccupancyGridVisualization::visualize: No grid map layer with name '" << layer_ << "' found."); 59 | return false; 60 | } 61 | nav_msgs::OccupancyGrid occupancyGrid; 62 | grid_map::GridMapRosConverter::toOccupancyGrid(map, layer_, dataMin_, dataMax_, occupancyGrid); 63 | publisher_.publish(occupancyGrid); 64 | return true; 65 | } 66 | 67 | } /* namespace */ 68 | -------------------------------------------------------------------------------- /grid_map_visualization/src/visualizations/PointCloudVisualization.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PointCloudVisualization.cpp 3 | * 4 | * Created on: Sep 11, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace grid_map_visualization { 15 | 16 | PointCloudVisualization::PointCloudVisualization(ros::NodeHandle& nodeHandle, const std::string& name) 17 | : VisualizationBase(nodeHandle, name) 18 | { 19 | } 20 | 21 | PointCloudVisualization::~PointCloudVisualization() 22 | { 23 | } 24 | 25 | bool PointCloudVisualization::readParameters(XmlRpc::XmlRpcValue& config) 26 | { 27 | VisualizationBase::readParameters(config); 28 | if (!getParam("layer", layer_)) { 29 | ROS_ERROR("PointCloudVisualization with name '%s' did not find a 'layer' parameter.", name_.c_str()); 30 | return false; 31 | } 32 | return true; 33 | } 34 | 35 | bool PointCloudVisualization::initialize() 36 | { 37 | publisher_ = nodeHandle_.advertise(name_, 1, true); 38 | return true; 39 | } 40 | 41 | bool PointCloudVisualization::visualize(const grid_map::GridMap& map) 42 | { 43 | if (!isActive()) return true; 44 | if (!map.exists(layer_)) { 45 | ROS_WARN_STREAM("PointCloudVisualization::visualize: No grid map layer with name '" << layer_ << "' found."); 46 | return false; 47 | } 48 | sensor_msgs::PointCloud2 pointCloud; 49 | grid_map::GridMapRosConverter::toPointCloud(map, layer_, pointCloud); 50 | publisher_.publish(pointCloud); 51 | return true; 52 | } 53 | 54 | } /* namespace */ 55 | -------------------------------------------------------------------------------- /grid_map_visualization/test/empty_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | TEST(Placeholder, emptyTest) { // NOLINT 4 | ASSERT_TRUE(true); 5 | } 6 | -------------------------------------------------------------------------------- /grid_map_visualization/test/test_grid_map_visualization.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_grid_map.cpp 3 | * 4 | * Created on: Feb 10, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | // gtest 10 | #include 11 | 12 | // Run all the tests that were declared with TEST() 13 | int main(int argc, char **argv) 14 | { 15 | testing::InitGoogleTest(&argc, argv); 16 | srand((int)time(0)); 17 | return RUN_ALL_TESTS(); 18 | } 19 | --------------------------------------------------------------------------------