diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 000000000..b7972978c --- /dev/null +++ b/.dockerignore @@ -0,0 +1,5 @@ +.circleci/ +.github/ +.gitignore/ +LICENSE/ +README.md diff --git a/.github/.mergify.yml b/.github/.mergify.yml new file mode 100644 index 000000000..1ba3b6a7a --- /dev/null +++ b/.github/.mergify.yml @@ -0,0 +1,36 @@ +--- +pull_request_rules: + - name: backport to iron at reviewers discretion + conditions: + - base=rolling + - label=backport-iron + actions: + backport: + branches: + - iron + + - name: backport to humble at reviewers discretion + conditions: + - base=rolling + - label=backport-humble + actions: + backport: + branches: + - humble + + - name: delete head branch after merge + conditions: + - merged + actions: + delete_head_branch: + + - name: ask to resolve conflict + conditions: + - conflict + - author!=mergify + actions: + comment: + message: This pull request is in conflict. Could you fix it @{{author}}? + +# TODO enable automatic merge of backports +# https://docs.mergify.com/workflow/actions/backport/#combining-automatic-merge diff --git a/.github/workflows/colcon.yaml b/.github/workflows/colcon.yaml new file mode 100644 index 000000000..c923ad037 --- /dev/null +++ b/.github/workflows/colcon.yaml @@ -0,0 +1,51 @@ +name: Colcon Build Test +on: + push: + branches: + - 'rolling' + - 'jazzy' + - 'iron' + - 'humble' + pull_request: + branches: + - '*' +jobs: + build-test: + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + matrix: + config: + - {rosdistro: 'jazzy', container: 'ros:jazzy'} + container: ${{ matrix.config.container }} + steps: + - uses: actions/checkout@v4 + with: + path: src/grid_map + - name: Pull in repos with vcs + run: | + apt-get update + vcs import --shallow --skip-existing --input grid_map/tools/ros2_dependencies.repos + shell: bash + working-directory: src + if: ${{ matrix.config.rosdistro == 'rolling' }} + - name: Install Dependencies with Rosdep + run: | + apt update + rosdep update + source /opt/ros/${{matrix.config.rosdistro}}/setup.bash + rosdep install --from-paths src --ignore-src -y --skip-keys "slam_toolbox turtlebot3_gazebo gazebo_ros_pkgs octomap_server" + shell: bash + - name: Colcon Build (Release) + run: | + source /opt/ros/${{matrix.config.rosdistro}}/setup.bash + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-up-to grid_map + shell: bash + - name: Test + run: | + source /opt/ros/${{matrix.config.rosdistro}}/setup.bash + source install/setup.bash + colcon test --paths src/grid_map/* --event-handlers=console_cohesion+ + colcon test-result --all --verbose + shell: bash + diff --git a/.github/workflows/industrial-ci.yaml b/.github/workflows/industrial-ci.yaml new file mode 100644 index 000000000..19aacbeb5 --- /dev/null +++ b/.github/workflows/industrial-ci.yaml @@ -0,0 +1,36 @@ +name: Industrial CI + +# Use industrial CI to ensure our packages express all dependencies. +# Attempts to find issues like #490 before merge. +# https://github.com/ros-industrial/industrial_ci?tab=readme-ov-file#for-github-actions + +on: + push: + branches: + - 'jazzy' + pull_request: + branches: + - '*' + +jobs: + industrial_ci: + strategy: + matrix: + env: + - {ROS_DISTRO: jazzy, ROS_REPO: testing} + - {ROS_DISTRO: jazzy, ROS_REPO: main} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + # nav2 on rolling is not available with binaries + - name: Set UPSTREAM_WORKSPACE to bring in nav2 as a source dependency + if: ${{ matrix.env.ROS_DISTRO == 'rolling' }} + run: echo "UPSTREAM_WORKSPACE=https://raw.githubusercontent.com/ANYbotics/grid_map/rolling/tools/ros2_dependencies.repos" >> $GITHUB_ENV + # We duplicate NAV2's key skipping because I doubt they intend this bespoke file to be used by other CI jobs: + # https://github.com/ros-navigation/navigation2/blob/main/tools/skip_keys.txt + # It also is out of date, so grid_map is just going to skip keys we are sure don't work. + - name: set ROSDEP_SKIP_KEYS to skip nav2 keys + if: ${{ matrix.env.ROS_DISTRO == 'rolling' }} + run: echo "ROSDEP_SKIP_KEYS=slam_toolbox" >> $GITHUB_ENV + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} diff --git a/.github/workflows/mirror-rolling-to-ros2.yaml b/.github/workflows/mirror-rolling-to-ros2.yaml new file mode 100644 index 000000000..fa4e69bf3 --- /dev/null +++ b/.github/workflows/mirror-rolling-to-ros2.yaml @@ -0,0 +1,13 @@ +name: Mirror rolling to ros2 + +on: + push: + branches: [ rolling ] + +jobs: + mirror-to-ros2: + runs-on: ubuntu-latest + steps: + - uses: zofrex/mirror-branch@v1 + with: + target-branch: ros2 diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 000000000..0ee0be8b5 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,36 @@ +ARG ROS_DISTRO=rolling +FROM ros:${ROS_DISTRO}-ros-core + +WORKDIR /root/ros2_ws/ + +# Install essential dependencies +RUN apt-get update && \ + apt-get install -y \ + ros-dev-tools \ + wget && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* + +# Clone dependencies +COPY tools/ros2_dependencies.repos . +RUN mkdir -p src && \ + vcs import --input ros2_dependencies.repos src + +# Initialize rosdep +RUN rosdep init + +# Copy source code +COPY . src/grid_map + +# Install dependencies +SHELL ["/bin/bash", "-c"] +RUN apt-get update && \ + rosdep update && \ + source /opt/ros/${ROS_DISTRO}/setup.bash && \ + rosdep install -y --ignore-src --from-paths src --skip-keys slam_toolbox --skip-keys gazebo_ros_pkgs --skip-keys turtlebot3_gazebo && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* + +# Build +RUN source /opt/ros/${ROS_DISTRO}/setup.bash && \ + colcon build --symlink-install --packages-up-to grid_map diff --git a/README.md b/README.md index 3bf4c0402..02a7a8557 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ Features: * **Visualizations:** The *grid_map_rviz_plugin* renders grid maps as 3d surface plots (height maps) in [RViz]. Additionally, the *grid_map_visualization* package helps to visualize grid maps as point clouds, occupancy grids, grid cells etc. * **Filters:** The *grid_map_filters* provides are range of filters to process grid maps as a sequence of filters. Parsing of mathematical expressions allows to flexibly setup powerful computations such as thresholding, normal vectors, smoothening, variance, inpainting, and matrix kernel convolutions. -The grid map package has been tested with [ROS] Indigo, Jade (under Ubuntu 14.04), Kinetic (under Ubuntu 16.04), and Melodic (under Ubuntu 18.04). This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. +The grid map package has been tested with ROS2 Foxy (under Ubuntu 20.04). This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. The source code is released under a [BSD 3-Clause license](LICENSE). @@ -65,36 +65,56 @@ The C++ API is documented here: * [grid_map_octomap](http://docs.ros.org/kinetic/api/grid_map_octomap/html/index.html) * [grid_map_pcl](http://docs.ros.org/kinetic/api/grid_map_pcl/html/index.html) -## Installation +## ~~Installation~~ -### Installation from Packages +### ~~Installation from Packages~~ -To install all packages from the grid map library as Debian packages use +~~To install all packages from the grid map library as Debian packages use~~ - sudo apt-get install ros-$ROS_DISTRO-grid-map +~~sudo apt-get install ros-$ROS_DISTRO-grid-map~~ ### Building from Source #### Dependencies -The *grid_map_core* package depends only on the linear algebra library [Eigen]. +Install ROS 2 rolling from [here](https://docs.ros.org/en/rolling/Installation/Alternatives/Ubuntu-Install-Binary.html). - sudo apt-get install libeigen3-dev +Source the ROS 2 underlay workspace. -The other packages depend additionally on the [ROS] standard installation (*roscpp*, *tf*, *filters*, *sensor_msgs*, *nav_msgs*, and *cv_bridge*). Other format specific conversion packages (e.g. *grid_map_cv*, *grid_map_pcl* etc.) depend on packages described below in *Packages Overview*. + source /opt/ros/rolling/setup.bash -#### Building +Clone and build grid_map ROS2 dependencies. +The *grid_map_core* package depends only on the linear algebra library [Eigen] which is installed through rosdep. -To build from source, clone the latest version from this repository into your catkin workspace and compile the package using - cd catkin_ws/src - git clone https://github.com/anybotics/grid_map.git + mkdir -p ~/gridmap_dep/src + cd ~/gridmap_dep + wget https://raw.githubusercontent.com/ANYbotics/grid_map/rolling/tools/ros2_dependencies.repos + vcs import src < ros2_dependencies.repos + rosdep install -y --ignore-src --from-paths src + colcon build --symlink-install --packages-up-to pcl_ros + +The other packages depend additionally on the [ROS] standard installation (*rclcpp*, *tf*, *filters*, *sensor_msgs*, *nav_msgs*, and *cv_bridge*). Other format specific conversion packages (e.g. *grid_map_cv*, *grid_map_pcl* etc.) depend on packages described below in *Packages Overview*. + +#### Building grid_map + +In a new terminal, source your underlay dependency workspace. + + source ~/gridmap_dep/install/setup.bash + +Clone the latest version from this repository and build it in a new grid_map workspace. + + mkdir -p ~/gridmap_ws/src + cd ~/gridmap_ws/src + git clone https://github.com/anybotics/grid_map.git --branch ros2 cd ../ - catkin_make + rosdep install -y --ignore-src --from-paths src + colcon build --symlink-install + To maximize performance, make sure to build in *Release* mode. You can specify the build type by setting - catkin_make -DCMAKE_BUILD_TYPE=Release + colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release ### Packages Overview @@ -106,7 +126,7 @@ This repository consists of following packages: * ***grid_map_ros*** is the main package for [ROS] dependent projects using the grid map library. It provides the interfaces to convert grid maps from and to several [ROS] message types. * ***grid_map_demos*** contains several nodes for demonstration purposes. * ***grid_map_filters*** builds on the [ROS Filters] package to process grid maps as a sequence of filters. -* ***grid_map_msgs*** holds the [ROS] message and service definitions around the [grid_map_msg/GridMap] message type. +* ***grid_map_msgs*** holds the [ROS] message and service definitions around the [grid_map_msg/msg/GridMap] message type. * ***grid_map_rviz_plugin*** is an [RViz] plugin to visualize grid maps as 3d surface plots (height maps). * ***grid_map_visualization*** contains a node written to convert GridMap messages to other [ROS] message types for example for visualization in [RViz]. @@ -121,13 +141,13 @@ Additional conversion packages: Run the unit tests with - catkin_make run_tests_grid_map_core run_tests_grid_map_ros + colcon test --packages-up-to grid_map -or +View test results with - catkin build grid_map --no-deps --verbose --catkin-make-args run_tests + colcon test-result --verbose -if you are using [catkin tools](http://catkin-tools.readthedocs.org/). +if you are using [colcon](https://colcon.readthedocs.io/en/released/index.html). ## Usage @@ -135,37 +155,43 @@ if you are using [catkin tools](http://catkin-tools.readthedocs.org/). The *grid_map_demos* package contains several demonstration nodes. Use this code to verify your installation of the grid map packages and to get you started with your own usage of the library. +*Note: The octomap_to_gridmap_demo_launch.py is not working at the moment, pending the port of octomap_server see issue [OctoMap/octomap_mapping/#76](https://github.com/OctoMap/octomap_mapping/issues/76).* + +Before running the demos make sure you source your grid_map overlay workspace + + source ~/gridmap_ws/install/setup.bash + * *[simple_demo](grid_map_demos/src/simple_demo_node.cpp)* demonstrates a simple example for using the grid map library. This ROS node creates a grid map, adds data to it, and publishes it. To see the result in RViz, execute the command - roslaunch grid_map_demos simple_demo.launch + ros2 launch grid_map_demos simple_demo_launch.py * *[tutorial_demo](grid_map_demos/src/tutorial_demo_node.cpp)* is an extended demonstration of the library's functionalities. Launch the *tutorial_demo* with - roslaunch grid_map_demos tutorial_demo.launch + ros2 launch grid_map_demos tutorial_demo_launch.py * *[iterators_demo](grid_map_demos/src/IteratorsDemo.cpp)* showcases the usage of the grid map iterators. Launch it with - roslaunch grid_map_demos iterators_demo.launch + ros2 launch grid_map_demos iterators_demo_launch.py * *[image_to_gridmap_demo](grid_map_demos/src/ImageToGridmapDemo.cpp)* demonstrates how to convert data from an [image](grid_map_demos/data/eth_logo.png) to a grid map. Start the demonstration with - roslaunch grid_map_demos image_to_gridmap_demo.launch + ros2 launch grid_map_demos image_to_gridmap_demo_launch.py ![Image to grid map demo result](grid_map_demos/doc/image_to_grid_map_demo_result.png) * *[opencv_demo](grid_map_demos/src/opencv_demo_node.cpp)* demonstrates map manipulations with help of [OpenCV] functions. Start the demonstration with - roslaunch grid_map_demos opencv_demo.launch + ros2 launch grid_map_demos opencv_demo_launch.py ![OpenCV demo result](grid_map_demos/doc/opencv_demo_result.gif) * *[resolution_change_demo](grid_map_demos/src/resolution_change_demo_node.cpp)* shows how the resolution of a grid map can be changed with help of the [OpenCV] image scaling methods. The see the results, use - roslaunch grid_map_demos resolution_change_demo.launch + ros2 launch grid_map_demos resolution_change_demo_launch.py * *[filters_demo](grid_map_demos/src/FiltersDemo.cpp)* uses a chain of [ROS Filters] to process a grid map. Starting from the elevation of a terrain map, the demo uses several filters to show how to compute surface normals, use inpainting to fill holes, smoothen/blur the map, and use math expressions to detect edges, compute roughness and traversability. The filter chain setup is configured in the [`filters_demo_filter_chain.yaml`](grid_map_demos/config/filters_demo_filter_chain.yaml) file. Launch the demo with - roslaunch grid_map_demos filters_demo.launch + ros2 launch grid_map_demos filters_demo_launch.py [![Filters demo results](grid_map_demos/doc/filters_demo_preview.gif)](grid_map_demos/doc/filters_demo.gif) @@ -173,7 +199,7 @@ The *grid_map_demos* package contains several demonstration nodes. Use this code * *[interpolation_demo](grid_map_demos/src/InterpolationDemo.cpp)* shows the result of different interpolation methods on the resulting surface. The start the demo, use - roslaunch grid_map_demos interpolation_demo.launch + ros2 launch grid_map_demos interpolation_demo_launch.py @@ -272,7 +298,7 @@ This [RViz] plugin visualizes a grid map layer as 3d surface plot (height map). ### grid_map_visualization -This node subscribes to a topic of type [grid_map_msgs/GridMap] and publishes messages that can be visualized in [RViz]. The published topics of the visualizer can be fully configure with a YAML parameter file. Any number of visualizations with different parameters can be added. An example is [here](grid_map_demos/config/tutorial_demo.yaml) for the configuration file of the *tutorial_demo*. +This node subscribes to a topic of type [grid_map_msgs/msg/GridMap] and publishes messages that can be visualized in [RViz]. The published topics of the visualizer can be fully configure with a YAML parameter file. Any number of visualizations with different parameters can be added. An example is [here](grid_map_demos/config/tutorial_demo.yaml) for the configuration file of the *tutorial_demo*. Point cloud | Vectors | Occupancy grid | Grid cells --- | --- | --- | --- @@ -287,7 +313,7 @@ Point cloud | Vectors | Occupancy grid | Grid cells #### Subscribed Topics -* **`/grid_map`** ([grid_map_msgs/GridMap]) +* **`/grid_map`** ([grid_map_msgs/msg/GridMap]) The grid map to visualize. @@ -296,7 +322,7 @@ Point cloud | Vectors | Occupancy grid | Grid cells The published topics are configured with the [YAML parameter file](grid_map_demos/config/tutorial_demo.yaml). Possible topics are: -* **`point_cloud`** ([sensor_msgs/PointCloud2]) +* **`point_cloud`** ([sensor_msgs/msg/PointCloud2]) Shows the grid map as a point cloud. Select which layer to transform as points with the `layer` parameter. @@ -306,7 +332,7 @@ The published topics are configured with the [YAML parameter file](grid_map_demo layer: elevation flat: false # optional -* **`flat_point_cloud`** ([sensor_msgs/PointCloud2]) +* **`flat_point_cloud`** ([sensor_msgs/msg/PointCloud2]) Shows the grid map as a "flat" point cloud, i.e. with all points at the same height *z*. This is convenient to visualize 2d maps or images (or even video streams) in [RViz] with help of its `Color Transformer`. The parameter `height` determines the desired *z*-position of the flat point cloud. @@ -317,7 +343,7 @@ The published topics are configured with the [YAML parameter file](grid_map_demo Note: In order to omit points in the flat point cloud from empty/invalid cells, specify the layers which should be checked for validity with `setBasicLayers(...)`. -* **`vectors`** ([visualization_msgs/Marker]) +* **`vectors`** ([visualization_msgs/msg/Marker]) Visualizes vector data of the grid map as visual markers. Specify the layers which hold the *x*-, *y*-, and *z*-components of the vectors with the `layer_prefix` parameter. The parameter `position_layer` defines the layer to be used as start point of the vectors. @@ -330,7 +356,7 @@ The published topics are configured with the [YAML parameter file](grid_map_demo line_width: 0.005 color: 15600153 # red -* **`occupancy_grid`** ([nav_msgs/OccupancyGrid]) +* **`occupancy_grid`** ([nav_msgs/msg/OccupancyGrid]) Visualizes a layer of the grid map as occupancy grid. Specify the layer to be visualized with the `layer` parameter, and the upper and lower bound with `data_min` and `data_max`. @@ -341,7 +367,7 @@ The published topics are configured with the [YAML parameter file](grid_map_demo data_min: -0.15 data_max: 0.15 -* **`grid_cells`** ([nav_msgs/GridCells]) +* **`grid_cells`** ([nav_msgs/msg/GridCells]) Visualizes a layer of the grid map as grid cells. Specify the layer to be visualized with the `layer` parameter, and the upper and lower bounds with `lower_threshold` and `upper_threshold`. @@ -352,7 +378,7 @@ The published topics are configured with the [YAML parameter file](grid_map_demo lower_threshold: -0.08 # optional, default: -inf upper_threshold: 0.08 # optional, default: inf -* **`region`** ([visualization_msgs/Marker]) +* **`region`** ([visualization_msgs/msg/Marker]) Shows the boundary of the grid map. @@ -516,13 +542,13 @@ Please report bugs and request features using the [Issue Tracker](https://github [OctoMap]: https://octomap.github.io/ [PCL]: http://pointclouds.org/ [costmap_2d]: http://wiki.ros.org/costmap_2d -[grid_map_msgs/GridMapInfo]: http://docs.ros.org/api/grid_map_msgs/html/msg/GridMapInfo.html -[grid_map_msgs/GridMap]: http://docs.ros.org/api/grid_map_msgs/html/msg/GridMap.html -[grid_map_msgs/GetGridMap]: http://docs.ros.org/api/grid_map_msgs/html/srv/GetGridMap.html -[sensor_msgs/PointCloud2]: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html -[visualization_msgs/Marker]: http://docs.ros.org/api/visualization_msgs/html/msg/Marker.html -[geometry_msgs/PolygonStamped]: http://docs.ros.org/api/geometry_msgs/html/msg/PolygonStamped.html -[nav_msgs/OccupancyGrid]: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html -[nav_msgs/GridCells]: http://docs.ros.org/api/nav_msgs/html/msg/GridCells.html +[grid_map_msgs/msg/GridMapInfo]: http://docs.ros.org/api/grid_map_msgs/msg/html/msg/GridMapInfo.html +[grid_map_msgs/msg/GridMap]: http://docs.ros.org/api/grid_map_msgs/msg/html/msg/GridMap.html +[grid_map_msgs/msg/GetGridMap]: http://docs.ros.org/api/grid_map_msgs/msg/html/srv/GetGridMap.html +[sensor_msgs/msg/PointCloud2]: http://docs.ros.org/api/sensor_msgs/msg/html/msg/PointCloud2.html +[visualization_msgs/msg/Marker]: http://docs.ros.org/api/visualization_msgs/msg/html/msg/Marker.html +[geometry_msgs/msg/PolygonStamped]: http://docs.ros.org/api/geometry_msgs/msg/html/msg/PolygonStamped.html +[nav_msgs/msg/OccupancyGrid]: http://docs.ros.org/api/nav_msgs/msg/html/msg/OccupancyGrid.html +[nav_msgs/msg/GridCells]: http://docs.ros.org/api/nav_msgs/msg/html/msg/GridCells.html [ROS Filters]: http://wiki.ros.org/filters [EigenLab]: https://github.com/leggedrobotics/EigenLab diff --git a/grid_map/CHANGELOG.rst b/grid_map/CHANGELOG.rst index bf72135e9..7f6b78143 100644 --- a/grid_map/CHANGELOG.rst +++ b/grid_map/CHANGELOG.rst @@ -2,6 +2,31 @@ Changelog for package grid_map ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Merge pull request `#420 `_ from Ryanf55/enfore-cpp17 + Enfore C++17 +* Enfore C++17 +* Contributors: Ryan, Ryan Friedman + +2.1.0 (2022-11-08) +------------------ + +2.0.0 (2022-09-13) +------------------ +* fix: build error on jammy +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map/CMakeLists.txt b/grid_map/CMakeLists.txt index 6d04c2c75..c6f567667 100644 --- a/grid_map/CMakeLists.txt +++ b/grid_map/CMakeLists.txt @@ -1,4 +1,9 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(grid_map) -find_package(catkin REQUIRED) -catkin_metapackage() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/grid_map/package.xml b/grid_map/package.xml index a150c1d10..11c5db340 100644 --- a/grid_map/package.xml +++ b/grid_map/package.xml @@ -1,25 +1,32 @@ - + + grid_map - 1.6.2 + 2.2.2 Meta-package for the universal grid map library. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Péter Fankhauser - catkin + ament_cmake + grid_map_cmake_helpers grid_map_core - grid_map_ros + grid_map_costmap_2d grid_map_cv - grid_map_msgs + grid_map_demos grid_map_filters - grid_map_visualization - grid_map_rviz_plugin grid_map_loader - grid_map_demos + grid_map_msgs + grid_map_octomap + grid_map_pcl + grid_map_ros + grid_map_rviz_plugin + grid_map_sdf + grid_map_visualization - + ament_cmake diff --git a/grid_map_cmake_helpers/CHANGELOG.rst b/grid_map_cmake_helpers/CHANGELOG.rst new file mode 100644 index 000000000..878f51c64 --- /dev/null +++ b/grid_map_cmake_helpers/CHANGELOG.rst @@ -0,0 +1,93 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package grid_map_cmake_helpers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Merge pull request `#419 `_ from Ryanf55/stop-using-CMAKE_COMPILER_IS_GNUCXX + Stop using deprecated CMAKE_COMPILER_IS_GNUCXX +* Merge pull request `#420 `_ from Ryanf55/enfore-cpp17 + Enfore C++17 +* Enfore C++17 +* Stop using deprecated CMAKE_COMPILER_IS_GNUCXX + * Switch to CMAKE_CXX_COMPILER_ID + * https://cmake.org/cmake/help/latest/variable/CMAKE_LANG_COMPILER_ID.html +* Contributors: Ryan, Ryan Friedman + +2.1.0 (2022-11-08) +------------------ + +2.0.0 (2022-09-13) +------------------ +* Initial ROS2 port +* Contributors: Steve Macenski + +* Update for rolliing (`#296 `_) + Co-authored-by: Francisco Martin Rico +* Initial ROS2 port +* added grid_map_cmake_helpers maintainers +* changed grid_map_common to grid_map_cmake_helpers +* Contributors: Daisuke Nishimatsu, Marwan Taher, Steve Macenski + +1.6.2 (2019-10-24) +------------------ + +1.6.1 (2019-02-27) +------------------ + +1.6.0 (2017-11-24) +------------------ + +1.5.2 (2017-07-25 19:54) +------------------------ + +1.5.1 (2017-07-25 11:25) +------------------------ + +1.5.0 (2017-07-18) +------------------ + +1.4.2 (2017-01-24) +------------------ + +1.4.1 (2016-10-23) +------------------ + +1.4.0 (2016-08-22) +------------------ + +1.3.3 (2016-05-10 20:35) +------------------------ + +1.3.1 (2016-05-10 16:41) +------------------------ + +1.3.0 (2016-04-26) +------------------ + +1.2.0 (2016-03-03) +------------------ + +1.1.3 (2016-01-11 17:55:26 +0100) +--------------------------------- + +1.1.2 (2016-01-11 16:38) +------------------------ + +1.1.1 (2016-01-11 13:51) +------------------------ + +1.1.0 (2016-01-08) +------------------ + +1.0.0 (2015-11-20) +------------------ diff --git a/grid_map_cmake_helpers/CMakeLists.txt b/grid_map_cmake_helpers/CMakeLists.txt new file mode 100644 index 000000000..adbc3a8bc --- /dev/null +++ b/grid_map_cmake_helpers/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.5) + +project(grid_map_cmake_helpers NONE) + +find_package(ament_cmake_core REQUIRED) + +ament_package( + CONFIG_EXTRAS "grid_map_cmake_helpers-extras.cmake" +) + +install( + DIRECTORY cmake + DESTINATION share/${PROJECT_NAME} +) diff --git a/grid_map_cmake_helpers/cmake/grid_map_package.cmake b/grid_map_cmake_helpers/cmake/grid_map_package.cmake new file mode 100644 index 000000000..4867a92f4 --- /dev/null +++ b/grid_map_cmake_helpers/cmake/grid_map_package.cmake @@ -0,0 +1,40 @@ +# Standard grid_map project setup +# +# @public +# +macro(grid_map_package) + if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Setting build type to Release as none was specified.") + set(CMAKE_BUILD_TYPE "Release" CACHE + STRING "Choose the type of build." FORCE) + # Set the possible values of build type for cmake-gui + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS + "Debug" "Release" "MinSizeRel" "RelWithDebInfo") + endif() + + if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + endif() + + if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC) + endif() + + if(COVERAGE_ENABLED) + add_compile_options(--coverage) + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --coverage") + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} --coverage") + endif() + + # Defaults for Microsoft C++ compiler + if(MSVC) + # https://blog.kitware.com/create-dlls-on-windows-without-declspec-using-new-cmake-export-all-feature/ + set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + + # Enable Math Constants + # https://docs.microsoft.com/en-us/cpp/c-runtime-library/math-constants?view=vs-2019 + add_compile_definitions( + _USE_MATH_DEFINES + ) + endif() +endmacro() diff --git a/grid_map_cmake_helpers/grid_map_cmake_helpers-extras.cmake b/grid_map_cmake_helpers/grid_map_cmake_helpers-extras.cmake new file mode 100644 index 000000000..b4d2b3af9 --- /dev/null +++ b/grid_map_cmake_helpers/grid_map_cmake_helpers-extras.cmake @@ -0,0 +1,3 @@ +set(AMENT_BUILD_CONFIGURATION_KEYWORD_SEPARATOR ":") + +include("${grid_map_cmake_helpers_DIR}/grid_map_package.cmake") diff --git a/grid_map_cmake_helpers/package.xml b/grid_map_cmake_helpers/package.xml new file mode 100644 index 000000000..4217c8bf6 --- /dev/null +++ b/grid_map_cmake_helpers/package.xml @@ -0,0 +1,19 @@ + + + + grid_map_cmake_helpers + 2.2.2 + CMake support functionality used throughout grid_map + Maximilian Wulf + Yoshua Nava + Ryan Friedman + BSD + + ament_cmake_core + + ament_cmake_core + + + ament_cmake + + diff --git a/grid_map_core/CHANGELOG.rst b/grid_map_core/CHANGELOG.rst index 413502356..1546c3b6a 100644 --- a/grid_map_core/CHANGELOG.rst +++ b/grid_map_core/CHANGELOG.rst @@ -2,6 +2,39 @@ Changelog for package grid_map_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ +* Fix UB in SpiralIterator::operator++ (`#481 `_) (`#487 `_) + * Calling pop_back on an iterator past the end is UB + (cherry picked from commit 74dbbc83ad695c09bc60dcc856aa8a75741311a9) + Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com> +* Contributors: mergify[bot] + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#458 `_ from ANYbotics/ci-temp-skip-octomap-server + build: treat several build issues on rolling +* suppress warning due to gcc13 bug +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Merge pull request `#404 `_ from Ryanf55/bugfix-403-cmake + grid_map_core: Use ament_export_targets and improve eigen linkage +* Update ament to latest recommendations + * Fixes include errors in grid_map_geo ros2 port +* Contributors: Ryan, Ryan Friedman, wep21 + +2.1.0 (2022-11-08) +------------------ + +2.0.0 (2022-09-13) +------------------ +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ * Implements a grid map transformation from one map frame to another map frame given the transform between the frames. diff --git a/grid_map_core/CMakeLists.txt b/grid_map_core/CMakeLists.txt index be6148752..cbf8ff5da 100644 --- a/grid_map_core/CMakeLists.txt +++ b/grid_map_core/CMakeLists.txt @@ -1,79 +1,49 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(grid_map_core) -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") +## Find ament_cmake macros and libraries +find_package(ament_cmake REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS) - -## Define Eigen addons. +## Define Eigen addons. include(cmake/${PROJECT_NAME}-extras.cmake) ## System dependencies are found with CMake's conventions -#find_package(Eigen3 REQUIRED) -# Solution to find Eigen3 with Saucy. -find_package(Eigen3 QUIET) -if(NOT EIGEN3_FOUND) - find_package(PkgConfig REQUIRED) - pkg_check_modules(EIGEN3 REQUIRED eigen3) - set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) -endif() +find_package(Eigen3 REQUIRED) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIR} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - DEPENDS - #Eigen3 - CFG_EXTRAS - ${PROJECT_NAME}-extras.cmake -) +grid_map_package() ########### ## Build ## ########### -## Specify additional locations of header files -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} -) - ## Declare a cpp library add_library(${PROJECT_NAME} - src/GridMap.cpp - src/GridMapMath.cpp - src/SubmapGeometry.cpp - src/BufferRegion.cpp - src/Polygon.cpp - src/CubicInterpolation.cpp - src/iterators/GridMapIterator.cpp - src/iterators/SubmapIterator.cpp - src/iterators/CircleIterator.cpp - src/iterators/EllipseIterator.cpp - src/iterators/SpiralIterator.cpp - src/iterators/PolygonIterator.cpp - src/iterators/LineIterator.cpp - src/iterators/SlidingWindowIterator.cpp + src/GridMap.cpp + src/GridMapMath.cpp + src/SubmapGeometry.cpp + src/BufferRegion.cpp + src/Polygon.cpp + src/CubicInterpolation.cpp + src/iterators/GridMapIterator.cpp + src/iterators/SubmapIterator.cpp + src/iterators/CircleIterator.cpp + src/iterators/EllipseIterator.cpp + src/iterators/SpiralIterator.cpp + src/iterators/PolygonIterator.cpp + src/iterators/LineIterator.cpp + src/iterators/SlidingWindowIterator.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +## Specify additional locations of header files +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$" ) +target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen) + ############# ## Install ## ############# @@ -81,50 +51,82 @@ target_link_libraries(${PROJECT_NAME} # Mark executables and/or libraries for installation install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) # Mark cpp header files for installation install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} FILES_MATCHING PATTERN "*.hpp" ) # Mark other files for installation install( DIRECTORY doc - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) ############# ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") -## Add gtest based cpp test target and link libraries -catkin_add_gtest(${PROJECT_NAME}-test - test/test_grid_map_core.cpp - test/test_helpers.cpp - test/CubicConvolutionInterpolationTest.cpp - test/CubicInterpolationTest.cpp - test/GridMapMathTest.cpp - test/GridMapTest.cpp - test/GridMapIteratorTest.cpp - test/LineIteratorTest.cpp - test/EllipseIteratorTest.cpp - test/SubmapIteratorTest.cpp - test/PolygonIteratorTest.cpp - test/PolygonTest.cpp - test/EigenPluginsTest.cpp - test/SpiralIteratorTest.cpp - test/SlidingWindowIteratorTest.cpp +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright -build/include_order + ) + endif() + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + ## Add gtest based cpp test target and link libraries + ament_add_gtest(${PROJECT_NAME}-test + test/test_grid_map_core.cpp + test/test_helpers.cpp + test/CubicConvolutionInterpolationTest.cpp + test/CubicInterpolationTest.cpp + test/GridMapMathTest.cpp + test/GridMapTest.cpp + test/GridMapIteratorTest.cpp + test/LineIteratorTest.cpp + test/EllipseIteratorTest.cpp + test/SubmapIteratorTest.cpp + test/PolygonIteratorTest.cpp + test/PolygonTest.cpp + test/EigenPluginsTest.cpp + test/SpiralIteratorTest.cpp + test/SlidingWindowIteratorTest.cpp ) endif() if(TARGET ${PROJECT_NAME}-test) target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) endif() + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(Eigen3) +ament_package(CONFIG_EXTRAS + cmake/${PROJECT_NAME}-extras.cmake +) diff --git a/grid_map_core/cmake/grid_map_core-extras.cmake b/grid_map_core/cmake/grid_map_core-extras.cmake index e63eae443..dd4089d4e 100644 --- a/grid_map_core/cmake/grid_map_core-extras.cmake +++ b/grid_map_core/cmake/grid_map_core-extras.cmake @@ -1,17 +1,17 @@ set(EIGEN_FUNCTORS_PLUGIN_PATH "grid_map_core/eigen_plugins/FunctorsPlugin.hpp") -if (EIGEN_FUNCTORS_PLUGIN) - if (NOT EIGEN_FUNCTORS_PLUGIN STREQUAL EIGEN_FUNCTORS_PLUGIN_PATH) - MESSAGE(FATAL_ERROR "EIGEN_FUNCTORS_PLUGIN already defined!") - endif () -else (EIGEN_FUNCTORS_PLUGIN) +if(EIGEN_FUNCTORS_PLUGIN) + if(NOT EIGEN_FUNCTORS_PLUGIN STREQUAL EIGEN_FUNCTORS_PLUGIN_PATH) + message(FATAL_ERROR "EIGEN_FUNCTORS_PLUGIN already defined") + endif() +else() add_definitions(-DEIGEN_FUNCTORS_PLUGIN=\"${EIGEN_FUNCTORS_PLUGIN_PATH}\") -endif (EIGEN_FUNCTORS_PLUGIN) +endif() set(EIGEN_DENSEBASE_PLUGIN_PATH "grid_map_core/eigen_plugins/DenseBasePlugin.hpp") -if (EIGEN_DENSEBASE_PLUGIN) - if (NOT EIGEN_DENSEBASE_PLUGIN STREQUAL EIGEN_DENSEBASE_PLUGIN_PATH) - MESSAGE(FATAL_ERROR "EIGEN_DENSEBASE_PLUGIN already defined!") - endif () -else (EIGEN_DENSEBASE_PLUGIN) +if(EIGEN_DENSEBASE_PLUGIN) + if(NOT EIGEN_DENSEBASE_PLUGIN STREQUAL EIGEN_DENSEBASE_PLUGIN_PATH) + message(FATAL_ERROR "EIGEN_DENSEBASE_PLUGIN already defined!") + endif() +else() add_definitions(-DEIGEN_DENSEBASE_PLUGIN=\"${EIGEN_DENSEBASE_PLUGIN_PATH}\") -endif (EIGEN_DENSEBASE_PLUGIN) \ No newline at end of file +endif() \ No newline at end of file diff --git a/grid_map_core/include/grid_map_core/BufferRegion.hpp b/grid_map_core/include/grid_map_core/BufferRegion.hpp index bbe95c776..fbe2d205d 100644 --- a/grid_map_core/include/grid_map_core/BufferRegion.hpp +++ b/grid_map_core/include/grid_map_core/BufferRegion.hpp @@ -6,11 +6,13 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CORE__BUFFERREGION_HPP_ +#define GRID_MAP_CORE__BUFFERREGION_HPP_ #include "grid_map_core/TypeDefs.hpp" -namespace grid_map { +namespace grid_map +{ /*! * This class holds information about a rectangular region @@ -18,8 +20,7 @@ namespace grid_map { */ class BufferRegion { - public: - +public: /*! * The definition of the buffer region positions. */ @@ -35,18 +36,19 @@ class BufferRegion constexpr static unsigned int nQuadrants = 4; BufferRegion(); - BufferRegion(const Index& startIndex, const Size& size, const BufferRegion::Quadrant& quadrant); - virtual ~BufferRegion(); + BufferRegion( + const Index & startIndex, const Size & size, + const BufferRegion::Quadrant & quadrant); + virtual ~BufferRegion() = default; - const Index& getStartIndex() const; - void setStartIndex(const Index& startIndex); - const Size& getSize() const; - void setSize(const Size& size); + const Index & getStartIndex() const; + void setStartIndex(const Index & startIndex); + const Size & getSize() const; + void setSize(const Size & size); BufferRegion::Quadrant getQuadrant() const; void setQuadrant(BufferRegion::Quadrant type); - private: - +private: //! Start index (typically top-left) of the buffer region. Index staretIndex_; @@ -56,8 +58,9 @@ class BufferRegion //! Quadrant type of the buffer region. Quadrant quadrant_; - public: +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /* namespace grid_map */ +} // namespace grid_map +#endif // GRID_MAP_CORE__BUFFERREGION_HPP_ diff --git a/grid_map_core/include/grid_map_core/CubicInterpolation.hpp b/grid_map_core/include/grid_map_core/CubicInterpolation.hpp index 7d6c6e6ec..26f9b7fb5 100644 --- a/grid_map_core/include/grid_map_core/CubicInterpolation.hpp +++ b/grid_map_core/include/grid_map_core/CubicInterpolation.hpp @@ -6,11 +6,14 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#pragma once + +#ifndef GRID_MAP_CORE__CUBICINTERPOLATION_HPP_ +#define GRID_MAP_CORE__CUBICINTERPOLATION_HPP_ #include #include #include +#include #include "grid_map_core/TypeDefs.hpp" /* @@ -26,7 +29,8 @@ * http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm */ -namespace grid_map { +namespace grid_map +{ class GridMap; @@ -59,19 +63,20 @@ unsigned int bindIndexToRange(int idReq, unsigned int nElem); * @param[in] colReq - column requested * @return - value of the layer at rowReq and colReq */ -double getLayerValue(const Matrix &layerMat, int rowReq, int colReq); +double getLayerValue(const Matrix & layerMat, int rowReq, int colReq); -namespace bicubic_conv { +namespace bicubic_conv +{ /*! * Matrix for cubic interpolation via convolution. Taken from: * https://en.wikipedia.org/wiki/Bicubic_interpolation */ static const Eigen::Matrix4d cubicInterpolationConvolutionMatrix { - (Eigen::Matrix4d() << 0.0, 2.0, 0.0, 0.0, - -1.0, 0.0, 1.0, 0.0, - 2.0, -5.0, 4.0, -1.0, - -1.0, 3.0, -3.0, 1.0).finished() }; + (Eigen::Matrix4d() << 0.0, 2.0, 0.0, 0.0, + -1.0, 0.0, 1.0, 0.0, + 2.0, -5.0, 4.0, -1.0, + -1.0, 3.0, -3.0, 1.0).finished()}; /* * Index of the middle knot for bicubic inteprolation. This is @@ -85,7 +90,9 @@ static const Eigen::Matrix4d cubicInterpolationConvolutionMatrix { * @param[out] index - indices of the middle knot for the interpolation * @return - true if success */ -bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosition, Index *index); +bool getIndicesOfMiddleKnot( + const GridMap & gridMap, const Position & queriedPosition, + Index * index); /* * Coordinates used for interpolation need to be shifted and scaled, @@ -96,8 +103,9 @@ bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosit * @param[out] position - normalized coordinates of the point for which the interpolation is requested * @return - true if success */ -bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPosition, - Position *position); +bool getNormalizedCoordinates( + const GridMap & gridMap, const Position & queriedPosition, + Position * position); /* * Queries the grid map for function values at the coordiantes which are neccesary for @@ -112,8 +120,9 @@ bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPos * for the details. * @return - true if success */ -bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &layer, - const Position &queriedPosition, FunctionValueMatrix *data); +bool assembleFunctionValueMatrix( + const GridMap & gridMap, const std::string & layer, + const Position & queriedPosition, FunctionValueMatrix * data); /* * Performs convolution in 1D. the function requires 4 function values @@ -123,7 +132,7 @@ bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &laye * interpolation in 1 dimension. * @return - interpolated value at normalized coordinate t */ -double convolve1D(double t, const Eigen::Vector4d &functionValues); +double convolve1D(double t, const Eigen::Vector4d & functionValues); /* * Performs convolution in 1D. the function requires 4 function values @@ -134,22 +143,25 @@ double convolve1D(double t, const Eigen::Vector4d &functionValues); * @param[out] interpolatedValue - interpolated value at queried point * @return - true if success */ -bool evaluateBicubicConvolutionInterpolation(const GridMap &gridMap, const std::string &layer, - const Position &queriedPosition, - double *interpolatedValue); +bool evaluateBicubicConvolutionInterpolation( + const GridMap & gridMap, const std::string & layer, + const Position & queriedPosition, + double * interpolatedValue); -} /* namespace bicubic_conv */ +} // namespace bicubic_conv -namespace bicubic { +namespace bicubic +{ /* * Enum for the derivatives direction * to perform interpolation one needs * derivatives w.r.t. to x and y dimension. */ -enum class Dim2D: int { - X, - Y +enum class Dim2D : int +{ + X, + Y }; /*! @@ -157,10 +169,10 @@ enum class Dim2D: int { * https://en.wikipedia.org/wiki/Bicubic_interpolation */ static const Eigen::Matrix4d bicubicInterpolationMatrix { - (Eigen::Matrix4d() << 1.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - -3.0, 3.0, -2.0, -1.0, - 2.0, -2.0, 1.0, 1.0).finished() }; + (Eigen::Matrix4d() << 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + -3.0, 3.0, -2.0, -1.0, + 2.0, -2.0, 1.0, 1.0).finished()}; /* * Data matrix that can hold function values @@ -185,10 +197,10 @@ struct DataMatrix */ struct IndicesMatrix { - Index topLeft_ { 0, 0 }; - Index topRight_ { 0, 0 }; - Index bottomLeft_ { 0, 0 }; - Index bottomRight_ { 0, 0 }; + Index topLeft_ {0, 0}; + Index topRight_ {0, 0}; + Index bottomLeft_ {0, 0}; + Index bottomRight_ {0, 0}; }; /* @@ -199,7 +211,7 @@ struct IndicesMatrix * @param[in/out] indices - indices that are bound to range, i.e. * rows and columns are with ranges */ -void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices); +void bindIndicesToRange(const GridMap & gridMap, IndicesMatrix * indices); /* * Performs bicubic interpolation at requested position. @@ -209,8 +221,9 @@ void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices); * @param[out] interpolatedValue - interpolated value at queried point * @return - true if success */ -bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &layer, - const Position &queriedPosition, double *interpolatedValue); +bool evaluateBicubicInterpolation( + const GridMap & gridMap, const std::string & layer, + const Position & queriedPosition, double * interpolatedValue); /* * Deduces which points in the grid map close a unit square around the @@ -221,8 +234,9 @@ bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &lay * around the queried point * @return - true if success */ -bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedPosition, - IndicesMatrix *indicesMatrix); +bool getUnitSquareCornerIndices( + const GridMap & gridMap, const Position & queriedPosition, + IndicesMatrix * indicesMatrix); /* * Get index (row and column number) of a point in grid map, which @@ -232,7 +246,9 @@ bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedP * @param[out] index - indices of the closest point in grid_map * @return - true if success */ -bool getClosestPointIndices(const GridMap &gridMap, const Position &queriedPosition, Index *index); +bool getClosestPointIndices( + const GridMap & gridMap, const Position & queriedPosition, + Index * index); /* * Retrieve function values from the grid map at requested indices. @@ -241,7 +257,7 @@ bool getClosestPointIndices(const GridMap &gridMap, const Position &queriedPosit * @param[out] data - requested function values * @return - true if success */ -bool getFunctionValues(const Matrix &layerData, const IndicesMatrix &indices, DataMatrix *data); +bool getFunctionValues(const Matrix & layerData, const IndicesMatrix & indices, DataMatrix * data); /* * Retrieve function derivative values from the grid map at requested indices. Function @@ -254,8 +270,9 @@ bool getFunctionValues(const Matrix &layerData, const IndicesMatrix &indices, Da * @param[out] derivatives - values of derivatives at requested indices * @return - true if success */ -bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, Dim2D dim, - double resolution, DataMatrix *derivatives); +bool getFirstOrderDerivatives( + const Matrix & layerData, const IndicesMatrix & indices, Dim2D dim, + double resolution, DataMatrix * derivatives); /* * Retrieve second order function derivative values from the grid map at requested indices. @@ -269,8 +286,9 @@ bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indi * @param[out] derivatives - values of second order mixed derivatives at requested indices * @return - true if success */ -bool getMixedSecondOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, - double resolution, DataMatrix *derivatives); +bool getMixedSecondOrderDerivatives( + const Matrix & layerData, const IndicesMatrix & indices, + double resolution, DataMatrix * derivatives); /* * First order derivative for a specific point determined by index. @@ -284,8 +302,9 @@ bool getMixedSecondOrderDerivatives(const Matrix &layerData, const IndicesMatrix * @param[in] resolution - resolution of the grid map * @return - value of the derivative at requested index */ -double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D dim, - double resolution); +double firstOrderDerivativeAt( + const Matrix & layerData, const Index & index, Dim2D dim, + double resolution); /* * Second order mixed derivative for a specific point determined by index. @@ -297,7 +316,9 @@ double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D * @param[in] resolution - resolution of the grid map * @return - value of the second order mixed derivative at requested index */ -double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution); +double mixedSecondOrderDerivativeAt( + const Matrix & layerData, const Index & index, + double resolution); /* * Evaluate polynomial at requested coordinates. the function will compute the polynomial @@ -310,7 +331,7 @@ double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, * @param[in] ty - normalized y coordinate for which the interpolation should be computed * @return - interpolated value at requested normalized coordinates. */ -double evaluatePolynomial(const FunctionValueMatrix &functionValues, double tx, double ty); +double evaluatePolynomial(const FunctionValueMatrix & functionValues, double tx, double ty); /* * Assemble function value matrix from small submatrices containing function values @@ -324,8 +345,9 @@ double evaluatePolynomial(const FunctionValueMatrix &functionValues, double tx, * @param[out] functionValues - function values and derivatives required to * compute polynomial coefficients */ -void assembleFunctionValueMatrix(const DataMatrix &f, const DataMatrix &dfx, const DataMatrix &dfy, - const DataMatrix &ddfxy, FunctionValueMatrix *functionValues); +void assembleFunctionValueMatrix( + const DataMatrix & f, const DataMatrix & dfx, const DataMatrix & dfy, + const DataMatrix & ddfxy, FunctionValueMatrix * functionValues); /* * Coordinates used for interpolation need to be shifter and scaled, @@ -337,9 +359,11 @@ void assembleFunctionValueMatrix(const DataMatrix &f, const DataMatrix &dfx, con * @param[out] position - normalized coordinates of the point for which the interpolation is requested * @return - true if success */ -bool computeNormalizedCoordinates(const GridMap &gridMap, const Index &originIndex, - const Position &queriedPosition, Position *normalizedCoordinates); +bool computeNormalizedCoordinates( + const GridMap & gridMap, const Index & originIndex, + const Position & queriedPosition, Position * normalizedCoordinates); -} /* namespace bicubic */ +} // namespace bicubic -} /* namespace grid_map*/ +} // namespace grid_map +#endif // GRID_MAP_CORE__CUBICINTERPOLATION_HPP_ diff --git a/grid_map_core/include/grid_map_core/GridMap.hpp b/grid_map_core/include/grid_map_core/GridMap.hpp index 9bc1d78f2..25902b5b0 100644 --- a/grid_map_core/include/grid_map_core/GridMap.hpp +++ b/grid_map_core/include/grid_map_core/GridMap.hpp @@ -6,21 +6,25 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CORE__GRIDMAP_HPP_ +#define GRID_MAP_CORE__GRIDMAP_HPP_ -#include "grid_map_core/TypeDefs.hpp" -#include "grid_map_core/SubmapGeometry.hpp" -#include "grid_map_core/BufferRegion.hpp" +// Eigen +#include +#include // STL #include +#include +#include #include -// Eigen -#include -#include +#include "grid_map_core/TypeDefs.hpp" +#include "grid_map_core/SubmapGeometry.hpp" +#include "grid_map_core/BufferRegion.hpp" -namespace grid_map { +namespace grid_map +{ class SubmapGeometry; @@ -39,7 +43,7 @@ class SubmapGeometry; */ class GridMap { - public: +public: // Type traits for use with template methods/classes using GridMap as a template parameter. typedef grid_map::DataType DataType; typedef grid_map::Matrix Matrix; @@ -48,7 +52,7 @@ class GridMap * Constructor. * @param layers a vector of strings containing the definition/description of the data layer. */ - GridMap(const std::vector& layers); + explicit GridMap(const std::vector & layers); /*! * Emtpy constructor. @@ -58,10 +62,10 @@ class GridMap /*! * Default copy assign and copy constructors. */ - GridMap(const GridMap&) = default; - GridMap& operator=(const GridMap&) = default; - GridMap(GridMap&&) = default; - GridMap& operator=(GridMap&&) = default; + GridMap(const GridMap &) = default; + GridMap & operator=(const GridMap &) = default; + GridMap(GridMap &&) = default; + GridMap & operator=(GridMap &&) = default; /*! * Destructor. @@ -74,35 +78,36 @@ class GridMap * @param resolution the cell size in [m/cell]. * @param position the 2d position of the grid map in the grid map frame [m]. */ - void setGeometry(const Length& length, const double resolution, - const Position& position = Position::Zero()); + void setGeometry( + const Length & length, const double resolution, + const Position & position = Position::Zero()); /*! * Set the geometry of the grid map from submap geometry information. * @param geometry the submap geometry information. */ - void setGeometry(const SubmapGeometry& geometry); + void setGeometry(const SubmapGeometry & geometry); /*! * Add a new empty data layer. * @param layer the name of the layer. * @value value the value to initialize the cells with. */ - void add(const std::string& layer, const double value = NAN); + void add(const std::string & layer, const double value = NAN); /*! * Add a new data layer (if the layer already exists, overwrite its data, otherwise add layer and data). * @param layer the name of the layer. * @param data the data to be added. */ - void add(const std::string& layer, const Matrix& data); + void add(const std::string & layer, const Matrix & data); /*! * Checks if data layer exists. * @param layer the name of the layer. * @return true if layer exists, false otherwise. */ - bool exists(const std::string& layer) const; + bool exists(const std::string & layer) const; /*! * Returns the grid map data for a layer as matrix. @@ -110,7 +115,7 @@ class GridMap * @return grid map data as matrix. * @throw std::out_of_range if no map layer with name `layer` is present. */ - const Matrix& get(const std::string& layer) const; + const Matrix & get(const std::string & layer) const; /*! * Returns the grid map data for a layer as non-const. Use this method @@ -119,7 +124,7 @@ class GridMap * @return grid map data. * @throw std::out_of_range if no map layer with name `layer` is present. */ - Matrix& get(const std::string& layer); + Matrix & get(const std::string & layer); /*! * Returns the grid map data for a layer as matrix. @@ -127,7 +132,7 @@ class GridMap * @return grid map data as matrix. * @throw std::out_of_range if no map layer with name `layer` is present. */ - const Matrix& operator [](const std::string& layer) const; + const Matrix & operator[](const std::string & layer) const; /*! * Returns the grid map data for a layer as non-const. Use this method @@ -136,20 +141,20 @@ class GridMap * @return grid map data. * @throw std::out_of_range if no map layer with name `layer` is present. */ - Matrix& operator [](const std::string& layer); + Matrix & operator[](const std::string & layer); /*! * Removes a layer from the grid map. * @param layer the name of the layer to be removed. * @return true if successful. */ - bool erase(const std::string& layer); + bool erase(const std::string & layer); /*! * Gets the names of the layers. * @return the names of the layers. */ - const std::vector& getLayers() const; + const std::vector & getLayers() const; /*! * Set the basic layers that need to be valid for a cell to be considered as valid. @@ -157,13 +162,13 @@ class GridMap * By default the list of basic layers is empty. * @param basicLayers the list of layers that are the basic layers of the map. */ - void setBasicLayers(const std::vector& basicLayers); + void setBasicLayers(const std::vector & basicLayers); /*! * Gets the names of the basic layers. * @return the names of the basic layers. */ - const std::vector& getBasicLayers() const; + const std::vector & getBasicLayers() const; /*! * True if basic layers are defined. @@ -178,7 +183,7 @@ class GridMap * @param other the other grid map. * @return true if the other grid map has the same layers, false otherwise. */ - bool hasSameLayers(const grid_map::GridMap& other) const; + bool hasSameLayers(const grid_map::GridMap & other) const; /*! * Get cell data at requested position. @@ -187,7 +192,7 @@ class GridMap * @return the data of the cell. * @throw std::out_of_range if no map layer with name `layer` is present. */ - float& atPosition(const std::string& layer, const Position& position); + float & atPosition(const std::string & layer, const Position & position); /*! * Get cell data at requested position. Const version form above. @@ -197,8 +202,9 @@ class GridMap * @throw std::out_of_range if no map layer with name `layer` is present. * @throw std::runtime_error if the specified interpolation method is not implemented. */ - float atPosition(const std::string& layer, const Position& position, - InterpolationMethods interpolationMethod = InterpolationMethods::INTER_NEAREST) const; + float atPosition( + const std::string & layer, const Position & position, + InterpolationMethods interpolationMethod = InterpolationMethods::INTER_NEAREST) const; /*! * Get cell data for requested index. @@ -207,7 +213,7 @@ class GridMap * @return the data of the cell. * @throw std::out_of_range if no map layer with name `layer` is present. */ - float& at(const std::string& layer, const Index& index); + float & at(const std::string & layer, const Index & index); /*! * Get cell data for requested index. Const version form above. @@ -216,7 +222,7 @@ class GridMap * @return the data of the cell. * @throw std::out_of_range if no map layer with name `layer` is present. */ - float at(const std::string& layer, const Index& index) const; + float at(const std::string & layer, const Index & index) const; /*! * Gets the corresponding cell index for a position. @@ -224,7 +230,7 @@ class GridMap * @param[out] index the corresponding index. * @return true if successful, false if position outside of map. */ - bool getIndex(const Position& position, Index& index) const; + bool getIndex(const Position & position, Index & index) const; /*! * Gets the 2d position of cell specified by the index (x, y of cell position) in @@ -233,14 +239,14 @@ class GridMap * @param[out] position the position of the data point in the parent frame. * @return true if successful, false if index not within range of buffer. */ - bool getPosition(const Index& index, Position& position) const; + bool getPosition(const Index & index, Position & position) const; /*! * Check if position is within the map boundaries. * @param position the position to be checked. * @return true if position is within map, false otherwise. */ - bool isInside(const Position& position) const; + bool isInside(const Position & position) const; /*! * Checks if the index of all layers defined as basic types are valid, @@ -248,7 +254,7 @@ class GridMap * @param index the index to check. * @return true if cell is valid, false otherwise. */ - bool isValid(const Index& index) const; + bool isValid(const Index & index) const; /*! * Checks if cell at index is a valid (finite) for a certain layer. @@ -256,7 +262,7 @@ class GridMap * @param layer the name of the layer to be checked for validity. * @return true if cell is valid, false otherwise. */ - bool isValid(const Index& index, const std::string& layer) const; + bool isValid(const Index & index, const std::string & layer) const; /*! * Checks if cell at index is a valid (finite) for certain layers. @@ -264,7 +270,7 @@ class GridMap * @param layers the layers to be checked for validity. * @return true if cell is valid, false otherwise. */ - bool isValid(const Index& index, const std::vector& layers) const; + bool isValid(const Index & index, const std::vector & layers) const; /*! * Gets the 3d position of a data point (x, y of cell position & cell value as z) in @@ -274,7 +280,7 @@ class GridMap * @param position the position of the data point in the parent frame. * @return true if successful, false if no valid data available. */ - bool getPosition3(const std::string& layer, const Index& index, Position3& position) const; + bool getPosition3(const std::string & layer, const Index & index, Position3 & position) const; /*! * Gets the 3d vector of three layers with suffixes 'x', 'y', and 'z'. @@ -283,8 +289,9 @@ class GridMap * @param vector the vector with the values of the data type. * @return true if successful, false if no valid data available. */ - bool getVector(const std::string& layerPrefix, const Index& index, - Eigen::Vector3d& vector) const; + bool getVector( + const std::string & layerPrefix, const Index & index, + Eigen::Vector3d & vector) const; /*! * Gets a submap from the map. The requested submap is specified with the requested @@ -296,21 +303,7 @@ class GridMap * @param[out] isSuccess true if successful, false otherwise. * @return submap (is empty if success is false). */ - GridMap getSubmap(const Position& position, const Length& length, bool& isSuccess) const; - - /*! - * Gets a submap from the map. The requested submap is specified with the requested - * location and length. - * Note: The returned submap may not have the requested length due to the borders - * of the map and discretization. - * @param[in] position the requested position of the submap (usually the center). - * @param[in] length the requested length of the submap. - * @param[out] indexInSubmap the index of the requested position in the submap. - * @param[out] isSuccess true if successful, false otherwise. - * @return submap (is empty if success is false). - */ - GridMap getSubmap(const Position& position, const Length& length, Index& indexInSubmap, - bool& isSuccess) const; + GridMap getSubmap(const Position & position, const Length & length, bool & isSuccess) const; /*! * Apply isometric transformation (rotation + offset) to grid map and returns the transformed map. @@ -325,19 +318,20 @@ class GridMap * @return transformed map. * @throw std::out_of_range if no map layer with name `heightLayerName` is present. */ - GridMap getTransformedMap(const Eigen::Isometry3d& transform, const std::string& heightLayerName, - const std::string& newFrameId, - const double sampleRatio = 0.0) const; + GridMap getTransformedMap( + const Eigen::Isometry3d & transform, const std::string & heightLayerName, + const std::string & newFrameId, + const double sampleRatio = 0.0) const; - /*! - * Set the position of the grid map. - * Note: This method does not change the data stored in the grid map and - * is complementary to the `move(...)` method. For a comparison between - * the `setPosition` and the `move` method, see the `move_demo_node.cpp` - * file of the `grid_map_demos` package. - * @param position the 2d position of the grid map in the grid map frame [m]. - */ - void setPosition(const Position& position); + /*! + * Set the position of the grid map. + * Note: This method does not change the data stored in the grid map and + * is complementary to the `move(...)` method. For a comparison between + * the `setPosition` and the `move` method, see the `move_demo_node.cpp` + * file of the `grid_map_demos` package. + * @param position the 2d position of the grid map in the grid map frame [m]. + */ + void setPosition(const Position & position); /*! * Move the grid map w.r.t. to the grid map frame. Use this to move the grid map @@ -349,7 +343,7 @@ class GridMap * @param newRegions the regions of the newly covered / previously uncovered regions of the buffer. * @return true if map has been moved, false otherwise. */ - bool move(const Position& position, std::vector& newRegions); + bool move(const Position & position, std::vector & newRegions); /*! * Move the grid map w.r.t. to the grid map frame. Use this to move the grid map @@ -358,7 +352,7 @@ class GridMap * @param position the new location of the grid map in the map frame. * @return true if map has been moved, false otherwise. */ - bool move(const Position& position); + bool move(const Position & position); /*! * Adds data from an other grid map to this grid map @@ -369,22 +363,23 @@ class GridMap * @param layers the layers that are copied if not all layers are used. * @return true if successful. */ - bool addDataFrom(const GridMap& other, bool extendMap, - bool overwriteData, bool copyAllLayers, - std::vector layers = std::vector()); + bool addDataFrom( + const GridMap & other, bool extendMap, + bool overwriteData, bool copyAllLayers, + std::vector layers = std::vector()); /*! * Extends the size of the grip map such that the other grid map fits within. * @param other the grid map to extend the size to. * @return true if successful. */ - bool extendToInclude(const GridMap& other); + bool extendToInclude(const GridMap & other); /*! * Clears all cells (set to NAN) for a layer. * @param layer the layer to be cleared. */ - void clear(const std::string& layer); + void clear(const std::string & layer); /*! * Clears all cells (set to NAN) for all basic layers. @@ -420,25 +415,25 @@ class GridMap * Set the frame id of the grid map. * @param frameId the frame id to set. */ - void setFrameId(const std::string& frameId); + void setFrameId(const std::string & frameId); /*! * Get the frameId of the grid map. * @return frameId. */ - const std::string& getFrameId() const; + const std::string & getFrameId() const; /*! * Get the side length of the grid map. * @return side length of the grid map. */ - const Length& getLength() const; + const Length & getLength() const; /*! * Get the 2d position of the grid map in the grid map frame. * @return position of the grid map in the grid map frame. */ - const Position& getPosition() const; + const Position & getPosition() const; /*! * Get the resolution of the grid map. @@ -450,20 +445,20 @@ class GridMap * Get the grid map size (rows and cols of the data structure). * @return grid map size. */ - const Size& getSize() const; + const Size & getSize() const; /*! * Set the start index of the circular buffer. * Use this method with caution! * @return buffer start index. */ - void setStartIndex(const Index& startIndex); + void setStartIndex(const Index & startIndex); /*! * Get the start index of the circular buffer. * @return buffer start index. */ - const Index& getStartIndex() const; + const Index & getStartIndex() const; /*! * Checks if the buffer is at start index (0,0). @@ -482,9 +477,9 @@ class GridMap * @param[in] position position that should be approached as close as possible. * @return position in map. */ - Position getClosestPositionInMap(const Position& position) const; + Position getClosestPositionInMap(const Position & position) const; - private: +private: /** * Defines data validation check * @param value @@ -513,7 +508,9 @@ class GridMap * @param value the data of the cell. * @return true if linear interpolation was successful. */ - bool atPositionLinearInterpolated(const std::string& layer, const Position& position, float& value) const; + bool atPositionLinearInterpolated( + const std::string & layer, const Position & position, + float & value) const; /*! @@ -527,8 +524,9 @@ class GridMap * @param[out] value the data of the cell. * @return true if bicubic convolution interpolation was successful. */ - bool atPositionBicubicConvolutionInterpolated(const std::string& layer, const Position& position, - float& value) const; + bool atPositionBicubicConvolutionInterpolated( + const std::string & layer, const Position & position, + float & value) const; /*! * Get cell data at requested position, cubic interpolated @@ -541,15 +539,16 @@ class GridMap * @param[out] value the data of the cell. * @return true if bicubic interpolation was successful. */ - bool atPositionBicubicInterpolated(const std::string& layer, const Position& position, - float& value) const; + bool atPositionBicubicInterpolated( + const std::string & layer, const Position & position, + float & value) const; /*! * Resize the buffer. * @param bufferSize the requested buffer size. */ - void resize(const Index& bufferSize); + void resize(const Index & bufferSize); //! Frame id of the grid map. std::string frameId_; @@ -583,8 +582,9 @@ class GridMap //! Circular buffer start indeces. Index startIndex_; - public: +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_CORE__GRIDMAP_HPP_ diff --git a/grid_map_core/include/grid_map_core/GridMapMath.hpp b/grid_map_core/include/grid_map_core/GridMapMath.hpp index b45f44aaa..fe2d5017e 100644 --- a/grid_map_core/include/grid_map_core/GridMapMath.hpp +++ b/grid_map_core/include/grid_map_core/GridMapMath.hpp @@ -6,16 +6,18 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once - -#include "grid_map_core/TypeDefs.hpp" -#include "grid_map_core/BufferRegion.hpp" +#ifndef GRID_MAP_CORE__GRIDMAPMATH_HPP_ +#define GRID_MAP_CORE__GRIDMAPMATH_HPP_ #include #include #include -namespace grid_map { +#include "grid_map_core/TypeDefs.hpp" +#include "grid_map_core/BufferRegion.hpp" + +namespace grid_map +{ /*! * Gets the position of a cell specified by its index in the map frame. @@ -28,13 +30,14 @@ namespace grid_map { * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional). * @return true if successful, false if index not within range of buffer. */ -bool getPositionFromIndex(Position& position, - const Index& index, - const Length& mapLength, - const Position& mapPosition, - const double& resolution, - const Size& bufferSize, - const Index& bufferStartIndex = Index::Zero()); +bool getPositionFromIndex( + Position & position, + const Index & index, + const Length & mapLength, + const Position & mapPosition, + const double & resolution, + const Size & bufferSize, + const Index & bufferStartIndex = Index::Zero()); /*! * Gets the index of the cell which contains a position in the map frame. @@ -47,13 +50,14 @@ bool getPositionFromIndex(Position& position, * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional). * @return true if successful, false if position outside of map. */ -bool getIndexFromPosition(Index& index, - const Position& position, - const Length& mapLength, - const Position& mapPosition, - const double& resolution, - const Size& bufferSize, - const Index& bufferStartIndex = Index::Zero()); +bool getIndexFromPosition( + Index & index, + const Position & position, + const Length & mapLength, + const Position & mapPosition, + const double & resolution, + const Size & bufferSize, + const Index & bufferStartIndex = Index::Zero()); /*! * Checks if position is within the map boundaries. @@ -62,9 +66,10 @@ bool getIndexFromPosition(Index& index, * @param[in] mapPosition the position of the map. * @return true if position is within map, false otherwise. */ -bool checkIfPositionWithinMap(const Position& position, - const Length& mapLength, - const Position& mapPosition); +bool checkIfPositionWithinMap( + const Position & position, + const Length & mapLength, + const Position & mapPosition); /*! * Gets the position of the data structure origin. @@ -72,9 +77,10 @@ bool checkIfPositionWithinMap(const Position& position, * @param[in] mapLength the map length. * @param[out] positionOfOrigin the position of the data structure origin. */ -void getPositionOfDataStructureOrigin(const Position& position, - const Length& mapLength, - Position& positionOfOrigin); +void getPositionOfDataStructureOrigin( + const Position & position, + const Length & mapLength, + Position & positionOfOrigin); /*! * Computes how many cells/indeces the map is moved based on a position shift in @@ -85,9 +91,10 @@ void getPositionOfDataStructureOrigin(const Position& position, * @param[in] resolution the resolution of the map. * @return true if successful. */ -bool getIndexShiftFromPositionShift(Index& indexShift, - const Vector& positionShift, - const double& resolution); +bool getIndexShiftFromPositionShift( + Index & indexShift, + const Vector & positionShift, + const double & resolution); /*! * Computes the corresponding position shift from a index shift. Use this function @@ -98,9 +105,10 @@ bool getIndexShiftFromPositionShift(Index& indexShift, * @param[in] resolution the resolution of the map. * @return true if successful. */ -bool getPositionShiftFromIndexShift(Vector& positionShift, - const Index& indexShift, - const double& resolution); +bool getPositionShiftFromIndexShift( + Vector & positionShift, + const Index & indexShift, + const double & resolution); /*! * Checks if index is within range of the buffer. @@ -108,7 +116,7 @@ bool getPositionShiftFromIndexShift(Vector& positionShift, * @param[in] bufferSize the size of the buffer. * @return true if index is within, and false if index is outside of the buffer. */ -bool checkIfIndexInRange(const Index& index, const Size& bufferSize); +bool checkIfIndexInRange(const Index & index, const Size & bufferSize); /*! * Bounds an index that runs out of the range of the buffer. @@ -117,7 +125,7 @@ bool checkIfIndexInRange(const Index& index, const Size& bufferSize); * @param[in/out] index the indeces that will be bounded to the valid region of the buffer. * @param[in] bufferSize the size of the buffer. */ -void boundIndexToRange(Index& index, const Size& bufferSize); +void boundIndexToRange(Index & index, const Size & bufferSize); /*! * Bounds an index that runs out of the range of the buffer. @@ -125,7 +133,7 @@ void boundIndexToRange(Index& index, const Size& bufferSize); * @param[in/out] index the index that will be bounded to the valid region of the buffer. * @param[in] bufferSize the size of the buffer. */ -void boundIndexToRange(int& index, const int& bufferSize); +void boundIndexToRange(int & index, const int & bufferSize); /*! * Wraps an index that runs out of the range of the buffer back into allowed the region. @@ -134,7 +142,7 @@ void boundIndexToRange(int& index, const int& bufferSize); * @param[in/out] index the indeces that will be wrapped into the valid region of the buffer. * @param[in] bufferSize the size of the buffer. */ -void wrapIndexToRange(Index& index, const Size& bufferSize); +void wrapIndexToRange(Index & index, const Size & bufferSize); /*! * Wraps an index that runs out of the range of the buffer back into allowed the region. @@ -142,7 +150,7 @@ void wrapIndexToRange(Index& index, const Size& bufferSize); * @param[in/out] index the index that will be wrapped into the valid region of the buffer. * @param[in] bufferSize the size of the buffer. */ -void wrapIndexToRange(int& index, int bufferSize); +void wrapIndexToRange(int & index, int bufferSize); /*! * Bound (cuts off) the position to lie inside the map. @@ -151,7 +159,9 @@ void wrapIndexToRange(int& index, int bufferSize); * @param[in] mapLength the lengths in x and y direction. * @param[in] mapPosition the position of the map. */ -void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition); +void boundPositionToRange( + Position & position, const Length & mapLength, + const Position & mapPosition); /*! * Provides the alignment transformation from the buffer order (outer/inner storage) @@ -179,18 +189,19 @@ const Eigen::Matrix2i getBufferOrderToMapFrameAlignment(); * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional). * @return true if successful. */ -bool getSubmapInformation(Index& submapTopLeftIndex, - Size& submapBufferSize, - Position& submapPosition, - Length& submapLength, - Index& requestedIndexInSubmap, - const Position& requestedSubmapPosition, - const Length& requestedSubmapLength, - const Length& mapLength, - const Position& mapPosition, - const double& resolution, - const Size& bufferSize, - const Index& bufferStartIndex = Index::Zero()); +bool getSubmapInformation( + Index & submapTopLeftIndex, + Size & submapBufferSize, + Position & submapPosition, + Length & submapLength, + Index & requestedIndexInSubmap, + const Position & requestedSubmapPosition, + const Length & requestedSubmapLength, + const Length & mapLength, + const Position & mapPosition, + const double & resolution, + const Size & bufferSize, + const Index & bufferStartIndex = Index::Zero()); /*! * Computes the buffer size of a submap given a top left and a lower right index. @@ -198,8 +209,9 @@ bool getSubmapInformation(Index& submapTopLeftIndex, * @param bottomRightIndex the bottom right index in the map. * @return buffer size for the submap. */ -Size getSubmapSizeFromCornerIndeces(const Index& topLeftIndex, const Index& bottomRightIndex, - const Size& bufferSize, const Index& bufferStartIndex); +Size getSubmapSizeFromCornerIndeces( + const Index & topLeftIndex, const Index & bottomRightIndex, + const Size & bufferSize, const Index & bufferStartIndex); /*! * Computes the regions in the circular buffer that make up the data for @@ -211,11 +223,12 @@ Size getSubmapSizeFromCornerIndeces(const Index& topLeftIndex, const Index& bott * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional). * @return true if successful, false if requested submap is not fully contained in the map. */ -bool getBufferRegionsForSubmap(std::vector& submapBufferRegions, - const Index& submapIndex, - const Size& submapBufferSize, - const Size& bufferSize, - const Index& bufferStartIndex = Index::Zero()); +bool getBufferRegionsForSubmap( + std::vector & submapBufferRegions, + const Index & submapIndex, + const Size & submapBufferSize, + const Size & bufferSize, + const Index & bufferStartIndex = Index::Zero()); /*! * Increases the index by one to iterate through the map. @@ -226,8 +239,9 @@ bool getBufferRegionsForSubmap(std::vector& submapBufferRegions, * @param[in] bufferStartIndex the map buffer start index. * @return true if successfully incremented indeces, false if end of iteration limits are reached. */ -bool incrementIndex(Index& index, const Size& bufferSize, - const Index& bufferStartIndex = Index::Zero()); +bool incrementIndex( + Index & index, const Size & bufferSize, + const Index & bufferStartIndex = Index::Zero()); /*! * Increases the index by one to iterate through the cells of a submap. @@ -245,9 +259,10 @@ bool incrementIndex(Index& index, const Size& bufferSize, * @param[in] bufferStartIndex the map buffer start index. * @return true if successfully incremented indeces, false if end of iteration limits are reached. */ -bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& submapTopLeftIndex, - const Size& submapBufferSize, const Size& bufferSize, - const Index& bufferStartIndex = Index::Zero()); +bool incrementIndexForSubmap( + Index & submapIndex, Index & index, const Index & submapTopLeftIndex, + const Size & submapBufferSize, const Size & bufferSize, + const Index & bufferStartIndex = Index::Zero()); /*! * Retrieve the index as unwrapped index, i.e., as the corresponding index of a @@ -257,8 +272,9 @@ bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& subm * @param bufferStartIndex the map buffer start index. * @return the unwrapped index. */ -Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize, - const Index& bufferStartIndex); +Index getIndexFromBufferIndex( + const Index & bufferIndex, const Size & bufferSize, + const Index & bufferStartIndex); /*! * Retrieve the index of the buffer from a unwrapped index (reverse from function above). @@ -267,7 +283,9 @@ Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize, * @param bufferStartIndex the map buffer start index. * @return the buffer index. */ -Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const Index& bufferStartIndex); +Index getBufferIndexFromIndex( + const Index & index, const Size & bufferSize, + const Index & bufferStartIndex); /*! * Returns the linear index (1-dim.) corresponding to the regular index (2-dim.) for either @@ -278,7 +296,9 @@ Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const * @param[in] (optional) rowMajor if the linear index is generated for row-major format. * @return the linear 1d index. */ -size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, const bool rowMajor = false); +size_t getLinearIndexFromIndex( + const Index & index, const Size & bufferSize, + const bool rowMajor = false); /*! * Returns the regular index (2-dim.) corresponding to the linear index (1-dim.) for a given buffer size. @@ -287,7 +307,9 @@ size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, const * @param[in] (optional) rowMajor if the linear index is generated for row-major format. * @return the regular 2d index. */ -Index getIndexFromLinearIndex(const size_t linearIndex, const Size& bufferSize, const bool rowMajor = false); +Index getIndexFromLinearIndex( + const size_t linearIndex, const Size & bufferSize, + const bool rowMajor = false); /*! * Generates a list of indices for a region in the map. @@ -295,8 +317,9 @@ Index getIndexFromLinearIndex(const size_t linearIndex, const Size& bufferSize, * @param regionSize the region size. * @param indices the list of indices of the region. */ -void getIndicesForRegion(const Index& regionIndex, const Size& regionSize, - std::vector indices); +void getIndicesForRegion( + const Index & regionIndex, const Size & regionSize, + std::vector indices); /*! * Generates a list of indices for multiple regions in the map. @@ -305,8 +328,9 @@ void getIndicesForRegion(const Index& regionIndex, const Size& regionSize, * @param regionSizes the regions' sizes. * @param indices the list of indices of the regions. */ -void getIndicesForRegions(const std::vector& regionIndeces, const Size& regionSizes, - std::vector indices); +void getIndicesForRegions( + const std::vector & regionIndeces, const Size & regionSizes, + std::vector indices); /*! * Transforms an int color value (concatenated RGB values) to an int color vector (RGB from 0-255). @@ -314,7 +338,7 @@ void getIndicesForRegions(const std::vector& regionIndeces, const Size& r * @param [out] colorVector the color vector in RGB from 0-255. * @return true if successful. */ -bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3i& colorVector); +bool colorValueToVector(const uint64_t & colorValue, Eigen::Vector3i & colorVector); /*! * Transforms an int color value (concatenated RGB values) to a float color vector (RGB from 0.0-1.0). @@ -322,7 +346,7 @@ bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3i& colorV * @param [out] colorVector the color vector in RGB from 0.0-1.0. * @return true if successful. */ -bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3f& colorVector); +bool colorValueToVector(const uint64_t & colorValue, Eigen::Vector3f & colorVector); /*! * Transforms a float color value (concatenated 3 single-byte value) to a float color vector (RGB from 0.0-1.0). @@ -330,7 +354,7 @@ bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3f& colorV * @param [out] colorVector the color vector in RGB from 0.0-1.0. * @return true if successful. */ -bool colorValueToVector(const float& colorValue, Eigen::Vector3f& colorVector); +bool colorValueToVector(const float & colorValue, Eigen::Vector3f & colorVector); /*! * Transforms an int color vector (RGB from 0-255) to a concatenated RGB int color. @@ -338,20 +362,21 @@ bool colorValueToVector(const float& colorValue, Eigen::Vector3f& colorVector); * @param [out] colorValue the concatenated RGB color value. * @return true if successful. */ -bool colorVectorToValue(const Eigen::Vector3i& colorVector, unsigned long& colorValue); +bool colorVectorToValue(const Eigen::Vector3i & colorVector, uint64_t & colorValue); /*! * Transforms a color vector (RGB from 0-255) to a concatenated 3 single-byte float value. * @param [in] colorVector the color vector in RGB from 0-255. * @param [out] colorValue the concatenated RGB color value. */ -void colorVectorToValue(const Eigen::Vector3i& colorVector, float& colorValue); +void colorVectorToValue(const Eigen::Vector3i & colorVector, float & colorValue); /*! * Transforms a color vector (RGB from 0.0-1.0) to a concatenated 3 single-byte float value. * @param [in] colorVector the color vector in RGB from 0.0-1.0. * @param [out] colorValue the concatenated RGB color value. */ -void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue); +void colorVectorToValue(const Eigen::Vector3f & colorVector, float & colorValue); -} // namespace +} // namespace grid_map +#endif // GRID_MAP_CORE__GRIDMAPMATH_HPP_ diff --git a/grid_map_core/include/grid_map_core/Polygon.hpp b/grid_map_core/include/grid_map_core/Polygon.hpp index cf486fbd8..9d7ad27a0 100644 --- a/grid_map_core/include/grid_map_core/Polygon.hpp +++ b/grid_map_core/include/grid_map_core/Polygon.hpp @@ -6,24 +6,27 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CORE__POLYGON_HPP_ +#define GRID_MAP_CORE__POLYGON_HPP_ + +// Eigen +#include #include // STD +#include #include -// Eigen -#include - -namespace grid_map { +namespace grid_map +{ class Polygon { - public: - - enum class TriangulationMethods { - FAN // Fan triangulation (only for convex polygons). +public: + enum class TriangulationMethods + { + FAN // Fan triangulation (only for convex polygons). }; /*! @@ -35,32 +38,32 @@ class Polygon * Constructor with vertices. * @param vertices the points of the polygon. */ - Polygon(std::vector vertices); + explicit Polygon(std::vector vertices); /*! * Destructor. */ - virtual ~Polygon(); + virtual ~Polygon() = default; /*! * Check if point is inside polygon. * @param point the point to be checked. * @return true if inside, false otherwise. */ - bool isInside(const Position& point) const; + bool isInside(const Position & point) const; /*! * Add a vertex to the polygon * @param vertex the point to be added. */ - void addVertex(const Position& vertex); + void addVertex(const Position & vertex); /*! * Get the vertex with index. * @param index the index of the requested vertex. * @return the requested vertex. */ - const Position& getVertex(const size_t index) const; + const Position & getVertex(const size_t index) const; /*! * Removes all vertices from the polygon. @@ -72,13 +75,13 @@ class Polygon * @param index the index of the requested vertex. * @return the requested vertex. */ - const Position& operator [](const size_t index) const; + const Position & operator[](const size_t index) const; /*! * Returns the vertices of the polygon. * @return the vertices of the polygon. */ - const std::vector& getVertices() const; + const std::vector & getVertices() const; /*! * Returns the number of vertices. @@ -107,13 +110,13 @@ class Polygon * Set the frame id of the polygon. * @param frameId the frame id to set. */ - void setFrameId(const std::string& frameId); + void setFrameId(const std::string & frameId); /*! * Get the frameId of the polygon. * @return frameId. */ - const std::string& getFrameId() const; + const std::string & getFrameId() const; /*! * Get the area of the polygon. The polygon has to be @@ -134,7 +137,7 @@ class Polygon * @param center the center of the bounding box. * @param length the side lengths of the bounding box. */ - void getBoundingBox(Position& center, Length& length) const; + void getBoundingBox(Position & center, Length & length) const; /*! * Convert polygon to inequality constraints which most tightly contain the points; i.e., @@ -147,8 +150,9 @@ class Polygon * @param b the b matrix in of the inequality constraint. * @return true if conversion successful, false otherwise. */ - bool convertToInequalityConstraints(Eigen::MatrixXd& A, - Eigen::VectorXd& b) const; + bool convertToInequalityConstraints( + Eigen::MatrixXd & A, + Eigen::VectorXd & b) const; /*! * Offsets the polygon inward (buffering) by a margin. @@ -170,7 +174,9 @@ class Polygon * Return a triangulated version of the polygon. * @return a list of triangle polygons covering the same polygon. */ - std::vector triangulate(const TriangulationMethods& method = TriangulationMethods::FAN) const; + std::vector triangulate( + const TriangulationMethods & method = + TriangulationMethods::FAN) const; /*! * Approximates a circle with a polygon. @@ -179,8 +185,9 @@ class Polygon * @param[in] nVertices number of vertices of the approximation polygon. Default = 20. * @return circle as polygon. */ - static Polygon fromCircle(const Position center, const double radius, - const int nVertices = 20); + static Polygon fromCircle( + const Position center, const double radius, + const int nVertices = 20); /*! * Approximates two circles with a convex hull and returns it as polygon. @@ -190,10 +197,11 @@ class Polygon * @param[in] nVertices number of vertices of the approximation polygon. Default = 20. * @return convex hull of the two circles as polygon. */ - static Polygon convexHullOfTwoCircles(const Position center1, - const Position center2, - const double radius, - const int nVertices = 20); + static Polygon convexHullOfTwoCircles( + const Position center1, + const Position center2, + const double radius, + const int nVertices = 20); /*! * Computes the convex hull of two polygons and returns it as polygon. @@ -201,32 +209,33 @@ class Polygon * @param[in] polygon2 the second input polygon. * @return convex hull as polygon. */ - static Polygon convexHull(Polygon& polygon1, Polygon& polygon2); + static Polygon convexHull(Polygon & polygon1, Polygon & polygon2); /*! * Computes the convex hull of given points, using Andrew's monotone chain convex hull algorithm, and returns it as polygon. * @param[in] points points to use to compute the convex hull used to create the polygon. * @return convex hull as polygon. */ - static Polygon monotoneChainConvexHullOfPoints(const std::vector& points); - - protected: + static Polygon monotoneChainConvexHullOfPoints(const std::vector & points); +protected: /*! * Returns true if the vector1 and vector2 are sorted lexicographically. * @param[in] vector1 the first input vector. * @param[in] vector2 the second input vector. */ - static bool sortVertices(const Eigen::Vector2d& vector1, - const Eigen::Vector2d& vector2); + static bool sortVertices( + const Eigen::Vector2d & vector1, + const Eigen::Vector2d & vector2); /*! * Returns the 2D cross product of vector1 and vector2. * @param[in] vector1 the first input vector. * @param[in] vector2 the second input vector. */ - static double computeCrossProduct2D(const Eigen::Vector2d& vector1, - const Eigen::Vector2d& vector2); + static double computeCrossProduct2D( + const Eigen::Vector2d & vector1, + const Eigen::Vector2d & vector2); /*! * Returns true if OAB makes a clockwise turn or if the OA and OB vectors are collinear. @@ -234,9 +243,10 @@ class Polygon * @param[in] pointA input point A, used to compute OA. * @param[in] pointB input point B, used to compute OB. */ - static double vectorsMakeClockwiseTurn(const Eigen::Vector2d& pointO, - const Eigen::Vector2d& pointA, - const Eigen::Vector2d& pointB); + static double vectorsMakeClockwiseTurn( + const Eigen::Vector2d & pointO, + const Eigen::Vector2d & pointA, + const Eigen::Vector2d & pointB); //! Frame id of the polygon. std::string frameId_; @@ -247,8 +257,9 @@ class Polygon //! Vertices of the polygon. std::vector vertices_; - public: +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /* namespace grid_map */ +} // namespace grid_map +#endif // GRID_MAP_CORE__POLYGON_HPP_ diff --git a/grid_map_core/include/grid_map_core/SubmapGeometry.hpp b/grid_map_core/include/grid_map_core/SubmapGeometry.hpp index 02143b5f0..2b03a1172 100644 --- a/grid_map_core/include/grid_map_core/SubmapGeometry.hpp +++ b/grid_map_core/include/grid_map_core/SubmapGeometry.hpp @@ -6,11 +6,14 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CORE__SUBMAPGEOMETRY_HPP_ +#define GRID_MAP_CORE__SUBMAPGEOMETRY_HPP_ + #include -namespace grid_map { +namespace grid_map +{ class GridMap; @@ -21,8 +24,7 @@ class GridMap; */ class SubmapGeometry { - public: - +public: /*! * Constructor. Note that the requested position and length * of the submap is adapted to fit the geometry of the parent @@ -32,23 +34,23 @@ class SubmapGeometry * @param[in] length the requested submap length. * @param[out] isSuccess true if successful, false otherwise. */ - SubmapGeometry(const GridMap& gridMap, const Position& position, const Length& length, - bool& isSuccess); + SubmapGeometry( + const GridMap & gridMap, const Position & position, const Length & length, + bool & isSuccess); virtual ~SubmapGeometry(); - const GridMap& getGridMap() const; - const Length& getLength() const; - const Position& getPosition() const; - const Index& getRequestedIndexInSubmap() const; - const Size& getSize() const; + const GridMap & getGridMap() const; + const Length & getLength() const; + const Position & getPosition() const; + const Index & getRequestedIndexInSubmap() const; + const Size & getSize() const; double getResolution() const; - const Index& getStartIndex() const; - - private: + const Index & getStartIndex() const; +private: //! Parent grid map of the submap. - const GridMap& gridMap_; + const GridMap & gridMap_; //! Start index (typically top left) index of the submap. Index startIndex_; @@ -66,8 +68,9 @@ class SubmapGeometry //! position of the submap. Index requestedIndexInSubmap_; - public: +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /* namespace grid_map */ +} // namespace grid_map +#endif // GRID_MAP_CORE__SUBMAPGEOMETRY_HPP_ diff --git a/grid_map_core/include/grid_map_core/TypeDefs.hpp b/grid_map_core/include/grid_map_core/TypeDefs.hpp index 7e19c63fc..270a2cdc0 100644 --- a/grid_map_core/include/grid_map_core/TypeDefs.hpp +++ b/grid_map_core/include/grid_map_core/TypeDefs.hpp @@ -6,41 +6,45 @@ * Institute: ETH Zurich, ANYbotics */ +#ifndef GRID_MAP_CORE__TYPEDEFS_HPP_ +#define GRID_MAP_CORE__TYPEDEFS_HPP_ + // Eigen #include -#pragma once - -namespace grid_map { +namespace grid_map +{ - typedef Eigen::MatrixXf Matrix; - typedef Matrix::Scalar DataType; - typedef Eigen::Vector2d Position; - typedef Eigen::Vector2d Vector; - typedef Eigen::Vector3d Position3; - typedef Eigen::Vector3d Vector3; - typedef Eigen::Array2i Index; - typedef Eigen::Array2i Size; - typedef Eigen::Array2d Length; - typedef uint64_t Time; +typedef Eigen::MatrixXf Matrix; +typedef Matrix::Scalar DataType; +typedef Eigen::Vector2d Position; +typedef Eigen::Vector2d Vector; +typedef Eigen::Vector3d Position3; +typedef Eigen::Vector3d Vector3; +typedef Eigen::Array2i Index; +typedef Eigen::Array2i Size; +typedef Eigen::Array2d Length; +typedef uint64_t Time; - /* - * Interpolations are ordered in the order - * of increasing accuracy and computational complexity. - * INTER_NEAREST - fastest, but least accurate, - * INTER_CUBIC - slowest, but the most accurate. - * see: - * https://en.wikipedia.org/wiki/Bicubic_interpolation - * https://web.archive.org/web/20051024202307/http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm - * for more info. Cubic convolution algorithm is also known as piecewise cubic - * interpolation and in general does not guarantee continuous - * first derivatives. - */ - enum class InterpolationMethods{ - INTER_NEAREST, // nearest neighbor interpolation - INTER_LINEAR, // bilinear interpolation - INTER_CUBIC_CONVOLUTION, //piecewise bicubic interpolation using convolution algorithm - INTER_CUBIC // standard bicubic interpolation - }; +/* + * Interpolations are ordered in the order + * of increasing accuracy and computational complexity. + * INTER_NEAREST - fastest, but least accurate, + * INTER_CUBIC - slowest, but the most accurate. + * see: + * https://en.wikipedia.org/wiki/Bicubic_interpolation + * https://web.archive.org/web/20051024202307/http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm + * for more info. Cubic convolution algorithm is also known as piecewise cubic + * interpolation and in general does not guarantee continuous + * first derivatives. + */ +enum class InterpolationMethods +{ + INTER_NEAREST, // nearest neighbor interpolation + INTER_LINEAR, // bilinear interpolation + INTER_CUBIC_CONVOLUTION, // piecewise bicubic interpolation using convolution algorithm + INTER_CUBIC // standard bicubic interpolation +}; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_CORE__TYPEDEFS_HPP_ diff --git a/grid_map_core/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp b/grid_map_core/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp index 8b3dd6f9b..e1f190330 100644 --- a/grid_map_core/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp +++ b/grid_map_core/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp @@ -1,18 +1,22 @@ +#ifndef GRID_MAP_CORE__EIGEN_PLUGINS__DENSEBASEPLUGIN_HPP_ +#define GRID_MAP_CORE__EIGEN_PLUGINS__DENSEBASEPLUGIN_HPP_ + Scalar numberOfFinites() const { - if (SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) return Scalar(0); + if (SizeAtCompileTime == 0 || (SizeAtCompileTime == Dynamic && size() == 0)) {return Scalar(0);} return Scalar((derived().array() == derived().array()).count()); } Scalar sumOfFinites() const { - if (SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) return Scalar(0); + if (SizeAtCompileTime == 0 || (SizeAtCompileTime == Dynamic && size() == 0)) {return Scalar(0);} return Scalar(this->redux(Eigen::internal::scalar_sum_of_finites_op())); } Scalar meanOfFinites() const { - return Scalar(this->redux(Eigen::internal::scalar_sum_of_finites_op())) / this->numberOfFinites(); + return Scalar(this->redux(Eigen::internal::scalar_sum_of_finites_op())) / + this->numberOfFinites(); } Scalar minCoeffOfFinites() const @@ -24,3 +28,4 @@ Scalar maxCoeffOfFinites() const { return Scalar(this->redux(Eigen::internal::scalar_max_of_finites_op())); } +#endif // GRID_MAP_CORE__EIGEN_PLUGINS__DENSEBASEPLUGIN_HPP_ diff --git a/grid_map_core/include/grid_map_core/eigen_plugins/Functors.hpp b/grid_map_core/include/grid_map_core/eigen_plugins/Functors.hpp index c4963c0fa..bbe3a4acc 100644 --- a/grid_map_core/include/grid_map_core/eigen_plugins/Functors.hpp +++ b/grid_map_core/include/grid_map_core/eigen_plugins/Functors.hpp @@ -6,23 +6,26 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CORE__EIGEN_PLUGINS__FUNCTORS_HPP_ +#define GRID_MAP_CORE__EIGEN_PLUGINS__FUNCTORS_HPP_ -namespace grid_map { +namespace grid_map +{ template struct Clamp { - Clamp(const Scalar& min, const Scalar& max) - : min_(min), - max_(max) + Clamp(const Scalar & min, const Scalar & max) + : min_(min), + max_(max) { } - const Scalar operator()(const Scalar& x) const + const Scalar operator()(const Scalar & x) const { return x < min_ ? min_ : (x > max_ ? max_ : x); } Scalar min_, max_; }; -} +} // namespace grid_map +#endif // GRID_MAP_CORE__EIGEN_PLUGINS__FUNCTORS_HPP_ diff --git a/grid_map_core/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp b/grid_map_core/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp index 79dcdd665..b5c3abba0 100644 --- a/grid_map_core/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp +++ b/grid_map_core/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp @@ -1,59 +1,76 @@ +#ifndef GRID_MAP_CORE__EIGEN_PLUGINS__FUNCTORSPLUGIN_HPP_ +#define GRID_MAP_CORE__EIGEN_PLUGINS__FUNCTORSPLUGIN_HPP_ + #include -template struct scalar_sum_of_finites_op { +template +struct scalar_sum_of_finites_op +{ EIGEN_EMPTY_STRUCT_CTOR(scalar_sum_of_finites_op) - EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { + EIGEN_STRONG_INLINE const Scalar operator()(const Scalar & a, const Scalar & b) const + { using std::isfinite; - if (isfinite(a) && isfinite(b)) return a + b; - if (isfinite(a)) return a; - if (isfinite(b)) return b; + if (isfinite(a) && isfinite(b)) {return a + b;} + if (isfinite(a)) {return a;} + if (isfinite(b)) {return b;} return a + b; } }; template -struct functor_traits > { - enum { +struct functor_traits> +{ + enum + { Cost = 2 * NumTraits::ReadCost + NumTraits::AddCost, PacketAccess = false }; }; template -struct scalar_min_of_finites_op { +struct scalar_min_of_finites_op +{ EIGEN_EMPTY_STRUCT_CTOR(scalar_min_of_finites_op) - EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { + EIGEN_STRONG_INLINE const Scalar operator()(const Scalar & a, const Scalar & b) const + { using std::min; using std::isfinite; - if (isfinite(a) && isfinite(b)) return (min)(a, b); - if (isfinite(a)) return a; - if (isfinite(b)) return b; + if (isfinite(a) && isfinite(b)) {return (min)(a, b);} + if (isfinite(a)) {return a;} + if (isfinite(b)) {return b;} return (min)(a, b); } }; template -struct functor_traits > { - enum { +struct functor_traits> +{ + enum + { Cost = NumTraits::AddCost, PacketAccess = false }; }; template -struct scalar_max_of_finites_op { +struct scalar_max_of_finites_op +{ EIGEN_EMPTY_STRUCT_CTOR(scalar_max_of_finites_op) - EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { + EIGEN_STRONG_INLINE const Scalar operator()(const Scalar & a, const Scalar & b) const + { using std::max; using std::isfinite; - if (isfinite(a) && isfinite(b)) return (max)(a, b); - if (isfinite(a)) return a; - if (isfinite(b)) return b; + if (isfinite(a) && isfinite(b)) {return (max)(a, b);} + if (isfinite(a)) {return a;} + if (isfinite(b)) {return b;} return (max)(a, b); } }; template -struct functor_traits > { - enum { +struct functor_traits> +{ + enum + { Cost = NumTraits::AddCost, PacketAccess = false }; }; +#endif // GRID_MAP_CORE__EIGEN_PLUGINS__FUNCTORSPLUGIN_HPP_ diff --git a/grid_map_core/include/grid_map_core/grid_map_core.hpp b/grid_map_core/include/grid_map_core/grid_map_core.hpp index d71431133..c7660ef59 100644 --- a/grid_map_core/include/grid_map_core/grid_map_core.hpp +++ b/grid_map_core/include/grid_map_core/grid_map_core.hpp @@ -6,7 +6,8 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CORE__GRID_MAP_CORE_HPP_ +#define GRID_MAP_CORE__GRID_MAP_CORE_HPP_ #include "grid_map_core/TypeDefs.hpp" #include "grid_map_core/GridMap.hpp" @@ -16,3 +17,5 @@ #include "grid_map_core/Polygon.hpp" #include "grid_map_core/iterators/iterators.hpp" #include "grid_map_core/eigen_plugins/Functors.hpp" + +#endif // GRID_MAP_CORE__GRID_MAP_CORE_HPP_ diff --git a/grid_map_core/include/grid_map_core/gtest_eigen.hpp b/grid_map_core/include/grid_map_core/gtest_eigen.hpp index 18c55b63a..f7599fd44 100644 --- a/grid_map_core/include/grid_map_core/gtest_eigen.hpp +++ b/grid_map_core/include/grid_map_core/gtest_eigen.hpp @@ -5,157 +5,208 @@ * @brief Code to simplify Eigen integration into GTest. Pretty basic but the error messages are good. */ -#pragma once + +#ifndef GRID_MAP_CORE__GTEST_EIGEN_HPP_ +#define GRID_MAP_CORE__GTEST_EIGEN_HPP_ #include #include #include #include - -namespace grid_map { - - template - Eigen::Matrix randomCovariance() - { - Eigen::Matrix U; - U.setRandom(); - return U.transpose() * U + 5.0 * Eigen::Matrix::Identity(); - } - - inline Eigen::MatrixXd randomCovarianceXd(int N) - { - Eigen::MatrixXd U(N,N); - U.setRandom(); - return U.transpose() * U + 5.0 * Eigen::MatrixXd::Identity(N,N); - } - - template - void assertEqual(const M1 & A, const M2 & B, std::string const & message = "") - { - ASSERT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n"; - ASSERT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n"; - - for(int r = 0; r < A.rows(); r++) - { - for(int c = 0; c < A.cols(); c++) - { - if (std::isnan(A(r,c))) { - ASSERT_TRUE(std::isnan(B(r,c))); - } else { - ASSERT_EQ(A(r,c),B(r,c)) << message << "\nEquality comparison failed at (" << r << "," << c << ")\n" - << "\nMatrix A:\n" << A << "\nand matrix B\n" << B; - } - } - } +#include +#include + +namespace grid_map +{ + +template +Eigen::Matrix randomCovariance() +{ + Eigen::Matrix U; + U.setRandom(); + return U.transpose() * U + 5.0 * Eigen::Matrix::Identity(); +} + +inline Eigen::MatrixXd randomCovarianceXd(int N) +{ + Eigen::MatrixXd U(N, N); + U.setRandom(); + return U.transpose() * U + 5.0 * Eigen::MatrixXd::Identity(N, N); +} + +template +void assertEqual(const M1 & A, const M2 & B, std::string const & message = "") +{ + ASSERT_EQ( + (size_t)A.rows(), + (size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << + "\nare not the same\n"; + ASSERT_EQ( + (size_t)A.cols(), + (size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << + "\nare not the same\n"; + + for (int r = 0; r < A.rows(); r++) { + for (int c = 0; c < A.cols(); c++) { + if (std::isnan(A(r, c))) { + ASSERT_TRUE(std::isnan(B(r, c))); + } else { + ASSERT_EQ( + A(r, c), + B(r, c)) << message << "\nEquality comparison failed at (" << r << "," << c << ")\n" << + "\nMatrix A:\n" << A << "\nand matrix B\n" << B; + } } - - template - void assertNear(const M1 & A, const M2 & B, T tolerance, std::string const & message = "") - { - // Note: If these assertions fail, they only abort this subroutine. - // see: http://code.google.com/p/googletest/wiki/AdvancedGuide#Using_Assertions_in_Sub-routines - // \todo better handling of this - ASSERT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n"; - ASSERT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n"; - - for(int r = 0; r < A.rows(); r++) - { - for(int c = 0; c < A.cols(); c++) - { - if (std::isnan(A(r,c))) { - ASSERT_TRUE(std::isnan(B(r,c))); - } else { - ASSERT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n" - << "\nMatrix A:\n" << A << "\nand matrix B\n" << B; - } - } - } + } +} + +template +void assertNear(const M1 & A, const M2 & B, T tolerance, std::string const & message = "") +{ + // Note: If these assertions fail, they only abort this subroutine. + // see: http://code.google.com/p/googletest/wiki/AdvancedGuide#Using_Assertions_in_Sub-routines + // \todo better handling of this + ASSERT_EQ( + (size_t)A.rows(), + (size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << + "\nare not the same\n"; + ASSERT_EQ( + (size_t)A.cols(), + (size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << + "\nare not the same\n"; + + for (int r = 0; r < A.rows(); r++) { + for (int c = 0; c < A.cols(); c++) { + if (std::isnan(A(r, c))) { + ASSERT_TRUE(std::isnan(B(r, c))); + } else { + ASSERT_NEAR( + A(r, c), B(r, c), + tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << + ")\n" << + "\nMatrix A:\n" << A << "\nand matrix B\n" << B; + } } - - template - void expectNear(const M1 & A, const M2 & B, T tolerance, std::string const & message = "") - { - EXPECT_EQ((size_t)A.rows(),(size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n"; - EXPECT_EQ((size_t)A.cols(),(size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << "\nare not the same\n"; - - for(int r = 0; r < A.rows(); r++) - { - for(int c = 0; c < A.cols(); c++) - { - if (std::isnan(A(r,c))) { - EXPECT_TRUE(std::isnan(B(r,c))); - } else { - EXPECT_NEAR(A(r,c),B(r,c),tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << ")\n" - << "\nMatrix A:\n" << A << "\nand matrix B\n" << B; - } - } - } + } +} + +template +void expectNear(const M1 & A, const M2 & B, T tolerance, std::string const & message = "") +{ + EXPECT_EQ( + (size_t)A.rows(), + (size_t)B.rows()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << + "\nare not the same\n"; + EXPECT_EQ( + (size_t)A.cols(), + (size_t)B.cols()) << message << "\nMatrix A:\n" << A << "\nand matrix B\n" << B << + "\nare not the same\n"; + + for (int r = 0; r < A.rows(); r++) { + for (int c = 0; c < A.cols(); c++) { + if (std::isnan(A(r, c))) { + EXPECT_TRUE(std::isnan(B(r, c))); + } else { + EXPECT_NEAR( + A(r, c), B(r, c), + tolerance) << message << "\nTolerance comparison failed at (" << r << "," << c << + ")\n" << + "\nMatrix A:\n" << A << "\nand matrix B\n" << B; + } } - - template - void assertFinite(const M1 & A, std::string const & message = "") - { - for(int r = 0; r < A.rows(); r++) - { - for(int c = 0; c < A.cols(); c++) - { - ASSERT_TRUE(std::isfinite(A(r,c))) << std::endl << "Check for finite values failed at A(" << r << "," << c << "). Matrix A:" << std::endl << A << std::endl; - } - } + } +} + +template +void assertFinite(const M1 & A, std::string const & /* message */) +{ + for (int r = 0; r < A.rows(); r++) { + for (int c = 0; c < A.cols(); c++) { + ASSERT_TRUE( + std::isfinite( + A( + r, + c))) << std::endl << "Check for finite values failed at A(" << r << "," << c << + "). Matrix A:" << std::endl << A << std::endl; } - - inline bool compareRelative(double a, double b, double percentTolerance, double * percentError = NULL) - { - // \todo: does anyone have a better idea? - double fa = fabs(a); - double fb = fabs(b); - if( (fa < 1e-15 && fb < 1e-15) || // Both zero. - (fa == 0.0 && fb < 1e-6) || // One exactly zero and the other small - (fb == 0.0 && fa < 1e-6) ) // ditto - return true; - - double diff = fabs(a - b)/std::max(fa,fb); - if(diff > percentTolerance * 1e-2) - { - if(percentError) - *percentError = diff * 100.0; - return false; - } - return true; + } +} + +inline bool compareRelative( + double a, double b, double percentTolerance, + double * percentError = NULL) +{ + // \todo: does anyone have a better idea? + double fa = fabs(a); + double fb = fabs(b); + if ( (fa < 1e-15 && fb < 1e-15) || // Both zero. + (fa == 0.0 && fb < 1e-6) || // One exactly zero and the other small + (fb == 0.0 && fa < 1e-6) ) // ditto + { + return true; + } + + double diff = fabs(a - b) / std::max(fa, fb); + if (diff > percentTolerance * 1e-2) { + if (percentError) { + *percentError = diff * 100.0; } - -#define ASSERT_DOUBLE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \ - ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \ - ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \ - for(int r = 0; r < (A).rows(); r++) \ - { \ - for(int c = 0; c < (A).cols(); c++) \ - { \ - double percentError = 0.0; \ - ASSERT_TRUE(grid_map::compareRelative( (A)(r,c), (B)(r,c), PERCENT_TOLERANCE, &percentError)) \ - << MSG << "\nComparing:\n" \ - << #A << "(" << r << "," << c << ") = " << (A)(r,c) << std::endl \ - << #B << "(" << r << "," << c << ") = " << (B)(r,c) << std::endl \ - << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \ - << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \ - } \ - } - -#define ASSERT_DOUBLE_SPARSE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \ - ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \ - ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \ - for(int r = 0; r < (A).rows(); r++) \ - { \ - for(int c = 0; c < (A).cols(); c++) \ - { \ - double percentError = 0.0; \ - ASSERT_TRUE(grid_map::compareRelative( (A).coeff(r,c), (B).coeff(r,c), PERCENT_TOLERANCE, &percentError)) \ - << MSG << "\nComparing:\n" \ - << #A << "(" << r << "," << c << ") = " << (A).coeff(r,c) << std::endl \ - << #B << "(" << r << "," << c << ") = " << (B).coeff(r,c) << std::endl \ + return false; + } + return true; +} + +#define ASSERT_DOUBLE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \ + ASSERT_EQ( \ + (size_t)(A).rows(), \ + (size_t)(B).rows()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << \ + "\n" << B << "\nare not the same size"; \ + ASSERT_EQ( \ + (size_t)(A).cols(), \ + (size_t)(B).cols()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << \ + "\n" << B << "\nare not the same size"; \ + for (int r = 0; r < (A).rows(); r++) \ + { \ + for (int c = 0; c < (A).cols(); c++) \ + { \ + double percentError = 0.0; \ + ASSERT_TRUE( \ + grid_map::compareRelative( \ + (A)(r, c), (B)(r, c), PERCENT_TOLERANCE, \ + &percentError)) \ + << MSG << "\nComparing:\n" \ + << #A << "(" << r << "," << c << ") = " << (A)(r, c) << std::endl \ + << #B << "(" << r << "," << c << ") = " << (B)(r, c) << std::endl \ << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \ << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \ - } \ - } + } \ + } + +#define ASSERT_DOUBLE_SPARSE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \ + ASSERT_EQ( \ + (size_t)(A).rows(), \ + (size_t)(B).rows()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << \ + "\n" << B << "\nare not the same size"; \ + ASSERT_EQ( \ + (size_t)(A).cols(), \ + (size_t)(B).cols()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << \ + "\n" << B << "\nare not the same size"; \ + for (int r = 0; r < (A).rows(); r++) \ + { \ + for (int c = 0; c < (A).cols(); c++) \ + { \ + double percentError = 0.0; \ + ASSERT_TRUE( \ + grid_map::compareRelative( \ + (A).coeff(r, c), (B).coeff(r, c), PERCENT_TOLERANCE, \ + &percentError)) \ + << MSG << "\nComparing:\n" \ + << #A << "(" << r << "," << c << ") = " << (A).coeff(r, c) << std::endl \ + << #B << "(" << r << "," << c << ") = " << (B).coeff(r, c) << std::endl \ + << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \ + << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \ + } \ + } -} // namespace +} // namespace grid_map +#endif // GRID_MAP_CORE__GTEST_EIGEN_HPP_ diff --git a/grid_map_core/include/grid_map_core/iterators/CircleIterator.hpp b/grid_map_core/include/grid_map_core/iterators/CircleIterator.hpp index 054819ba6..38edcc454 100644 --- a/grid_map_core/include/grid_map_core/iterators/CircleIterator.hpp +++ b/grid_map_core/include/grid_map_core/iterators/CircleIterator.hpp @@ -5,17 +5,18 @@ * Author: Péter Fankhauser * Institute: ETH Zurich, ANYbotics */ - -#pragma once - -#include "grid_map_core/GridMap.hpp" -#include "grid_map_core/iterators/SubmapIterator.hpp" +#ifndef GRID_MAP_CORE__ITERATORS__CIRCLEITERATOR_HPP_ +#define GRID_MAP_CORE__ITERATORS__CIRCLEITERATOR_HPP_ #include #include -namespace grid_map { +#include "grid_map_core/GridMap.hpp" +#include "grid_map_core/iterators/SubmapIterator.hpp" + +namespace grid_map +{ /*! * Iterator class to iterate through a circular area of the map. @@ -23,39 +24,38 @@ namespace grid_map { class CircleIterator { public: - /*! * Constructor. * @param gridMap the grid map to iterate on. * @param center the position of the circle center. * @param radius the radius of the circle. */ - CircleIterator(const GridMap& gridMap, const Position& center, const double radius); + CircleIterator(const GridMap & gridMap, const Position & center, const double radius); /*! * Assignment operator. * @param iterator the iterator to copy data from. * @return a reference to *this. */ - CircleIterator& operator =(const CircleIterator& other); + CircleIterator & operator=(const CircleIterator & other); /*! * Compare to another iterator. * @return whether the current iterator points to a different address than the other one. */ - bool operator !=(const CircleIterator& other) const; + bool operator!=(const CircleIterator & other) const; /*! * Dereference the iterator with const. * @return the value to which the iterator is pointing. */ - const Index& operator *() const; + const Index & operator*() const; /*! * Increase the iterator to the next element. * @return a reference to the updated iterator. */ - CircleIterator& operator ++(); + CircleIterator & operator++(); /*! * Indicates if iterator is past end. @@ -64,7 +64,6 @@ class CircleIterator bool isPastEnd() const; private: - /*! * Check if current index is inside the circle. * @return true if inside, false otherwise. @@ -78,8 +77,9 @@ class CircleIterator * @param[out] startIndex the start index of the submap. * @param[out] bufferSize the buffer size of the submap. */ - void findSubmapParameters(const Position& center, const double radius, - Index& startIndex, Size& bufferSize) const; + void findSubmapParameters( + const Position & center, const double radius, + Index & startIndex, Size & bufferSize) const; //! Position of the circle center; Position center_; @@ -90,7 +90,7 @@ class CircleIterator //! Square of the radius (for efficiency). double radiusSquare_; - //! Grid submap iterator. // TODO Think of using unique_ptr instead. + //! Grid submap iterator. // TODO(needs_assignment): Think of using unique_ptr instead. std::shared_ptr internalIterator_; //! Map information needed to get position from iterator. @@ -100,8 +100,9 @@ class CircleIterator Size bufferSize_; Index bufferStartIndex_; - public: +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_CORE__ITERATORS__CIRCLEITERATOR_HPP_ diff --git a/grid_map_core/include/grid_map_core/iterators/EllipseIterator.hpp b/grid_map_core/include/grid_map_core/iterators/EllipseIterator.hpp index 2506e4ac9..39ab40077 100644 --- a/grid_map_core/include/grid_map_core/iterators/EllipseIterator.hpp +++ b/grid_map_core/include/grid_map_core/iterators/EllipseIterator.hpp @@ -5,17 +5,17 @@ * Author: Péter Fankhauser * Institute: ETH Zurich, ANYbotics */ +#ifndef GRID_MAP_CORE__ITERATORS__ELLIPSEITERATOR_HPP_ +#define GRID_MAP_CORE__ITERATORS__ELLIPSEITERATOR_HPP_ -#pragma once +#include +#include #include "grid_map_core/GridMap.hpp" #include "grid_map_core/iterators/SubmapIterator.hpp" -#include - -#include - -namespace grid_map { +namespace grid_map +{ /*! * Iterator class to iterate through a ellipsoid area of the map. @@ -24,7 +24,6 @@ namespace grid_map { class EllipseIterator { public: - /*! * Constructor. * @param gridMap the grid map to iterate on. @@ -32,32 +31,34 @@ class EllipseIterator * @param length the length of the main axis. * @param angle the rotation angle of the ellipse (in [rad]). */ - EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation = 0.0); + EllipseIterator( + const GridMap & gridMap, const Position & center, const Length & length, + const double rotation = 0.0); /*! * Assignment operator. * @param iterator the iterator to copy data from. * @return a reference to *this. */ - EllipseIterator& operator =(const EllipseIterator& other); + EllipseIterator & operator=(const EllipseIterator & other); /*! * Compare to another iterator. * @return whether the current iterator points to a different address than the other one. */ - bool operator !=(const EllipseIterator& other) const; + bool operator!=(const EllipseIterator & other) const; /*! * Dereference the iterator with const. * @return the value to which the iterator is pointing. */ - const Index& operator *() const; + const Index & operator*() const; /*! * Increase the iterator to the next element. * @return a reference to the updated iterator. */ - EllipseIterator& operator ++(); + EllipseIterator & operator++(); /*! * Indicates if iterator is past end. @@ -69,10 +70,9 @@ class EllipseIterator * Returns the size of the submap covered by the iterator. * @return the size of the submap covered by the iterator. */ - const Size& getSubmapSize() const; + const Size & getSubmapSize() const; private: - /*! * Check if current index is inside the ellipse. * @return true if inside, false otherwise. @@ -87,8 +87,9 @@ class EllipseIterator * @param[out] startIndex the start index of the submap. * @param[out] bufferSize the buffer size of the submap. */ - void findSubmapParameters(const Position& center, const Length& length, const double rotation, - Index& startIndex, Size& bufferSize) const; + void findSubmapParameters( + const Position & center, const Length & length, const double rotation, + Index & startIndex, Size & bufferSize) const; //! Position of the circle center; Position center_; @@ -99,7 +100,7 @@ class EllipseIterator //! Sine and cosine values of the rotation angle as transformation matrix. Eigen::Matrix2d transformMatrix_; - //! Grid submap iterator. // TODO Think of using unique_ptr instead. + //! Grid submap iterator. // TODO(needs_assignment): Think of using unique_ptr instead. std::shared_ptr internalIterator_; //! Map information needed to get position from iterator. @@ -109,8 +110,9 @@ class EllipseIterator Size bufferSize_; Index bufferStartIndex_; - public: +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_CORE__ITERATORS__ELLIPSEITERATOR_HPP_ diff --git a/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp b/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp index 2beeca89a..86a621be4 100644 --- a/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp +++ b/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp @@ -5,15 +5,15 @@ * Author: Péter Fankhauser * Institute: ETH Zurich, ANYbotics */ +#ifndef GRID_MAP_CORE__ITERATORS__GRIDMAPITERATOR_HPP_ +#define GRID_MAP_CORE__ITERATORS__GRIDMAPITERATOR_HPP_ -#pragma once +#include #include "grid_map_core/GridMap.hpp" -// Eigen -#include - -namespace grid_map { +namespace grid_map +{ /*! * Iterator class to iterate trough the entire grid map. @@ -21,38 +21,37 @@ namespace grid_map { class GridMapIterator { public: - /*! * Constructor. * @param gridMap the grid map to iterate on. */ - GridMapIterator(const grid_map::GridMap &gridMap); + explicit GridMapIterator(const grid_map::GridMap & gridMap); /*! * Copy constructor. * @param other the object to copy. */ - GridMapIterator(const GridMapIterator* other); + explicit GridMapIterator(const GridMapIterator * other); /*! * Assignment operator. * @param iterator the iterator to copy data from. * @return a reference to *this. */ - GridMapIterator& operator =(const GridMapIterator& other); + GridMapIterator & operator=(const GridMapIterator & other); /*! * Compare to another iterator. * @return whether the current iterator points to a different address than the other one. */ - bool operator !=(const GridMapIterator& other) const; + bool operator!=(const GridMapIterator & other) const; /*! * Dereference the iterator to return the regular index (2-dim.) of the cell * to which the iterator is pointing at. * @return the regular index (2-dim.) of the cell on which the iterator is pointing. */ - const Index operator *() const; + const Index operator*() const; /*! * Returns the the linear (1-dim.) index of the cell the iterator is pointing at. @@ -60,7 +59,7 @@ class GridMapIterator * Example: See `runGridMapIteratorVersion3()` of `grid_map_demos/src/iterator_benchmark.cpp`. * @return the 1d linear index. */ - const size_t& getLinearIndex() const; + const size_t & getLinearIndex() const; /*! * Retrieve the index as unwrapped index, i.e., as the corresponding index of a @@ -72,7 +71,7 @@ class GridMapIterator * Increase the iterator to the next element. * @return a reference to the updated iterator. */ - virtual GridMapIterator& operator ++(); + virtual GridMapIterator & operator++(); /*! * Return the end iterator @@ -87,7 +86,6 @@ class GridMapIterator bool isPastEnd() const; protected: - //! Size of the buffer. Size size_; @@ -103,8 +101,10 @@ class GridMapIterator //! Is iterator out of scope. bool isPastEnd_; - public: +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /* namespace */ +} // namespace grid_map + +#endif // GRID_MAP_CORE__ITERATORS__GRIDMAPITERATOR_HPP_ diff --git a/grid_map_core/include/grid_map_core/iterators/LineIterator.hpp b/grid_map_core/include/grid_map_core/iterators/LineIterator.hpp index 868b74b38..85778a103 100644 --- a/grid_map_core/include/grid_map_core/iterators/LineIterator.hpp +++ b/grid_map_core/include/grid_map_core/iterators/LineIterator.hpp @@ -6,14 +6,16 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CORE__ITERATORS__LINEITERATOR_HPP_ +#define GRID_MAP_CORE__ITERATORS__LINEITERATOR_HPP_ + +#include #include "grid_map_core/GridMap.hpp" #include "grid_map_core/iterators/SubmapIterator.hpp" -#include - -namespace grid_map { +namespace grid_map +{ /*! * Iterator class to iterate over a line in the map. @@ -22,7 +24,6 @@ namespace grid_map { class LineIterator { public: - /*! * Constructor. * @param gridMap the grid map to iterate on. @@ -30,7 +31,7 @@ class LineIterator * @param end the ending point of the line. * @throw std::invalid_argument if start and end impose an ill conditioned line iteration. */ - LineIterator(const grid_map::GridMap& gridMap, const Position& start, const Position& end); + LineIterator(const grid_map::GridMap & gridMap, const Position & start, const Position & end); /*! * Constructor. @@ -38,32 +39,32 @@ class LineIterator * @param start the starting index of the line. * @param end the ending index of the line. */ - LineIterator(const grid_map::GridMap& gridMap, const Index& start, const Index& end); + LineIterator(const grid_map::GridMap & gridMap, const Index & start, const Index & end); /*! * Assignment operator. * @param iterator the iterator to copy data from. * @return a reference to *this. */ - LineIterator& operator =(const LineIterator& other); + LineIterator & operator=(const LineIterator & other); /*! * Compare to another iterator. * @return whether the current iterator points to a different address than the other one. */ - bool operator !=(const LineIterator& other) const; + bool operator!=(const LineIterator & other) const; /*! * Dereference the iterator with const. * @return the value to which the iterator is pointing. */ - const Index& operator *() const; + const Index & operator*() const; /*! * Increase the iterator to the next element. * @return a reference to the updated iterator. */ - LineIterator& operator ++(); + LineIterator & operator++(); /*! * Indicates if iterator is past end. @@ -72,8 +73,6 @@ class LineIterator bool isPastEnd() const; private: - - /*! * Construct function. * @param gridMap the grid map to iterate on. @@ -81,7 +80,7 @@ class LineIterator * @param end the ending index of the line. * @return true if successful, false otherwise. */ - bool initialize(const grid_map::GridMap& gridMap, const Index& start, const Index& end); + bool initialize(const grid_map::GridMap & gridMap, const Index & start, const Index & end); /*! * Computes the parameters requires for the line drawing algorithm. @@ -96,8 +95,9 @@ class LineIterator * @param[out] index the index of the moved start position. * @return true if successful, false otherwise. */ - bool getIndexLimitedToMapRange(const grid_map::GridMap& gridMap, const Position& start, - const Position& end, Index& index); + bool getIndexLimitedToMapRange( + const grid_map::GridMap & gridMap, const Position & start, + const Position & end, Index & index); //! Current index. Index index_; @@ -125,8 +125,9 @@ class LineIterator Size bufferSize_; Index bufferStartIndex_; - public: +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_CORE__ITERATORS__LINEITERATOR_HPP_ diff --git a/grid_map_core/include/grid_map_core/iterators/PolygonIterator.hpp b/grid_map_core/include/grid_map_core/iterators/PolygonIterator.hpp index da68fd65c..4665f52cf 100644 --- a/grid_map_core/include/grid_map_core/iterators/PolygonIterator.hpp +++ b/grid_map_core/include/grid_map_core/iterators/PolygonIterator.hpp @@ -6,15 +6,17 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CORE__ITERATORS__POLYGONITERATOR_HPP_ +#define GRID_MAP_CORE__ITERATORS__POLYGONITERATOR_HPP_ + +#include #include "grid_map_core/GridMap.hpp" #include "grid_map_core/Polygon.hpp" #include "grid_map_core/iterators/SubmapIterator.hpp" -#include - -namespace grid_map { +namespace grid_map +{ /*! * Iterator class to iterate through a polygonal area of the map. @@ -22,38 +24,37 @@ namespace grid_map { class PolygonIterator { public: - /*! * Constructor. * @param gridMap the grid map to iterate on. * @param polygon the polygonal area to iterate on. */ - PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon); + PolygonIterator(const grid_map::GridMap & gridMap, const grid_map::Polygon & polygon); /*! * Assignment operator. * @param iterator the iterator to copy data from. * @return a reference to *this. */ - PolygonIterator& operator =(const PolygonIterator& other); + PolygonIterator & operator=(const PolygonIterator & other); /*! * Compare to another iterator. * @return whether the current iterator points to a different address than the other one. */ - bool operator !=(const PolygonIterator& other) const; + bool operator!=(const PolygonIterator & other) const; /*! * Dereference the iterator with const. * @return the value to which the iterator is pointing. */ - const Index& operator *() const; + const Index & operator*() const; /*! * Increase the iterator to the next element. * @return a reference to the updated iterator. */ - PolygonIterator& operator ++(); + PolygonIterator & operator++(); /*! * Indicates if iterator is past end. @@ -62,7 +63,6 @@ class PolygonIterator bool isPastEnd() const; private: - /*! * Check if current index is inside polygon. * @return true if inside, false otherwise. @@ -75,7 +75,9 @@ class PolygonIterator * @param[out] startIndex the start index of the submap. * @param[out] bufferSize the buffer size of the submap. */ - void findSubmapParameters(const grid_map::Polygon& polygon, Index& startIndex,Size& bufferSize) const; + void findSubmapParameters( + const grid_map::Polygon & polygon, Index & startIndex, + Size & bufferSize) const; //! Polygon to iterate on. grid_map::Polygon polygon_; @@ -90,8 +92,10 @@ class PolygonIterator Size bufferSize_; Index bufferStartIndex_; - public: +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /* namespace */ +} // namespace grid_map + +#endif // GRID_MAP_CORE__ITERATORS__POLYGONITERATOR_HPP_ diff --git a/grid_map_core/include/grid_map_core/iterators/SlidingWindowIterator.hpp b/grid_map_core/include/grid_map_core/iterators/SlidingWindowIterator.hpp index c581f8fbf..5614be04b 100644 --- a/grid_map_core/include/grid_map_core/iterators/SlidingWindowIterator.hpp +++ b/grid_map_core/include/grid_map_core/iterators/SlidingWindowIterator.hpp @@ -6,15 +6,17 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CORE__ITERATORS__SLIDINGWINDOWITERATOR_HPP_ +#define GRID_MAP_CORE__ITERATORS__SLIDINGWINDOWITERATOR_HPP_ -#include "grid_map_core/GridMap.hpp" +#include +#include +#include "grid_map_core/GridMap.hpp" #include "grid_map_core/iterators/GridMapIterator.hpp" -#include - -namespace grid_map { +namespace grid_map +{ /*! * Iterator class to iterate trough the entire grid map with access to a layer's @@ -24,12 +26,12 @@ namespace grid_map { class SlidingWindowIterator : public GridMapIterator { public: - - enum class EdgeHandling { - INSIDE, // Only visit indices that are surrounded by a full window. - CROP, // Crop data matrix with missing cells at edges. - EMPTY, // Fill in missing edges with empty cells (NAN-value). - MEAN // Fill in missing edges with MEAN of valid values. + enum class EdgeHandling + { + INSIDE, // Only visit indices that are surrounded by a full window. + CROP, // Crop data matrix with missing cells at edges. + EMPTY, // Fill in missing edges with empty cells (NAN-value). + MEAN // Fill in missing edges with MEAN of valid values. }; /*! @@ -39,28 +41,29 @@ class SlidingWindowIterator : public GridMapIterator * @param edgeHandling the method to handle edges of the map. * @param windowSize the size of the moving window in number of cells (has to be an odd number!). */ - SlidingWindowIterator(const GridMap& gridMap, const std::string& layer, - const EdgeHandling& edgeHandling = EdgeHandling::CROP, - const size_t windowSize = 3); + SlidingWindowIterator( + const GridMap & gridMap, const std::string & layer, + const EdgeHandling & edgeHandling = EdgeHandling::CROP, + const size_t windowSize = 3); /*! * Copy constructor. * @param other the object to copy. */ - SlidingWindowIterator(const SlidingWindowIterator* other); + explicit SlidingWindowIterator(const SlidingWindowIterator * other); /*! * Set the side length of the moving window (in m). * @param gridMap the grid map to iterate on. * @param windowLength the side length of the window (in m). */ - void setWindowLength(const GridMap& gridMap, const double windowLength); + void setWindowLength(const GridMap & gridMap, const double windowLength); /*! * Increase the iterator to the next element. * @return a reference to the updated iterator. */ - SlidingWindowIterator& operator ++(); + SlidingWindowIterator & operator++(); /*! * Return the data of the sliding window. @@ -70,7 +73,7 @@ class SlidingWindowIterator : public GridMapIterator private: //! Setup members. - void setup(const GridMap& gridMap); + void setup(const GridMap & gridMap); //! Check if data for current index is fully inside map. bool dataInsideMap() const; @@ -79,7 +82,7 @@ class SlidingWindowIterator : public GridMapIterator const EdgeHandling edgeHandling_; //! Data. - const Matrix& data_; + const Matrix & data_; //! Size of the sliding window. size_t windowSize_; @@ -87,8 +90,10 @@ class SlidingWindowIterator : public GridMapIterator //! Size of the border of the window around the center cell. size_t windowMargin_; - public: +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /* namespace */ +} // namespace grid_map + +#endif // GRID_MAP_CORE__ITERATORS__SLIDINGWINDOWITERATOR_HPP_ diff --git a/grid_map_core/include/grid_map_core/iterators/SpiralIterator.hpp b/grid_map_core/include/grid_map_core/iterators/SpiralIterator.hpp index b3715cb3d..d268b2240 100644 --- a/grid_map_core/include/grid_map_core/iterators/SpiralIterator.hpp +++ b/grid_map_core/include/grid_map_core/iterators/SpiralIterator.hpp @@ -6,15 +6,18 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once -#include "grid_map_core/GridMap.hpp" +#ifndef GRID_MAP_CORE__ITERATORS__SPIRALITERATOR_HPP_ +#define GRID_MAP_CORE__ITERATORS__SPIRALITERATOR_HPP_ #include #include #include -namespace grid_map { +#include "grid_map_core/GridMap.hpp" + +namespace grid_map +{ /*! * Iterator class to iterate through a circular area of the map with a spiral. @@ -22,39 +25,40 @@ namespace grid_map { class SpiralIterator { public: - /*! * Constructor. * @param gridMap the grid map to iterate on. * @param center the position of the circle center. * @param radius the radius of the circle. */ - SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center, const double radius); + SpiralIterator( + const grid_map::GridMap & gridMap, const Eigen::Vector2d & center, + const double radius); /*! * Assignment operator. * @param iterator the iterator to copy data from. * @return a reference to *this. */ - SpiralIterator& operator =(const SpiralIterator& other); + SpiralIterator & operator=(const SpiralIterator & other); /*! * Compare to another iterator. * @return whether the current iterator points to a different address than the other one. */ - bool operator !=(const SpiralIterator& other) const; + bool operator!=(const SpiralIterator & other) const; /*! * Dereference the iterator with const. * @return the value to which the iterator is pointing. */ - const Eigen::Array2i& operator *() const; + const Eigen::Array2i & operator*() const; /*! * Increase the iterator to the next element. * @return a reference to the updated iterator. */ - SpiralIterator& operator ++(); + SpiralIterator & operator++(); /*! * Indicates if iterator is past end. @@ -69,7 +73,6 @@ class SpiralIterator double getCurrentRadius() const; private: - /*! * Check if index is inside the circle. * @return true if inside, false otherwise. @@ -82,8 +85,9 @@ class SpiralIterator */ void generateRing(); - int signum(const int val) { - return (0 < val) - (val < 0); + int signum(const int val) + { + return (0 < val) - (val < 0); } //! Position of the circle center; @@ -108,8 +112,9 @@ class SpiralIterator double resolution_; Size bufferSize_; - public: +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_CORE__ITERATORS__SPIRALITERATOR_HPP_ diff --git a/grid_map_core/include/grid_map_core/iterators/SubmapIterator.hpp b/grid_map_core/include/grid_map_core/iterators/SubmapIterator.hpp index 9ca7706bd..f88c48ce6 100644 --- a/grid_map_core/include/grid_map_core/iterators/SubmapIterator.hpp +++ b/grid_map_core/include/grid_map_core/iterators/SubmapIterator.hpp @@ -6,15 +6,17 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CORE__ITERATORS__SUBMAPITERATOR_HPP_ +#define GRID_MAP_CORE__ITERATORS__SUBMAPITERATOR_HPP_ + +#include #include "grid_map_core/GridMap.hpp" #include "grid_map_core/SubmapGeometry.hpp" #include "grid_map_core/BufferRegion.hpp" -#include - -namespace grid_map { +namespace grid_map +{ /*! * Iterator class to iterate through a rectangular part of the map (submap). @@ -24,18 +26,17 @@ namespace grid_map { class SubmapIterator { public: - /*! * Constructor. * @param submap the submap geometry to iterate over. */ - SubmapIterator(const grid_map::SubmapGeometry& submap); + explicit SubmapIterator(const grid_map::SubmapGeometry & submap); /*! * Constructor. * @param submap the buffer region of a grid map to iterate over. */ - SubmapIterator(const grid_map::GridMap& gridMap, const grid_map::BufferRegion& bufferRegion); + SubmapIterator(const grid_map::GridMap & gridMap, const grid_map::BufferRegion & bufferRegion); /*! * Constructor. @@ -43,45 +44,46 @@ class SubmapIterator * @param submapStartIndex the start index of the submap, typically top-left index. * @param submapSize the size of the submap to iterate on. */ - SubmapIterator(const grid_map::GridMap& gridMap, const Index& submapStartIndex, - const Size& submapSize); + SubmapIterator( + const grid_map::GridMap & gridMap, const Index & submapStartIndex, + const Size & submapSize); /*! * Copy constructor. * @param other the object to copy. */ - SubmapIterator(const SubmapIterator* other); + explicit SubmapIterator(const SubmapIterator * other); /*! * Assignment operator. * @param iterator the iterator to copy data from. * @return a reference to *this. */ - SubmapIterator& operator =(const SubmapIterator& other); + SubmapIterator & operator=(const SubmapIterator & other); /*! * Compare to another iterator. * @return whether the current iterator points to a different address than the other one. */ - bool operator !=(const SubmapIterator& other) const; + bool operator!=(const SubmapIterator & other) const; /*! * Dereference the iterator with const. * @return the value to which the iterator is pointing. */ - const Index& operator *() const; + const Index & operator*() const; /*! * Get the current index in the submap. * @return the current index in the submap. */ - const Index& getSubmapIndex() const; + const Index & getSubmapIndex() const; /*! * Increase the iterator to the next element. * @return a reference to the updated iterator. */ - SubmapIterator& operator ++(); + SubmapIterator & operator++(); /*! * Indicates if iterator is past end. @@ -93,10 +95,9 @@ class SubmapIterator * Returns the size of the submap covered by the iterator. * @return the size of the submap covered by the iterator. */ - const Size& getSubmapSize() const; + const Size & getSubmapSize() const; private: - //! Size of the buffer. Size size_; @@ -118,8 +119,10 @@ class SubmapIterator //! Is iterator out of scope. bool isPastEnd_; - public: +public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} /* namespace */ +} // namespace grid_map + +#endif // GRID_MAP_CORE__ITERATORS__SUBMAPITERATOR_HPP_ diff --git a/grid_map_core/include/grid_map_core/iterators/iterators.hpp b/grid_map_core/include/grid_map_core/iterators/iterators.hpp index 1d919aadc..e54f9dd90 100644 --- a/grid_map_core/include/grid_map_core/iterators/iterators.hpp +++ b/grid_map_core/include/grid_map_core/iterators/iterators.hpp @@ -6,7 +6,9 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CORE__ITERATORS__ITERATORS_HPP_ +#define GRID_MAP_CORE__ITERATORS__ITERATORS_HPP_ + #include "grid_map_core/iterators/GridMapIterator.hpp" #include "grid_map_core/iterators/SubmapIterator.hpp" @@ -16,3 +18,5 @@ #include "grid_map_core/iterators/LineIterator.hpp" #include "grid_map_core/iterators/PolygonIterator.hpp" #include "grid_map_core/iterators/SlidingWindowIterator.hpp" + +#endif // GRID_MAP_CORE__ITERATORS__ITERATORS_HPP_ diff --git a/grid_map_core/package.xml b/grid_map_core/package.xml index 58dc495f6..b49c7b1d5 100644 --- a/grid_map_core/package.xml +++ b/grid_map_core/package.xml @@ -1,14 +1,27 @@ - + + grid_map_core - 1.6.2 + 2.2.2 Universal grid map library to manage two-dimensional grid maps with multiple data layers. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Péter Fankhauser - catkin + + ament_cmake + grid_map_cmake_helpers + eigen + + ament_cmake_gtest + ament_lint_common + ament_lint_auto + + + ament_cmake + diff --git a/grid_map_core/src/BufferRegion.cpp b/grid_map_core/src/BufferRegion.cpp index bc4782b80..550c096eb 100644 --- a/grid_map_core/src/BufferRegion.cpp +++ b/grid_map_core/src/BufferRegion.cpp @@ -7,42 +7,41 @@ */ #include -namespace grid_map { - -BufferRegion::BufferRegion() : - staretIndex_(Index::Zero()), - size_(Size::Zero()), - quadrant_(BufferRegion::Quadrant::Undefined) +namespace grid_map { -} -BufferRegion::BufferRegion(const Index& index, const Size& size, const BufferRegion::Quadrant& quadrant) : - staretIndex_(index), - size_(size), - quadrant_(quadrant) +BufferRegion::BufferRegion() +: staretIndex_(Index::Zero()), + size_(Size::Zero()), + quadrant_(BufferRegion::Quadrant::Undefined) { } -BufferRegion::~BufferRegion() +BufferRegion::BufferRegion( + const Index & index, const Size & size, + const BufferRegion::Quadrant & quadrant) +: staretIndex_(index), + size_(size), + quadrant_(quadrant) { } -const Index& BufferRegion::getStartIndex() const +const Index & BufferRegion::getStartIndex() const { return staretIndex_; } -void BufferRegion::setStartIndex(const Index& staretIndex) +void BufferRegion::setStartIndex(const Index & staretIndex) { staretIndex_ = staretIndex; } -const Size& BufferRegion::getSize() const +const Size & BufferRegion::getSize() const { return size_; } -void BufferRegion::setSize(const Size& size) +void BufferRegion::setSize(const Size & size) { size_ = size; } @@ -57,6 +56,4 @@ void BufferRegion::setQuadrant(BufferRegion::Quadrant type) quadrant_ = type; } -} /* namespace grid_map */ - - +} // namespace grid_map diff --git a/grid_map_core/src/CubicInterpolation.cpp b/grid_map_core/src/CubicInterpolation.cpp index 89ce6b32f..456e1f21a 100644 --- a/grid_map_core/src/CubicInterpolation.cpp +++ b/grid_map_core/src/CubicInterpolation.cpp @@ -6,25 +6,27 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#include "grid_map_core/CubicInterpolation.hpp" +#include +#include "grid_map_core/CubicInterpolation.hpp" #include "grid_map_core/GridMap.hpp" #include "grid_map_core/GridMapMath.hpp" -namespace grid_map { +namespace grid_map +{ unsigned int bindIndexToRange(int idReq, unsigned int nElem) { if (idReq < 0) { return 0; } - if (idReq >= nElem) { + if ((unsigned)idReq >= nElem) { return static_cast(nElem - 1); } return static_cast(idReq); } -double getLayerValue(const Matrix &layerMat, int rowReq, int colReq) +double getLayerValue(const Matrix & layerMat, int rowReq, int colReq) { const auto numCol = layerMat.cols(); const auto numRow = layerMat.rows(); @@ -43,11 +45,13 @@ double getLayerValue(const Matrix &layerMat, int rowReq, int colReq) * https://web.archive.org/web/20051024202307/http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm */ -namespace bicubic_conv { +namespace bicubic_conv +{ -bool evaluateBicubicConvolutionInterpolation(const GridMap &gridMap, const std::string &layer, - const Position &queriedPosition, - double *interpolatedValue) +bool evaluateBicubicConvolutionInterpolation( + const GridMap & gridMap, const std::string & layer, + const Position & queriedPosition, + double * interpolatedValue) { FunctionValueMatrix functionValues; if (!assembleFunctionValueMatrix(gridMap, layer, queriedPosition, &functionValues)) { @@ -62,8 +66,8 @@ bool evaluateBicubicConvolutionInterpolation(const GridMap &gridMap, const std:: const double tx = normalizedCoordinate.x(); const double ty = normalizedCoordinate.y(); - //bm1 stands for b minus one, i.e. index decreased by one - //b2 stands for b plus 2, i.e. index increased by two + // bm1 stands for b minus one, i.e. index decreased by one + // b2 stands for b plus 2, i.e. index increased by two const double bm1 = convolve1D(tx, functionValues.row(0)); const double b0 = convolve1D(tx, functionValues.row(1)); const double b1 = convolve1D(tx, functionValues.row(2)); @@ -73,29 +77,29 @@ bool evaluateBicubicConvolutionInterpolation(const GridMap &gridMap, const std:: return true; } -double convolve1D(double t, const Eigen::Vector4d &functionValues) +double convolve1D(double t, const Eigen::Vector4d & functionValues) { const Eigen::Vector4d tVector(1.0, t, t * t, t * t * t); - const Eigen::Vector4d temp = cubicInterpolationConvolutionMatrix - * functionValues; + const Eigen::Vector4d temp = cubicInterpolationConvolutionMatrix * + functionValues; const double retVal = 0.5 * tVector.transpose() * temp; return retVal; } -bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &layer, - const Position &queriedPosition, FunctionValueMatrix *data) +bool assembleFunctionValueMatrix( + const GridMap & gridMap, const std::string & layer, + const Position & queriedPosition, FunctionValueMatrix * data) { - Index middleKnotIndex; if (!getIndicesOfMiddleKnot(gridMap, queriedPosition, &middleKnotIndex)) { return false; } - const Matrix &layerMatrix = gridMap.get(layer); + const Matrix & layerMatrix = gridMap.get(layer); auto f = [&layerMatrix](int rowReq, int colReq) { - double retVal = getLayerValue(layerMatrix, rowReq, colReq); - return retVal; - }; + double retVal = getLayerValue(layerMatrix, rowReq, colReq); + return retVal; + }; const unsigned int i = middleKnotIndex.x(); const unsigned int j = middleKnotIndex.y(); @@ -108,14 +112,15 @@ bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &laye * https://github.com/ANYbotics/grid_map */ *data << f(i + 1, j + 1), f(i, j + 1), f(i - 1, j + 1), f(i - 2, j + 1), f(i + 1, j), f(i, j), f( - i - 1, j), f(i - 2, j), f(i + 1, j - 1), f(i, j - 1), f(i - 1, j - 1), f(i - 2, j - 1), f( - i + 1, j - 2), f(i, j - 2), f(i - 1, j - 2), f(i - 2, j - 2); + i - 1, j), f(i - 2, j), f(i + 1, j - 1), f(i, j - 1), f(i - 1, j - 1), f(i - 2, j - 1), f( + i + 1, j - 2), f(i, j - 2), f(i - 1, j - 2), f(i - 2, j - 2); return true; } -bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPosition, - Position *position) +bool getNormalizedCoordinates( + const GridMap & gridMap, const Position & queriedPosition, + Position * position) { Index index; if (!getIndicesOfMiddleKnot(gridMap, queriedPosition, &index)) { @@ -133,16 +138,17 @@ bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPos return true; } -bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosition, Index *index) +bool getIndicesOfMiddleKnot( + const GridMap & gridMap, const Position & queriedPosition, + Index * index) { - if (!gridMap.getIndex(queriedPosition, *index)) { return false; } return true; } -} /* namespace bicubic_conv */ +} // namespace bicubic_conv /** * BICUBIC INTERPOLATION ALGORITHM @@ -153,13 +159,14 @@ bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosit * https://web.archive.org/web/20051024202307/http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm */ -namespace bicubic { - -bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &layer, - const Position &queriedPosition, double *interpolatedValue) +namespace bicubic { - const Matrix& layerMat = gridMap.get(layer); +bool evaluateBicubicInterpolation( + const GridMap & gridMap, const std::string & layer, + const Position & queriedPosition, double * interpolatedValue) +{ + const Matrix & layerMat = gridMap.get(layer); const double resolution = gridMap.getResolution(); // get indices of data points needed for interpolation @@ -197,22 +204,25 @@ bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &lay // get normalized coordiantes Position normalizedCoordinates; - if (!computeNormalizedCoordinates(gridMap, unitSquareCornerIndices.bottomLeft_, queriedPosition, - &normalizedCoordinates)) { + if (!computeNormalizedCoordinates( + gridMap, unitSquareCornerIndices.bottomLeft_, queriedPosition, + &normalizedCoordinates)) + { return false; } // evaluate polynomial - *interpolatedValue = evaluatePolynomial(functionValues, normalizedCoordinates.x(), - normalizedCoordinates.y()); + *interpolatedValue = evaluatePolynomial( + functionValues, normalizedCoordinates.x(), + normalizedCoordinates.y()); return true; } -bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedPosition, - IndicesMatrix *indicesMatrix) +bool getUnitSquareCornerIndices( + const GridMap & gridMap, const Position & queriedPosition, + IndicesMatrix * indicesMatrix) { - Index closestPointId; if (!getClosestPointIndices(gridMap, queriedPosition, &closestPointId)) { return false; @@ -230,8 +240,8 @@ bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedP const double x = queriedPosition.x(); const double y = queriedPosition.y(); - if (x > x0) { //first or fourth quadrant - if (y > y0) { //first quadrant + if (x > x0) { // first or fourth quadrant + if (y > y0) { // first quadrant indicesMatrix->topLeft_ = Index(idx0, idy0 - 1); indicesMatrix->topRight_ = Index(idx0 - 1, idy0 - 1); indicesMatrix->bottomLeft_ = Index(idx0, idy0); @@ -243,7 +253,7 @@ bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedP indicesMatrix->bottomRight_ = Index(idx0 - 1, idy0 + 1); } } else { // second or third quadrant - if (y > y0) { //second quadrant + if (y > y0) { // second quadrant indicesMatrix->topLeft_ = Index(idx0 + 1, idy0 - 1); indicesMatrix->topRight_ = Index(idx0, idy0 - 1); indicesMatrix->bottomLeft_ = Index(idx0 + 1, idy0); @@ -259,22 +269,22 @@ bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedP bindIndicesToRange(gridMap, indicesMatrix); return true; - } -bool getClosestPointIndices(const GridMap &gridMap, const Position &queriedPosition, Index *index) +bool getClosestPointIndices( + const GridMap & gridMap, const Position & queriedPosition, + Index * index) { - if (!gridMap.getIndex(queriedPosition, *index)) { return false; } return true; } -bool computeNormalizedCoordinates(const GridMap &gridMap, const Index &originIndex, - const Position &queriedPosition, Position *normalizedCoordinates) +bool computeNormalizedCoordinates( + const GridMap & gridMap, const Index & originIndex, + const Position & queriedPosition, Position * normalizedCoordinates) { - Position origin; if (!gridMap.getPosition(originIndex, origin)) { return false; @@ -284,10 +294,9 @@ bool computeNormalizedCoordinates(const GridMap &gridMap, const Index &originInd normalizedCoordinates->y() = (queriedPosition.y() - origin.y()) / gridMap.getResolution(); return true; - } -bool getFunctionValues(const Matrix &layerData, const IndicesMatrix &indices, DataMatrix *data) +bool getFunctionValues(const Matrix & layerData, const IndicesMatrix & indices, DataMatrix * data) { data->topLeft_ = layerData(indices.topLeft_.x(), indices.topLeft_.y()); data->topRight_ = layerData(indices.topRight_.x(), indices.topRight_.y()); @@ -296,55 +305,58 @@ bool getFunctionValues(const Matrix &layerData, const IndicesMatrix &indices, Da return true; } -void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices) +void bindIndicesToRange(const GridMap & gridMap, IndicesMatrix * indices) { const int numCol = gridMap.getSize().y(); const int numRow = gridMap.getSize().x(); - //top left + // top left { const unsigned int iBoundToRange = bindIndexToRange(indices->topLeft_.x(), numRow); const unsigned int jBoundToRange = bindIndexToRange(indices->topLeft_.y(), numCol); indices->topLeft_ = Index(iBoundToRange, jBoundToRange); } - //top right + // top right { const unsigned int iBoundToRange = bindIndexToRange(indices->topRight_.x(), numRow); const unsigned int jBoundToRange = bindIndexToRange(indices->topRight_.y(), numCol); indices->topRight_ = Index(iBoundToRange, jBoundToRange); } - //bottom left + // bottom left { const unsigned int iBoundToRange = bindIndexToRange(indices->bottomLeft_.x(), numRow); const unsigned int jBoundToRange = bindIndexToRange(indices->bottomLeft_.y(), numCol); indices->bottomLeft_ = Index(iBoundToRange, jBoundToRange); } - //bottom right + // bottom right { const unsigned int iBoundToRange = bindIndexToRange(indices->bottomRight_.x(), numRow); const unsigned int jBoundToRange = bindIndexToRange(indices->bottomRight_.y(), numCol); indices->bottomRight_ = Index(iBoundToRange, jBoundToRange); } - } -bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, Dim2D dim, - double resolution, DataMatrix *derivatives) +bool getFirstOrderDerivatives( + const Matrix & layerData, const IndicesMatrix & indices, Dim2D dim, + double resolution, DataMatrix * derivatives) { derivatives->topLeft_ = firstOrderDerivativeAt(layerData, indices.topLeft_, dim, resolution); derivatives->topRight_ = firstOrderDerivativeAt(layerData, indices.topRight_, dim, resolution); - derivatives->bottomLeft_ = firstOrderDerivativeAt(layerData, indices.bottomLeft_, dim, - resolution); - derivatives->bottomRight_ = firstOrderDerivativeAt(layerData, indices.bottomRight_, dim, - resolution); + derivatives->bottomLeft_ = firstOrderDerivativeAt( + layerData, indices.bottomLeft_, dim, + resolution); + derivatives->bottomRight_ = firstOrderDerivativeAt( + layerData, indices.bottomRight_, dim, + resolution); return true; } -double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D dim, - double resolution) +double firstOrderDerivativeAt( + const Matrix & layerData, const Index & index, Dim2D dim, + double resolution) { const int numCol = layerData.cols(); const int numRow = layerData.rows(); @@ -352,18 +364,18 @@ double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D double left, right; switch (dim) { case Dim2D::X: { - left = layerData(bindIndexToRange(index.x() + 1, numRow), index.y()); - right = layerData(bindIndexToRange(index.x() - 1, numRow), index.y()); - break; - } + left = layerData(bindIndexToRange(index.x() + 1, numRow), index.y()); + right = layerData(bindIndexToRange(index.x() - 1, numRow), index.y()); + break; + } case Dim2D::Y: { - left = layerData(index.x(), bindIndexToRange(index.y() + 1, numCol)); - right = layerData(index.x(), bindIndexToRange(index.y() - 1, numCol)); - break; - } + left = layerData(index.x(), bindIndexToRange(index.y() + 1, numCol)); + right = layerData(index.x(), bindIndexToRange(index.y() - 1, numCol)); + break; + } default: { - throw std::runtime_error("Unknown derivative direction"); - } + throw std::runtime_error("Unknown derivative direction"); + } } const double perturbation = resolution; @@ -373,7 +385,9 @@ double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D return (right - left) / (2.0 * perturbation) * resolution; } -double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution) +double mixedSecondOrderDerivativeAt( + const Matrix & layerData, const Index & index, + double resolution) { /* * no need for dimensions since the we have to differentiate w.r.t. x and y @@ -384,58 +398,66 @@ double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, const int numCol = layerData.cols(); const int numRow = layerData.rows(); - const double f11 = layerData(bindIndexToRange(index.x() - 1, numRow), - bindIndexToRange(index.y() - 1, numCol)); - const double f1m1 = layerData(bindIndexToRange(index.x() - 1, numRow), - bindIndexToRange(index.y() + 1, numCol)); - const double fm11 = layerData(bindIndexToRange(index.x() + 1, numRow), - bindIndexToRange(index.y() - 1, numCol)); - const double fm1m1 = layerData(bindIndexToRange(index.x() + 1, numRow), - bindIndexToRange(index.y() + 1, numCol)); + const double f11 = layerData( + bindIndexToRange(index.x() - 1, numRow), + bindIndexToRange(index.y() - 1, numCol)); + const double f1m1 = layerData( + bindIndexToRange(index.x() - 1, numRow), + bindIndexToRange(index.y() + 1, numCol)); + const double fm11 = layerData( + bindIndexToRange(index.x() + 1, numRow), + bindIndexToRange(index.y() - 1, numCol)); + const double fm1m1 = layerData( + bindIndexToRange(index.x() + 1, numRow), + bindIndexToRange(index.y() + 1, numCol)); const double perturbation = resolution; // central difference approximation // we need to multiply with resolution^2 since we are // operating in scaled coordinates. Second derivative scales // with the square of the resolution - return (f11 - f1m1 - fm11 + fm1m1) / (4.0 * perturbation * perturbation) * resolution * resolution; - + return (f11 - f1m1 - fm11 + fm1m1) / (4.0 * perturbation * perturbation) * resolution * + resolution; } -bool getMixedSecondOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, - double resolution, DataMatrix *derivatives) +bool getMixedSecondOrderDerivatives( + const Matrix & layerData, const IndicesMatrix & indices, + double resolution, DataMatrix * derivatives) { derivatives->topLeft_ = mixedSecondOrderDerivativeAt(layerData, indices.topLeft_, resolution); derivatives->topRight_ = mixedSecondOrderDerivativeAt(layerData, indices.topRight_, resolution); - derivatives->bottomLeft_ = mixedSecondOrderDerivativeAt(layerData, indices.bottomLeft_, - resolution); - derivatives->bottomRight_ = mixedSecondOrderDerivativeAt(layerData, indices.bottomRight_, - resolution); + derivatives->bottomLeft_ = mixedSecondOrderDerivativeAt( + layerData, indices.bottomLeft_, + resolution); + derivatives->bottomRight_ = mixedSecondOrderDerivativeAt( + layerData, indices.bottomRight_, + resolution); return true; } -double evaluatePolynomial(const FunctionValueMatrix &functionValues, double tx, double ty) +double evaluatePolynomial(const FunctionValueMatrix & functionValues, double tx, double ty) { const Eigen::Vector4d xVector(1, tx, tx * tx, tx * tx * tx); const Eigen::Vector4d yVector(1, ty, ty * ty, ty * ty * ty); - const Eigen::Matrix4d tempMat = functionValues - * bicubicInterpolationMatrix.transpose(); + const Eigen::Matrix4d tempMat = functionValues * + bicubicInterpolationMatrix.transpose(); const Eigen::Matrix4d polynomialCoeffMatrix = bicubicInterpolationMatrix * tempMat; const Eigen::Vector4d tempVec = polynomialCoeffMatrix * yVector; return xVector.transpose() * tempVec; } -void assembleFunctionValueMatrix(const DataMatrix &f, const DataMatrix &dfx, const DataMatrix &dfy, - const DataMatrix &ddfxy, FunctionValueMatrix *functionValues) +void assembleFunctionValueMatrix( + const DataMatrix & f, const DataMatrix & dfx, const DataMatrix & dfy, + const DataMatrix & ddfxy, FunctionValueMatrix * functionValues) { - auto toEigenMatrix = [](const DataMatrix &d)-> Eigen::Matrix2d { - Eigen::Matrix2d e; - e(0,0) = d.bottomLeft_; - e(1,0) = d.bottomRight_; - e(0,1) = d.topLeft_; - e(1,1) = d.topRight_; - return e; - }; + auto toEigenMatrix = [](const DataMatrix & d) -> Eigen::Matrix2d { + Eigen::Matrix2d e; + e(0, 0) = d.bottomLeft_; + e(1, 0) = d.bottomRight_; + e(0, 1) = d.topLeft_; + e(1, 1) = d.topRight_; + return e; + }; functionValues->block<2, 2>(0, 0) = toEigenMatrix(f); functionValues->block<2, 2>(2, 2) = toEigenMatrix(ddfxy); @@ -443,6 +465,6 @@ void assembleFunctionValueMatrix(const DataMatrix &f, const DataMatrix &dfx, con functionValues->block<2, 2>(2, 0) = toEigenMatrix(dfx); } -} /* namespace bicubic*/ +} // namespace bicubic -} /* namespace grid_map */ +} // namespace grid_map diff --git a/grid_map_core/src/GridMap.cpp b/grid_map_core/src/GridMap.cpp index 71695a4aa..cbee49dff 100644 --- a/grid_map_core/src/GridMap.cpp +++ b/grid_map_core/src/GridMap.cpp @@ -6,13 +6,6 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/GridMap.hpp" - -#include "grid_map_core/CubicInterpolation.hpp" -#include "grid_map_core/GridMapMath.hpp" -#include "grid_map_core/SubmapGeometry.hpp" -#include "grid_map_core/iterators/GridMapIterator.hpp" - #include #include @@ -20,13 +13,23 @@ #include #include #include +#include +#include +#include + +#include "grid_map_core/GridMap.hpp" + +#include "grid_map_core/CubicInterpolation.hpp" +#include "grid_map_core/GridMapMath.hpp" +#include "grid_map_core/SubmapGeometry.hpp" +#include "grid_map_core/iterators/GridMapIterator.hpp" -using namespace std; -using namespace grid_map; -namespace grid_map { +namespace grid_map +{ -GridMap::GridMap(const std::vector& layers) { +GridMap::GridMap(const std::vector & layers) +{ position_.setZero(); length_.setZero(); resolution_ = 0.0; @@ -35,20 +38,23 @@ GridMap::GridMap(const std::vector& layers) { timestamp_ = 0; layers_ = layers; - for (auto& layer : layers_) { + for (auto & layer : layers_) { data_.insert(std::pair(layer, Matrix())); } } -GridMap::GridMap() : GridMap(std::vector()) {} +GridMap::GridMap() +: GridMap(std::vector()) {} -void GridMap::setGeometry(const Length& length, const double resolution, const Position& position) { +void GridMap::setGeometry(const Length & length, const double resolution, const Position & position) +{ assert(length(0) > 0.0); assert(length(1) > 0.0); assert(resolution > 0.0); Size size; - size(0) = static_cast(round(length(0) / resolution)); // There is no round() function in Eigen. + // There is no round() function in Eigen. + size(0) = static_cast(round(length(0) / resolution)); size(1) = static_cast(round(length(1) / resolution)); resize(size); clearAll(); @@ -57,28 +63,31 @@ void GridMap::setGeometry(const Length& length, const double resolution, const P length_ = (size_.cast() * resolution_).matrix(); position_ = position; startIndex_.setZero(); - - return; } -void GridMap::setGeometry(const SubmapGeometry& geometry) { +void GridMap::setGeometry(const SubmapGeometry & geometry) +{ setGeometry(geometry.getLength(), geometry.getResolution(), geometry.getPosition()); } -void GridMap::setBasicLayers(const std::vector& basicLayers) { +void GridMap::setBasicLayers(const std::vector & basicLayers) +{ basicLayers_ = basicLayers; } -const std::vector& GridMap::getBasicLayers() const { +const std::vector & GridMap::getBasicLayers() const +{ return basicLayers_; } -bool GridMap::hasBasicLayers() const { +bool GridMap::hasBasicLayers() const +{ return basicLayers_.size() > 0; } -bool GridMap::hasSameLayers(const GridMap& other) const { - for (const auto& layer : layers_) { +bool GridMap::hasSameLayers(const GridMap & other) const +{ + for (const auto & layer : layers_) { if (!other.exists(layer)) { return false; } @@ -86,11 +95,13 @@ bool GridMap::hasSameLayers(const GridMap& other) const { return true; } -void GridMap::add(const std::string& layer, const double value) { +void GridMap::add(const std::string & layer, const double value) +{ add(layer, Matrix::Constant(size_(0), size_(1), value)); } -void GridMap::add(const std::string& layer, const Matrix& data) { +void GridMap::add(const std::string & layer, const Matrix & data) +{ assert(size_(0) == data.rows()); assert(size_(1) == data.cols()); @@ -104,35 +115,41 @@ void GridMap::add(const std::string& layer, const Matrix& data) { } } -bool GridMap::exists(const std::string& layer) const { +bool GridMap::exists(const std::string & layer) const +{ return !(data_.find(layer) == data_.end()); } -const Matrix& GridMap::get(const std::string& layer) const { +const Matrix & GridMap::get(const std::string & layer) const +{ try { return data_.at(layer); - } catch (const std::out_of_range& exception) { + } catch (const std::out_of_range & exception) { throw std::out_of_range("GridMap::get(...) : No map layer '" + layer + "' available."); } } -Matrix& GridMap::get(const std::string& layer) { +Matrix & GridMap::get(const std::string & layer) +{ try { return data_.at(layer); - } catch (const std::out_of_range& exception) { + } catch (const std::out_of_range & exception) { throw std::out_of_range("GridMap::get(...) : No map layer of type '" + layer + "' available."); } } -const Matrix& GridMap::operator[](const std::string& layer) const { +const Matrix & GridMap::operator[](const std::string & layer) const +{ return get(layer); } -Matrix& GridMap::operator[](const std::string& layer) { +Matrix & GridMap::operator[](const std::string & layer) +{ return get(layer); } -bool GridMap::erase(const std::string& layer) { +bool GridMap::erase(const std::string & layer) +{ const auto dataIterator = data_.find(layer); if (dataIterator == data_.end()) { return false; @@ -153,11 +170,13 @@ bool GridMap::erase(const std::string& layer) { return true; } -const std::vector& GridMap::getLayers() const { +const std::vector & GridMap::getLayers() const +{ return layers_; } -float& GridMap::atPosition(const std::string& layer, const Position& position) { +float & GridMap::atPosition(const std::string & layer, const Position & position) +{ Index index; if (getIndex(position, index)) { return at(layer, index); @@ -165,96 +184,106 @@ float& GridMap::atPosition(const std::string& layer, const Position& position) { throw std::out_of_range("GridMap::atPosition(...) : Position is out of range."); } -float GridMap::atPosition(const std::string& layer, const Position& position, InterpolationMethods interpolationMethod) const { - - bool skipNextSwitchCase = false; - switch (interpolationMethod) { - case InterpolationMethods::INTER_CUBIC_CONVOLUTION: { - float value; - if (atPositionBicubicConvolutionInterpolated(layer, position, value)) { - return value; - } else { - interpolationMethod = InterpolationMethods::INTER_LINEAR; - skipNextSwitchCase = true; - } +float GridMap::atPosition( + const std::string & layer, const Position & position, + InterpolationMethods interpolationMethod) const +{ + if (interpolationMethod == InterpolationMethods::INTER_CUBIC_CONVOLUTION) { + float value; + if (atPositionBicubicConvolutionInterpolated(layer, position, value)) { + return value; + } else { + interpolationMethod = InterpolationMethods::INTER_LINEAR; } - case InterpolationMethods::INTER_CUBIC: { - if (!skipNextSwitchCase) { - float value; - if (atPositionBicubicInterpolated(layer, position, value)) { - return value; - } else { - interpolationMethod = InterpolationMethods::INTER_LINEAR; - } - } + } + + if (interpolationMethod == InterpolationMethods::INTER_CUBIC) { + float value; + if (atPositionBicubicInterpolated(layer, position, value)) { + return value; + } else { + interpolationMethod = InterpolationMethods::INTER_LINEAR; } - case InterpolationMethods::INTER_LINEAR: { - float value; - if (atPositionLinearInterpolated(layer, position, value)) - return value; - else - interpolationMethod = InterpolationMethods::INTER_NEAREST; + } + + if (interpolationMethod == InterpolationMethods::INTER_LINEAR) { + float value; + if (atPositionLinearInterpolated(layer, position, value)) { + return value; + } else { + interpolationMethod = InterpolationMethods::INTER_NEAREST; } - case InterpolationMethods::INTER_NEAREST: { - Index index; - if (getIndex(position, index)) { - return at(layer, index); - } else - throw std::out_of_range("GridMap::atPosition(...) : Position is out of range."); - break; + } + + if (interpolationMethod == InterpolationMethods::INTER_NEAREST) { + Index index; + if (getIndex(position, index)) { + return at(layer, index); + } else { + throw std::out_of_range("GridMap::atPosition(...) : Position is out of range."); } - default: - throw std::runtime_error( - "GridMap::atPosition(...) : Specified " - "interpolation method not implemented."); + } else { + throw std::runtime_error( + "GridMap::atPosition(...) : Specified " + "interpolation method not implemented."); } } -float& GridMap::at(const std::string& layer, const Index& index) { +float & GridMap::at(const std::string & layer, const Index & index) +{ try { + // cppcheck-suppress returnTempReference return data_.at(layer)(index(0), index(1)); - } catch (const std::out_of_range& exception) { + } catch (const std::out_of_range & exception) { throw std::out_of_range("GridMap::at(...) : No map layer '" + layer + "' available."); } } -float GridMap::at(const std::string& layer, const Index& index) const { +float GridMap::at(const std::string & layer, const Index & index) const +{ try { return data_.at(layer)(index(0), index(1)); - } catch (const std::out_of_range& exception) { + } catch (const std::out_of_range & exception) { throw std::out_of_range("GridMap::at(...) : No map layer '" + layer + "' available."); } } -bool GridMap::getIndex(const Position& position, Index& index) const { +bool GridMap::getIndex(const Position & position, Index & index) const +{ return getIndexFromPosition(index, position, length_, position_, resolution_, size_, startIndex_); } -bool GridMap::getPosition(const Index& index, Position& position) const { +bool GridMap::getPosition(const Index & index, Position & position) const +{ return getPositionFromIndex(position, index, length_, position_, resolution_, size_, startIndex_); } -bool GridMap::isInside(const Position& position) const { +bool GridMap::isInside(const Position & position) const +{ return checkIfPositionWithinMap(position, length_, position_); } -bool GridMap::isValid(DataType value) const { - return isfinite(value); +bool GridMap::isValid(DataType value) const +{ + return std::isfinite(value); } -bool GridMap::isValid(const Index& index) const { +bool GridMap::isValid(const Index & index) const +{ return isValid(index, basicLayers_); } -bool GridMap::isValid(const Index& index, const std::string& layer) const { +bool GridMap::isValid(const Index & index, const std::string & layer) const +{ return isValid(at(layer, index)); } -bool GridMap::isValid(const Index& index, const std::vector& layers) const { +bool GridMap::isValid(const Index & index, const std::vector & layers) const +{ if (layers.empty()) { return false; } - for (const auto& layer : layers) { + for (const auto & layer : layers) { if (!isValid(index, layer)) { return false; } @@ -262,7 +291,10 @@ bool GridMap::isValid(const Index& index, const std::vector& layers return true; } -bool GridMap::getPosition3(const std::string& layer, const Index& index, Position3& position) const { +bool GridMap::getPosition3( + const std::string & layer, const Index & index, + Position3 & position) const +{ const auto value = at(layer, index); if (!isValid(value)) { return false; @@ -274,8 +306,12 @@ bool GridMap::getPosition3(const std::string& layer, const Index& index, Positio return true; } -bool GridMap::getVector(const std::string& layerPrefix, const Index& index, Eigen::Vector3d& vector) const { - Eigen::Vector3d temp{at(layerPrefix + "x", index), at(layerPrefix + "y", index), at(layerPrefix + "z", index)}; +bool GridMap::getVector( + const std::string & layerPrefix, const Index & index, + Eigen::Vector3d & vector) const +{ + Eigen::Vector3d temp{at(layerPrefix + "x", index), at(layerPrefix + "y", index), at( + layerPrefix + "z", index)}; if (!isValid(temp[0]) || !isValid(temp[1]) || !isValid(temp[2])) { return false; } else { @@ -284,12 +320,8 @@ bool GridMap::getVector(const std::string& layerPrefix, const Index& index, Eige } } -GridMap GridMap::getSubmap(const Position& position, const Length& length, bool& isSuccess) const { - Index index; - return getSubmap(position, length, index, isSuccess); -} - -GridMap GridMap::getSubmap(const Position& position, const Length& length, Index& indexInSubmap, bool& isSuccess) const { +GridMap GridMap::getSubmap(const Position & position, const Length & length, bool & isSuccess) const +{ // Submap the generate. GridMap submap(layers_); submap.setBasicLayers(basicLayers_); @@ -307,25 +339,36 @@ GridMap GridMap::getSubmap(const Position& position, const Length& length, Index // Copy data. std::vector bufferRegions; - if (!getBufferRegionsForSubmap(bufferRegions, submapInformation.getStartIndex(), submap.getSize(), size_, startIndex_)) { - cout << "Cannot access submap of this size." << endl; + if (!getBufferRegionsForSubmap( + bufferRegions, submapInformation.getStartIndex(), submap.getSize(), + size_, startIndex_)) + { + std::cout << "Cannot access submap of this size." << std::endl; isSuccess = false; return GridMap(layers_); } - for (const auto& data : data_) { - for (const auto& bufferRegion : bufferRegions) { + for (const auto & data : data_) { + for (const auto & bufferRegion : bufferRegions) { Index index = bufferRegion.getStartIndex(); Size size = bufferRegion.getSize(); if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopLeft) { - submap.data_[data.first].topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1)); + submap.data_[data.first].topLeftCorner(size(0), size(1)) = data.second.block( + index(0), index( + 1), size(0), size(1)); } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopRight) { - submap.data_[data.first].topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1)); + submap.data_[data.first].topRightCorner(size(0), size(1)) = data.second.block( + index( + 0), index(1), size(0), size(1)); } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomLeft) { - submap.data_[data.first].bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1)); + submap.data_[data.first].bottomLeftCorner(size(0), size(1)) = data.second.block( + index( + 0), index(1), size(0), size(1)); } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomRight) { - submap.data_[data.first].bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1)); + submap.data_[data.first].bottomRightCorner(size(0), size(1)) = data.second.block( + index( + 0), index(1), size(0), size(1)); } } } @@ -334,11 +377,15 @@ GridMap GridMap::getSubmap(const Position& position, const Length& length, Index return submap; } -GridMap GridMap::getTransformedMap(const Eigen::Isometry3d& transform, const std::string& heightLayerName, const std::string& newFrameId, - const double sampleRatio) const { +GridMap GridMap::getTransformedMap( + const Eigen::Isometry3d & transform, const std::string & heightLayerName, + const std::string & newFrameId, + const double sampleRatio) const +{ // Check if height layer is valid. if (!exists(heightLayerName)) { - throw std::out_of_range("GridMap::getTransformedMap(...) : No map layer '" + heightLayerName + "' available."); + throw std::out_of_range( + "GridMap::getTransformedMap(...) : No map layer '" + heightLayerName + "' available."); } // Initialization. @@ -365,17 +412,19 @@ GridMap GridMap::getTransformedMap(const Eigen::Isometry3d& transform, const std // Find new grid center. Position3 newCenter = Position3::Zero(); - for (const auto& newEdge : newEdges) { + for (const auto & newEdge : newEdges) { newCenter += newEdge; } newCenter *= 0.25; // Find new grid length. Length maxLengthFromCenter = Length(0.0, 0.0); - for (const auto& newEdge : newEdges) { + for (const auto & newEdge : newEdges) { Position3 positionCenterToEdge = newEdge - newCenter; - maxLengthFromCenter.x() = std::fmax(std::fabs(positionCenterToEdge.x()), maxLengthFromCenter.x()); - maxLengthFromCenter.y() = std::fmax(std::fabs(positionCenterToEdge.y()), maxLengthFromCenter.y()); + maxLengthFromCenter.x() = + std::fmax(std::fabs(positionCenterToEdge.x()), maxLengthFromCenter.x()); + maxLengthFromCenter.y() = + std::fmax(std::fabs(positionCenterToEdge.y()), maxLengthFromCenter.y()); } Length newLength = 2.0 * maxLengthFromCenter; @@ -408,7 +457,7 @@ GridMap GridMap::getTransformedMap(const Eigen::Isometry3d& transform, const std } // Transform the sampled points and register to the new map. - for (const auto& position : positionSamples) { + for (const auto & position : positionSamples) { const Position3 transformedPosition = transform * position; // Get new index. @@ -424,13 +473,12 @@ GridMap GridMap::getTransformedMap(const Eigen::Isometry3d& transform, const std } // Copy the layers. - for (const auto& layer : layers_) { + for (const auto & layer : layers_) { const auto currentValueInOldGrid = at(layer, *iterator); - auto& newValue = newMap.at(layer, newIndex); + auto & newValue = newMap.at(layer, newIndex); if (layer == heightLayerName) { - newValue = transformedPosition.z(); - } // adjust height - else { + newValue = transformedPosition.z(); // adjust height + } else { newValue = currentValueInOldGrid; } // re-assign } @@ -440,11 +488,13 @@ GridMap GridMap::getTransformedMap(const Eigen::Isometry3d& transform, const std return newMap; } -void GridMap::setPosition(const Position& position) { +void GridMap::setPosition(const Position & position) +{ position_ = position; } -bool GridMap::move(const Position& position, std::vector& newRegions) { +bool GridMap::move(const Position & position, std::vector & newRegions) +{ Index indexShift; Position positionShift = position - position_; getIndexShiftFromPositionShift(indexShift, positionShift, resolution_); @@ -457,7 +507,10 @@ bool GridMap::move(const Position& position, std::vector& newRegio if (abs(indexShift(i)) >= getSize()(i)) { // Entire map is dropped. clearAll(); - newRegions.push_back(BufferRegion(Index(0, 0), getSize(), BufferRegion::Quadrant::Undefined)); + newRegions.push_back( + BufferRegion( + Index(0, 0), getSize(), + BufferRegion::Quadrant::Undefined)); } else { // Drop cells out of map. int sign = (indexShift(i) > 0 ? 1 : -1); @@ -471,10 +524,16 @@ bool GridMap::move(const Position& position, std::vector& newRegio // One region to drop. if (i == 0) { clearRows(index, nCells); - newRegions.push_back(BufferRegion(Index(index, 0), Size(nCells, getSize()(1)), BufferRegion::Quadrant::Undefined)); + newRegions.push_back( + BufferRegion( + Index(index, 0), Size(nCells, getSize()(1)), + BufferRegion::Quadrant::Undefined)); } else if (i == 1) { clearCols(index, nCells); - newRegions.push_back(BufferRegion(Index(0, index), Size(getSize()(0), nCells), BufferRegion::Quadrant::Undefined)); + newRegions.push_back( + BufferRegion( + Index(0, index), Size(getSize()(0), nCells), + BufferRegion::Quadrant::Undefined)); } } else { // Two regions to drop. @@ -482,20 +541,32 @@ bool GridMap::move(const Position& position, std::vector& newRegio int firstNCells = getSize()(i) - firstIndex; if (i == 0) { clearRows(firstIndex, firstNCells); - newRegions.push_back(BufferRegion(Index(firstIndex, 0), Size(firstNCells, getSize()(1)), BufferRegion::Quadrant::Undefined)); + newRegions.push_back( + BufferRegion( + Index(firstIndex, 0), Size(firstNCells, getSize()(1)), + BufferRegion::Quadrant::Undefined)); } else if (i == 1) { clearCols(firstIndex, firstNCells); - newRegions.push_back(BufferRegion(Index(0, firstIndex), Size(getSize()(0), firstNCells), BufferRegion::Quadrant::Undefined)); + newRegions.push_back( + BufferRegion( + Index(0, firstIndex), Size(getSize()(0), firstNCells), + BufferRegion::Quadrant::Undefined)); } int secondIndex = 0; int secondNCells = nCells - firstNCells; if (i == 0) { clearRows(secondIndex, secondNCells); - newRegions.push_back(BufferRegion(Index(secondIndex, 0), Size(secondNCells, getSize()(1)), BufferRegion::Quadrant::Undefined)); + newRegions.push_back( + BufferRegion( + Index(secondIndex, 0), + Size(secondNCells, getSize()(1)), BufferRegion::Quadrant::Undefined)); } else if (i == 1) { clearCols(secondIndex, secondNCells); - newRegions.push_back(BufferRegion(Index(0, secondIndex), Size(getSize()(0), secondNCells), BufferRegion::Quadrant::Undefined)); + newRegions.push_back( + BufferRegion( + Index(0, secondIndex), + Size(getSize()(0), secondNCells), BufferRegion::Quadrant::Undefined)); } } } @@ -508,37 +579,41 @@ bool GridMap::move(const Position& position, std::vector& newRegio position_ += alignedPositionShift; // Check if map has been moved at all. - return (indexShift.any() != 0); + return indexShift.any() != 0; } -bool GridMap::move(const Position& position) { +bool GridMap::move(const Position & position) +{ std::vector newRegions; - return move(position, newRegions); + return GridMap::move(position, newRegions); } -bool GridMap::addDataFrom(const GridMap& other, bool extendMap, bool overwriteData, bool copyAllLayers, std::vector layers) { +bool GridMap::addDataFrom( + const GridMap & other, bool extendMap, bool overwriteData, + bool copyAllLayers, std::vector layers) +{ // Set the layers to copy. - if (copyAllLayers) layers = other.getLayers(); + if (copyAllLayers) {layers = other.getLayers();} // Resize map. - if (extendMap) extendToInclude(other); + if (extendMap) {extendToInclude(other);} // Check if all layers to copy exist and add missing layers. - for (const auto& layer : layers) { + for (const auto & layer : layers) { if (std::find(layers_.begin(), layers_.end(), layer) == layers_.end()) { add(layer); } } // Copy data. for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) { - if (isValid(*iterator) && !overwriteData) continue; + if (isValid(*iterator) && !overwriteData) {continue;} Position position; getPosition(*iterator, position); Index index; - if (!other.isInside(position)) continue; + if (!other.isInside(position)) {continue;} other.getIndex(position, index); - for (const auto& layer : layers) { - if (!other.isValid(index, layer)) continue; + for (const auto & layer : layers) { + if (!other.isValid(index, layer)) {continue;} at(layer, *iterator) = other.at(layer, index); } } @@ -546,13 +621,15 @@ bool GridMap::addDataFrom(const GridMap& other, bool extendMap, bool overwriteDa return true; } -bool GridMap::extendToInclude(const GridMap& other) { +bool GridMap::extendToInclude(const GridMap & other) +{ // Get dimension of maps. Position topLeftCorner(position_.x() + length_.x() / 2.0, position_.y() + length_.y() / 2.0); Position bottomRightCorner(position_.x() - length_.x() / 2.0, position_.y() - length_.y() / 2.0); - Position topLeftCornerOther(other.getPosition().x() + other.getLength().x() / 2.0, other.getPosition().y() + other.getLength().y() / 2.0); + Position topLeftCornerOther(other.getPosition().x() + other.getLength().x() / 2.0, + other.getPosition().y() + other.getLength().y() / 2.0); Position bottomRightCornerOther(other.getPosition().x() - other.getLength().x() / 2.0, - other.getPosition().y() - other.getLength().y() / 2.0); + other.getPosition().y() - other.getLength().y() / 2.0); // Check if map needs to be resized. bool resizeMap = false; Position extendedMapPosition = position_; @@ -603,13 +680,13 @@ bool GridMap::extendToInclude(const GridMap& other) { } // Copy data. for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) { - if (isValid(*iterator)) continue; + if (isValid(*iterator)) {continue;} Position position; getPosition(*iterator, position); Index index; - if (!mapCopy.isInside(position)) continue; + if (!mapCopy.isInside(position)) {continue;} mapCopy.getIndex(position, index); - for (const auto& layer : layers_) { + for (const auto & layer : layers_) { at(layer, *iterator) = mapCopy.at(layer, index); } } @@ -617,76 +694,97 @@ bool GridMap::extendToInclude(const GridMap& other) { return true; } -void GridMap::setTimestamp(const Time timestamp) { +void GridMap::setTimestamp(const Time timestamp) +{ timestamp_ = timestamp; } -Time GridMap::getTimestamp() const { +Time GridMap::getTimestamp() const +{ return timestamp_; } -void GridMap::resetTimestamp() { +void GridMap::resetTimestamp() +{ timestamp_ = 0.0; } -void GridMap::setFrameId(const std::string& frameId) { +void GridMap::setFrameId(const std::string & frameId) +{ frameId_ = frameId; } -const std::string& GridMap::getFrameId() const { +const std::string & GridMap::getFrameId() const +{ return frameId_; } -const Length& GridMap::getLength() const { +const Length & GridMap::getLength() const +{ return length_; } -const Position& GridMap::getPosition() const { +const Position & GridMap::getPosition() const +{ return position_; } -double GridMap::getResolution() const { +double GridMap::getResolution() const +{ return resolution_; } -const Size& GridMap::getSize() const { +const Size & GridMap::getSize() const +{ return size_; } -void GridMap::setStartIndex(const Index& startIndex) { +void GridMap::setStartIndex(const Index & startIndex) +{ startIndex_ = startIndex; } -const Index& GridMap::getStartIndex() const { +const Index & GridMap::getStartIndex() const +{ return startIndex_; } -bool GridMap::isDefaultStartIndex() const { +bool GridMap::isDefaultStartIndex() const +{ return (startIndex_ == 0).all(); } -void GridMap::convertToDefaultStartIndex() { - if (isDefaultStartIndex()) return; +void GridMap::convertToDefaultStartIndex() +{ + if (isDefaultStartIndex()) {return;} std::vector bufferRegions; if (!getBufferRegionsForSubmap(bufferRegions, startIndex_, size_, size_, startIndex_)) { throw std::out_of_range("Cannot access submap of this size."); } - for (auto& data : data_) { + for (auto & data : data_) { auto tempData(data.second); - for (const auto& bufferRegion : bufferRegions) { + for (const auto & bufferRegion : bufferRegions) { Index index = bufferRegion.getStartIndex(); Size size = bufferRegion.getSize(); if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopLeft) { - tempData.topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1)); + tempData.topLeftCorner(size(0), size(1)) = data.second.block( + index(0), index(1), size( + 0), size(1)); } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopRight) { - tempData.topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1)); + tempData.topRightCorner(size(0), size(1)) = data.second.block( + index(0), index(1), size( + 0), size(1)); } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomLeft) { - tempData.bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1)); + tempData.bottomLeftCorner(size(0), size(1)) = data.second.block( + index(0), index(1), size( + 0), size(1)); } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomRight) { - tempData.bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1)); + tempData.bottomRightCorner(size(0), size(1)) = data.second.block( + index(0), index(1), size( + 0), size(1)); } } data.second = tempData; @@ -695,8 +793,9 @@ void GridMap::convertToDefaultStartIndex() { startIndex_.setZero(); } -Position GridMap::getClosestPositionInMap(const Position& position) const { - if (getSize().x() < 1u || getSize().y() < 1u) { +Position GridMap::getClosestPositionInMap(const Position & position) const +{ + if (getSize().x() < 1 || getSize().y() < 1) { return position_; } @@ -730,49 +829,59 @@ Position GridMap::getClosestPositionInMap(const Position& position) const { return positionInMap; } -void GridMap::clear(const std::string& layer) { +void GridMap::clear(const std::string & layer) +{ try { data_.at(layer).setConstant(NAN); - } catch (const std::out_of_range& exception) { + } catch (const std::out_of_range & exception) { throw std::out_of_range("GridMap::clear(...) : No map layer '" + layer + "' available."); } } -void GridMap::clearBasic() { - for (auto& layer : basicLayers_) { +void GridMap::clearBasic() +{ + for (auto & layer : basicLayers_) { clear(layer); } } -void GridMap::clearAll() { - for (auto& data : data_) { +void GridMap::clearAll() +{ + for (auto & data : data_) { data.second.setConstant(NAN); } } -void GridMap::clearRows(unsigned int index, unsigned int nRows) { +void GridMap::clearRows(unsigned int index, unsigned int nRows) +{ std::vector layersToClear; - if (basicLayers_.size() > 0) + if (basicLayers_.size() > 0) { layersToClear = basicLayers_; - else + } else { layersToClear = layers_; - for (auto& layer : layersToClear) { + } + for (auto & layer : layersToClear) { data_.at(layer).block(index, 0, nRows, getSize()(1)).setConstant(NAN); } } -void GridMap::clearCols(unsigned int index, unsigned int nCols) { +void GridMap::clearCols(unsigned int index, unsigned int nCols) +{ std::vector layersToClear; - if (basicLayers_.size() > 0) + if (basicLayers_.size() > 0) { layersToClear = basicLayers_; - else + } else { layersToClear = layers_; - for (auto& layer : layersToClear) { + } + for (auto & layer : layersToClear) { data_.at(layer).block(0, index, getSize()(0), nCols).setConstant(NAN); } } -bool GridMap::atPositionLinearInterpolated(const std::string& layer, const Position& position, float& value) const { +bool GridMap::atPositionLinearInterpolated( + const std::string & layer, const Position & position, + float & value) const +{ Position point; Index indices[4]; bool idxTempDir; @@ -819,16 +928,16 @@ bool GridMap::atPositionLinearInterpolated(const std::string& layer, const Posit indices[3].x() = indices[1].x(); indices[3].y() = indices[2].y(); - const Size& mapSize = getSize(); + const Size & mapSize = getSize(); const size_t bufferSize = mapSize(0) * mapSize(1); const size_t startIndexLin = getLinearIndexFromIndex(startIndex_, mapSize); const size_t endIndexLin = startIndexLin + bufferSize; - const auto& layerMat = operator[](layer); + const auto & layerMat = operator[](layer); float f[4]; for (size_t i = 0; i < 4; ++i) { const size_t indexLin = getLinearIndexFromIndex(indices[idxShift[i]], mapSize); - if ((indexLin < startIndexLin) || (indexLin > endIndexLin)) return false; + if ((indexLin < startIndexLin) || (indexLin > endIndexLin)) {return false;} f[i] = layerMat(indexLin); } @@ -836,24 +945,30 @@ bool GridMap::atPositionLinearInterpolated(const std::string& layer, const Posit const Position positionRed = (position - point) / resolution_; const Position positionRedFlip = Position(1., 1.) - positionRed; - value = f[0] * positionRedFlip.x() * positionRedFlip.y() + f[1] * positionRed.x() * positionRedFlip.y() + - f[2] * positionRedFlip.x() * positionRed.y() + f[3] * positionRed.x() * positionRed.y(); + value = f[0] * positionRedFlip.x() * positionRedFlip.y() + f[1] * positionRed.x() * + positionRedFlip.y() + + f[2] * positionRedFlip.x() * positionRed.y() + f[3] * positionRed.x() * positionRed.y(); return true; } -void GridMap::resize(const Index& size) { +void GridMap::resize(const Index & size) +{ size_ = size; - for (auto& data : data_) { + for (auto & data : data_) { data.second.resize(size_(0), size_(1)); } } -bool GridMap::atPositionBicubicConvolutionInterpolated(const std::string& layer, const Position& position, - float& value) const +bool GridMap::atPositionBicubicConvolutionInterpolated( + const std::string & layer, const Position & position, + float & value) const { double interpolatedValue = 0.0; - if (!bicubic_conv::evaluateBicubicConvolutionInterpolation(*this, layer, position, &interpolatedValue)) { + if (!bicubic_conv::evaluateBicubicConvolutionInterpolation( + *this, layer, position, + &interpolatedValue)) + { return false; } @@ -865,8 +980,9 @@ bool GridMap::atPositionBicubicConvolutionInterpolated(const std::string& layer, return true; } -bool GridMap::atPositionBicubicInterpolated(const std::string& layer, const Position& position, - float& value) const +bool GridMap::atPositionBicubicInterpolated( + const std::string & layer, const Position & position, + float & value) const { double interpolatedValue = 0.0; if (!bicubic::evaluateBicubicInterpolation(*this, layer, position, &interpolatedValue)) { @@ -879,7 +995,6 @@ bool GridMap::atPositionBicubicInterpolated(const std::string& layer, const Posi value = interpolatedValue; return true; - } diff --git a/grid_map_core/src/GridMapMath.cpp b/grid_map_core/src/GridMapMath.cpp index 361fe6357..b12162c42 100644 --- a/grid_map_core/src/GridMapMath.cpp +++ b/grid_map_core/src/GridMapMath.cpp @@ -6,19 +6,22 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/GridMapMath.hpp" - // fabs #include // Limits #include -using namespace std; +#include + +#include "grid_map_core/GridMapMath.hpp" + -namespace grid_map { +namespace grid_map +{ -namespace internal { +namespace internal +{ /*! * Gets the vector from the center of the map to the origin @@ -27,7 +30,7 @@ namespace internal { * @param[in] mapLength the lengths in x and y direction. * @return true if successful. */ -inline bool getVectorToOrigin(Vector& vectorToOrigin, const Length& mapLength) +inline bool getVectorToOrigin(Vector & vectorToOrigin, const Length & mapLength) { vectorToOrigin = (0.5 * mapLength).matrix(); return true; @@ -41,8 +44,9 @@ inline bool getVectorToOrigin(Vector& vectorToOrigin, const Length& mapLength) * @param[in] resolution the resolution of the map. * @return true if successful. */ -inline bool getVectorToFirstCell(Vector& vectorToFirstCell, - const Length& mapLength, const double& resolution) +inline bool getVectorToFirstCell( + Vector & vectorToFirstCell, + const Length & mapLength, const double & resolution) { Vector vectorToOrigin; getVectorToOrigin(vectorToOrigin, mapLength); @@ -57,7 +61,8 @@ inline Eigen::Matrix2i getBufferOrderToMapFrameTransformation() return -Eigen::Matrix2i::Identity(); } -inline Vector transformBufferOrderToMapFrame(const Index& index) { +inline Vector transformBufferOrderToMapFrame(const Index & index) +{ return {-index[0], -index[1]}; } @@ -66,23 +71,25 @@ inline Eigen::Matrix2i getMapFrameToBufferOrderTransformation() return getBufferOrderToMapFrameTransformation().transpose(); } -inline Index transformMapFrameToBufferOrder(const Vector& vector) { +inline Index transformMapFrameToBufferOrder(const Vector & vector) +{ return {-vector[0], -vector[1]}; } -inline Index transformMapFrameToBufferOrder(const Eigen::Vector2i& vector) { +inline Index transformMapFrameToBufferOrder(const Eigen::Vector2i & vector) +{ return {-vector[0], -vector[1]}; } -inline bool checkIfStartIndexAtDefaultPosition(const Index& bufferStartIndex) +inline bool checkIfStartIndexAtDefaultPosition(const Index & bufferStartIndex) { - return ((bufferStartIndex == 0).all()); + return (bufferStartIndex == 0).all(); } inline Vector getIndexVectorFromIndex( - const Index& index, - const Size& bufferSize, - const Index& bufferStartIndex) + const Index & index, + const Size & bufferSize, + const Index & bufferStartIndex) { Index unwrappedIndex; unwrappedIndex = getIndexFromBufferIndex(index, bufferSize, bufferStartIndex); @@ -90,135 +97,150 @@ inline Vector getIndexVectorFromIndex( } inline Index getIndexFromIndexVector( - const Vector& indexVector, - const Size& bufferSize, - const Index& bufferStartIndex) + const Vector & indexVector, + const Size & bufferSize, + const Index & bufferStartIndex) { Index index = transformMapFrameToBufferOrder(indexVector); return getBufferIndexFromIndex(index, bufferSize, bufferStartIndex); } -inline BufferRegion::Quadrant getQuadrant(const Index& index, const Index& bufferStartIndex) +inline BufferRegion::Quadrant getQuadrant(const Index & index, const Index & bufferStartIndex) { - if (index[0] >= bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) return BufferRegion::Quadrant::TopLeft; - if (index[0] >= bufferStartIndex[0] && index[1] < bufferStartIndex[1]) return BufferRegion::Quadrant::TopRight; - if (index[0] < bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) return BufferRegion::Quadrant::BottomLeft; - if (index[0] < bufferStartIndex[0] && index[1] < bufferStartIndex[1]) return BufferRegion::Quadrant::BottomRight; + if (index[0] >= bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) { + return BufferRegion::Quadrant::TopLeft; + } + if (index[0] >= bufferStartIndex[0] && index[1] < bufferStartIndex[1]) { + return BufferRegion::Quadrant::TopRight; + } + if (index[0] < bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) { + return BufferRegion::Quadrant::BottomLeft; + } + if (index[0] < bufferStartIndex[0] && index[1] < bufferStartIndex[1]) { + return BufferRegion::Quadrant::BottomRight; + } return BufferRegion::Quadrant::Undefined; } -} // namespace - -using namespace internal; +} // namespace internal -bool getPositionFromIndex(Position& position, - const Index& index, - const Length& mapLength, - const Position& mapPosition, - const double& resolution, - const Size& bufferSize, - const Index& bufferStartIndex) +bool getPositionFromIndex( + Position & position, + const Index & index, + const Length & mapLength, + const Position & mapPosition, + const double & resolution, + const Size & bufferSize, + const Index & bufferStartIndex) { - if (!checkIfIndexInRange(index, bufferSize)) return false; + if (!checkIfIndexInRange(index, bufferSize)) {return false;} Vector offset; - getVectorToFirstCell(offset, mapLength, resolution); - position = mapPosition + offset + resolution * getIndexVectorFromIndex(index, bufferSize, bufferStartIndex); + internal::getVectorToFirstCell(offset, mapLength, resolution); + position = mapPosition + offset + resolution * internal::getIndexVectorFromIndex( + index, bufferSize, + bufferStartIndex); return true; } -bool getIndexFromPosition(Index& index, - const Position& position, - const Length& mapLength, - const Position& mapPosition, - const double& resolution, - const Size& bufferSize, - const Index& bufferStartIndex) +bool getIndexFromPosition( + Index & index, + const Position & position, + const Length & mapLength, + const Position & mapPosition, + const double & resolution, + const Size & bufferSize, + const Index & bufferStartIndex) { Vector offset; - getVectorToOrigin(offset, mapLength); + internal::getVectorToOrigin(offset, mapLength); Vector indexVector = ((position - offset - mapPosition).array() / resolution).matrix(); - index = getIndexFromIndexVector(indexVector, bufferSize, bufferStartIndex); - if (!checkIfPositionWithinMap(position, mapLength, mapPosition)) return false; + index = internal::getIndexFromIndexVector(indexVector, bufferSize, bufferStartIndex); + if (!checkIfPositionWithinMap(position, mapLength, mapPosition)) {return false;} return true; } -bool checkIfPositionWithinMap(const Position& position, - const Length& mapLength, - const Position& mapPosition) +bool checkIfPositionWithinMap( + const Position & position, + const Length & mapLength, + const Position & mapPosition) { Vector offset; - getVectorToOrigin(offset, mapLength); - Position positionTransformed = getMapFrameToBufferOrderTransformation().cast() * (position - mapPosition - offset); + internal::getVectorToOrigin(offset, mapLength); + Position positionTransformed = internal::getMapFrameToBufferOrderTransformation().cast() * + (position - mapPosition - offset); - if (positionTransformed.x() >= 0.0 && positionTransformed.y() >= 0.0 - && positionTransformed.x() < mapLength(0) && positionTransformed.y() < mapLength(1)) { + if (positionTransformed.x() >= 0.0 && positionTransformed.y() >= 0.0 && + positionTransformed.x() < mapLength(0) && positionTransformed.y() < mapLength(1)) + { return true; } return false; } -void getPositionOfDataStructureOrigin(const Position& position, - const Length& mapLength, - Position& positionOfOrigin) +void getPositionOfDataStructureOrigin( + const Position & position, + const Length & mapLength, + Position & positionOfOrigin) { Vector vectorToOrigin; - getVectorToOrigin(vectorToOrigin, mapLength); + internal::getVectorToOrigin(vectorToOrigin, mapLength); positionOfOrigin = position + vectorToOrigin; } -bool getIndexShiftFromPositionShift(Index& indexShift, - const Vector& positionShift, - const double& resolution) +bool getIndexShiftFromPositionShift( + Index & indexShift, + const Vector & positionShift, + const double & resolution) { Vector indexShiftVectorTemp = (positionShift.array() / resolution).matrix(); Eigen::Vector2i indexShiftVector; for (int i = 0; i < indexShiftVector.size(); i++) { - indexShiftVector[i] = static_cast(indexShiftVectorTemp[i] + 0.5 * (indexShiftVectorTemp[i] > 0 ? 1 : -1)); + indexShiftVector[i] = + static_cast(indexShiftVectorTemp[i] + 0.5 * (indexShiftVectorTemp[i] > 0 ? 1 : -1)); } - indexShift = transformMapFrameToBufferOrder(indexShiftVector); + indexShift = internal::transformMapFrameToBufferOrder(indexShiftVector); return true; } -bool getPositionShiftFromIndexShift(Vector& positionShift, - const Index& indexShift, - const double& resolution) +bool getPositionShiftFromIndexShift( + Vector & positionShift, + const Index & indexShift, + const double & resolution) { - positionShift = transformBufferOrderToMapFrame(indexShift) * resolution; + positionShift = internal::transformBufferOrderToMapFrame(indexShift) * resolution; return true; } -bool checkIfIndexInRange(const Index& index, const Size& bufferSize) +bool checkIfIndexInRange(const Index & index, const Size & bufferSize) { - if (index[0] >= 0 && index[1] >= 0 && index[0] < bufferSize[0] && index[1] < bufferSize[1]) - { + if (index[0] >= 0 && index[1] >= 0 && index[0] < bufferSize[0] && index[1] < bufferSize[1]) { return true; } return false; } -void boundIndexToRange(Index& index, const Size& bufferSize) +void boundIndexToRange(Index & index, const Size & bufferSize) { for (int i = 0; i < index.size(); i++) { boundIndexToRange(index[i], bufferSize[i]); } } -void boundIndexToRange(int& index, const int& bufferSize) +void boundIndexToRange(int & index, const int & bufferSize) { - if (index < 0) index = 0; - else if (index >= bufferSize) index = bufferSize - 1; + if (index < 0) {index = 0;} else if (index >= bufferSize) {index = bufferSize - 1;} } -void wrapIndexToRange(Index& index, const Size& bufferSize) +void wrapIndexToRange(Index & index, const Size & bufferSize) { for (int i = 0; i < index.size(); i++) { wrapIndexToRange(index[i], bufferSize[i]); } } -void wrapIndexToRange(int& index, int bufferSize) +void wrapIndexToRange(int & index, int bufferSize) { index = index % bufferSize; if (index < 0) { @@ -226,17 +248,19 @@ void wrapIndexToRange(int& index, int bufferSize) } } -void boundPositionToRange(Position& position, const Length& mapLength, const Position& mapPosition) +void boundPositionToRange( + Position & position, const Length & mapLength, + const Position & mapPosition) { Vector vectorToOrigin; - getVectorToOrigin(vectorToOrigin, mapLength); + internal::getVectorToOrigin(vectorToOrigin, mapLength); Position positionShifted = position - mapPosition + vectorToOrigin; // We have to make sure to stay inside the map. for (int i = 0; i < positionShifted.size(); i++) { - - double epsilon = 10.0 * numeric_limits::epsilon(); // TODO Why is the factor 10 necessary. - if (std::fabs(position(i)) > 1.0) epsilon *= std::fabs(position(i)); + // TODO(needs_assignment): Why is the factor 10 necessary. + double epsilon = 10.0 * std::numeric_limits::epsilon(); + if (std::fabs(position(i)) > 1.0) {epsilon *= std::fabs(position(i));} if (positionShifted(i) <= 0) { positionShifted(i) = epsilon; @@ -253,41 +277,51 @@ void boundPositionToRange(Position& position, const Length& mapLength, const Pos const Eigen::Matrix2i getBufferOrderToMapFrameAlignment() { - return getBufferOrderToMapFrameTransformation().array().abs().matrix(); + return internal::getBufferOrderToMapFrameTransformation().array().abs().matrix(); } -bool getSubmapInformation(Index& submapTopLeftIndex, - Size& submapBufferSize, - Position& submapPosition, - Length& submapLength, - Index& requestedIndexInSubmap, - const Position& requestedSubmapPosition, - const Length& requestedSubmapLength, - const Length& mapLength, - const Position& mapPosition, - const double& resolution, - const Size& bufferSize, - const Index& bufferStartIndex) +bool getSubmapInformation( + Index & submapTopLeftIndex, + Size & submapBufferSize, + Position & submapPosition, + Length & submapLength, + Index & requestedIndexInSubmap, + const Position & requestedSubmapPosition, + const Length & requestedSubmapLength, + const Length & mapLength, + const Position & mapPosition, + const double & resolution, + const Size & bufferSize, + const Index & bufferStartIndex) { // (Top left / bottom right corresponds to the position in the matrix, not the map frame) - const Eigen::Matrix2d halfTransform = 0.5 * getMapFrameToBufferOrderTransformation().cast(); + const Eigen::Matrix2d halfTransform = 0.5 * + internal::getMapFrameToBufferOrderTransformation().cast(); // Corners of submap. - Position topLeftPosition = requestedSubmapPosition - halfTransform * requestedSubmapLength.matrix(); + Position topLeftPosition = requestedSubmapPosition - halfTransform * + requestedSubmapLength.matrix(); boundPositionToRange(topLeftPosition, mapLength, mapPosition); - if(!getIndexFromPosition(submapTopLeftIndex, topLeftPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false; + if (!getIndexFromPosition( + submapTopLeftIndex, topLeftPosition, mapLength, mapPosition, resolution, + bufferSize, bufferStartIndex)) {return false;} Index topLeftIndex; topLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex); - Position bottomRightPosition = requestedSubmapPosition + halfTransform * requestedSubmapLength.matrix(); + Position bottomRightPosition = requestedSubmapPosition + halfTransform * + requestedSubmapLength.matrix(); boundPositionToRange(bottomRightPosition, mapLength, mapPosition); Index bottomRightIndex; - if(!getIndexFromPosition(bottomRightIndex, bottomRightPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false; + if (!getIndexFromPosition( + bottomRightIndex, bottomRightPosition, mapLength, mapPosition, + resolution, bufferSize, bufferStartIndex)) {return false;} bottomRightIndex = getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex); // Get the position of the top left corner of the generated submap. Position topLeftCorner; - if(!getPositionFromIndex(topLeftCorner, submapTopLeftIndex, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false; + if (!getPositionFromIndex( + topLeftCorner, submapTopLeftIndex, mapLength, mapPosition, resolution, + bufferSize, bufferStartIndex)) {return false;} topLeftCorner -= halfTransform * Position::Constant(resolution); // Size of submap. @@ -298,131 +332,182 @@ bool getSubmapInformation(Index& submapTopLeftIndex, // Position of submap. Vector vectorToSubmapOrigin; - getVectorToOrigin(vectorToSubmapOrigin, submapLength); + internal::getVectorToOrigin(vectorToSubmapOrigin, submapLength); submapPosition = topLeftCorner - vectorToSubmapOrigin; // Get the index of the cell which corresponds the requested // position of the submap. - return getIndexFromPosition(requestedIndexInSubmap, requestedSubmapPosition, submapLength, submapPosition, resolution, submapBufferSize); + return getIndexFromPosition( + requestedIndexInSubmap, requestedSubmapPosition, submapLength, + submapPosition, resolution, submapBufferSize); } -Size getSubmapSizeFromCornerIndeces(const Index& topLeftIndex, const Index& bottomRightIndex, - const Size& bufferSize, const Index& bufferStartIndex) +Size getSubmapSizeFromCornerIndeces( + const Index & topLeftIndex, const Index & bottomRightIndex, + const Size & bufferSize, const Index & bufferStartIndex) { - const Index unwrappedTopLeftIndex = getIndexFromBufferIndex(topLeftIndex, bufferSize, bufferStartIndex); - const Index unwrappedBottomRightIndex = getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex); + const Index unwrappedTopLeftIndex = getIndexFromBufferIndex( + topLeftIndex, bufferSize, + bufferStartIndex); + const Index unwrappedBottomRightIndex = getIndexFromBufferIndex( + bottomRightIndex, bufferSize, + bufferStartIndex); return Size(unwrappedBottomRightIndex - unwrappedTopLeftIndex + Size::Ones()); } -bool getBufferRegionsForSubmap(std::vector& submapBufferRegions, - const Index& submapIndex, - const Size& submapBufferSize, - const Size& bufferSize, - const Index& bufferStartIndex) +bool getBufferRegionsForSubmap( + std::vector & submapBufferRegions, + const Index & submapIndex, + const Size & submapBufferSize, + const Size & bufferSize, + const Index & bufferStartIndex) { - if ((getIndexFromBufferIndex(submapIndex, bufferSize, bufferStartIndex) + submapBufferSize > bufferSize).any()) return false; + if ((getIndexFromBufferIndex( + submapIndex, bufferSize, + bufferStartIndex) + submapBufferSize > bufferSize).any()) {return false;} submapBufferRegions.clear(); Index bottomRightIndex = submapIndex + submapBufferSize - Index::Ones(); wrapIndexToRange(bottomRightIndex, bufferSize); - BufferRegion::Quadrant quadrantOfTopLeft = getQuadrant(submapIndex, bufferStartIndex); - BufferRegion::Quadrant quadrantOfBottomRight = getQuadrant(bottomRightIndex, bufferStartIndex); + BufferRegion::Quadrant quadrantOfTopLeft = internal::getQuadrant(submapIndex, bufferStartIndex); + BufferRegion::Quadrant quadrantOfBottomRight = internal::getQuadrant( + bottomRightIndex, + bufferStartIndex); if (quadrantOfTopLeft == BufferRegion::Quadrant::TopLeft) { - if (quadrantOfBottomRight == BufferRegion::Quadrant::TopLeft) { - submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopLeft)); + submapBufferRegions.push_back( + BufferRegion( + submapIndex, submapBufferSize, + BufferRegion::Quadrant::TopLeft)); return true; } if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) { Size topLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1)); - submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft)); + submapBufferRegions.push_back( + BufferRegion( + submapIndex, topLeftSize, + BufferRegion::Quadrant::TopLeft)); Index topRightIndex(submapIndex(0), 0); Size topRightSize(submapBufferSize(0), submapBufferSize(1) - topLeftSize(1)); - submapBufferRegions.push_back(BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight)); + submapBufferRegions.push_back( + BufferRegion( + topRightIndex, topRightSize, + BufferRegion::Quadrant::TopRight)); return true; } if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) { Size topLeftSize(bufferSize(0) - submapIndex(0), submapBufferSize(1)); - submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft)); + submapBufferRegions.push_back( + BufferRegion( + submapIndex, topLeftSize, + BufferRegion::Quadrant::TopLeft)); Index bottomLeftIndex(0, submapIndex(1)); Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), submapBufferSize(1)); - submapBufferRegions.push_back(BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft)); + submapBufferRegions.push_back( + BufferRegion( + bottomLeftIndex, bottomLeftSize, + BufferRegion::Quadrant::BottomLeft)); return true; } if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) { Size topLeftSize(bufferSize(0) - submapIndex(0), bufferSize(1) - submapIndex(1)); - submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft)); + submapBufferRegions.push_back( + BufferRegion( + submapIndex, topLeftSize, + BufferRegion::Quadrant::TopLeft)); Index topRightIndex(submapIndex(0), 0); Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1) - topLeftSize(1)); - submapBufferRegions.push_back(BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight)); + submapBufferRegions.push_back( + BufferRegion( + topRightIndex, topRightSize, + BufferRegion::Quadrant::TopRight)); Index bottomLeftIndex(0, submapIndex(1)); Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), bufferSize(1) - submapIndex(1)); - submapBufferRegions.push_back(BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft)); + submapBufferRegions.push_back( + BufferRegion( + bottomLeftIndex, bottomLeftSize, + BufferRegion::Quadrant::BottomLeft)); Index bottomRightIndex = Index::Zero(); Size bottomRightSize(bottomLeftSize(0), topRightSize(1)); - submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight)); + submapBufferRegions.push_back( + BufferRegion( + bottomRightIndex, bottomRightSize, + BufferRegion::Quadrant::BottomRight)); return true; } - } else if (quadrantOfTopLeft == BufferRegion::Quadrant::TopRight) { - if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) { - submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopRight)); + submapBufferRegions.push_back( + BufferRegion( + submapIndex, submapBufferSize, + BufferRegion::Quadrant::TopRight)); return true; } if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) { - Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1)); - submapBufferRegions.push_back(BufferRegion(submapIndex, topRightSize, BufferRegion::Quadrant::TopRight)); + submapBufferRegions.push_back( + BufferRegion( + submapIndex, topRightSize, + BufferRegion::Quadrant::TopRight)); Index bottomRightIndex(0, submapIndex(1)); Size bottomRightSize(submapBufferSize(0) - topRightSize(0), submapBufferSize(1)); - submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight)); + submapBufferRegions.push_back( + BufferRegion( + bottomRightIndex, bottomRightSize, + BufferRegion::Quadrant::BottomRight)); return true; } - } else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomLeft) { - if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) { - submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomLeft)); + submapBufferRegions.push_back( + BufferRegion( + submapIndex, submapBufferSize, + BufferRegion::Quadrant::BottomLeft)); return true; } if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) { Size bottomLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1)); - submapBufferRegions.push_back(BufferRegion(submapIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft)); + submapBufferRegions.push_back( + BufferRegion( + submapIndex, bottomLeftSize, + BufferRegion::Quadrant::BottomLeft)); Index bottomRightIndex(submapIndex(0), 0); Size bottomRightSize(submapBufferSize(0), submapBufferSize(1) - bottomLeftSize(1)); - submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight)); + submapBufferRegions.push_back( + BufferRegion( + bottomRightIndex, bottomRightSize, + BufferRegion::Quadrant::BottomRight)); return true; } - } else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomRight) { - if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) { - submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomRight)); + submapBufferRegions.push_back( + BufferRegion( + submapIndex, submapBufferSize, + BufferRegion::Quadrant::BottomRight)); return true; } - } return false; } -bool incrementIndex(Index& index, const Size& bufferSize, const Index& bufferStartIndex) +bool incrementIndex(Index & index, const Size & bufferSize, const Index & bufferStartIndex) { Index unwrappedIndex = getIndexFromBufferIndex(index, bufferSize, bufferStartIndex); @@ -437,16 +522,17 @@ bool incrementIndex(Index& index, const Size& bufferSize, const Index& bufferSta } // End of iterations reached. - if (!checkIfIndexInRange(unwrappedIndex, bufferSize)) return false; + if (!checkIfIndexInRange(unwrappedIndex, bufferSize)) {return false;} // Return true iterated index. index = getBufferIndexFromIndex(unwrappedIndex, bufferSize, bufferStartIndex); return true; } -bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& submapTopLeftIndex, - const Size& submapBufferSize, const Size& bufferSize, - const Index& bufferStartIndex) +bool incrementIndexForSubmap( + Index & submapIndex, Index & index, const Index & submapTopLeftIndex, + const Size & submapBufferSize, const Size & bufferSize, + const Index & bufferStartIndex) { // Copy the data first, only copy it back if everything is within range. Index tempIndex = index; @@ -463,11 +549,15 @@ bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& subm } // End of iterations reached. - if (!checkIfIndexInRange(tempSubmapIndex, submapBufferSize)) return false; + if (!checkIfIndexInRange(tempSubmapIndex, submapBufferSize)) {return false;} // Get corresponding index in map. - Index unwrappedSubmapTopLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex); - tempIndex = getBufferIndexFromIndex(unwrappedSubmapTopLeftIndex + tempSubmapIndex, bufferSize, bufferStartIndex); + Index unwrappedSubmapTopLeftIndex = getIndexFromBufferIndex( + submapTopLeftIndex, bufferSize, + bufferStartIndex); + tempIndex = getBufferIndexFromIndex( + unwrappedSubmapTopLeftIndex + tempSubmapIndex, bufferSize, + bufferStartIndex); // Copy data back. index = tempIndex; @@ -475,60 +565,74 @@ bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& subm return true; } -Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize, const Index& bufferStartIndex) +Index getIndexFromBufferIndex( + const Index & bufferIndex, const Size & bufferSize, + const Index & bufferStartIndex) { - if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) return bufferIndex; + if (internal::checkIfStartIndexAtDefaultPosition(bufferStartIndex)) {return bufferIndex;} Index index = bufferIndex - bufferStartIndex; wrapIndexToRange(index, bufferSize); return index; } -Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const Index& bufferStartIndex) +Index getBufferIndexFromIndex( + const Index & index, const Size & bufferSize, + const Index & bufferStartIndex) { - if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) return index; + if (internal::checkIfStartIndexAtDefaultPosition(bufferStartIndex)) {return index;} Index bufferIndex = index + bufferStartIndex; wrapIndexToRange(bufferIndex, bufferSize); return bufferIndex; } -size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, const bool rowMajor) +size_t getLinearIndexFromIndex(const Index & index, const Size & bufferSize, const bool rowMajor) { - if (!rowMajor) return index(1) * bufferSize(0) + index(0); + if (!rowMajor) {return index(1) * bufferSize(0) + index(0);} return index(0) * bufferSize(1) + index(1); } -Index getIndexFromLinearIndex(const size_t linearIndex, const Size& bufferSize, const bool rowMajor) -{ - if (!rowMajor) return Index((int)linearIndex % bufferSize(0), (int)linearIndex / bufferSize(0)); - return Index((int)linearIndex / bufferSize(1), (int)linearIndex % bufferSize(1)); -} - -void getIndicesForRegion(const Index& regionIndex, const Size& regionSize, - std::vector indices) -{ -// for (int i = line.index_; col < line.endIndex(); col++) { -// for (int i = 0; i < getSize()(0); i++) { -// -// } -// } -} - -void getIndicesForRegions(const std::vector& regionIndeces, const Size& regionSizes, - std::vector indices) +Index getIndexFromLinearIndex( + const size_t linearIndex, const Size & bufferSize, + const bool rowMajor) { -} - -bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3i& colorVector) + if (!rowMajor) { + return Index( + static_cast(linearIndex) % bufferSize( + 0), static_cast(linearIndex) / bufferSize(0)); + } + return Index( + static_cast(linearIndex) / bufferSize( + 1), static_cast(linearIndex) % bufferSize(1)); +} + +// void getIndicesForRegion( +// const Index & regionIndex, const Size & regionSize, +// std::vector indices) +// { +// // for (int i = line.index_; col < line.endIndex(); col++) { +// // for (int i = 0; i < getSize()(0); i++) { +// // +// // } +// // } +// } + +// void getIndicesForRegions( +// const std::vector & regionIndeces, const Size & regionSizes, +// std::vector indices) +// { +// } + +bool colorValueToVector(const uint64_t & colorValue, Eigen::Vector3i & colorVector) { colorVector(0) = (colorValue >> 16) & 0x0000ff; colorVector(1) = (colorValue >> 8) & 0x0000ff; - colorVector(2) = colorValue & 0x0000ff; + colorVector(2) = colorValue & 0x0000ff; return true; } -bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3f& colorVector) +bool colorValueToVector(const uint64_t & colorValue, Eigen::Vector3f & colorVector) { Eigen::Vector3i tempColorVector; colorValueToVector(colorValue, tempColorVector); @@ -536,32 +640,32 @@ bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3f& colorV return true; } -bool colorValueToVector(const float& colorValue, Eigen::Vector3f& colorVector) +bool colorValueToVector(const float & colorValue, Eigen::Vector3f & colorVector) { // cppcheck-suppress invalidPointerCast - const unsigned long tempColorValue = *reinterpret_cast(&colorValue); + const uint64_t tempColorValue = *reinterpret_cast(&colorValue); colorValueToVector(tempColorValue, colorVector); return true; } -bool colorVectorToValue(const Eigen::Vector3i& colorVector, unsigned long& colorValue) +bool colorVectorToValue(const Eigen::Vector3i & colorVector, uint64_t & colorValue) { - colorValue = ((int)colorVector(0)) << 16 | ((int)colorVector(1)) << 8 | ((int)colorVector(2)); + colorValue = static_cast(colorVector(0)) << 16 | static_cast(colorVector(1)) << 8 | + static_cast(colorVector(2)); return true; } -void colorVectorToValue(const Eigen::Vector3i& colorVector, float& colorValue) +void colorVectorToValue(const Eigen::Vector3i & colorVector, float & colorValue) { - unsigned long color = (colorVector(0) << 16) + (colorVector(1) << 8) + colorVector(2); - // cppcheck-suppress invalidPointerCast - colorValue = *reinterpret_cast(&color); + uint64_t color = (colorVector(0) << 16) + (colorVector(1) << 8) + colorVector(2); + float * temp = reinterpret_cast(&color); + colorValue = *temp; } -void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue) +void colorVectorToValue(const Eigen::Vector3f & colorVector, float & colorValue) { Eigen::Vector3i tempColorVector = (colorVector * 255.0).cast(); colorVectorToValue(tempColorVector, colorValue); } -} // namespace - +} // namespace grid_map diff --git a/grid_map_core/src/Polygon.cpp b/grid_map_core/src/Polygon.cpp index 715aec4c6..fff4f500b 100644 --- a/grid_map_core/src/Polygon.cpp +++ b/grid_map_core/src/Polygon.cpp @@ -6,49 +6,50 @@ * Institute: ETH Zurich, ANYbotics */ -#include - #include #include +#include + +#include +#include #include #include -namespace grid_map { +namespace grid_map +{ Polygon::Polygon() - : timestamp_(0) +: timestamp_(0) { } Polygon::Polygon(std::vector vertices) - : Polygon() +: Polygon() { vertices_ = vertices; } -Polygon::~Polygon() {} - -bool Polygon::isInside(const Position& point) const +bool Polygon::isInside(const Position & point) const { int cross = 0; - for (int i = 0, j = vertices_.size() - 1; i < vertices_.size(); j = i++) { - if ( ((vertices_[i].y() > point.y()) != (vertices_[j].y() > point.y())) - && (point.x() < (vertices_[j].x() - vertices_[i].x()) * (point.y() - vertices_[i].y()) / - (vertices_[j].y() - vertices_[i].y()) + vertices_[i].x()) ) + for (std::size_t i = 0, j = vertices_.size() - 1; i < vertices_.size(); j = i++) { + if ( ((vertices_[i].y() > point.y()) != (vertices_[j].y() > point.y())) && + (point.x() < (vertices_[j].x() - vertices_[i].x()) * (point.y() - vertices_[i].y()) / + (vertices_[j].y() - vertices_[i].y()) + vertices_[i].x()) ) { cross++; } } - return bool(cross % 2); + return static_cast(cross % 2); } -void Polygon::addVertex(const Position& vertex) +void Polygon::addVertex(const Position & vertex) { vertices_.push_back(vertex); } -const Position& Polygon::getVertex(const size_t index) const +const Position & Polygon::getVertex(const size_t index) const { return vertices_.at(index); } @@ -58,12 +59,12 @@ void Polygon::removeVertices() vertices_.clear(); } -const Position& Polygon::operator [](const size_t index) const +const Position & Polygon::operator[](const size_t index) const { return getVertex(index); } -const std::vector& Polygon::getVertices() const +const std::vector & Polygon::getVertices() const { return vertices_; } @@ -73,12 +74,12 @@ size_t Polygon::nVertices() const return vertices_.size(); } -const std::string& Polygon::getFrameId() const +const std::string & Polygon::getFrameId() const { return frameId_; } -void Polygon::setFrameId(const std::string& frameId) +void Polygon::setFrameId(const std::string & frameId) { frameId_ = frameId; } @@ -102,9 +103,9 @@ double Polygon::getArea() const { double area = 0.0; int j = vertices_.size() - 1; - for (int i = 0; i < vertices_.size(); i++) { - area += (vertices_.at(j).x() + vertices_.at(i).x()) - * (vertices_.at(j).y() - vertices_.at(i).y()); + for (std::size_t i = 0; i < vertices_.size(); i++) { + area += (vertices_.at(j).x() + vertices_.at(i).x()) * + (vertices_.at(j).y() - vertices_.at(i).y()); j = i; } return std::abs(area / 2.0); @@ -116,28 +117,28 @@ Position Polygon::getCentroid() const std::vector vertices = getVertices(); vertices.push_back(vertices.at(0)); double area = 0.0; - for (int i = 0; i < vertices.size() - 1; i++) { - const double a = vertices[i].x() * vertices[i+1].y() - vertices[i+1].x() * vertices[i].y(); + for (std::size_t i = 0; i < vertices.size() - 1; i++) { + const double a = vertices[i].x() * vertices[i + 1].y() - vertices[i + 1].x() * vertices[i].y(); area += a; - centroid.x() += a * (vertices[i].x() + vertices[i+1].x()); - centroid.y() += a * (vertices[i].y() + vertices[i+1].y()); + centroid.x() += a * (vertices[i].x() + vertices[i + 1].x()); + centroid.y() += a * (vertices[i].y() + vertices[i + 1].y()); } area *= 0.5; centroid /= (6.0 * area); return centroid; } -void Polygon::getBoundingBox(Position& center, Length& length) const +void Polygon::getBoundingBox(Position & center, Length & length) const { double minX = std::numeric_limits::infinity(); double maxX = -std::numeric_limits::infinity(); double minY = std::numeric_limits::infinity(); double maxY = -std::numeric_limits::infinity(); - for (const auto& vertex : vertices_) { - if (vertex.x() > maxX) maxX = vertex.x(); - if (vertex.y() > maxY) maxY = vertex.y(); - if (vertex.x() < minX) minX = vertex.x(); - if (vertex.y() < minY) minY = vertex.y(); + for (const auto & vertex : vertices_) { + if (vertex.x() > maxX) {maxX = vertex.x();} + if (vertex.y() > maxY) {maxY = vertex.y();} + if (vertex.x() < minX) {minX = vertex.x();} + if (vertex.y() < minY) {minY = vertex.y();} } center.x() = (minX + maxX) / 2.0; center.y() = (minY + maxY) / 2.0; @@ -145,19 +146,21 @@ void Polygon::getBoundingBox(Position& center, Length& length) const length.y() = (maxY - minY); } -bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd& A, Eigen::VectorXd& b) const +bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd & A, Eigen::VectorXd & b) const { Eigen::MatrixXd V(nVertices(), 2); - for (unsigned int i = 0; i < nVertices(); ++i) + for (unsigned int i = 0; i < nVertices(); ++i) { V.row(i) = vertices_[i]; + } // Create k, a list of indices from V forming the convex hull. - // TODO: Assuming counter-clockwise ordered convex polygon. + // TODO(needs_assignment): Assuming counter-clockwise ordered convex polygon. // MATLAB: k = convhulln(V); Eigen::MatrixXi k; k.resizeLike(V); - for (unsigned int i = 0; i < V.rows(); ++i) - k.row(i) << i, (i+1) % V.rows(); + for (unsigned int i = 0; i < V.rows(); ++i) { + k.row(i) << i, (i + 1) % V.rows(); + } Eigen::RowVectorXd c = V.colwise().mean(); V.rowwise() -= c; A = Eigen::MatrixXd::Constant(k.rows(), V.cols(), NAN); @@ -183,7 +186,7 @@ bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd& A, Eigen::VectorXd bool Polygon::thickenLine(const double thickness) { - if (vertices_.size() != 2) return false; + if (vertices_.size() != 2) {return false;} const Vector connection(vertices_[1] - vertices_[0]); const Vector orthogonal = thickness * Vector(connection.y(), -connection.x()).normalized(); std::vector newVertices; @@ -199,12 +202,12 @@ bool Polygon::thickenLine(const double thickness) bool Polygon::offsetInward(const double margin) { // Create a list of indices of the neighbours of each vertex. - // TODO: Assuming counter-clockwise ordered convex polygon. + // TODO(needs_assignment): Assuming counter-clockwise ordered convex polygon. std::vector neighbourIndices; const unsigned int n = nVertices(); neighbourIndices.resize(n); for (unsigned int i = 0; i < n; ++i) { - neighbourIndices[i] << (i > 0 ? (i-1)%n : n-1), (i + 1) % n; + neighbourIndices[i] << (i > 0 ? (i - 1) % n : n - 1), (i + 1) % n; } std::vector copy(vertices_); @@ -220,13 +223,15 @@ bool Polygon::offsetInward(const double margin) return true; } -std::vector Polygon::triangulate(const TriangulationMethods& method) const +std::vector Polygon::triangulate(const TriangulationMethods & method) const { - // TODO Add more triangulation methods. + // TODO(needs_assignment): Add more triangulation methods. // https://en.wikipedia.org/wiki/Polygon_triangulation + (void)(method); // method parameter unused, should be removed once used. std::vector polygons; - if (vertices_.size() < 3) + if (vertices_.size() < 3) { return polygons; + } size_t nPolygons = vertices_.size() - 2; polygons.reserve(nPolygons); @@ -245,8 +250,9 @@ std::vector Polygon::triangulate(const TriangulationMethods& method) co return polygons; } -Polygon Polygon::fromCircle(const Position center, const double radius, - const int nVertices) +Polygon Polygon::fromCircle( + const Position center, const double radius, + const int nVertices) { Eigen::Vector2d centerToVertex(radius, 0.0), centerToVertexTemp; @@ -260,11 +266,12 @@ Polygon Polygon::fromCircle(const Position center, const double radius, return polygon; } -Polygon Polygon::convexHullOfTwoCircles(const Position center1, - const Position center2, const double radius, - const int nVertices) +Polygon Polygon::convexHullOfTwoCircles( + const Position center1, + const Position center2, const double radius, + const int nVertices) { - if (center1 == center2) return fromCircle(center1, radius, nVertices); + if (center1 == center2) {return fromCircle(center1, radius, nVertices);} Eigen::Vector2d centerToVertex, centerToVertexTemp; centerToVertex = center2 - center1; centerToVertex.normalize(); @@ -286,7 +293,7 @@ Polygon Polygon::convexHullOfTwoCircles(const Position center1, return polygon; } -Polygon Polygon::convexHull(Polygon& polygon1, Polygon& polygon2) +Polygon Polygon::convexHull(Polygon & polygon1, Polygon & polygon2) { std::vector vertices; vertices.reserve(polygon1.nVertices() + polygon2.nVertices()); @@ -296,7 +303,7 @@ Polygon Polygon::convexHull(Polygon& polygon1, Polygon& polygon2) return monotoneChainConvexHullOfPoints(vertices); } -Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector& points) +Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector & points) { // Adapted from https://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain if (points.size() <= 3) { @@ -311,8 +318,12 @@ Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector& po int k = 0; // Build lower hull - for (int i = 0; i < sortedPoints.size(); ++i) { - while (k >= 2 && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) { + for (std::size_t i = 0; i < sortedPoints.size(); ++i) { + while (k >= 2 && + vectorsMakeClockwiseTurn( + pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), + sortedPoints.at(i))) + { k--; } pointsConvexHull.at(k++) = sortedPoints.at(i); @@ -320,7 +331,11 @@ Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector& po // Build upper hull. for (int i = sortedPoints.size() - 2, t = k + 1; i >= 0; i--) { - while (k >= t && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) { + while (k >= t && + vectorsMakeClockwiseTurn( + pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), + sortedPoints.at(i))) + { k--; } pointsConvexHull.at(k++) = sortedPoints.at(i); @@ -331,24 +346,27 @@ Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector& po return polygon; } -bool Polygon::sortVertices(const Eigen::Vector2d& vector1, - const Eigen::Vector2d& vector2) +bool Polygon::sortVertices( + const Eigen::Vector2d & vector1, + const Eigen::Vector2d & vector2) { - return (vector1.x() < vector2.x() - || (vector1.x() == vector2.x() && vector1.y() < vector2.y())); + return vector1.x() < vector2.x() || + (vector1.x() == vector2.x() && vector1.y() < vector2.y()); } -double Polygon::computeCrossProduct2D(const Eigen::Vector2d& vector1, - const Eigen::Vector2d& vector2) +double Polygon::computeCrossProduct2D( + const Eigen::Vector2d & vector1, + const Eigen::Vector2d & vector2) { - return (vector1.x() * vector2.y() - vector1.y() * vector2.x()); + return vector1.x() * vector2.y() - vector1.y() * vector2.x(); } -double Polygon::vectorsMakeClockwiseTurn(const Eigen::Vector2d &pointOrigin, - const Eigen::Vector2d &pointA, - const Eigen::Vector2d &pointB) +double Polygon::vectorsMakeClockwiseTurn( + const Eigen::Vector2d & pointOrigin, + const Eigen::Vector2d & pointA, + const Eigen::Vector2d & pointB) { return computeCrossProduct2D(pointA - pointOrigin, pointB - pointOrigin) <= 0; } -} /* namespace grid_map */ +} // namespace grid_map diff --git a/grid_map_core/src/SubmapGeometry.cpp b/grid_map_core/src/SubmapGeometry.cpp index df2a41eee..34ee37fce 100644 --- a/grid_map_core/src/SubmapGeometry.cpp +++ b/grid_map_core/src/SubmapGeometry.cpp @@ -9,43 +9,46 @@ #include #include -namespace grid_map { +namespace grid_map +{ -SubmapGeometry::SubmapGeometry(const GridMap& gridMap, const Position& position, - const Length& length, bool& isSuccess) - : gridMap_(gridMap) +SubmapGeometry::SubmapGeometry( + const GridMap & gridMap, const Position & position, + const Length & length, bool & isSuccess) +: gridMap_(gridMap) { - isSuccess = getSubmapInformation(startIndex_, size_, position_, length_, - requestedIndexInSubmap_, position, length, gridMap_.getLength(), - gridMap_.getPosition(), gridMap_.getResolution(), - gridMap_.getSize(), gridMap_.getStartIndex()); + isSuccess = getSubmapInformation( + startIndex_, size_, position_, length_, + requestedIndexInSubmap_, position, length, gridMap_.getLength(), + gridMap_.getPosition(), gridMap_.getResolution(), + gridMap_.getSize(), gridMap_.getStartIndex()); } SubmapGeometry::~SubmapGeometry() { } -const GridMap& SubmapGeometry::getGridMap() const +const GridMap & SubmapGeometry::getGridMap() const { return gridMap_; } -const Length& SubmapGeometry::getLength() const +const Length & SubmapGeometry::getLength() const { return length_; } -const Position& SubmapGeometry::getPosition() const +const Position & SubmapGeometry::getPosition() const { return position_; } -const Index& SubmapGeometry::getRequestedIndexInSubmap() const +const Index & SubmapGeometry::getRequestedIndexInSubmap() const { return requestedIndexInSubmap_; } -const Size& SubmapGeometry::getSize() const +const Size & SubmapGeometry::getSize() const { return size_; } @@ -55,9 +58,9 @@ double SubmapGeometry::getResolution() const return gridMap_.getResolution(); } -const Index& SubmapGeometry::getStartIndex() const +const Index & SubmapGeometry::getStartIndex() const { return startIndex_; } -} /* namespace grid_map */ +} // namespace grid_map diff --git a/grid_map_core/src/iterators/CircleIterator.cpp b/grid_map_core/src/iterators/CircleIterator.cpp index ecdc5fe15..acbcb6a69 100644 --- a/grid_map_core/src/iterators/CircleIterator.cpp +++ b/grid_map_core/src/iterators/CircleIterator.cpp @@ -6,16 +6,19 @@ * Institute: ETH Zurich, ANYbotics */ +#include + #include "grid_map_core/iterators/CircleIterator.hpp" #include "grid_map_core/GridMapMath.hpp" -using namespace std; - -namespace grid_map { +namespace grid_map +{ -CircleIterator::CircleIterator(const GridMap& gridMap, const Position& center, const double radius) - : center_(center), - radius_(radius) +CircleIterator::CircleIterator( + const GridMap & gridMap, const Position & center, + const double radius) +: center_(center), + radius_(radius) { radiusSquare_ = pow(radius_, 2); mapLength_ = gridMap.getLength(); @@ -26,11 +29,15 @@ CircleIterator::CircleIterator(const GridMap& gridMap, const Position& center, c Index submapStartIndex; Index submapBufferSize; findSubmapParameters(center, radius, submapStartIndex, submapBufferSize); - internalIterator_ = std::shared_ptr(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize)); - if(!isInside()) ++(*this); + internalIterator_ = + std::shared_ptr( + new SubmapIterator( + gridMap, submapStartIndex, + submapBufferSize)); + if (!isInside()) {++(*this);} } -CircleIterator& CircleIterator::operator =(const CircleIterator& other) +CircleIterator & CircleIterator::operator=(const CircleIterator & other) { center_ = other.center_; radius_ = other.radius_; @@ -44,23 +51,23 @@ CircleIterator& CircleIterator::operator =(const CircleIterator& other) return *this; } -bool CircleIterator::operator !=(const CircleIterator& other) const +bool CircleIterator::operator!=(const CircleIterator & other) const { - return (internalIterator_ != other.internalIterator_); + return internalIterator_ != other.internalIterator_; } -const Index& CircleIterator::operator *() const +const Index & CircleIterator::operator*() const { return *(*internalIterator_); } -CircleIterator& CircleIterator::operator ++() +CircleIterator & CircleIterator::operator++() { ++(*internalIterator_); - if (internalIterator_->isPastEnd()) return *this; + if (internalIterator_->isPastEnd()) {return *this;} for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) { - if (isInside()) break; + if (isInside()) {break;} } return *this; @@ -74,23 +81,29 @@ bool CircleIterator::isPastEnd() const bool CircleIterator::isInside() const { Position position; - getPositionFromIndex(position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); + getPositionFromIndex( + position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, + bufferSize_, bufferStartIndex_); double squareNorm = (position - center_).array().square().sum(); - return (squareNorm <= radiusSquare_); + return squareNorm <= radiusSquare_; } -void CircleIterator::findSubmapParameters(const Position& center, const double radius, - Index& startIndex, Size& bufferSize) const +void CircleIterator::findSubmapParameters( + const Position & center, const double radius, + Index & startIndex, Size & bufferSize) const { Position topLeft = center.array() + radius; Position bottomRight = center.array() - radius; boundPositionToRange(topLeft, mapLength_, mapPosition_); boundPositionToRange(bottomRight, mapLength_, mapPosition_); - getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); + getIndexFromPosition( + startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, + bufferStartIndex_); Index endIndex; - getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); + getIndexFromPosition( + endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, + bufferStartIndex_); bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_); } -} /* namespace grid_map */ - +} // namespace grid_map diff --git a/grid_map_core/src/iterators/EllipseIterator.cpp b/grid_map_core/src/iterators/EllipseIterator.cpp index 6463c509c..c191323ae 100644 --- a/grid_map_core/src/iterators/EllipseIterator.cpp +++ b/grid_map_core/src/iterators/EllipseIterator.cpp @@ -6,18 +6,21 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/iterators/EllipseIterator.hpp" -#include "grid_map_core/GridMapMath.hpp" - #include #include +#include + +#include "grid_map_core/iterators/EllipseIterator.hpp" +#include "grid_map_core/GridMapMath.hpp" -using namespace std; -namespace grid_map { +namespace grid_map +{ -EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation) - : center_(center) +EllipseIterator::EllipseIterator( + const GridMap & gridMap, const Position & center, + const Length & length, const double rotation) +: center_(center) { semiAxisSquare_ = (0.5 * length).square(); double sinRotation = sin(rotation); @@ -31,11 +34,15 @@ EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, Index submapStartIndex; Index submapBufferSize; findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize); - internalIterator_ = std::shared_ptr(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize)); - if(!isInside()) ++(*this); + internalIterator_ = + std::shared_ptr( + new SubmapIterator( + gridMap, submapStartIndex, + submapBufferSize)); + if (!isInside()) {++(*this);} } -EllipseIterator& EllipseIterator::operator =(const EllipseIterator& other) +EllipseIterator & EllipseIterator::operator=(const EllipseIterator & other) { center_ = other.center_; semiAxisSquare_ = other.semiAxisSquare_; @@ -49,23 +56,23 @@ EllipseIterator& EllipseIterator::operator =(const EllipseIterator& other) return *this; } -bool EllipseIterator::operator !=(const EllipseIterator& other) const +bool EllipseIterator::operator!=(const EllipseIterator & other) const { - return (internalIterator_ != other.internalIterator_); + return internalIterator_ != other.internalIterator_; } -const Eigen::Array2i& EllipseIterator::operator *() const +const Eigen::Array2i & EllipseIterator::operator*() const { return *(*internalIterator_); } -EllipseIterator& EllipseIterator::operator ++() +EllipseIterator & EllipseIterator::operator++() { ++(*internalIterator_); - if (internalIterator_->isPastEnd()) return *this; + if (internalIterator_->isPastEnd()) {return *this;} for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) { - if (isInside()) break; + if (isInside()) {break;} } return *this; @@ -76,7 +83,7 @@ bool EllipseIterator::isPastEnd() const return internalIterator_->isPastEnd(); } -const Size& EllipseIterator::getSubmapSize() const +const Size & EllipseIterator::getSubmapSize() const { return internalIterator_->getSubmapSize(); } @@ -84,13 +91,17 @@ const Size& EllipseIterator::getSubmapSize() const bool EllipseIterator::isInside() const { Position position; - getPositionFromIndex(position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); - double value = ((transformMatrix_ * (position - center_)).array().square() / semiAxisSquare_).sum(); - return (value <= 1); + getPositionFromIndex( + position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, + bufferSize_, bufferStartIndex_); + double value = + ((transformMatrix_ * (position - center_)).array().square() / semiAxisSquare_).sum(); + return value <= 1; } -void EllipseIterator::findSubmapParameters(const Position& center, const Length& length, const double rotation, - Index& startIndex, Size& bufferSize) const +void EllipseIterator::findSubmapParameters( + const Position & center, const Length & length, const double rotation, + Index & startIndex, Size & bufferSize) const { const Eigen::Rotation2Dd rotationMatrix(rotation); Eigen::Vector2d u = rotationMatrix * Eigen::Vector2d(length(0), 0.0); @@ -100,11 +111,14 @@ void EllipseIterator::findSubmapParameters(const Position& center, const Length& Position bottomRight = center.array() - boundingBoxHalfLength; boundPositionToRange(topLeft, mapLength_, mapPosition_); boundPositionToRange(bottomRight, mapLength_, mapPosition_); - getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); + getIndexFromPosition( + startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, + bufferStartIndex_); Index endIndex; - getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); + getIndexFromPosition( + endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, + bufferStartIndex_); bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_); } -} /* namespace grid_map */ - +} // namespace grid_map diff --git a/grid_map_core/src/iterators/GridMapIterator.cpp b/grid_map_core/src/iterators/GridMapIterator.cpp index 68ae57b73..5f499896b 100644 --- a/grid_map_core/src/iterators/GridMapIterator.cpp +++ b/grid_map_core/src/iterators/GridMapIterator.cpp @@ -9,9 +9,10 @@ #include "grid_map_core/iterators/GridMapIterator.hpp" #include "grid_map_core/GridMapMath.hpp" -namespace grid_map { +namespace grid_map +{ -GridMapIterator::GridMapIterator(const grid_map::GridMap& gridMap) +GridMapIterator::GridMapIterator(const grid_map::GridMap & gridMap) { size_ = gridMap.getSize(); startIndex_ = gridMap.getStartIndex(); @@ -20,7 +21,7 @@ GridMapIterator::GridMapIterator(const grid_map::GridMap& gridMap) isPastEnd_ = false; } -GridMapIterator::GridMapIterator(const GridMapIterator* other) +GridMapIterator::GridMapIterator(const GridMapIterator * other) { size_ = other->size_; startIndex_ = other->startIndex_; @@ -29,7 +30,7 @@ GridMapIterator::GridMapIterator(const GridMapIterator* other) isPastEnd_ = other->isPastEnd_; } -GridMapIterator& GridMapIterator::operator =(const GridMapIterator& other) +GridMapIterator & GridMapIterator::operator=(const GridMapIterator & other) { size_ = other.size_; startIndex_ = other.startIndex_; @@ -39,17 +40,17 @@ GridMapIterator& GridMapIterator::operator =(const GridMapIterator& other) return *this; } -bool GridMapIterator::operator !=(const GridMapIterator& other) const +bool GridMapIterator::operator!=(const GridMapIterator & other) const { return linearIndex_ != other.linearIndex_; } -const Index GridMapIterator::operator *() const +const Index GridMapIterator::operator*() const { return getIndexFromLinearIndex(linearIndex_, size_); } -const size_t& GridMapIterator::getLinearIndex() const +const size_t & GridMapIterator::getLinearIndex() const { return linearIndex_; } @@ -59,7 +60,7 @@ const Index GridMapIterator::getUnwrappedIndex() const return getIndexFromBufferIndex(*(*this), size_, startIndex_); } -GridMapIterator& GridMapIterator::operator ++() +GridMapIterator & GridMapIterator::operator++() { size_t newIndex = linearIndex_ + 1; if (newIndex < linearSize_) { @@ -74,7 +75,7 @@ GridMapIterator GridMapIterator::end() const { GridMapIterator res(this); res.linearIndex_ = linearSize_ - 1; - return res; + return GridMapIterator(&res); } bool GridMapIterator::isPastEnd() const @@ -82,4 +83,4 @@ bool GridMapIterator::isPastEnd() const return isPastEnd_; } -} /* namespace grid_map */ +} // namespace grid_map diff --git a/grid_map_core/src/iterators/LineIterator.cpp b/grid_map_core/src/iterators/LineIterator.cpp index 0afc4eb97..66e82e578 100644 --- a/grid_map_core/src/iterators/LineIterator.cpp +++ b/grid_map_core/src/iterators/LineIterator.cpp @@ -6,32 +6,36 @@ * Institute: ETH Zurich, ANYbotics */ +#include + #include "grid_map_core/iterators/LineIterator.hpp" #include "grid_map_core/GridMapMath.hpp" -using namespace std; - -namespace grid_map { +namespace grid_map +{ -LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Position& start, - const Position& end) +LineIterator::LineIterator( + const grid_map::GridMap & gridMap, const Position & start, + const Position & end) { Index startIndex, endIndex; - if (getIndexLimitedToMapRange(gridMap, start, end, startIndex) - && getIndexLimitedToMapRange(gridMap, end, start, endIndex)) { + if (getIndexLimitedToMapRange(gridMap, start, end, startIndex) && + getIndexLimitedToMapRange(gridMap, end, start, endIndex)) + { initialize(gridMap, startIndex, endIndex); - } - else { + } else { throw std::invalid_argument("Failed to construct LineIterator."); } } -LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Index& start, const Index& end) +LineIterator::LineIterator( + const grid_map::GridMap & gridMap, const Index & start, + const Index & end) { initialize(gridMap, start, end); } -LineIterator& LineIterator::operator =(const LineIterator& other) +LineIterator & LineIterator::operator=(const LineIterator & other) { index_ = other.index_; start_ = other.start_; @@ -51,25 +55,27 @@ LineIterator& LineIterator::operator =(const LineIterator& other) return *this; } -bool LineIterator::operator !=(const LineIterator& other) const +bool LineIterator::operator!=(const LineIterator & other) const { return (index_ != other.index_).any(); } -const Index& LineIterator::operator *() const +const Index & LineIterator::operator*() const { return index_; } -LineIterator& LineIterator::operator ++() +LineIterator & LineIterator::operator++() { numerator_ += numeratorAdd_; // Increase the numerator by the top of the fraction. if (numerator_ >= denominator_) { numerator_ -= denominator_; - const Index unwrappedIndex = getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment1_; + const Index unwrappedIndex = + getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment1_; index_ = getBufferIndexFromIndex(unwrappedIndex, bufferSize_, bufferStartIndex_); } - const Index unwrappedIndex = getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment2_; + const Index unwrappedIndex = + getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment2_; index_ = getBufferIndexFromIndex(unwrappedIndex, bufferSize_, bufferStartIndex_); ++iCell_; return *this; @@ -80,29 +86,35 @@ bool LineIterator::isPastEnd() const return iCell_ >= nCells_; } -bool LineIterator::initialize(const grid_map::GridMap& gridMap, const Index& start, const Index& end) +bool LineIterator::initialize( + const grid_map::GridMap & gridMap, const Index & start, + const Index & end) { - start_ = start; - end_ = end; - mapLength_ = gridMap.getLength(); - mapPosition_ = gridMap.getPosition(); - resolution_ = gridMap.getResolution(); - bufferSize_ = gridMap.getSize(); - bufferStartIndex_ = gridMap.getStartIndex(); - initializeIterationParameters(); - return true; + start_ = start; + end_ = end; + mapLength_ = gridMap.getLength(); + mapPosition_ = gridMap.getPosition(); + resolution_ = gridMap.getResolution(); + bufferSize_ = gridMap.getSize(); + bufferStartIndex_ = gridMap.getStartIndex(); + initializeIterationParameters(); + return true; } -bool LineIterator::getIndexLimitedToMapRange(const grid_map::GridMap& gridMap, - const Position& start, const Position& end, - Index& index) +bool LineIterator::getIndexLimitedToMapRange( + const grid_map::GridMap & gridMap, + const Position & start, const Position & end, + Index & index) { Position newStart = start; Vector direction = (end - start).normalized(); while (!gridMap.getIndex(newStart, index)) { newStart += (gridMap.getResolution() - std::numeric_limits::epsilon()) * direction; - if ((end - newStart).norm() < gridMap.getResolution() - std::numeric_limits::epsilon()) + if ((end - newStart).norm() < + gridMap.getResolution() - std::numeric_limits::epsilon()) + { return false; + } } return true; } @@ -138,21 +150,21 @@ void LineIterator::initializeIterationParameters() if (delta.x() >= delta.y()) { // There is at least one x-value for every y-value. - increment1_.x() = 0; // Do not change the x when numerator >= denominator. - increment2_.y() = 0; // Do not change the y for every iteration. + increment1_.x() = 0; // Do not change the x when numerator >= denominator. + increment2_.y() = 0; // Do not change the y for every iteration. denominator_ = delta.x(); numerator_ = delta.x() / 2; numeratorAdd_ = delta.y(); - nCells_ = delta.x() + 1; // There are more x-values than y-values. + nCells_ = delta.x() + 1; // There are more x-values than y-values. } else { // There is at least one y-value for every x-value - increment2_.x() = 0; // Do not change the x for every iteration. - increment1_.y() = 0; // Do not change the y when numerator >= denominator. + increment2_.x() = 0; // Do not change the x for every iteration. + increment1_.y() = 0; // Do not change the y when numerator >= denominator. denominator_ = delta.y(); numerator_ = delta.y() / 2; numeratorAdd_ = delta.x(); - nCells_ = delta.y() + 1; // There are more y-values than x-values. + nCells_ = delta.y() + 1; // There are more y-values than x-values. } } -} /* namespace grid_map */ +} // namespace grid_map diff --git a/grid_map_core/src/iterators/PolygonIterator.cpp b/grid_map_core/src/iterators/PolygonIterator.cpp index e20849ff9..036562ca9 100644 --- a/grid_map_core/src/iterators/PolygonIterator.cpp +++ b/grid_map_core/src/iterators/PolygonIterator.cpp @@ -6,15 +6,18 @@ * Institute: ETH Zurich, ANYbotics */ +#include + #include "grid_map_core/iterators/PolygonIterator.hpp" #include "grid_map_core/GridMapMath.hpp" -using namespace std; - -namespace grid_map { +namespace grid_map +{ -PolygonIterator::PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon) - : polygon_(polygon) +PolygonIterator::PolygonIterator( + const grid_map::GridMap & gridMap, + const grid_map::Polygon & polygon) +: polygon_(polygon) { mapLength_ = gridMap.getLength(); mapPosition_ = gridMap.getPosition(); @@ -24,11 +27,15 @@ PolygonIterator::PolygonIterator(const grid_map::GridMap& gridMap, const grid_ma Index submapStartIndex; Size submapBufferSize; findSubmapParameters(polygon, submapStartIndex, submapBufferSize); - internalIterator_ = std::shared_ptr(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize)); - if(!isInside()) ++(*this); + internalIterator_ = + std::shared_ptr( + new SubmapIterator( + gridMap, submapStartIndex, + submapBufferSize)); + if (!isInside()) {++(*this);} } -PolygonIterator& PolygonIterator::operator =(const PolygonIterator& other) +PolygonIterator & PolygonIterator::operator=(const PolygonIterator & other) { polygon_ = other.polygon_; internalIterator_ = other.internalIterator_; @@ -40,23 +47,23 @@ PolygonIterator& PolygonIterator::operator =(const PolygonIterator& other) return *this; } -bool PolygonIterator::operator !=(const PolygonIterator& other) const +bool PolygonIterator::operator!=(const PolygonIterator & other) const { - return (internalIterator_ != other.internalIterator_); + return internalIterator_ != other.internalIterator_; } -const Index& PolygonIterator::operator *() const +const Index & PolygonIterator::operator*() const { return *(*internalIterator_); } -PolygonIterator& PolygonIterator::operator ++() +PolygonIterator & PolygonIterator::operator++() { ++(*internalIterator_); - if (internalIterator_->isPastEnd()) return *this; + if (internalIterator_->isPastEnd()) {return *this;} for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) { - if (isInside()) break; + if (isInside()) {break;} } return *this; @@ -70,25 +77,33 @@ bool PolygonIterator::isPastEnd() const bool PolygonIterator::isInside() const { Position position; - getPositionFromIndex(position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); + getPositionFromIndex( + position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, + bufferSize_, bufferStartIndex_); return polygon_.isInside(position); } -void PolygonIterator::findSubmapParameters(const grid_map::Polygon& polygon, Index& startIndex, Size& bufferSize) const +void PolygonIterator::findSubmapParameters( + const grid_map::Polygon & polygon, Index & startIndex, + Size & bufferSize) const { + (void)(polygon); // polygon parameter unused, should be removed when used. Position topLeft = polygon_.getVertices()[0]; Position bottomRight = topLeft; - for (const auto& vertex : polygon_.getVertices()) { + for (const auto & vertex : polygon_.getVertices()) { topLeft = topLeft.array().max(vertex.array()); bottomRight = bottomRight.array().min(vertex.array()); } boundPositionToRange(topLeft, mapLength_, mapPosition_); boundPositionToRange(bottomRight, mapLength_, mapPosition_); - getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); + getIndexFromPosition( + startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, + bufferStartIndex_); Index endIndex; - getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_); + getIndexFromPosition( + endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, + bufferStartIndex_); bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_); } -} /* namespace grid_map */ - +} // namespace grid_map diff --git a/grid_map_core/src/iterators/SlidingWindowIterator.cpp b/grid_map_core/src/iterators/SlidingWindowIterator.cpp index 6ade5a4b3..e72352deb 100644 --- a/grid_map_core/src/iterators/SlidingWindowIterator.cpp +++ b/grid_map_core/src/iterators/SlidingWindowIterator.cpp @@ -6,45 +6,48 @@ * Institute: ETH Zurich, ANYbotics */ +#include +#include + #include "grid_map_core/iterators/SlidingWindowIterator.hpp" #include "grid_map_core/GridMapMath.hpp" -#include - -namespace grid_map { +namespace grid_map +{ -SlidingWindowIterator::SlidingWindowIterator(const GridMap& gridMap, const std::string& layer, - const EdgeHandling& edgeHandling, const size_t windowSize) - : GridMapIterator(gridMap), - edgeHandling_(edgeHandling), - data_(gridMap[layer]) +SlidingWindowIterator::SlidingWindowIterator( + const GridMap & gridMap, const std::string & layer, + const EdgeHandling & edgeHandling, const size_t windowSize) +: GridMapIterator(gridMap), + edgeHandling_(edgeHandling), + data_(gridMap[layer]) { windowSize_ = windowSize; setup(gridMap); } -SlidingWindowIterator::SlidingWindowIterator(const SlidingWindowIterator* other) - : GridMapIterator(other), - edgeHandling_(other->edgeHandling_), - data_(other->data_) +SlidingWindowIterator::SlidingWindowIterator(const SlidingWindowIterator * other) +: GridMapIterator(other), + edgeHandling_(other->edgeHandling_), + data_(other->data_) { windowSize_ = other->windowSize_; windowMargin_ = other->windowMargin_; } -void SlidingWindowIterator::setWindowLength(const GridMap& gridMap, const double windowLength) +void SlidingWindowIterator::setWindowLength(const GridMap & gridMap, const double windowLength) { windowSize_ = std::round(windowLength / gridMap.getResolution()); - if (windowSize_ % 2 != 1) ++windowSize_; + if (windowSize_ % 2 != 1) {++windowSize_;} setup(gridMap); } -SlidingWindowIterator& SlidingWindowIterator::operator ++() +SlidingWindowIterator & SlidingWindowIterator::operator++() { if (edgeHandling_ == EdgeHandling::INSIDE) { while (!isPastEnd()) { GridMapIterator::operator++(); - if (dataInsideMap()) break; + if (dataInsideMap()) {break;} } } else { GridMapIterator::operator++(); @@ -66,27 +69,41 @@ const Matrix SlidingWindowIterator::getData() const switch (edgeHandling_) { case EdgeHandling::INSIDE: case EdgeHandling::CROP: - return data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1)); + return data_.block( + topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize( + 1)); case EdgeHandling::EMPTY: case EdgeHandling::MEAN: - const Matrix data = data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1)); + const Matrix data = data_.block( + topLeftIndex(0), topLeftIndex(1), adjustedWindowSize( + 0), adjustedWindowSize(1)); Matrix returnData(windowSize_, windowSize_); - if (edgeHandling_ == EdgeHandling::EMPTY) returnData.setConstant(NAN); - else if (edgeHandling_ == EdgeHandling::MEAN) returnData.setConstant(data.meanOfFinites()); + if (edgeHandling_ == EdgeHandling::EMPTY) { + returnData.setConstant(NAN); + } else if (edgeHandling_ == EdgeHandling::MEAN) { + returnData.setConstant(data.meanOfFinites()); + } const Index topLeftIndexShift(topLeftIndex - originalTopLeftIndex); - returnData.block(topLeftIndexShift(0), topLeftIndexShift(1), adjustedWindowSize(0), adjustedWindowSize(1)) = - data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1)); + returnData.block( + topLeftIndexShift(0), topLeftIndexShift(1), adjustedWindowSize( + 0), adjustedWindowSize(1)) = + data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1)); return returnData; } return Matrix::Zero(0, 0); } -void SlidingWindowIterator::setup(const GridMap& gridMap) +void SlidingWindowIterator::setup(const GridMap & gridMap) { - if (!gridMap.isDefaultStartIndex()) throw std::runtime_error( - "SlidingWindowIterator cannot be used with grid maps that don't have a default buffer start index."); - if (windowSize_ % 2 == 0) throw std::runtime_error( - "SlidingWindowIterator has a wrong window size!"); + if (!gridMap.isDefaultStartIndex()) { + throw std::runtime_error( + "SlidingWindowIterator cannot be used with grid maps" + "that don't have a default buffer start index."); + } + if (windowSize_ % 2 == 0) { + throw std::runtime_error( + "SlidingWindowIterator has a wrong window size!"); + } windowMargin_ = (windowSize_ - 1) / 2; if (edgeHandling_ == EdgeHandling::INSIDE) { @@ -105,4 +122,4 @@ bool SlidingWindowIterator::dataInsideMap() const return checkIfIndexInRange(topLeftIndex, size_) && checkIfIndexInRange(bottomRightIndex, size_); } -} /* namespace grid_map */ +} // namespace grid_map diff --git a/grid_map_core/src/iterators/SpiralIterator.cpp b/grid_map_core/src/iterators/SpiralIterator.cpp index 2f8899e6e..168fb8564 100644 --- a/grid_map_core/src/iterators/SpiralIterator.cpp +++ b/grid_map_core/src/iterators/SpiralIterator.cpp @@ -6,20 +6,20 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/iterators/SpiralIterator.hpp" -#include "grid_map_core/GridMapMath.hpp" - #include -using namespace std; +#include "grid_map_core/iterators/SpiralIterator.hpp" +#include "grid_map_core/GridMapMath.hpp" -namespace grid_map { +namespace grid_map +{ -SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center, - const double radius) - : center_(center), - radius_(radius), - distance_(0) +SpiralIterator::SpiralIterator( + const grid_map::GridMap & gridMap, const Eigen::Vector2d & center, + const double radius) +: center_(center), + radius_(radius), + distance_(0) { radiusSquare_ = radius_ * radius_; mapLength_ = gridMap.getLength(); @@ -28,14 +28,16 @@ SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Ve bufferSize_ = gridMap.getSize(); gridMap.getIndex(center_, indexCenter_); nRings_ = std::ceil(radius_ / resolution_); - if (checkIfIndexInRange(indexCenter_, bufferSize_)) + if (checkIfIndexInRange(indexCenter_, bufferSize_)) { pointsRing_.push_back(indexCenter_); - else - while(pointsRing_.empty() && !isPastEnd()) + } else { + while (pointsRing_.empty() && !isPastEnd()) { generateRing(); + } + } } -SpiralIterator& SpiralIterator::operator =(const SpiralIterator& other) +SpiralIterator & SpiralIterator::operator=(const SpiralIterator & other) { center_ = other.center_; indexCenter_ = other.indexCenter_; @@ -51,26 +53,30 @@ SpiralIterator& SpiralIterator::operator =(const SpiralIterator& other) return *this; } -bool SpiralIterator::operator !=(const SpiralIterator& other) const +bool SpiralIterator::operator!=(const SpiralIterator & other) const { + (void)(other); // other parameter unused, should be removed when used. return (pointsRing_.back() != pointsRing_.back()).any(); } -const Eigen::Array2i& SpiralIterator::operator *() const +const Eigen::Array2i & SpiralIterator::operator*() const { return pointsRing_.back(); } -SpiralIterator& SpiralIterator::operator ++() +SpiralIterator & SpiralIterator::operator++() { + if (isPastEnd()) { + return *this; + } pointsRing_.pop_back(); - if (pointsRing_.empty() && !isPastEnd()) generateRing(); + if (pointsRing_.empty() && !isPastEnd()) {generateRing();} return *this; } bool SpiralIterator::isPastEnd() const { - return (distance_ == nRings_ && pointsRing_.empty()); + return distance_ == nRings_ && pointsRing_.empty(); } bool SpiralIterator::isInside(const Index index) const @@ -78,7 +84,7 @@ bool SpiralIterator::isInside(const Index index) const Eigen::Vector2d position; getPositionFromIndex(position, index, mapLength_, mapPosition_, resolution_, bufferSize_); double squareNorm = (position - center_).array().square().sum(); - return (squareNorm <= radiusSquare_); + return squareNorm <= radiusSquare_; } void SpiralIterator::generateRing() @@ -92,25 +98,28 @@ void SpiralIterator::generateRing() pointInMap.y() = point.y() + indexCenter_.y(); if (checkIfIndexInRange(pointInMap, bufferSize_)) { if (distance_ == nRings_ || distance_ == nRings_ - 1) { - if (isInside(pointInMap)) + if (isInside(pointInMap)) { pointsRing_.push_back(pointInMap); + } } else { pointsRing_.push_back(pointInMap); } } normal.x() = -signum(point.y()); normal.y() = signum(point.x()); - if (normal.x() != 0 - && (int) Vector(point.x() + normal.x(), point.y()).norm() == distance_) + if (normal.x() != 0 && + static_cast(Vector(point.x() + normal.x(), point.y()).norm()) == distance_) + { point.x() += normal.x(); - else if (normal.y() != 0 - && (int) Vector(point.x(), point.y() + normal.y()).norm() == distance_) + } else if (normal.y() != 0 && // NOLINT + static_cast(Vector(point.x(), point.y() + normal.y()).norm()) == distance_) + { point.y() += normal.y(); - else { + } else { point.x() += normal.x(); point.y() += normal.y(); } - } while (point.x() != distance_ || point.y() != 0); + } while ((unsigned)point.x() != distance_ || point.y() != 0); } double SpiralIterator::getCurrentRadius() const @@ -119,5 +128,4 @@ double SpiralIterator::getCurrentRadius() const return radius.matrix().norm() * resolution_; } -} /* namespace grid_map */ - +} // namespace grid_map diff --git a/grid_map_core/src/iterators/SubmapIterator.cpp b/grid_map_core/src/iterators/SubmapIterator.cpp index bf1232547..c46350dd2 100644 --- a/grid_map_core/src/iterators/SubmapIterator.cpp +++ b/grid_map_core/src/iterators/SubmapIterator.cpp @@ -9,24 +9,25 @@ #include "grid_map_core/iterators/SubmapIterator.hpp" #include "grid_map_core/GridMapMath.hpp" -using namespace std; - -namespace grid_map { +namespace grid_map +{ -SubmapIterator::SubmapIterator(const grid_map::SubmapGeometry& submap) - : SubmapIterator(submap.getGridMap(), submap.getStartIndex(), submap.getSize()) +SubmapIterator::SubmapIterator(const grid_map::SubmapGeometry & submap) +: SubmapIterator(submap.getGridMap(), submap.getStartIndex(), submap.getSize()) { } -SubmapIterator::SubmapIterator(const grid_map::GridMap& gridMap, - const grid_map::BufferRegion& bufferRegion) - : SubmapIterator(gridMap, bufferRegion.getStartIndex(), bufferRegion.getSize()) +SubmapIterator::SubmapIterator( + const grid_map::GridMap & gridMap, + const grid_map::BufferRegion & bufferRegion) +: SubmapIterator(gridMap, bufferRegion.getStartIndex(), bufferRegion.getSize()) { } -SubmapIterator::SubmapIterator(const grid_map::GridMap& gridMap, const Index& submapStartIndex, - const Size& submapSize) +SubmapIterator::SubmapIterator( + const grid_map::GridMap & gridMap, const Index & submapStartIndex, + const Size & submapSize) { size_ = gridMap.getSize(); startIndex_ = gridMap.getStartIndex(); @@ -37,7 +38,7 @@ SubmapIterator::SubmapIterator(const grid_map::GridMap& gridMap, const Index& su isPastEnd_ = false; } -SubmapIterator::SubmapIterator(const SubmapIterator* other) +SubmapIterator::SubmapIterator(const SubmapIterator * other) { size_ = other->size_; startIndex_ = other->startIndex_; @@ -48,7 +49,7 @@ SubmapIterator::SubmapIterator(const SubmapIterator* other) isPastEnd_ = other->isPastEnd_; } -SubmapIterator& SubmapIterator::operator =(const SubmapIterator& other) +SubmapIterator & SubmapIterator::operator=(const SubmapIterator & other) { size_ = other.size_; startIndex_ = other.startIndex_; @@ -60,25 +61,26 @@ SubmapIterator& SubmapIterator::operator =(const SubmapIterator& other) return *this; } -bool SubmapIterator::operator !=(const SubmapIterator& other) const +bool SubmapIterator::operator!=(const SubmapIterator & other) const { return (index_ != other.index_).any(); } -const Index& SubmapIterator::operator *() const +const Index & SubmapIterator::operator*() const { return index_; } -const Index& SubmapIterator::getSubmapIndex() const +const Index & SubmapIterator::getSubmapIndex() const { return submapIndex_; } -SubmapIterator& SubmapIterator::operator ++() +SubmapIterator & SubmapIterator::operator++() { - isPastEnd_ = !incrementIndexForSubmap(submapIndex_, index_, submapStartIndex_, - submapSize_, size_, startIndex_); + isPastEnd_ = !incrementIndexForSubmap( + submapIndex_, index_, submapStartIndex_, + submapSize_, size_, startIndex_); return *this; } @@ -87,10 +89,9 @@ bool SubmapIterator::isPastEnd() const return isPastEnd_; } -const Size& SubmapIterator::getSubmapSize() const +const Size & SubmapIterator::getSubmapSize() const { return submapSize_; } -} /* namespace grid_map */ - +} // namespace grid_map diff --git a/grid_map_core/test/CubicConvolutionInterpolationTest.cpp b/grid_map_core/test/CubicConvolutionInterpolationTest.cpp index 4e884f2e5..45646ea06 100644 --- a/grid_map_core/test/CubicConvolutionInterpolationTest.cpp +++ b/grid_map_core/test/CubicConvolutionInterpolationTest.cpp @@ -6,125 +6,140 @@ * Institute: ETH Zurich, Robotic Systems Lab */ +// gtest +#include + #include "test_helpers.hpp" #include "grid_map_core/GridMap.hpp" -// gtest -#include - namespace gm = grid_map; namespace gmt = grid_map_test; TEST(CubicConvolutionInterpolation, FlatWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(2.0, 2.0), 0.1, gm::Position(0.0, 0.0)); auto trueValues = gmt::createFlatWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 100); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); if (::testing::Test::HasFailure()) { - std::cout << "\n Test CubicConvolutionInterpolation, FlatWorld failed with seed: " << seed - << std::endl; + std::cout << "\n Test CubicConvolutionInterpolation, FlatWorld failed with seed: " << seed << + std::endl; } } TEST(CubicConvolutionInterpolation, RationalFunctionWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.01, gm::Position(0.0, 0.0)); auto trueValues = gmt::createRationalFunctionWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); if (::testing::Test::HasFailure()) { - std::cout << "\n Test CubicConvolutionInterpolation, RationalFunctionWorld failed with seed: " - << seed << std::endl; + std::cout << + "\n Test CubicConvolutionInterpolation, RationalFunctionWorld failed with seed: " << + seed << std::endl; } } TEST(CubicConvolutionInterpolation, SaddleWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.1, gm::Position(0.0, 0.0)); auto trueValues = gmt::createSaddleWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); if (::testing::Test::HasFailure()) { - std::cout << "\n Test CubicConvolutionInterpolation, SaddleWorld failed with seed: " << seed - << std::endl; + std::cout << "\n Test CubicConvolutionInterpolation, SaddleWorld failed with seed: " << seed << + std::endl; } } TEST(CubicConvolutionInterpolation, SecondOrderPolyWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.1, gm::Position(0.0, 0.0)); auto trueValues = gmt::createSecondOrderPolyWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); if (::testing::Test::HasFailure()) { - std::cout << "\n Test CubicConvolutionInterpolation, SecondOrderPolyWorld failed with seed: " - << seed << std::endl; + std::cout << "\n Test CubicConvolutionInterpolation, SecondOrderPolyWorld failed with seed: " << + seed << std::endl; } } TEST(CubicConvolutionInterpolation, SineWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.01, gm::Position(0.0, 0.0)); auto trueValues = gmt::createSineWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); if (::testing::Test::HasFailure()) { - std::cout << "\n Test CubicConvolutionInterpolation, SineWorld failed with seed: " << seed - << std::endl; + std::cout << "\n Test CubicConvolutionInterpolation, SineWorld failed with seed: " << seed << + std::endl; } } TEST(CubicConvolutionInterpolation, TanhWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(2.5, 2.5), 0.02, gm::Position(0.0, 0.0)); auto trueValues = gmt::createTanhWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); if (::testing::Test::HasFailure()) { - std::cout << "\n Test CubicConvolutionInterpolation, TanhWorld failed with seed: " << seed - << std::endl; + std::cout << "\n Test CubicConvolutionInterpolation, TanhWorld failed with seed: " << seed << + std::endl; } } TEST(CubicConvolutionInterpolation, GaussianWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.02, gm::Position(0.0, 0.0)); auto trueValues = gmt::createGaussianWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC_CONVOLUTION); if (::testing::Test::HasFailure()) { - std::cout << "\n Test CubicConvolutionInterpolation, GaussianWorld failed with seed: " << seed - << std::endl; + std::cout << "\n Test CubicConvolutionInterpolation, GaussianWorld failed with seed: " << + seed << + std::endl; } } - diff --git a/grid_map_core/test/CubicInterpolationTest.cpp b/grid_map_core/test/CubicInterpolationTest.cpp index eb4a9cfa4..fb248a95d 100644 --- a/grid_map_core/test/CubicInterpolationTest.cpp +++ b/grid_map_core/test/CubicInterpolationTest.cpp @@ -6,25 +6,27 @@ * Institute: ETH Zurich, Robotic Systems Lab */ +// gtest +#include + #include "test_helpers.hpp" #include "grid_map_core/GridMap.hpp" -// gtest -#include - namespace gm = grid_map; namespace gmt = grid_map_test; TEST(CubicInterpolation, FlatWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(2.0, 2.0), 0.1, gm::Position(0.0, 0.0)); auto trueValues = gmt::createFlatWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 100); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC); if (::testing::Test::HasFailure()) { std::cout << "\n Test CubicInterpolation, FlatWorld failed with seed: " << seed << std::endl; @@ -33,29 +35,33 @@ TEST(CubicInterpolation, FlatWorld) TEST(CubicInterpolation, RationalFunctionWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.01, gm::Position(0.0, 0.0)); auto trueValues = gmt::createRationalFunctionWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC); if (::testing::Test::HasFailure()) { - std::cout << "\n Test CubicInterpolation, RationalFunctionWorld failed with seed: " << seed - << std::endl; + std::cout << "\n Test CubicInterpolation, RationalFunctionWorld failed with seed: " << seed << + std::endl; } } TEST(CubicInterpolation, SaddleWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.1, gm::Position(0.0, 0.0)); auto trueValues = gmt::createSaddleWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC); if (::testing::Test::HasFailure()) { std::cout << "\n Test CubicInterpolation, SaddleWorld failed with seed: " << seed << std::endl; @@ -64,29 +70,33 @@ TEST(CubicInterpolation, SaddleWorld) TEST(CubicInterpolation, SecondOrderPolyWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.1, gm::Position(0.0, 0.0)); auto trueValues = gmt::createSecondOrderPolyWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC); if (::testing::Test::HasFailure()) { - std::cout << "\n Test CubicInterpolation, SecondOrderPolyWorld failed with seed: " << seed - << std::endl; + std::cout << "\n Test CubicInterpolation, SecondOrderPolyWorld failed with seed: " << seed << + std::endl; } } TEST(CubicInterpolation, SineWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.01, gm::Position(0.0, 0.0)); auto trueValues = gmt::createSineWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC); if (::testing::Test::HasFailure()) { std::cout << "\n Test CubicInterpolation, SineWorld failed with seed: " << seed << std::endl; @@ -95,13 +105,15 @@ TEST(CubicInterpolation, SineWorld) TEST(CubicInterpolation, TanhWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(2.5, 2.5), 0.02, gm::Position(0.0, 0.0)); auto trueValues = gmt::createTanhWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC); if (::testing::Test::HasFailure()) { std::cout << "\n Test CubicInterpolation, TanhWorld failed with seed: " << seed << std::endl; @@ -110,17 +122,18 @@ TEST(CubicInterpolation, TanhWorld) TEST(CubicInterpolation, GaussianWorld) { - const int seed = rand(); + const int seed = std::rand(); gmt::rndGenerator.seed(seed); auto map = gmt::createMap(gm::Length(3.0, 3.0), 0.02, gm::Position(0.0, 0.0)); auto trueValues = gmt::createGaussianWorld(&map); const auto queryPoints = gmt::uniformlyDitributedPointsWithinMap(map, 1000); - verifyValuesAtQueryPointsAreClose(map, trueValues, queryPoints, gm::InterpolationMethods::INTER_CUBIC); + verifyValuesAtQueryPointsAreClose( + map, trueValues, queryPoints, + gm::InterpolationMethods::INTER_CUBIC); if (::testing::Test::HasFailure()) { - std::cout << "\n Test CubicInterpolation, GaussianWorld failed with seed: " << seed - << std::endl; + std::cout << "\n Test CubicInterpolation, GaussianWorld failed with seed: " << seed << + std::endl; } } - diff --git a/grid_map_core/test/EigenPluginsTest.cpp b/grid_map_core/test/EigenPluginsTest.cpp index 0ec5cf652..e168c5910 100644 --- a/grid_map_core/test/EigenPluginsTest.cpp +++ b/grid_map_core/test/EigenPluginsTest.cpp @@ -6,16 +6,21 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/grid_map_core.hpp" - // gtest #include // Eigen #include -using namespace std; -using namespace Eigen; +#include "grid_map_core/grid_map_core.hpp" + +// GCC 13 has false positive warnings around array-bounds. +// Suppress them until this is fixed in upstream gcc. See +// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=114758 for more details. +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#endif TEST(EigenMatrixBaseAddons, numberOfFinites) { @@ -24,11 +29,11 @@ TEST(EigenMatrixBaseAddons, numberOfFinites) matrix(1, 0) = NAN; EXPECT_EQ(7, matrix.numberOfFinites()); - Matrix matrix2; + Eigen::Matrix matrix2; matrix2.setOnes(); EXPECT_EQ(matrix2.rows() * matrix2.cols(), matrix2.numberOfFinites()); - Matrix matrix3; + Eigen::Matrix matrix3; matrix3.setConstant(NAN); matrix3.col(3).setConstant(0.0); EXPECT_EQ(matrix3.rows(), matrix3.numberOfFinites()); @@ -36,7 +41,7 @@ TEST(EigenMatrixBaseAddons, numberOfFinites) TEST(EigenMatrixBaseAddons, sumOfFinites) { - Matrix matrix; + Eigen::Matrix matrix; matrix.setRandom(); EXPECT_NEAR(matrix.sum(), matrix.sumOfFinites(), 1e-10); double finiteSum = matrix.sum() - matrix(0, 0) - matrix(1, 2) - matrix(3, 6) - matrix(6, 12); @@ -58,14 +63,14 @@ TEST(EigenMatrixBaseAddons, meanOfFinites) matrix(1, 1) = NAN; EXPECT_DOUBLE_EQ(1.0, matrix.meanOfFinites()); - Matrix matrix2; + Eigen::Matrix matrix2; matrix2.setRandom(); EXPECT_NEAR(matrix2.mean(), matrix2.meanOfFinites(), 1e-10); } TEST(EigenMatrixBaseAddons, minCoeffOfFinites) { - Matrix matrix; + Eigen::Matrix matrix; matrix.setRandom(); double min = matrix.minCoeff(); EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10); @@ -83,7 +88,7 @@ TEST(EigenMatrixBaseAddons, minCoeffOfFinites) TEST(EigenMatrixBaseAddons, maxCoeffOfFinites) { - Matrix matrix; + Eigen::Matrix matrix; matrix.setRandom(); double max = matrix.maxCoeff(); EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10); diff --git a/grid_map_core/test/EllipseIteratorTest.cpp b/grid_map_core/test/EllipseIteratorTest.cpp index 801cd9552..f6e1739f9 100644 --- a/grid_map_core/test/EllipseIteratorTest.cpp +++ b/grid_map_core/test/EllipseIteratorTest.cpp @@ -6,9 +6,6 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/iterators/EllipseIterator.hpp" -#include "grid_map_core/GridMap.hpp" - // Eigen #include @@ -21,16 +18,16 @@ // Vector #include -using namespace std; -using namespace Eigen; -using namespace grid_map; +#include "grid_map_core/iterators/EllipseIterator.hpp" +#include "grid_map_core/GridMap.hpp" + TEST(EllipseIterator, OneCellWideEllipse) { - GridMap map( { "types" }); - map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); + grid_map::GridMap map({"types"}); + map.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); - EllipseIterator iterator(map, Position(0.0, 0.0), Length(8.0, 1.0)); + grid_map::EllipseIterator iterator(map, grid_map::Position(0.0, 0.0), grid_map::Length(8.0, 1.0)); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(0, (*iterator)(0)); diff --git a/grid_map_core/test/GridMapIteratorTest.cpp b/grid_map_core/test/GridMapIteratorTest.cpp index c04e472d5..695871864 100644 --- a/grid_map_core/test/GridMapIteratorTest.cpp +++ b/grid_map_core/test/GridMapIteratorTest.cpp @@ -6,9 +6,6 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/iterators/GridMapIterator.hpp" -#include "grid_map_core/GridMap.hpp" - // Eigen #include @@ -21,15 +18,17 @@ // Vector #include -using namespace std; -using namespace grid_map; +#include "grid_map_core/iterators/GridMapIterator.hpp" +#include "grid_map_core/GridMap.hpp" + TEST(GridMapIterator, Simple) { - GridMap map; - map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + grid_map::GridMap map; + map.setGeometry( + grid_map::Length(8.1, 5.1), 1.0, grid_map::Position(0.0, 0.0)); // bufferSize(8, 5) map.add("layer", 0.0); - GridMapIterator iterator(map); + grid_map::GridMapIterator iterator(map); unsigned int i = 0; for (; !iterator.isPastEnd(); ++iterator, ++i) { @@ -37,19 +36,20 @@ TEST(GridMapIterator, Simple) EXPECT_FALSE(iterator.isPastEnd()); } - EXPECT_EQ(40, i); + EXPECT_EQ(40u, i); EXPECT_TRUE(iterator.isPastEnd()); EXPECT_TRUE((map["layer"].array() == 1.0f).all()); } TEST(GridMapIterator, LinearIndex) { - GridMap map; - map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + grid_map::GridMap map; + map.setGeometry( + grid_map::Length(8.1, 5.1), 1.0, grid_map::Position(0.0, 0.0)); // bufferSize(8, 5) map.add("layer", 0.0); - GridMapIterator iterator(map); + grid_map::GridMapIterator iterator(map); - auto& data = map["layer"]; + auto & data = map["layer"]; unsigned int i = 0; for (; !iterator.isPastEnd(); ++iterator, ++i) { data(iterator.getLinearIndex()) = 1.0; @@ -57,7 +57,7 @@ TEST(GridMapIterator, LinearIndex) EXPECT_FALSE(iterator.isPastEnd()); } - EXPECT_EQ(40, i); + EXPECT_EQ(40u, i); EXPECT_TRUE(iterator.isPastEnd()); EXPECT_TRUE((map["layer"].array() == 1.0f).all()); } diff --git a/grid_map_core/test/GridMapMathTest.cpp b/grid_map_core/test/GridMapMathTest.cpp index 0bb17aca8..be266af00 100644 --- a/grid_map_core/test/GridMapMathTest.cpp +++ b/grid_map_core/test/GridMapMathTest.cpp @@ -6,8 +6,6 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/GridMapMath.hpp" - // Eigen #include @@ -20,200 +18,338 @@ // Vector #include -using namespace std; -using namespace grid_map; +#include + +#include "grid_map_core/GridMapMath.hpp" + TEST(PositionFromIndex, Simple) { - Length mapLength(3.0, 2.0); - Position mapPosition(-1.0, 2.0); + grid_map::Length mapLength(3.0, 2.0); + grid_map::Position mapPosition(-1.0, 2.0); double resolution = 1.0; - Size bufferSize(3, 2); - Position position; + grid_map::Size bufferSize(3, 2); + grid_map::Position position; - EXPECT_TRUE(getPositionFromIndex(position, Index(0, 0), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getPositionFromIndex( + position, grid_map::Index(0, 0), mapLength, mapPosition, resolution, + bufferSize)); EXPECT_DOUBLE_EQ(1.0 + mapPosition.x(), position.x()); EXPECT_DOUBLE_EQ(0.5 + mapPosition.y(), position.y()); - EXPECT_TRUE(getPositionFromIndex(position, Index(1, 0), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getPositionFromIndex( + position, grid_map::Index(1, 0), mapLength, mapPosition, resolution, + bufferSize)); EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x()); EXPECT_DOUBLE_EQ(0.5 + mapPosition.y(), position.y()); - EXPECT_TRUE(getPositionFromIndex(position, Index(1, 1), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getPositionFromIndex( + position, grid_map::Index(1, 1), mapLength, mapPosition, resolution, + bufferSize)); EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x()); EXPECT_DOUBLE_EQ(-0.5 + mapPosition.y(), position.y()); - EXPECT_TRUE(getPositionFromIndex(position, Index(2, 1), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getPositionFromIndex( + position, grid_map::Index(2, 1), mapLength, mapPosition, resolution, + bufferSize)); EXPECT_DOUBLE_EQ(-1.0 + mapPosition.x(), position.x()); EXPECT_DOUBLE_EQ(-0.5 + mapPosition.y(), position.y()); - EXPECT_FALSE(getPositionFromIndex(position, Index(3, 1), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_FALSE( + grid_map::getPositionFromIndex( + position, grid_map::Index(3, 1), mapLength, mapPosition, resolution, + bufferSize)); } TEST(PositionFromIndex, CircularBuffer) { - Length mapLength(0.5, 0.4); - Position mapPosition(-0.1, 13.4); + grid_map::Length mapLength(0.5, 0.4); + grid_map::Position mapPosition(-0.1, 13.4); double resolution = 0.1; - Size bufferSize(5, 4); - Index bufferStartIndex(3, 1); - Position position; - - EXPECT_TRUE(getPositionFromIndex(position, Index(3, 1), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + grid_map::Size bufferSize(5, 4); + grid_map::Index bufferStartIndex(3, 1); + grid_map::Position position; + + EXPECT_TRUE( + grid_map::getPositionFromIndex( + position, grid_map::Index(3, 1), mapLength, mapPosition, resolution, + bufferSize, bufferStartIndex)); EXPECT_DOUBLE_EQ(0.2 + mapPosition.x(), position.x()); EXPECT_DOUBLE_EQ(0.15 + mapPosition.y(), position.y()); - EXPECT_TRUE(getPositionFromIndex(position, Index(4, 2), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_TRUE( + grid_map::getPositionFromIndex( + position, grid_map::Index(4, 2), mapLength, mapPosition, resolution, + bufferSize, bufferStartIndex)); EXPECT_DOUBLE_EQ(0.1 + mapPosition.x(), position.x()); EXPECT_DOUBLE_EQ(0.05 + mapPosition.y(), position.y()); - EXPECT_TRUE(getPositionFromIndex(position, Index(2, 0), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_TRUE( + grid_map::getPositionFromIndex( + position, grid_map::Index(2, 0), mapLength, mapPosition, resolution, + bufferSize, bufferStartIndex)); EXPECT_DOUBLE_EQ(-0.2 + mapPosition.x(), position.x()); EXPECT_DOUBLE_EQ(-0.15 + mapPosition.y(), position.y()); - EXPECT_TRUE(getPositionFromIndex(position, Index(0, 0), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_TRUE( + grid_map::getPositionFromIndex( + position, grid_map::Index(0, 0), mapLength, mapPosition, resolution, + bufferSize, bufferStartIndex)); EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x()); EXPECT_DOUBLE_EQ(-0.15 + mapPosition.y(), position.y()); - EXPECT_TRUE(getPositionFromIndex(position, Index(4, 3), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_TRUE( + grid_map::getPositionFromIndex( + position, grid_map::Index(4, 3), mapLength, mapPosition, resolution, + bufferSize, bufferStartIndex)); EXPECT_DOUBLE_EQ(0.1 + mapPosition.x(), position.x()); EXPECT_DOUBLE_EQ(-0.05 + mapPosition.y(), position.y()); - EXPECT_FALSE(getPositionFromIndex(position, Index(5, 3), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_FALSE( + grid_map::getPositionFromIndex( + position, grid_map::Index(5, 3), mapLength, mapPosition, resolution, + bufferSize, bufferStartIndex)); } TEST(IndexFromPosition, Simple) { - Length mapLength(3.0, 2.0); - Position mapPosition(-12.4, -7.1); + grid_map::Length mapLength(3.0, 2.0); + grid_map::Position mapPosition(-12.4, -7.1); double resolution = 1.0; - Index bufferSize(3, 2); - Index index; + grid_map::Index bufferSize(3, 2); + grid_map::Index index; - EXPECT_TRUE(getIndexFromPosition(index, Position(1.0, 0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getIndexFromPosition( + index, grid_map::Position(1.0, 0.5) + mapPosition, mapLength, mapPosition, + resolution, bufferSize)); EXPECT_EQ(0, index(0)); EXPECT_EQ(0, index(1)); - EXPECT_TRUE(getIndexFromPosition(index, Position(-1.0, -0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getIndexFromPosition( + index, grid_map::Position(-1.0, -0.5) + mapPosition, mapLength, + mapPosition, resolution, bufferSize)); EXPECT_EQ(2, index(0)); EXPECT_EQ(1, index(1)); - EXPECT_TRUE(getIndexFromPosition(index, Position(0.6, 0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getIndexFromPosition( + index, grid_map::Position(0.6, 0.1) + mapPosition, mapLength, mapPosition, + resolution, bufferSize)); EXPECT_EQ(0, index(0)); EXPECT_EQ(0, index(1)); - EXPECT_TRUE(getIndexFromPosition(index, Position(0.4, -0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getIndexFromPosition( + index, grid_map::Position(0.4, -0.1) + mapPosition, mapLength, mapPosition, + resolution, bufferSize)); EXPECT_EQ(1, index(0)); EXPECT_EQ(1, index(1)); - EXPECT_TRUE(getIndexFromPosition(index, Position(0.4, 0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getIndexFromPosition( + index, grid_map::Position(0.4, 0.1) + mapPosition, mapLength, mapPosition, + resolution, bufferSize)); EXPECT_EQ(1, index(0)); EXPECT_EQ(0, index(1)); - EXPECT_FALSE(getIndexFromPosition(index, Position(4.0, 0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_FALSE( + grid_map::getIndexFromPosition( + index, grid_map::Position(4.0, 0.5) + mapPosition, mapLength, mapPosition, + resolution, bufferSize)); } TEST(IndexFromPosition, EdgeCases) { - Length mapLength(3.0, 2.0); - Position mapPosition(0.0, 0.0); + grid_map::Length mapLength(3.0, 2.0); + grid_map::Position mapPosition(0.0, 0.0); double resolution = 1.0; - Size bufferSize(3, 2); - Index index; + grid_map::Size bufferSize(3, 2); + grid_map::Index index; - EXPECT_TRUE(getIndexFromPosition(index, Position(0.0, DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getIndexFromPosition( + index, grid_map::Position(0.0, DBL_EPSILON), mapLength, mapPosition, + resolution, bufferSize)); EXPECT_EQ(1, index(0)); EXPECT_EQ(0, index(1)); - EXPECT_TRUE(getIndexFromPosition(index, Position(0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getIndexFromPosition( + index, grid_map::Position(0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, + mapPosition, resolution, bufferSize)); EXPECT_EQ(1, index(0)); EXPECT_EQ(1, index(1)); - EXPECT_TRUE(getIndexFromPosition(index, Position(-0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getIndexFromPosition( + index, grid_map::Position(-0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, + mapPosition, resolution, bufferSize)); EXPECT_EQ(2, index(0)); EXPECT_EQ(1, index(1)); - EXPECT_FALSE(getIndexFromPosition(index, Position(-1.5, 1.0), mapLength, mapPosition, resolution, bufferSize)); + EXPECT_FALSE( + grid_map::getIndexFromPosition( + index, grid_map::Position(-1.5, 1.0), mapLength, mapPosition, resolution, + bufferSize)); } TEST(IndexFromPosition, CircularBuffer) { - Length mapLength(0.5, 0.4); - Position mapPosition(0.4, -0.9); + grid_map::Length mapLength(0.5, 0.4); + grid_map::Position mapPosition(0.4, -0.9); double resolution = 0.1; - Size bufferSize(5, 4); - Index bufferStartIndex(3, 1); - Index index; - - EXPECT_TRUE(getIndexFromPosition(index, Position(0.2, 0.15) + mapPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + grid_map::Size bufferSize(5, 4); + grid_map::Index bufferStartIndex(3, 1); + grid_map::Index index; + + EXPECT_TRUE( + grid_map::getIndexFromPosition( + index, grid_map::Position(0.2, 0.15) + mapPosition, mapLength, mapPosition, + resolution, bufferSize, bufferStartIndex)); EXPECT_EQ(3, index(0)); EXPECT_EQ(1, index(1)); - EXPECT_TRUE(getIndexFromPosition(index, Position(0.03, -0.17) + mapPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_TRUE( + grid_map::getIndexFromPosition( + index, grid_map::Position(0.03, -0.17) + mapPosition, mapLength, + mapPosition, resolution, bufferSize, bufferStartIndex)); EXPECT_EQ(0, index(0)); EXPECT_EQ(0, index(1)); } TEST(checkIfPositionWithinMap, Inside) { - Length mapLength(50.0, 25.0); - Position mapPosition(11.4, 0.0); - - EXPECT_TRUE(checkIfPositionWithinMap(Position(0.0, 0.0) + mapPosition, mapLength, mapPosition)); - EXPECT_TRUE(checkIfPositionWithinMap(Position(5.0, 5.0) + mapPosition, mapLength, mapPosition)); - EXPECT_TRUE(checkIfPositionWithinMap(Position(20.0, 10.0) + mapPosition, mapLength, mapPosition)); - EXPECT_TRUE(checkIfPositionWithinMap(Position(20.0, -10.0) + mapPosition, mapLength, mapPosition)); - EXPECT_TRUE(checkIfPositionWithinMap(Position(-20.0, 10.0) + mapPosition, mapLength, mapPosition)); - EXPECT_TRUE(checkIfPositionWithinMap(Position(-20.0, -10.0) + mapPosition, mapLength, mapPosition)); + grid_map::Length mapLength(50.0, 25.0); + grid_map::Position mapPosition(11.4, 0.0); + + EXPECT_TRUE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(0.0, 0.0) + mapPosition, + mapLength, mapPosition)); + EXPECT_TRUE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(5.0, 5.0) + mapPosition, + mapLength, mapPosition)); + EXPECT_TRUE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(20.0, 10.0) + mapPosition, + mapLength, mapPosition)); + EXPECT_TRUE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(20.0, -10.0) + mapPosition, mapLength, + mapPosition)); + EXPECT_TRUE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-20.0, 10.0) + mapPosition, mapLength, + mapPosition)); + EXPECT_TRUE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-20.0, -10.0) + mapPosition, mapLength, + mapPosition)); } TEST(checkIfPositionWithinMap, Outside) { - Length mapLength(10.0, 5.0); - Position mapPosition(-3.0, 145.2); - - EXPECT_FALSE(checkIfPositionWithinMap(Position(5.5, 0.0) + mapPosition, mapLength, mapPosition)); - EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, 0.0) + mapPosition, mapLength, mapPosition)); - EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, 3.0) + mapPosition, mapLength, mapPosition)); - EXPECT_FALSE(checkIfPositionWithinMap(Position(-5.5, -3.0) + mapPosition, mapLength, mapPosition)); - EXPECT_FALSE(checkIfPositionWithinMap(Position(3.0, 3.0) + mapPosition, mapLength, mapPosition)); + grid_map::Length mapLength(10.0, 5.0); + grid_map::Position mapPosition(-3.0, 145.2); + + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(5.5, 0.0) + mapPosition, + mapLength, mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-5.5, 0.0) + mapPosition, + mapLength, mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-5.5, 3.0) + mapPosition, + mapLength, mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-5.5, -3.0) + mapPosition, mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(3.0, 3.0) + mapPosition, + mapLength, mapPosition)); } TEST(checkIfPositionWithinMap, EdgeCases) { - Length mapLength(2.0, 3.0); - Position mapPosition(0.0, 0.0); - - EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0, -1.5), mapLength, mapPosition)); - EXPECT_FALSE(checkIfPositionWithinMap(Position(-1.0, 1.5), mapLength, mapPosition)); - EXPECT_FALSE(checkIfPositionWithinMap(Position(1.0 + DBL_EPSILON, 1.0), mapLength, mapPosition)); - EXPECT_TRUE(checkIfPositionWithinMap(Position((2.0 + DBL_EPSILON) / 2.0, 1.0), mapLength, mapPosition)); - EXPECT_FALSE(checkIfPositionWithinMap(Position(0.5, -1.5 - (2.0 * DBL_EPSILON)), mapLength, mapPosition)); - EXPECT_TRUE(checkIfPositionWithinMap(Position(-0.5, (3.0 + DBL_EPSILON) / 2.0), mapLength, mapPosition)); + grid_map::Length mapLength(2.0, 3.0); + grid_map::Position mapPosition(0.0, 0.0); + + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(1.0, -1.5), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-1.0, 1.5), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(1.0 + DBL_EPSILON, 1.0), + mapLength, mapPosition)); + EXPECT_TRUE( + grid_map::checkIfPositionWithinMap( + grid_map::Position((2.0 + DBL_EPSILON) / 2.0, 1.0), mapLength, + mapPosition)); + EXPECT_FALSE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(0.5, -1.5 - (2.0 * DBL_EPSILON)), mapLength, + mapPosition)); + EXPECT_TRUE( + grid_map::checkIfPositionWithinMap( + grid_map::Position(-0.5, (3.0 + DBL_EPSILON) / 2.0), mapLength, + mapPosition)); } TEST(getIndexShiftFromPositionShift, All) { double resolution = 1.0; - Index indexShift; + grid_map::Index indexShift; - EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.0, 0.0), resolution)); + EXPECT_TRUE( + grid_map::getIndexShiftFromPositionShift( + indexShift, grid_map::Vector(0.0, 0.0), + resolution)); EXPECT_EQ(0, indexShift(0)); EXPECT_EQ(0, indexShift(1)); - EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.35, -0.45), resolution)); + EXPECT_TRUE( + grid_map::getIndexShiftFromPositionShift( + indexShift, grid_map::Vector(0.35, -0.45), + resolution)); EXPECT_EQ(0, indexShift(0)); EXPECT_EQ(0, indexShift(1)); - EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(0.55, -0.45), resolution)); + EXPECT_TRUE( + grid_map::getIndexShiftFromPositionShift( + indexShift, grid_map::Vector(0.55, -0.45), + resolution)); EXPECT_EQ(-1, indexShift(0)); EXPECT_EQ(0, indexShift(1)); - EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(-1.3, -2.65), resolution)); + EXPECT_TRUE( + grid_map::getIndexShiftFromPositionShift( + indexShift, grid_map::Vector(-1.3, -2.65), + resolution)); EXPECT_EQ(1, indexShift(0)); EXPECT_EQ(3, indexShift(1)); - EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector(-0.4, 0.09), 0.2)); + EXPECT_TRUE( + grid_map::getIndexShiftFromPositionShift( + indexShift, grid_map::Vector( + -0.4, + 0.09), 0.2)); EXPECT_EQ(2, indexShift(0)); EXPECT_EQ(0, indexShift(1)); } @@ -221,30 +357,39 @@ TEST(getIndexShiftFromPositionShift, All) TEST(getPositionShiftFromIndexShift, All) { double resolution = 0.3; - Vector positionShift; + grid_map::Vector positionShift; - EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(0, 0), resolution)); + EXPECT_TRUE( + grid_map::getPositionShiftFromIndexShift( + positionShift, grid_map::Index(0, 0), + resolution)); EXPECT_DOUBLE_EQ(0.0, positionShift.x()); EXPECT_DOUBLE_EQ(0.0, positionShift.y()); - EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(1, -1), resolution)); + EXPECT_TRUE( + grid_map::getPositionShiftFromIndexShift( + positionShift, grid_map::Index(1, -1), + resolution)); EXPECT_DOUBLE_EQ(-0.3, positionShift.x()); EXPECT_DOUBLE_EQ(0.3, positionShift.y()); - EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Index(2, 1), resolution)); + EXPECT_TRUE( + grid_map::getPositionShiftFromIndexShift( + positionShift, grid_map::Index(2, 1), + resolution)); EXPECT_DOUBLE_EQ(-0.6, positionShift.x()); EXPECT_DOUBLE_EQ(-0.3, positionShift.y()); } TEST(checkIfIndexInRange, All) { - Size bufferSize(10, 15); - EXPECT_TRUE(checkIfIndexInRange(Index(0, 0), bufferSize)); - EXPECT_TRUE(checkIfIndexInRange(Index(9, 14), bufferSize)); - EXPECT_FALSE(checkIfIndexInRange(Index(10, 5), bufferSize)); - EXPECT_FALSE(checkIfIndexInRange(Index(5, 300), bufferSize)); - EXPECT_FALSE(checkIfIndexInRange(Index(-1, 0), bufferSize)); - EXPECT_FALSE(checkIfIndexInRange(Index(0, -300), bufferSize)); + grid_map::Size bufferSize(10, 15); + EXPECT_TRUE(grid_map::checkIfIndexInRange(grid_map::Index(0, 0), bufferSize)); + EXPECT_TRUE(grid_map::checkIfIndexInRange(grid_map::Index(9, 14), bufferSize)); + EXPECT_FALSE(grid_map::checkIfIndexInRange(grid_map::Index(10, 5), bufferSize)); + EXPECT_FALSE(grid_map::checkIfIndexInRange(grid_map::Index(5, 300), bufferSize)); + EXPECT_FALSE(grid_map::checkIfIndexInRange(grid_map::Index(-1, 0), bufferSize)); + EXPECT_FALSE(grid_map::checkIfIndexInRange(grid_map::Index(0, -300), bufferSize)); } TEST(boundIndexToRange, All) @@ -253,31 +398,31 @@ TEST(boundIndexToRange, All) int bufferSize = 10; index = 0; - boundIndexToRange(index, bufferSize); + grid_map::boundIndexToRange(index, bufferSize); EXPECT_EQ(0, index); index = 1; - boundIndexToRange(index, bufferSize); + grid_map::boundIndexToRange(index, bufferSize); EXPECT_EQ(1, index); index = -1; - boundIndexToRange(index, bufferSize); + grid_map::boundIndexToRange(index, bufferSize); EXPECT_EQ(0, index); index = 9; - boundIndexToRange(index, bufferSize); + grid_map::boundIndexToRange(index, bufferSize); EXPECT_EQ(9, index); index = 10; - boundIndexToRange(index, bufferSize); + grid_map::boundIndexToRange(index, bufferSize); EXPECT_EQ(9, index); index = 35; - boundIndexToRange(index, bufferSize); + grid_map::boundIndexToRange(index, bufferSize); EXPECT_EQ(9, index); index = -19; - boundIndexToRange(index, bufferSize); + grid_map::boundIndexToRange(index, bufferSize); EXPECT_EQ(0, index); } @@ -287,92 +432,92 @@ TEST(wrapIndexToRange, All) int bufferSize = 10; index = 0; - wrapIndexToRange(index, bufferSize); + grid_map::wrapIndexToRange(index, bufferSize); EXPECT_EQ(0, index); index = 1; - wrapIndexToRange(index, bufferSize); + grid_map::wrapIndexToRange(index, bufferSize); EXPECT_EQ(1, index); index = -1; - wrapIndexToRange(index, bufferSize); + grid_map::wrapIndexToRange(index, bufferSize); EXPECT_EQ(9, index); index = 9; - wrapIndexToRange(index, bufferSize); + grid_map::wrapIndexToRange(index, bufferSize); EXPECT_EQ(9, index); index = 10; - wrapIndexToRange(index, bufferSize); + grid_map::wrapIndexToRange(index, bufferSize); EXPECT_EQ(0, index); index = 11; - wrapIndexToRange(index, bufferSize); + grid_map::wrapIndexToRange(index, bufferSize); EXPECT_EQ(1, index); index = 35; - wrapIndexToRange(index, bufferSize); + grid_map::wrapIndexToRange(index, bufferSize); EXPECT_EQ(5, index); index = -9; - wrapIndexToRange(index, bufferSize); + grid_map::wrapIndexToRange(index, bufferSize); EXPECT_EQ(1, index); index = -19; - wrapIndexToRange(index, bufferSize); + grid_map::wrapIndexToRange(index, bufferSize); EXPECT_EQ(1, index); } TEST(boundPositionToRange, Simple) { - double epsilon = 11.0 * numeric_limits::epsilon(); + double epsilon = 11.0 * std::numeric_limits::epsilon(); - Length mapLength(30.0, 10.0); - Position mapPosition(0.0, 0.0); - Position position; + grid_map::Length mapLength(30.0, 10.0); + grid_map::Position mapPosition(0.0, 0.0); + grid_map::Position position; position << 0.0, 0.0; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_DOUBLE_EQ(0.0, position.x()); EXPECT_DOUBLE_EQ(0.0, position.y()); position << 15.0, 5.0; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_NEAR(15.0, position.x(), 15.0 * epsilon); EXPECT_GE(15.0, position.x()); EXPECT_NEAR(5.0, position.y(), 5.0 * epsilon); EXPECT_GE(5.0, position.y()); position << -15.0, -5.0; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_NEAR(-15.0, position.x(), 15.0 * epsilon); EXPECT_LE(-15.0, position.x()); EXPECT_NEAR(-5.0, position.y(), 5.0 * epsilon); EXPECT_LE(-5.0, position.y()); position << 16.0, 6.0; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_NEAR(15.0, position.x(), 16.0 * epsilon); EXPECT_GE(15.0, position.x()); EXPECT_NEAR(5.0, position.y(), 6.0 * epsilon); EXPECT_GE(5.0, position.y()); position << -16.0, -6.0; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_NEAR(-15.0, position.x(), 16.0 * epsilon); EXPECT_LE(-15.0, position.x()); EXPECT_NEAR(-5.0, position.y(), 6.0 * epsilon); EXPECT_LE(-5.0, position.y()); position << 1e6, 1e6; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_NEAR(15.0, position.x(), 1e6 * epsilon); EXPECT_GE(15.0, position.x()); EXPECT_NEAR(5.0, position.y(), 1e6 * epsilon); EXPECT_GE(5.0, position.y()); position << -1e6, -1e6; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_NEAR(-15.0, position.x(), 1e6 * epsilon); EXPECT_LE(-15.0, position.x()); EXPECT_NEAR(-5.0, position.y(), 1e6 * epsilon); @@ -381,54 +526,54 @@ TEST(boundPositionToRange, Simple) TEST(boundPositionToRange, Position) { - double epsilon = 11.0 * numeric_limits::epsilon(); + double epsilon = 11.0 * std::numeric_limits::epsilon(); - Length mapLength(30.0, 10.0); - Position mapPosition(1.0, 2.0); - Position position; + grid_map::Length mapLength(30.0, 10.0); + grid_map::Position mapPosition(1.0, 2.0); + grid_map::Position position; position << 0.0, 0.0; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_DOUBLE_EQ(0.0, position.x()); EXPECT_DOUBLE_EQ(0.0, position.y()); position << 16.0, 7.0; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_NEAR(16.0, position.x(), 16.0 * epsilon); EXPECT_GE(16.0, position.x()); EXPECT_NEAR(7.0, position.y(), 7.0 * epsilon); EXPECT_GE(7.0, position.y()); position << -14.0, -3.0; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_NEAR(-14.0, position.x(), 14.0 * epsilon); EXPECT_LE(-14.0, position.x()); EXPECT_NEAR(-3.0, position.y(), 3.0 * epsilon); EXPECT_LE(-3.0, position.y()); position << 17.0, 8.0; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_NEAR(16.0, position.x(), 17.0 * epsilon); EXPECT_GE(16.0, position.x()); EXPECT_NEAR(7.0, position.y(), 8.0 * epsilon); EXPECT_GE(7.0, position.y()); position << -15.0, -4.0; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_NEAR(-14.0, position.x(), 15.0 * epsilon); EXPECT_LE(-14.0, position.x()); EXPECT_NEAR(-3.0, position.y(), 4.0 * epsilon); EXPECT_LE(-3.0, position.y()); position << 1e6, 1e6; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_NEAR(16.0, position.x(), 1e6 * epsilon); EXPECT_GE(16.0, position.x()); EXPECT_NEAR(7.0, position.y(), 1e6 * epsilon); EXPECT_GE(7.0, position.y()); position << -1e6, -1e6; - boundPositionToRange(position, mapLength, mapPosition); + grid_map::boundPositionToRange(position, mapLength, mapPosition); EXPECT_NEAR(-14.0, position.x(), 1e6 * epsilon); EXPECT_LE(-14.0, position.x()); EXPECT_NEAR(-3.0, position.y(), 1e6 * epsilon); @@ -438,26 +583,29 @@ TEST(boundPositionToRange, Position) TEST(getSubmapInformation, Simple) { // Map - Length mapLength(5.0, 4.0); - Position mapPosition(0.0, 0.0); + grid_map::Length mapLength(5.0, 4.0); + grid_map::Position mapPosition(0.0, 0.0); double resolution = 1.0; - Size bufferSize(5, 4); + grid_map::Size bufferSize(5, 4); // Requested submap - Position requestedSubmapPosition; - Position requestedSubmapLength; + grid_map::Position requestedSubmapPosition; + grid_map::Position requestedSubmapLength; // The returned submap indeces - Index submapTopLeftIndex; - Index submapSize; - Position submapPosition; - Length submapLength; - Index requestedIndexInSubmap; + grid_map::Index submapTopLeftIndex; + grid_map::Index submapSize; + grid_map::Position submapPosition; + grid_map::Length submapLength; + grid_map::Index requestedIndexInSubmap; requestedSubmapPosition << 0.0, 0.5; requestedSubmapLength << 0.9, 2.9; - EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, - requestedSubmapPosition, requestedSubmapLength, mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getSubmapInformation( + submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, mapLength, mapPosition, resolution, + bufferSize)); EXPECT_EQ(2, submapTopLeftIndex(0)); EXPECT_EQ(0, submapTopLeftIndex(1)); EXPECT_EQ(1, submapSize(0)); @@ -473,27 +621,29 @@ TEST(getSubmapInformation, Simple) TEST(getSubmapInformation, Zero) { // Map - Length mapLength(5.0, 4.0); - Position mapPosition(0.0, 0.0); + grid_map::Length mapLength(5.0, 4.0); + grid_map::Position mapPosition(0.0, 0.0); double resolution = 1.0; - Size bufferSize(5, 4); + grid_map::Size bufferSize(5, 4); // Requested submap - Position requestedSubmapPosition; - Length requestedSubmapLength; + grid_map::Position requestedSubmapPosition; + grid_map::Length requestedSubmapLength; // The returned submap indeces - Index submapTopLeftIndex; - Index submapSize; - Position submapPosition; - Length submapLength; - Index requestedIndexInSubmap; + grid_map::Index submapTopLeftIndex; + grid_map::Index submapSize; + grid_map::Position submapPosition; + grid_map::Length submapLength; + grid_map::Index requestedIndexInSubmap; requestedSubmapPosition << -1.0, -0.5; requestedSubmapLength << 0.0, 0.0; - EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, - requestedSubmapPosition, requestedSubmapLength, - mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getSubmapInformation( + submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize)); EXPECT_EQ(3, submapTopLeftIndex(0)); EXPECT_EQ(2, submapTopLeftIndex(1)); EXPECT_EQ(1, submapSize(0)); @@ -509,27 +659,29 @@ TEST(getSubmapInformation, Zero) TEST(getSubmapInformation, ExceedingBoundaries) { // Map - Length mapLength(5.0, 4.0); - Position mapPosition(0.0, 0.0); + grid_map::Length mapLength(5.0, 4.0); + grid_map::Position mapPosition(0.0, 0.0); double resolution = 1.0; - Size bufferSize(5, 4); + grid_map::Size bufferSize(5, 4); // Requested submap - Position requestedSubmapPosition; - Length requestedSubmapLength; + grid_map::Position requestedSubmapPosition; + grid_map::Length requestedSubmapLength; // The returned submap indeces - Index submapTopLeftIndex; - Size submapSize; - Position submapPosition; - Length submapLength; - Index requestedIndexInSubmap; + grid_map::Index submapTopLeftIndex; + grid_map::Size submapSize; + grid_map::Position submapPosition; + grid_map::Length submapLength; + grid_map::Index requestedIndexInSubmap; requestedSubmapPosition << 2.0, 1.5; requestedSubmapLength << 2.9, 2.9; - EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, - requestedSubmapPosition, requestedSubmapLength, - mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getSubmapInformation( + submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize)); EXPECT_EQ(0, submapTopLeftIndex(0)); EXPECT_EQ(0, submapTopLeftIndex(1)); EXPECT_EQ(2, submapSize(0)); @@ -543,9 +695,11 @@ TEST(getSubmapInformation, ExceedingBoundaries) requestedSubmapPosition << 0.0, 0.0; requestedSubmapLength << 1e6, 1e6; - EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, - requestedSubmapPosition, requestedSubmapLength, - mapLength, mapPosition, resolution, bufferSize)); + EXPECT_TRUE( + grid_map::getSubmapInformation( + submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize)); EXPECT_EQ(0, submapTopLeftIndex(0)); EXPECT_EQ(0, submapTopLeftIndex(1)); EXPECT_EQ(bufferSize(0), submapSize(0)); @@ -562,28 +716,30 @@ TEST(getSubmapInformation, ExceedingBoundaries) TEST(getSubmapInformation, CircularBuffer) { // Map - Length mapLength(5.0, 4.0); - Position mapPosition(0.0, 0.0); + grid_map::Length mapLength(5.0, 4.0); + grid_map::Position mapPosition(0.0, 0.0); double resolution = 1.0; - Size bufferSize(5, 4); - Index bufferStartIndex(2, 1); + grid_map::Size bufferSize(5, 4); + grid_map::Index bufferStartIndex(2, 1); // Requested submap - Position requestedSubmapPosition; - Length requestedSubmapLength; + grid_map::Position requestedSubmapPosition; + grid_map::Length requestedSubmapLength; // The returned submap indeces - Index submapTopLeftIndex; - Size submapSize; - Position submapPosition; - Length submapLength; - Index requestedIndexInSubmap; + grid_map::Index submapTopLeftIndex; + grid_map::Size submapSize; + grid_map::Position submapPosition; + grid_map::Length submapLength; + grid_map::Index requestedIndexInSubmap; requestedSubmapPosition << 0.0, 0.5; requestedSubmapLength << 0.9, 2.9; - EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, - requestedSubmapPosition, requestedSubmapLength, - mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_TRUE( + grid_map::getSubmapInformation( + submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); EXPECT_EQ(4, submapTopLeftIndex(0)); EXPECT_EQ(1, submapTopLeftIndex(1)); EXPECT_EQ(1, submapSize(0)); @@ -597,9 +753,11 @@ TEST(getSubmapInformation, CircularBuffer) requestedSubmapPosition << 2.0, 1.5; requestedSubmapLength << 2.9, 2.9; - EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, - requestedSubmapPosition, requestedSubmapLength, - mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_TRUE( + grid_map::getSubmapInformation( + submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); EXPECT_EQ(2, submapTopLeftIndex(0)); EXPECT_EQ(1, submapTopLeftIndex(1)); EXPECT_EQ(2, submapSize(0)); @@ -613,9 +771,11 @@ TEST(getSubmapInformation, CircularBuffer) requestedSubmapPosition << 0.0, 0.0; requestedSubmapLength << 1e6, 1e6; - EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, - requestedSubmapPosition, requestedSubmapLength, - mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + EXPECT_TRUE( + grid_map::getSubmapInformation( + submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); EXPECT_EQ(2, submapTopLeftIndex(0)); EXPECT_EQ(1, submapTopLeftIndex(1)); EXPECT_EQ(bufferSize(0), submapSize(0)); @@ -632,26 +792,28 @@ TEST(getSubmapInformation, CircularBuffer) TEST(getSubmapInformation, Debug1) { // Map - Length mapLength(4.98, 4.98); - Position mapPosition(-4.98, -5.76); + grid_map::Length mapLength(4.98, 4.98); + grid_map::Position mapPosition(-4.98, -5.76); double resolution = 0.06; - Size bufferSize(83, 83); - Index bufferStartIndex(0, 13); + grid_map::Size bufferSize(83, 83); + grid_map::Index bufferStartIndex(0, 13); // Requested submap - Position requestedSubmapPosition(-7.44, -3.42); - Length requestedSubmapLength(0.12, 0.12); + grid_map::Position requestedSubmapPosition(-7.44, -3.42); + grid_map::Length requestedSubmapLength(0.12, 0.12); // The returned submap indeces - Index submapTopLeftIndex; - Size submapSize; - Position submapPosition; - Length submapLength; - Index requestedIndexInSubmap; - - EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, - requestedSubmapPosition, requestedSubmapLength, - mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + grid_map::Index submapTopLeftIndex; + grid_map::Size submapSize; + grid_map::Position submapPosition; + grid_map::Length submapLength; + grid_map::Index requestedIndexInSubmap; + + EXPECT_TRUE( + grid_map::getSubmapInformation( + submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); EXPECT_EQ(2, submapSize(0)); EXPECT_EQ(3, submapSize(1)); EXPECT_DOUBLE_EQ(0.12, submapLength(0)); @@ -661,26 +823,28 @@ TEST(getSubmapInformation, Debug1) TEST(getSubmapInformation, Debug2) { // Map - Length mapLength(4.98, 4.98); - Position mapPosition(2.46, -25.26); + grid_map::Length mapLength(4.98, 4.98); + grid_map::Position mapPosition(2.46, -25.26); double resolution = 0.06; - Size bufferSize(83, 83); - Index bufferStartIndex(42, 6); + grid_map::Size bufferSize(83, 83); + grid_map::Index bufferStartIndex(42, 6); // Requested submap - Position requestedSubmapPosition(0.24, -26.82); - Length requestedSubmapLength(0.624614, 0.462276); + grid_map::Position requestedSubmapPosition(0.24, -26.82); + grid_map::Length requestedSubmapLength(0.624614, 0.462276); // The returned submap indeces - Index submapTopLeftIndex; - Size submapSize; - Position submapPosition; - Length submapLength; - Index requestedIndexInSubmap; - - EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, - requestedSubmapPosition, requestedSubmapLength, - mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); + grid_map::Index submapTopLeftIndex; + grid_map::Size submapSize; + grid_map::Position submapPosition; + grid_map::Length submapLength; + grid_map::Index requestedIndexInSubmap; + + EXPECT_TRUE( + grid_map::getSubmapInformation( + submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap, + requestedSubmapPosition, requestedSubmapLength, + mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)); EXPECT_LT(0, submapSize(0)); EXPECT_LT(0, submapSize(1)); EXPECT_LT(0.0, submapLength(0)); @@ -689,36 +853,36 @@ TEST(getSubmapInformation, Debug2) TEST(getBufferRegionsForSubmap, Trivial) { - Size bufferSize(5, 4); - Index submapIndex(0, 0); - Size submapSize(0, 0); - std::vector regions; - - EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize)); - EXPECT_EQ(1, regions.size()); - EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); + grid_map::Size bufferSize(5, 4); + grid_map::Index submapIndex(0, 0); + grid_map::Size submapSize(0, 0); + std::vector regions; + + EXPECT_TRUE(grid_map::getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize)); + EXPECT_EQ(1u, regions.size()); + EXPECT_EQ(grid_map::BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); EXPECT_EQ(0, regions[0].getStartIndex()[0]); EXPECT_EQ(0, regions[0].getStartIndex()[1]); EXPECT_EQ(0, regions[0].getSize()[0]); EXPECT_EQ(0, regions[0].getSize()[1]); submapSize << 0, 7; - EXPECT_FALSE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize)); + EXPECT_FALSE(grid_map::getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize)); submapSize << 6, 7; - EXPECT_FALSE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize)); + EXPECT_FALSE(grid_map::getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize)); } TEST(getBufferRegionsForSubmap, Simple) { - Size bufferSize(5, 4); - Index submapIndex(1, 2); - Size submapSize(3, 2); - std::vector regions; - - EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize)); - EXPECT_EQ(1, regions.size()); - EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); + grid_map::Size bufferSize(5, 4); + grid_map::Index submapIndex(1, 2); + grid_map::Size submapSize(3, 2); + std::vector regions; + + EXPECT_TRUE(grid_map::getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize)); + EXPECT_EQ(1u, regions.size()); + EXPECT_EQ(grid_map::BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); EXPECT_EQ(1, regions[0].getStartIndex()[0]); EXPECT_EQ(2, regions[0].getStartIndex()[1]); EXPECT_EQ(3, regions[0].getSize()[0]); @@ -727,17 +891,20 @@ TEST(getBufferRegionsForSubmap, Simple) TEST(getBufferRegionsForSubmap, CircularBuffer) { - Size bufferSize(5, 4); - Index submapIndex; - Size submapSize; - Index bufferStartIndex(3, 1); - std::vector regions; + grid_map::Size bufferSize(5, 4); + grid_map::Index submapIndex; + grid_map::Size submapSize; + grid_map::Index bufferStartIndex(3, 1); + std::vector regions; submapIndex << 3, 1; submapSize << 2, 3; - EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex)); - EXPECT_EQ(1, regions.size()); - EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); + EXPECT_TRUE( + grid_map::getBufferRegionsForSubmap( + regions, submapIndex, submapSize, bufferSize, + bufferStartIndex)); + EXPECT_EQ(1u, regions.size()); + EXPECT_EQ(grid_map::BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); EXPECT_EQ(3, regions[0].getStartIndex()[0]); EXPECT_EQ(1, regions[0].getStartIndex()[1]); EXPECT_EQ(2, regions[0].getSize()[0]); @@ -745,14 +912,17 @@ TEST(getBufferRegionsForSubmap, CircularBuffer) submapIndex << 4, 1; submapSize << 2, 3; - EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex)); - EXPECT_EQ(2, regions.size()); - EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); + EXPECT_TRUE( + grid_map::getBufferRegionsForSubmap( + regions, submapIndex, submapSize, bufferSize, + bufferStartIndex)); + EXPECT_EQ(2u, regions.size()); + EXPECT_EQ(grid_map::BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); EXPECT_EQ(4, regions[0].getStartIndex()[0]); EXPECT_EQ(1, regions[0].getStartIndex()[1]); EXPECT_EQ(1, regions[0].getSize()[0]); EXPECT_EQ(3, regions[0].getSize()[1]); - EXPECT_EQ(BufferRegion::Quadrant::BottomLeft, regions[1].getQuadrant()); + EXPECT_EQ(grid_map::BufferRegion::Quadrant::BottomLeft, regions[1].getQuadrant()); EXPECT_EQ(0, regions[1].getStartIndex()[0]); EXPECT_EQ(1, regions[1].getStartIndex()[1]); EXPECT_EQ(1, regions[1].getSize()[0]); @@ -760,9 +930,12 @@ TEST(getBufferRegionsForSubmap, CircularBuffer) submapIndex << 1, 0; submapSize << 2, 1; - EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex)); - EXPECT_EQ(1, regions.size()); - EXPECT_EQ(BufferRegion::Quadrant::BottomRight, regions[0].getQuadrant()); + EXPECT_TRUE( + grid_map::getBufferRegionsForSubmap( + regions, submapIndex, submapSize, bufferSize, + bufferStartIndex)); + EXPECT_EQ(1u, regions.size()); + EXPECT_EQ(grid_map::BufferRegion::Quadrant::BottomRight, regions[0].getQuadrant()); EXPECT_EQ(1, regions[0].getStartIndex()[0]); EXPECT_EQ(0, regions[0].getStartIndex()[1]); EXPECT_EQ(2, regions[0].getSize()[0]); @@ -770,24 +943,27 @@ TEST(getBufferRegionsForSubmap, CircularBuffer) submapIndex << 3, 1; submapSize << 5, 4; - EXPECT_TRUE(getBufferRegionsForSubmap(regions, submapIndex, submapSize, bufferSize, bufferStartIndex));\ - EXPECT_EQ(4, regions.size()); - EXPECT_EQ(BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); + EXPECT_TRUE( + grid_map::getBufferRegionsForSubmap( + regions, submapIndex, submapSize, bufferSize, + bufferStartIndex)); \ + EXPECT_EQ(4u, regions.size()); + EXPECT_EQ(grid_map::BufferRegion::Quadrant::TopLeft, regions[0].getQuadrant()); EXPECT_EQ(3, regions[0].getStartIndex()[0]); EXPECT_EQ(1, regions[0].getStartIndex()[1]); EXPECT_EQ(2, regions[0].getSize()[0]); EXPECT_EQ(3, regions[0].getSize()[1]); - EXPECT_EQ(BufferRegion::Quadrant::TopRight, regions[1].getQuadrant()); + EXPECT_EQ(grid_map::BufferRegion::Quadrant::TopRight, regions[1].getQuadrant()); EXPECT_EQ(3, regions[1].getStartIndex()[0]); EXPECT_EQ(0, regions[1].getStartIndex()[1]); EXPECT_EQ(2, regions[1].getSize()[0]); EXPECT_EQ(1, regions[1].getSize()[1]); - EXPECT_EQ(BufferRegion::Quadrant::BottomLeft, regions[2].getQuadrant()); + EXPECT_EQ(grid_map::BufferRegion::Quadrant::BottomLeft, regions[2].getQuadrant()); EXPECT_EQ(0, regions[2].getStartIndex()[0]); EXPECT_EQ(1, regions[2].getStartIndex()[1]); EXPECT_EQ(3, regions[2].getSize()[0]); EXPECT_EQ(3, regions[2].getSize()[1]); - EXPECT_EQ(BufferRegion::Quadrant::BottomRight, regions[3].getQuadrant()); + EXPECT_EQ(grid_map::BufferRegion::Quadrant::BottomRight, regions[3].getQuadrant()); EXPECT_EQ(0, regions[3].getStartIndex()[0]); EXPECT_EQ(0, regions[3].getStartIndex()[1]); EXPECT_EQ(3, regions[3].getSize()[0]); @@ -796,192 +972,252 @@ TEST(getBufferRegionsForSubmap, CircularBuffer) TEST(checkIncrementIndex, Simple) { - Index index(0, 0); - Size bufferSize(4, 3); + grid_map::Index index(0, 0); + grid_map::Size bufferSize(4, 3); - EXPECT_TRUE(incrementIndex(index, bufferSize)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize)); EXPECT_EQ(0, index[0]); EXPECT_EQ(1, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize)); EXPECT_EQ(0, index[0]); EXPECT_EQ(2, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize)); EXPECT_EQ(1, index[0]); EXPECT_EQ(0, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize)); EXPECT_EQ(1, index[0]); EXPECT_EQ(1, index[1]); for (int i = 0; i < 6; i++) { - EXPECT_TRUE(incrementIndex(index, bufferSize)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize)); } EXPECT_EQ(3, index[0]); EXPECT_EQ(1, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize)); EXPECT_EQ(3, index[0]); EXPECT_EQ(2, index[1]); - EXPECT_FALSE(incrementIndex(index, bufferSize)); + EXPECT_FALSE(grid_map::incrementIndex(index, bufferSize)); EXPECT_EQ(index[0], index[0]); EXPECT_EQ(index[1], index[1]); } TEST(checkIncrementIndex, CircularBuffer) { - Size bufferSize(4, 3); - Index bufferStartIndex(2, 1); - Index index(bufferStartIndex); + grid_map::Size bufferSize(4, 3); + grid_map::Index bufferStartIndex(2, 1); + grid_map::Index index(bufferStartIndex); - EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize, bufferStartIndex)); EXPECT_EQ(2, index[0]); EXPECT_EQ(2, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize, bufferStartIndex)); EXPECT_EQ(2, index[0]); EXPECT_EQ(0, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize, bufferStartIndex)); EXPECT_EQ(3, index[0]); EXPECT_EQ(1, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize, bufferStartIndex)); EXPECT_EQ(3, index[0]); EXPECT_EQ(2, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize, bufferStartIndex)); EXPECT_EQ(3, index[0]); EXPECT_EQ(0, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize, bufferStartIndex)); EXPECT_EQ(0, index[0]); EXPECT_EQ(1, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize, bufferStartIndex)); EXPECT_EQ(0, index[0]); EXPECT_EQ(2, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize, bufferStartIndex)); EXPECT_EQ(0, index[0]); EXPECT_EQ(0, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize, bufferStartIndex)); EXPECT_EQ(1, index[0]); EXPECT_EQ(1, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize, bufferStartIndex)); EXPECT_EQ(1, index[0]); EXPECT_EQ(2, index[1]); - EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_TRUE(grid_map::incrementIndex(index, bufferSize, bufferStartIndex)); EXPECT_EQ(1, index[0]); EXPECT_EQ(0, index[1]); - EXPECT_FALSE(incrementIndex(index, bufferSize, bufferStartIndex)); + EXPECT_FALSE(grid_map::incrementIndex(index, bufferSize, bufferStartIndex)); EXPECT_EQ(index[0], index[0]); EXPECT_EQ(index[1], index[1]); } TEST(checkIncrementIndexForSubmap, Simple) { - Index submapIndex(0, 0); - Index index; - Index submapTopLeftIndex(3, 1); - Size submapBufferSize(2, 4); - Size bufferSize(8, 5); - - EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + grid_map::Index submapIndex(0, 0); + grid_map::Index index; + grid_map::Index submapTopLeftIndex(3, 1); + grid_map::Size submapBufferSize(2, 4); + grid_map::Size bufferSize(8, 5); + + EXPECT_TRUE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize)); EXPECT_EQ(0, submapIndex[0]); EXPECT_EQ(1, submapIndex[1]); EXPECT_EQ(3, index[0]); EXPECT_EQ(2, index[1]); - EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + EXPECT_TRUE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize)); EXPECT_EQ(0, submapIndex[0]); EXPECT_EQ(2, submapIndex[1]); EXPECT_EQ(3, index[0]); EXPECT_EQ(3, index[1]); - EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + EXPECT_TRUE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize)); EXPECT_EQ(0, submapIndex[0]); EXPECT_EQ(3, submapIndex[1]); EXPECT_EQ(3, index[0]); EXPECT_EQ(4, index[1]); - EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + EXPECT_TRUE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize)); EXPECT_EQ(1, submapIndex[0]); EXPECT_EQ(0, submapIndex[1]); EXPECT_EQ(4, index[0]); EXPECT_EQ(1, index[1]); submapIndex << 1, 2; - EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + EXPECT_TRUE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize)); EXPECT_EQ(1, submapIndex[0]); EXPECT_EQ(3, submapIndex[1]); EXPECT_EQ(4, index[0]); EXPECT_EQ(4, index[1]); - EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + EXPECT_FALSE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize)); submapIndex << 2, 0; - EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize)); + EXPECT_FALSE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize)); } TEST(checkIncrementIndexForSubmap, CircularBuffer) { - Index submapIndex(0, 0); - Index index; - Index submapTopLeftIndex(6, 3); - Size submapBufferSize(2, 4); - Size bufferSize(8, 5); - Index bufferStartIndex(3, 2); - - EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + grid_map::Index submapIndex(0, 0); + grid_map::Index index; + grid_map::Index submapTopLeftIndex(6, 3); + grid_map::Size submapBufferSize(2, 4); + grid_map::Size bufferSize(8, 5); + grid_map::Index bufferStartIndex(3, 2); + + EXPECT_TRUE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize, bufferStartIndex)); EXPECT_EQ(0, submapIndex[0]); EXPECT_EQ(1, submapIndex[1]); EXPECT_EQ(6, index[0]); EXPECT_EQ(4, index[1]); - EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + EXPECT_TRUE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize, bufferStartIndex)); EXPECT_EQ(0, submapIndex[0]); EXPECT_EQ(2, submapIndex[1]); EXPECT_EQ(6, index[0]); EXPECT_EQ(0, index[1]); - EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + EXPECT_TRUE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize, bufferStartIndex)); EXPECT_EQ(0, submapIndex[0]); EXPECT_EQ(3, submapIndex[1]); EXPECT_EQ(6, index[0]); EXPECT_EQ(1, index[1]); - EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + EXPECT_TRUE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize, bufferStartIndex)); EXPECT_EQ(1, submapIndex[0]); EXPECT_EQ(0, submapIndex[1]); EXPECT_EQ(7, index[0]); EXPECT_EQ(3, index[1]); submapIndex << 1, 2; - EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + EXPECT_TRUE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize, bufferStartIndex)); EXPECT_EQ(1, submapIndex[0]); EXPECT_EQ(3, submapIndex[1]); EXPECT_EQ(7, index[0]); EXPECT_EQ(1, index[1]); - EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + EXPECT_FALSE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize, bufferStartIndex)); submapIndex << 2, 0; - EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex)); + EXPECT_FALSE( + grid_map::incrementIndexForSubmap( + submapIndex, index, submapTopLeftIndex, submapBufferSize, + bufferSize, bufferStartIndex)); } TEST(getIndexFromLinearIndex, Simple) { - EXPECT_TRUE((Index(0, 0) == getIndexFromLinearIndex(0, Size(8, 5), false)).all()); - EXPECT_TRUE((Index(1, 0) == getIndexFromLinearIndex(1, Size(8, 5), false)).all()); - EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(1, Size(8, 5), true)).all()); - EXPECT_TRUE((Index(2, 0) == getIndexFromLinearIndex(2, Size(8, 5), false)).all()); - EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(8, Size(8, 5), false)).all()); - EXPECT_TRUE((Index(7, 4) == getIndexFromLinearIndex(39, Size(8, 5), false)).all()); + EXPECT_TRUE( + (grid_map::Index( + 0, + 0) == grid_map::getIndexFromLinearIndex(0, grid_map::Size(8, 5), false)).all()); + EXPECT_TRUE( + (grid_map::Index( + 1, + 0) == grid_map::getIndexFromLinearIndex(1, grid_map::Size(8, 5), false)).all()); + EXPECT_TRUE( + (grid_map::Index( + 0, + 1) == grid_map::getIndexFromLinearIndex(1, grid_map::Size(8, 5), true)).all()); + EXPECT_TRUE( + (grid_map::Index( + 2, + 0) == grid_map::getIndexFromLinearIndex(2, grid_map::Size(8, 5), false)).all()); + EXPECT_TRUE( + (grid_map::Index( + 0, + 1) == grid_map::getIndexFromLinearIndex(8, grid_map::Size(8, 5), false)).all()); + EXPECT_TRUE( + (grid_map::Index( + 7, + 4) == grid_map::getIndexFromLinearIndex(39, grid_map::Size(8, 5), false)).all()); } diff --git a/grid_map_core/test/GridMapTest.cpp b/grid_map_core/test/GridMapTest.cpp index 0acf5e08b..141623805 100644 --- a/grid_map_core/test/GridMapTest.cpp +++ b/grid_map_core/test/GridMapTest.cpp @@ -6,24 +6,25 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/GridMap.hpp" - // gtest #include // Math #include -using namespace std; -using namespace grid_map; +#include +#include + +#include "grid_map_core/GridMap.hpp" + TEST(GridMap, CopyConstructor) { - GridMap map({"layer_a", "layer_b"}); - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2)); + grid_map::GridMap map({"layer_a", "layer_b"}); + map.setGeometry(grid_map::Length(1.0, 2.0), 0.1, grid_map::Position(0.1, 0.2)); map["layer_a"].setConstant(1.0); map["layer_b"].setConstant(2.0); - GridMap mapCopy(map); + grid_map::GridMap mapCopy(map); EXPECT_EQ(map.getSize()[0], mapCopy.getSize()[0]); EXPECT_EQ(map.getSize()[1], mapCopy.getSize()[1]); EXPECT_EQ(map.getLength().x(), mapCopy.getLength().x()); @@ -37,11 +38,11 @@ TEST(GridMap, CopyConstructor) TEST(GridMap, CopyAssign) { - GridMap map({"layer_a", "layer_b"}); - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2)); + grid_map::GridMap map({"layer_a", "layer_b"}); + map.setGeometry(grid_map::Length(1.0, 2.0), 0.1, grid_map::Position(0.1, 0.2)); map["layer_a"].setConstant(1.0); map["layer_b"].setConstant(2.0); - GridMap mapCopy; + grid_map::GridMap mapCopy; mapCopy = map; EXPECT_EQ(map.getSize()[0], mapCopy.getSize()[0]); EXPECT_EQ(map.getSize()[1], mapCopy.getSize()[1]); @@ -56,24 +57,25 @@ TEST(GridMap, CopyAssign) TEST(GridMap, Move) { - GridMap map; - map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + grid_map::GridMap map; + // bufferSize(8, 5) + map.setGeometry(grid_map::Length(8.1, 5.1), 1.0, grid_map::Position(0.0, 0.0)); map.add("layer", 0.0); map.setBasicLayers(map.getLayers()); - std::vector regions; - map.move(Position(-3.0, -2.0), regions); - Index startIndex = map.getStartIndex(); + std::vector regions; + map.move(grid_map::Position(-3.0, -2.0), regions); + grid_map::Index startIndex = map.getStartIndex(); EXPECT_EQ(3, startIndex(0)); EXPECT_EQ(2, startIndex(1)); - EXPECT_FALSE(map.isValid(Index(0, 0))); // TODO Check entire map. - EXPECT_TRUE(map.isValid(Index(3, 2))); - EXPECT_FALSE(map.isValid(Index(2, 2))); - EXPECT_FALSE(map.isValid(Index(3, 1))); - EXPECT_TRUE(map.isValid(Index(7, 4))); + EXPECT_FALSE(map.isValid(grid_map::Index(0, 0))); // TODO(needs_assignment): Check entire map. + EXPECT_TRUE(map.isValid(grid_map::Index(3, 2))); + EXPECT_FALSE(map.isValid(grid_map::Index(2, 2))); + EXPECT_FALSE(map.isValid(grid_map::Index(3, 1))); + EXPECT_TRUE(map.isValid(grid_map::Index(7, 4))); - EXPECT_EQ(2, regions.size()); + EXPECT_EQ(2u, regions.size()); EXPECT_EQ(0, regions[0].getStartIndex()[0]); EXPECT_EQ(0, regions[0].getStartIndex()[1]); EXPECT_EQ(3, regions[0].getSize()[0]); @@ -87,13 +89,13 @@ TEST(GridMap, Move) TEST(GridMap, Transform) { // Initial map. - GridMap map; + grid_map::GridMap map; const auto heightLayerName = "height"; - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0)); + map.setGeometry(grid_map::Length(1.0, 2.0), 0.1, grid_map::Position(0.0, 0.0)); map.add(heightLayerName, 0.0); map.setBasicLayers(map.getLayers()); - map.get(heightLayerName)(0,0) = 1.0; + map.get(heightLayerName)(0, 0) = 1.0; // Transformation (90° rotation). Eigen::Isometry3d transform; @@ -102,43 +104,47 @@ TEST(GridMap, Transform) transform.translation().y() = 0.0; transform.translation().z() = 0.0; - transform.linear()(0,0) = 0.0; - transform.linear()(0,1) = -1.0; - transform.linear()(0,2) = 0.0; + transform.linear()(0, 0) = 0.0; + transform.linear()(0, 1) = -1.0; + transform.linear()(0, 2) = 0.0; - transform.linear()(1,0) = 1.0; - transform.linear()(1,1) = 0.0; - transform.linear()(1,2) = 0.0; + transform.linear()(1, 0) = 1.0; + transform.linear()(1, 1) = 0.0; + transform.linear()(1, 2) = 0.0; - transform.linear()(2,0) = 0.0; - transform.linear()(2,1) = 0.0; - transform.linear()(2,2) = 1.0; + transform.linear()(2, 0) = 0.0; + transform.linear()(2, 1) = 0.0; + transform.linear()(2, 2) = 1.0; // Apply affine transformation. - const GridMap transformedMap = map.getTransformedMap(transform, heightLayerName, map.getFrameId(), 0.25); + const grid_map::GridMap transformedMap = map.getTransformedMap( + transform, heightLayerName, + map.getFrameId(), 0.25); // Check if map has been rotated by 90° about z EXPECT_NEAR(map.getLength().x(), transformedMap.getLength().y(), 1e-6); EXPECT_NEAR(map.getLength().y(), transformedMap.getLength().x(), 1e-6); EXPECT_EQ(map.get(heightLayerName).size(), transformedMap.get(heightLayerName).size()); - EXPECT_DOUBLE_EQ(map.get(heightLayerName)(0,0), transformedMap.get(heightLayerName)(19,0)); + EXPECT_DOUBLE_EQ(map.get(heightLayerName)(0, 0), transformedMap.get(heightLayerName)(19, 0)); } TEST(GridMap, ClipToMap) { - GridMap map({"layer_a", "layer_b"}); - map.setGeometry(Length(1.0, 1.0), 0.1, Position(0.5, 0.5)); + grid_map::GridMap map({"layer_a", "layer_b"}); + map.setGeometry(grid_map::Length(1.0, 1.0), 0.1, grid_map::Position(0.5, 0.5)); map["layer_a"].setConstant(1.0); map["layer_b"].setConstant(2.0); - const Position positionInMap = Position(0.4, 0.3); // position located inside the map - const Position positionOutMap = Position(1.0, 2.0); // position located outside the map + // position located outside the map + const grid_map::Position positionInMap = grid_map::Position(0.4, 0.3); + // position located outside the map + const grid_map::Position positionOutMap = grid_map::Position(1.0, 2.0); - const Position clippedPositionInMap = map.getClosestPositionInMap(positionInMap); - const Position clippedPositionOutMap = map.getClosestPositionInMap(positionOutMap); + const grid_map::Position clippedPositionInMap = map.getClosestPositionInMap(positionInMap); + const grid_map::Position clippedPositionOutMap = map.getClosestPositionInMap(positionOutMap); // Check if position-in-map remains unchanged. - EXPECT_NEAR(clippedPositionInMap.x(),positionInMap.x(), 1e-6); + EXPECT_NEAR(clippedPositionInMap.x(), positionInMap.x(), 1e-6); EXPECT_NEAR(clippedPositionInMap.y(), positionInMap.y(), 1e-6); // Check if position-out-map is indeed outside of the map. @@ -150,13 +156,14 @@ TEST(GridMap, ClipToMap) TEST(AddDataFrom, ExtendMapAligned) { - GridMap map1, map2; - map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(5, 5) + grid_map::GridMap map1, map2; + // bufferSize(5, 5) + map1.setGeometry(grid_map::Length(5.1, 5.1), 1.0, grid_map::Position(0.0, 0.0)); map1.add("zero", 0.0); map1.add("one", 1.0); map1.setBasicLayers(map1.getLayers()); - map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0)); + map2.setGeometry(grid_map::Length(3.1, 3.1), 1.0, grid_map::Position(2.0, 2.0)); map2.add("one", 1.1); map2.add("two", 2.0); map2.setBasicLayers(map1.getLayers()); @@ -164,26 +171,27 @@ TEST(AddDataFrom, ExtendMapAligned) map1.addDataFrom(map2, true, true, true); EXPECT_TRUE(map1.exists("two")); - EXPECT_TRUE(map1.isInside(Position(3.0, 3.0))); + EXPECT_TRUE(map1.isInside(grid_map::Position(3.0, 3.0))); EXPECT_DOUBLE_EQ(6.0, map1.getLength().x()); EXPECT_DOUBLE_EQ(6.0, map1.getLength().y()); EXPECT_DOUBLE_EQ(0.5, map1.getPosition().x()); EXPECT_DOUBLE_EQ(0.5, map1.getPosition().y()); - EXPECT_NEAR(1.1, map1.atPosition("one", Position(2, 2)), 1e-4); - EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(-2, -2))); - EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", Position(0.0, 0.0))); + EXPECT_NEAR(1.1, map1.atPosition("one", grid_map::Position(2, 2)), 1e-4); + EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", grid_map::Position(-2, -2))); + EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", grid_map::Position(0.0, 0.0))); } TEST(AddDataFrom, ExtendMapNotAligned) { - GridMap map1, map2; - map1.setGeometry(Length(6.1, 6.1), 1.0, Position(0.0, 0.0)); // bufferSize(6, 6) + grid_map::GridMap map1, map2; + // bufferSize(6, 6) + map1.setGeometry(grid_map::Length(6.1, 6.1), 1.0, grid_map::Position(0.0, 0.0)); map1.add("nan"); map1.add("one", 1.0); map1.add("zero", 0.0); map1.setBasicLayers(map1.getLayers()); - map2.setGeometry(Length(3.1, 3.1), 1.0, Position(3.2, 3.2)); + map2.setGeometry(grid_map::Length(3.1, 3.1), 1.0, grid_map::Position(3.2, 3.2)); map2.add("nan", 1.0); map2.add("one", 1.1); map2.add("two", 2.0); @@ -192,96 +200,109 @@ TEST(AddDataFrom, ExtendMapNotAligned) std::vector stringVector; stringVector.push_back("nan"); map1.addDataFrom(map2, true, false, false, stringVector); - Index index; - map1.getIndex(Position(-2, -2), index); + grid_map::Index index; + map1.getIndex(grid_map::Position(-2, -2), index); EXPECT_FALSE(map1.exists("two")); - EXPECT_TRUE(map1.isInside(Position(4.0, 4.0))); + EXPECT_TRUE(map1.isInside(grid_map::Position(4.0, 4.0))); EXPECT_DOUBLE_EQ(8.0, map1.getLength().x()); EXPECT_DOUBLE_EQ(8.0, map1.getLength().y()); EXPECT_DOUBLE_EQ(1.0, map1.getPosition().x()); EXPECT_DOUBLE_EQ(1.0, map1.getPosition().y()); EXPECT_FALSE(map1.isValid(index, "nan")); - EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(0.0, 0.0))); - EXPECT_DOUBLE_EQ(1.0, map1.atPosition("nan", Position(3.0, 3.0))); + EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", grid_map::Position(0.0, 0.0))); + EXPECT_DOUBLE_EQ(1.0, map1.atPosition("nan", grid_map::Position(3.0, 3.0))); } TEST(AddDataFrom, CopyData) { - GridMap map1, map2; - map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(5, 5) + grid_map::GridMap map1, map2; + // bufferSize(5, 5) + map1.setGeometry(grid_map::Length(5.1, 5.1), 1.0, grid_map::Position(0.0, 0.0)); map1.add("zero", 0.0); map1.add("one"); map1.setBasicLayers(map1.getLayers()); - map2.setGeometry(Length(3.1, 3.1), 1.0, Position(2.0, 2.0)); + map2.setGeometry(grid_map::Length(3.1, 3.1), 1.0, grid_map::Position(2.0, 2.0)); map2.add("one", 1.0); map2.add("two", 2.0); map2.setBasicLayers(map1.getLayers()); map1.addDataFrom(map2, false, false, true); - Index index; - map1.getIndex(Position(-2, -2), index); + grid_map::Index index; + map1.getIndex(grid_map::Position(-2, -2), index); EXPECT_TRUE(map1.exists("two")); - EXPECT_FALSE(map1.isInside(Position(3.0, 3.0))); + EXPECT_FALSE(map1.isInside(grid_map::Position(3.0, 3.0))); EXPECT_DOUBLE_EQ(5.0, map1.getLength().x()); EXPECT_DOUBLE_EQ(5.0, map1.getLength().y()); EXPECT_DOUBLE_EQ(0.0, map1.getPosition().x()); EXPECT_DOUBLE_EQ(0.0, map1.getPosition().y()); - EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", Position(2, 2))); + EXPECT_DOUBLE_EQ(1.0, map1.atPosition("one", grid_map::Position(2, 2))); EXPECT_FALSE(map1.isValid(index, "one")); - EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", Position(0.0, 0.0))); + EXPECT_DOUBLE_EQ(0.0, map1.atPosition("zero", grid_map::Position(0.0, 0.0))); } TEST(ValueAtPosition, NearestNeighbor) { - GridMap map( { "types" }); - map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0)); - - map.at("types", Index(0,0)) = 0.5; - map.at("types", Index(0,1)) = 3.8; - map.at("types", Index(0,2)) = 2.0; - map.at("types", Index(1,0)) = 2.1; - map.at("types", Index(1,1)) = 1.0; - map.at("types", Index(1,2)) = 2.0; - map.at("types", Index(2,0)) = 1.0; - map.at("types", Index(2,1)) = 2.0; - map.at("types", Index(2,2)) = 2.0; + grid_map::GridMap map({"types"}); + map.setGeometry(grid_map::Length(3.0, 3.0), 1.0, grid_map::Position(0.0, 0.0)); + + map.at("types", grid_map::Index(0, 0)) = 0.5; + map.at("types", grid_map::Index(0, 1)) = 3.8; + map.at("types", grid_map::Index(0, 2)) = 2.0; + map.at("types", grid_map::Index(1, 0)) = 2.1; + map.at("types", grid_map::Index(1, 1)) = 1.0; + map.at("types", grid_map::Index(1, 2)) = 2.0; + map.at("types", grid_map::Index(2, 0)) = 1.0; + map.at("types", grid_map::Index(2, 1)) = 2.0; + map.at("types", grid_map::Index(2, 2)) = 2.0; double value; - value = map.atPosition("types", Position(1.35,-0.4)); + value = map.atPosition("types", grid_map::Position(1.35, -0.4)); EXPECT_DOUBLE_EQ((float)3.8, value); - value = map.atPosition("types", Position(-0.3,0.0)); + value = map.atPosition("types", grid_map::Position(-0.3, 0.0)); EXPECT_DOUBLE_EQ(1.0, value); } TEST(ValueAtPosition, LinearInterpolated) { - GridMap map( { "types" }); - map.setGeometry(Length(3.0, 3.0), 1.0, Position(0.0, 0.0)); - - map.at("types", Index(0,0)) = 0.5; - map.at("types", Index(0,1)) = 3.8; - map.at("types", Index(0,2)) = 2.0; - map.at("types", Index(1,0)) = 2.1; - map.at("types", Index(1,1)) = 1.0; - map.at("types", Index(1,2)) = 2.0; - map.at("types", Index(2,0)) = 1.0; - map.at("types", Index(2,1)) = 2.0; - map.at("types", Index(2,2)) = 2.0; + grid_map::GridMap map({"types"}); + map.setGeometry(grid_map::Length(3.0, 3.0), 1.0, grid_map::Position(0.0, 0.0)); + + map.at("types", grid_map::Index(0, 0)) = 0.5; + map.at("types", grid_map::Index(0, 1)) = 3.8; + map.at("types", grid_map::Index(0, 2)) = 2.0; + map.at("types", grid_map::Index(1, 0)) = 2.1; + map.at("types", grid_map::Index(1, 1)) = 1.0; + map.at("types", grid_map::Index(1, 2)) = 2.0; + map.at("types", grid_map::Index(2, 0)) = 1.0; + map.at("types", grid_map::Index(2, 1)) = 2.0; + map.at("types", grid_map::Index(2, 2)) = 2.0; double value; // Close to the border -> reverting to INTER_NEAREST. - value = map.atPosition("types", Position(-0.5,-1.2), InterpolationMethods::INTER_LINEAR); + value = map.atPosition( + "types", grid_map::Position( + -0.5, + -1.2), + grid_map::InterpolationMethods::INTER_LINEAR); EXPECT_DOUBLE_EQ(2.0, value); // In between 1.0 and 2.0 field. - value = map.atPosition("types", Position(-0.5,0.0), InterpolationMethods::INTER_LINEAR); + value = map.atPosition( + "types", grid_map::Position( + -0.5, + 0.0), + grid_map::InterpolationMethods::INTER_LINEAR); EXPECT_DOUBLE_EQ(1.5, value); // Calculated "by Hand". - value = map.atPosition("types", Position(0.69,0.38), InterpolationMethods::INTER_LINEAR); + value = map.atPosition( + "types", grid_map::Position( + 0.69, + 0.38), + grid_map::InterpolationMethods::INTER_LINEAR); EXPECT_NEAR(2.1963200, value, 0.0000001); } diff --git a/grid_map_core/test/LineIteratorTest.cpp b/grid_map_core/test/LineIteratorTest.cpp index 664267bef..1bb969a6e 100644 --- a/grid_map_core/test/LineIteratorTest.cpp +++ b/grid_map_core/test/LineIteratorTest.cpp @@ -6,24 +6,26 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/iterators/LineIterator.hpp" -#include "grid_map_core/GridMap.hpp" - // gtest #include // Limits #include -using namespace grid_map; +#include "grid_map_core/iterators/LineIterator.hpp" +#include "grid_map_core/GridMap.hpp" + TEST(LineIterator, StartOutsideMap) { - GridMap map( { "types" }); - map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); + grid_map::GridMap map({"types"}); + map.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); - EXPECT_NO_THROW(LineIterator iterator(map, Position(2.0, 2.0), Position(0.0, 0.0))); - LineIterator iterator(map, Position(2.0, 2.0), Position(0.0, 0.0)); + EXPECT_NO_THROW( + grid_map::LineIterator iterator( + map, grid_map::Position(2.0, 2.0), + grid_map::Position(0.0, 0.0))); + grid_map::LineIterator iterator(map, grid_map::Position(2.0, 2.0), grid_map::Position(0.0, 0.0)); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(2, (*iterator)(0)); @@ -45,11 +47,14 @@ TEST(LineIterator, StartOutsideMap) TEST(LineIterator, EndOutsideMap) { - GridMap map( { "types" }); - map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); + grid_map::GridMap map({"types"}); + map.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); - EXPECT_NO_THROW(LineIterator iterator(map, Position(0.0, 0.0), Position(9.0, 6.0))); - LineIterator iterator(map, Position(0.0, 0.0), Position(9.0, 6.0)); + EXPECT_NO_THROW( + grid_map::LineIterator iterator( + map, grid_map::Position(0.0, 0.0), + grid_map::Position(9.0, 6.0))); + grid_map::LineIterator iterator(map, grid_map::Position(0.0, 0.0), grid_map::Position(9.0, 6.0)); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(4, (*iterator)(0)); @@ -75,11 +80,15 @@ TEST(LineIterator, EndOutsideMap) TEST(LineIterator, StartAndEndOutsideMap) { - GridMap map( { "types" }); - map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); + grid_map::GridMap map({"types"}); + map.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); - EXPECT_NO_THROW(LineIterator iterator(map, Position(-7.0, -9.0), Position(8.0, 8.0))); - LineIterator iterator(map, Position(-7.0, -9.0), Position(8.0, 8.0)); + EXPECT_NO_THROW( + grid_map::LineIterator iterator( + map, grid_map::Position(-7.0, -9.0), + grid_map::Position(8.0, 8.0))); + grid_map::LineIterator iterator(map, grid_map::Position(-7.0, -9.0), + grid_map::Position(8.0, 8.0)); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(5, (*iterator)(0)); @@ -103,21 +112,29 @@ TEST(LineIterator, StartAndEndOutsideMap) TEST(LineIterator, StartAndEndOutsideMapWithoutIntersectingMap) { - GridMap map( { "types" }); - map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); - - EXPECT_THROW(LineIterator iterator(map, Position(-8.0, 8.0), Position(8.0, 8.0)), std::invalid_argument); + grid_map::GridMap map({"types"}); + map.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); + + EXPECT_THROW( + grid_map::LineIterator iterator( + map, grid_map::Position(-8.0, 8.0), grid_map::Position( + 8.0, + 8.0)), + std::invalid_argument); } TEST(LineIterator, MovedMap) { - GridMap map( { "types" }); - map.setGeometry(Length(7.0, 5.0), 1.0, Position(0.0, 0.0)); - map.move(Position(2.0, 2.0)); + grid_map::GridMap map({"types"}); + map.setGeometry(grid_map::Length(7.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); + map.move(grid_map::Position(2.0, 2.0)); - EXPECT_NO_THROW(LineIterator iterator(map, Position(0.0, 0.0), Position(2.0, 2.0))); - LineIterator iterator(map, Position(0.0, 0.0), Position(2.0, 2.0)); - Position point; + EXPECT_NO_THROW( + grid_map::LineIterator iterator( + map, grid_map::Position(0.0, 0.0), + grid_map::Position(2.0, 2.0))); + grid_map::LineIterator iterator(map, grid_map::Position(0.0, 0.0), grid_map::Position(2.0, 2.0)); + grid_map::Position point; EXPECT_FALSE(iterator.isPastEnd()); map.getPosition(*iterator, point); @@ -142,13 +159,16 @@ TEST(LineIterator, MovedMap) TEST(LineIterator, StartAndEndOutsideMovedMap) { - GridMap map( { "types" }); - map.setGeometry(Length(7.0, 5.0), 1.0, Position(0.0, 0.0)); - map.move(Position(2.0, 2.0)); - - EXPECT_NO_THROW(LineIterator iterator(map, Position(0.0, 0.0), Position(8.0, 8.0))); - LineIterator iterator(map, Position(0.0, 0.0), Position(8.0, 8.0)); - Position point; + grid_map::GridMap map({"types"}); + map.setGeometry(grid_map::Length(7.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); + map.move(grid_map::Position(2.0, 2.0)); + + EXPECT_NO_THROW( + grid_map::LineIterator iterator( + map, grid_map::Position(0.0, 0.0), + grid_map::Position(8.0, 8.0))); + grid_map::LineIterator iterator(map, grid_map::Position(0.0, 0.0), grid_map::Position(8.0, 8.0)); + grid_map::Position point; EXPECT_FALSE(iterator.isPastEnd()); map.getPosition(*iterator, point); diff --git a/grid_map_core/test/PolygonIteratorTest.cpp b/grid_map_core/test/PolygonIteratorTest.cpp index 4bbc5d6c2..4a5116377 100644 --- a/grid_map_core/test/PolygonIteratorTest.cpp +++ b/grid_map_core/test/PolygonIteratorTest.cpp @@ -6,40 +6,40 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/iterators/PolygonIterator.hpp" -#include "grid_map_core/GridMap.hpp" -#include "grid_map_core/Polygon.hpp" +// gtest +#include // Eigen #include -// gtest -#include - // Limits #include // Vector #include -using namespace std; -using namespace Eigen; -using namespace grid_map; +#include + +#include "grid_map_core/iterators/PolygonIterator.hpp" +#include "grid_map_core/GridMap.hpp" +#include "grid_map_core/Polygon.hpp" + TEST(PolygonIterator, FullCover) { - vector types; + std::vector types; types.push_back("type"); - GridMap map(types); - map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + grid_map::GridMap map(types); + // bufferSize(8, 5) + map.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); - Polygon polygon; - polygon.addVertex(Position(-100.0, 100.0)); - polygon.addVertex(Position(100.0, 100.0)); - polygon.addVertex(Position(100.0, -100.0)); - polygon.addVertex(Position(-100.0, -100.0)); + grid_map::Polygon polygon; + polygon.addVertex(grid_map::Position(-100.0, 100.0)); + polygon.addVertex(grid_map::Position(100.0, 100.0)); + polygon.addVertex(grid_map::Position(100.0, -100.0)); + polygon.addVertex(grid_map::Position(-100.0, -100.0)); - PolygonIterator iterator(map, polygon); + grid_map::PolygonIterator iterator(map, polygon); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(0, (*iterator)(0)); @@ -55,7 +55,9 @@ TEST(PolygonIterator, FullCover) EXPECT_EQ(0, (*iterator)(0)); EXPECT_EQ(2, (*iterator)(1)); - for (int i = 0; i < 37; ++i) ++iterator; + for (int i = 0; i < 37; ++i) { + ++iterator; + } EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(7, (*iterator)(0)); @@ -67,32 +69,34 @@ TEST(PolygonIterator, FullCover) TEST(PolygonIterator, Outside) { - GridMap map({"types"}); - map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + grid_map::GridMap map({"types"}); + // bufferSize(8, 5) + map.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); - Polygon polygon; - polygon.addVertex(Position(99.0, 101.0)); - polygon.addVertex(Position(101.0, 101.0)); - polygon.addVertex(Position(101.0, 99.0)); - polygon.addVertex(Position(99.0, 99.0)); + grid_map::Polygon polygon; + polygon.addVertex(grid_map::Position(99.0, 101.0)); + polygon.addVertex(grid_map::Position(101.0, 101.0)); + polygon.addVertex(grid_map::Position(101.0, 99.0)); + polygon.addVertex(grid_map::Position(99.0, 99.0)); - PolygonIterator iterator(map, polygon); + grid_map::PolygonIterator iterator(map, polygon); EXPECT_TRUE(iterator.isPastEnd()); } TEST(PolygonIterator, Square) { - GridMap map({"types"}); - map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + grid_map::GridMap map({"types"}); + // bufferSize(8, 5) + map.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); - Polygon polygon; - polygon.addVertex(Position(-1.0, 1.5)); - polygon.addVertex(Position(1.0, 1.5)); - polygon.addVertex(Position(1.0, -1.5)); - polygon.addVertex(Position(-1.0, -1.5)); + grid_map::Polygon polygon; + polygon.addVertex(grid_map::Position(-1.0, 1.5)); + polygon.addVertex(grid_map::Position(1.0, 1.5)); + polygon.addVertex(grid_map::Position(1.0, -1.5)); + polygon.addVertex(grid_map::Position(-1.0, -1.5)); - PolygonIterator iterator(map, polygon); + grid_map::PolygonIterator iterator(map, polygon); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(3, (*iterator)(0)); @@ -129,15 +133,16 @@ TEST(PolygonIterator, Square) TEST(PolygonIterator, TopLeftTriangle) { - GridMap map({"types"}); - map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + grid_map::GridMap map({"types"}); + // bufferSize(8, 5) + map.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); - Polygon polygon; - polygon.addVertex(Position(-40.1, 20.6)); - polygon.addVertex(Position(40.1, 20.4)); - polygon.addVertex(Position(-40.1, -20.6)); + grid_map::Polygon polygon; + polygon.addVertex(grid_map::Position(-40.1, 20.6)); + polygon.addVertex(grid_map::Position(40.1, 20.4)); + polygon.addVertex(grid_map::Position(-40.1, -20.6)); - PolygonIterator iterator(map, polygon); + grid_map::PolygonIterator iterator(map, polygon); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(0, (*iterator)(0)); @@ -148,21 +153,22 @@ TEST(PolygonIterator, TopLeftTriangle) EXPECT_EQ(1, (*iterator)(0)); EXPECT_EQ(0, (*iterator)(1)); - // TODO Extend. + // TODO(needs_assignment): Extend. } TEST(PolygonIterator, MoveMap) { - GridMap map({"layer"}); - map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) - map.move(Position(2.0, 0.0)); + grid_map::GridMap map({"layer"}); + // bufferSize(8, 5) + map.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); + map.move(grid_map::Position(2.0, 0.0)); - Polygon polygon; - polygon.addVertex(Position(6.1, 1.6)); - polygon.addVertex(Position(0.9, 1.6)); - polygon.addVertex(Position(0.9, -1.6)); - polygon.addVertex(Position(6.1, -1.6)); - PolygonIterator iterator(map, polygon); + grid_map::Polygon polygon; + polygon.addVertex(grid_map::Position(6.1, 1.6)); + polygon.addVertex(grid_map::Position(0.9, 1.6)); + polygon.addVertex(grid_map::Position(0.9, -1.6)); + polygon.addVertex(grid_map::Position(6.1, -1.6)); + grid_map::PolygonIterator iterator(map, polygon); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(6, (*iterator)(0)); @@ -173,7 +179,9 @@ TEST(PolygonIterator, MoveMap) EXPECT_EQ(6, (*iterator)(0)); EXPECT_EQ(2, (*iterator)(1)); - for (int i = 0; i < 4; ++i) ++iterator; + for (int i = 0; i < 4; ++i) { + ++iterator; + } EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(7, (*iterator)(0)); @@ -184,7 +192,9 @@ TEST(PolygonIterator, MoveMap) EXPECT_EQ(0, (*iterator)(0)); EXPECT_EQ(1, (*iterator)(1)); - for (int i = 0; i < 8; ++i) ++iterator; + for (int i = 0; i < 8; ++i) { + ++iterator; + } EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(2, (*iterator)(0)); diff --git a/grid_map_core/test/PolygonTest.cpp b/grid_map_core/test/PolygonTest.cpp index 5540377ca..a6cb13b69 100644 --- a/grid_map_core/test/PolygonTest.cpp +++ b/grid_map_core/test/PolygonTest.cpp @@ -6,8 +6,6 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/Polygon.hpp" - // gtest #include @@ -15,50 +13,51 @@ #include #include -using namespace std; -using namespace Eigen; -using namespace grid_map; +#include + +#include "grid_map_core/Polygon.hpp" + TEST(Polygon, getCentroidTriangle) { - Polygon triangle; - triangle.addVertex(Vector2d(0.0, 0.0)); - triangle.addVertex(Vector2d(1.0, 0.0)); - triangle.addVertex(Vector2d(0.5, 1.0)); + grid_map::Polygon triangle; + triangle.addVertex(Eigen::Vector2d(0.0, 0.0)); + triangle.addVertex(Eigen::Vector2d(1.0, 0.0)); + triangle.addVertex(Eigen::Vector2d(0.5, 1.0)); - Position expectedCentroid; + grid_map::Position expectedCentroid; expectedCentroid.x() = 1.0 / 3.0 * (1.0 + 0.5); expectedCentroid.y() = 1.0 / 3.0; - Position centroid = triangle.getCentroid(); + grid_map::Position centroid = triangle.getCentroid(); EXPECT_DOUBLE_EQ(expectedCentroid.x(), centroid.x()); EXPECT_DOUBLE_EQ(expectedCentroid.y(), centroid.y()); } TEST(Polygon, getCentroidRectangle) { - Polygon rectangle; - rectangle.addVertex(Vector2d(-2.0, -1.0)); - rectangle.addVertex(Vector2d(-2.0, 2.0)); - rectangle.addVertex(Vector2d(1.0, 2.0)); - rectangle.addVertex(Vector2d(1.0, -1.0)); - - Position expectedCentroid(-0.5, 0.5); - Position centroid = rectangle.getCentroid(); + grid_map::Polygon rectangle; + rectangle.addVertex(Eigen::Vector2d(-2.0, -1.0)); + rectangle.addVertex(Eigen::Vector2d(-2.0, 2.0)); + rectangle.addVertex(Eigen::Vector2d(1.0, 2.0)); + rectangle.addVertex(Eigen::Vector2d(1.0, -1.0)); + + grid_map::Position expectedCentroid(-0.5, 0.5); + grid_map::Position centroid = rectangle.getCentroid(); EXPECT_DOUBLE_EQ(expectedCentroid.x(), centroid.x()); EXPECT_DOUBLE_EQ(expectedCentroid.y(), centroid.y()); } TEST(Polygon, getBoundingBox) { - Polygon triangle; - triangle.addVertex(Vector2d(0.0, 0.0)); - triangle.addVertex(Vector2d(0.5, -1.2)); - triangle.addVertex(Vector2d(1.0, 0.0)); - - Position expectedCenter(0.5, -0.6); - Length expectedLength(1.0, 1.2); - Position center; - Length length; + grid_map::Polygon triangle; + triangle.addVertex(Eigen::Vector2d(0.0, 0.0)); + triangle.addVertex(Eigen::Vector2d(0.5, -1.2)); + triangle.addVertex(Eigen::Vector2d(1.0, 0.0)); + + grid_map::Position expectedCenter(0.5, -0.6); + grid_map::Length expectedLength(1.0, 1.2); + grid_map::Position center; + grid_map::Length length; triangle.getBoundingBox(center, length); EXPECT_DOUBLE_EQ(expectedCenter.x(), center.x()); @@ -69,165 +68,169 @@ TEST(Polygon, getBoundingBox) TEST(Polygon, convexHullPoints) { - // Test that points which already create a convex shape (square) can be used to create a convex polygon. - std::vector points1; - points1.push_back(Vector2d(0.0, 0.0)); - points1.push_back(Vector2d(1.0, 0.0)); - points1.push_back(Vector2d(1.0, 1.0)); - points1.push_back(Vector2d(0.0, 1.0)); - Polygon polygon1 = Polygon::monotoneChainConvexHullOfPoints(points1); - EXPECT_EQ(4, polygon1.nVertices()); - EXPECT_TRUE(polygon1.isInside(Vector2d(0.5, 0.5))); - EXPECT_FALSE(polygon1.isInside(Vector2d(-0.01, 0.5))); + // Test that points which already create a convex shape (square) + // can be used to create a convex polygon. + std::vector points1; + points1.push_back(Eigen::Vector2d(0.0, 0.0)); + points1.push_back(Eigen::Vector2d(1.0, 0.0)); + points1.push_back(Eigen::Vector2d(1.0, 1.0)); + points1.push_back(Eigen::Vector2d(0.0, 1.0)); + grid_map::Polygon polygon1 = grid_map::Polygon::monotoneChainConvexHullOfPoints(points1); + EXPECT_EQ(4u, polygon1.nVertices()); + EXPECT_TRUE(polygon1.isInside(Eigen::Vector2d(0.5, 0.5))); + EXPECT_FALSE(polygon1.isInside(Eigen::Vector2d(-0.01, 0.5))); // Test that a random set of points can be used to create a convex polygon. - std::vector points2; - points2.push_back(Vector2d(0.0, 0.0)); - points2.push_back(Vector2d(1.0, 0.0)); - points2.push_back(Vector2d(2.0, 1.0)); - points2.push_back(Vector2d(1.0, 2.0)); - points2.push_back(Vector2d(-1.0, 2.0)); - points2.push_back(Vector2d(-1.0, -2.0)); - points2.push_back(Vector2d(0.0, 1.0)); - points2.push_back(Vector2d(1.0, 1.0)); - Polygon polygon2 = Polygon::monotoneChainConvexHullOfPoints(points2); - EXPECT_EQ(4, polygon2.nVertices()); - EXPECT_TRUE(polygon2.isInside(Vector2d(0.5, 0.5))); - EXPECT_TRUE(polygon2.isInside(Vector2d(0.0, 1.0))); - EXPECT_TRUE(polygon2.isInside(Vector2d(-0.5, -0.5))); - EXPECT_FALSE(polygon2.isInside(Vector2d(2.0, 0.0))); - EXPECT_FALSE(polygon2.isInside(Vector2d(-0.5, -2))); - EXPECT_FALSE(polygon2.isInside(Vector2d(1.75, 1.75))); + std::vector points2; + points2.push_back(Eigen::Vector2d(0.0, 0.0)); + points2.push_back(Eigen::Vector2d(1.0, 0.0)); + points2.push_back(Eigen::Vector2d(2.0, 1.0)); + points2.push_back(Eigen::Vector2d(1.0, 2.0)); + points2.push_back(Eigen::Vector2d(-1.0, 2.0)); + points2.push_back(Eigen::Vector2d(-1.0, -2.0)); + points2.push_back(Eigen::Vector2d(0.0, 1.0)); + points2.push_back(Eigen::Vector2d(1.0, 1.0)); + grid_map::Polygon polygon2 = grid_map::Polygon::monotoneChainConvexHullOfPoints(points2); + EXPECT_EQ(4u, polygon2.nVertices()); + EXPECT_TRUE(polygon2.isInside(Eigen::Vector2d(0.5, 0.5))); + EXPECT_TRUE(polygon2.isInside(Eigen::Vector2d(0.0, 1.0))); + EXPECT_TRUE(polygon2.isInside(Eigen::Vector2d(-0.5, -0.5))); + EXPECT_FALSE(polygon2.isInside(Eigen::Vector2d(2.0, 0.0))); + EXPECT_FALSE(polygon2.isInside(Eigen::Vector2d(-0.5, -2))); + EXPECT_FALSE(polygon2.isInside(Eigen::Vector2d(1.75, 1.75))); } TEST(Polygon, convexHullPolygon) { - Polygon polygon1; - polygon1.addVertex(Vector2d(0.0, 0.0)); - polygon1.addVertex(Vector2d(1.0, 1.0)); - polygon1.addVertex(Vector2d(0.0, 1.0)); - polygon1.addVertex(Vector2d(1.0, 0.0)); - - Polygon polygon2; - polygon2.addVertex(Vector2d(0.5, 0.5)); - polygon2.addVertex(Vector2d(0.5, 1.5)); - polygon2.addVertex(Vector2d(1.5, 0.5)); - polygon2.addVertex(Vector2d(1.5, 1.5)); - - Polygon hull = Polygon::convexHull(polygon1, polygon2); - - EXPECT_EQ(6, hull.nVertices()); - EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.5))); - EXPECT_FALSE(hull.isInside(Vector2d(0.01, 1.49))); + grid_map::Polygon polygon1; + polygon1.addVertex(Eigen::Vector2d(0.0, 0.0)); + polygon1.addVertex(Eigen::Vector2d(1.0, 1.0)); + polygon1.addVertex(Eigen::Vector2d(0.0, 1.0)); + polygon1.addVertex(Eigen::Vector2d(1.0, 0.0)); + + grid_map::Polygon polygon2; + polygon2.addVertex(Eigen::Vector2d(0.5, 0.5)); + polygon2.addVertex(Eigen::Vector2d(0.5, 1.5)); + polygon2.addVertex(Eigen::Vector2d(1.5, 0.5)); + polygon2.addVertex(Eigen::Vector2d(1.5, 1.5)); + + grid_map::Polygon hull = grid_map::Polygon::convexHull(polygon1, polygon2); + + EXPECT_EQ(6u, hull.nVertices()); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.5, 0.5))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.01, 1.49))); } TEST(Polygon, convexHullCircles) { - Position center1(0.0, 0.0); - Position center2(1.0, 0.0); + grid_map::Position center1(0.0, 0.0); + grid_map::Position center2(1.0, 0.0); double radius = 0.5; const int nVertices = 15; - Polygon hull = Polygon::convexHullOfTwoCircles(center1, center2, radius); - EXPECT_EQ(20, hull.nVertices()); - EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0))); - EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.0))); - EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.4))); - EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.6))); - EXPECT_FALSE(hull.isInside(Vector2d(1.5, 0.2))); - - hull = Polygon::convexHullOfTwoCircles(center1, center2, radius, nVertices); - EXPECT_EQ(nVertices + 1, hull.nVertices()); - EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0))); - EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.0))); - EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.4))); - EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.6))); - EXPECT_FALSE(hull.isInside(Vector2d(1.5, 0.2))); - - hull = Polygon::convexHullOfTwoCircles(center1, center1, radius); - EXPECT_EQ(20, hull.nVertices()); - EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0))); - EXPECT_TRUE(hull.isInside(Vector2d(0.25, 0.0))); - EXPECT_TRUE(hull.isInside(Vector2d(0.0, 0.25))); - EXPECT_TRUE(hull.isInside(Vector2d(0.0, -0.25))); - EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.5))); - EXPECT_FALSE(hull.isInside(Vector2d(0.6, 0.0))); - EXPECT_FALSE(hull.isInside(Vector2d(-0.6, 0.0))); - EXPECT_FALSE(hull.isInside(Vector2d(0.0, 0.6))); - EXPECT_FALSE(hull.isInside(Vector2d(0.0, -0.6))); - - hull = Polygon::convexHullOfTwoCircles(center1, center1, radius, nVertices); - EXPECT_EQ(nVertices, hull.nVertices()); - EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0))); - EXPECT_TRUE(hull.isInside(Vector2d(0.25, 0.0))); - EXPECT_TRUE(hull.isInside(Vector2d(0.0, 0.25))); - EXPECT_TRUE(hull.isInside(Vector2d(0.0, -0.25))); - EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.5))); - EXPECT_FALSE(hull.isInside(Vector2d(0.6, 0.0))); - EXPECT_FALSE(hull.isInside(Vector2d(-0.6, 0.0))); - EXPECT_FALSE(hull.isInside(Vector2d(0.0, 0.6))); - EXPECT_FALSE(hull.isInside(Vector2d(0.0, -0.6))); + grid_map::Polygon hull = grid_map::Polygon::convexHullOfTwoCircles(center1, center2, radius); + EXPECT_EQ(20u, hull.nVertices()); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(-0.25, 0.0))); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.5, 0.0))); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.5, 0.4))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.5, 0.6))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(1.5, 0.2))); + + hull = grid_map::Polygon::convexHullOfTwoCircles(center1, center2, radius, nVertices); + EXPECT_EQ(static_cast(nVertices + 1), hull.nVertices()); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(-0.25, 0.0))); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.5, 0.0))); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.5, 0.4))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.5, 0.6))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(1.5, 0.2))); + + hull = grid_map::Polygon::convexHullOfTwoCircles(center1, center1, radius); + EXPECT_EQ(20u, hull.nVertices()); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(-0.25, 0.0))); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.25, 0.0))); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.0, 0.25))); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.0, -0.25))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.5, 0.5))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.6, 0.0))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(-0.6, 0.0))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.0, 0.6))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.0, -0.6))); + + hull = grid_map::Polygon::convexHullOfTwoCircles(center1, center1, radius, nVertices); + EXPECT_EQ(static_cast(nVertices), hull.nVertices()); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(-0.25, 0.0))); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.25, 0.0))); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.0, 0.25))); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.0, -0.25))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.5, 0.5))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.6, 0.0))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(-0.6, 0.0))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.0, 0.6))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.0, -0.6))); } TEST(Polygon, convexHullCircle) { - Position center(0.0, 0.0); + grid_map::Position center(0.0, 0.0); double radius = 0.5; const int nVertices = 15; - Polygon hull = Polygon::fromCircle(center, radius); + grid_map::Polygon hull = grid_map::Polygon::fromCircle(center, radius); - EXPECT_EQ(20, hull.nVertices()); - EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0))); - EXPECT_TRUE(hull.isInside(Vector2d(0.49, 0.0))); - EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.4))); - EXPECT_FALSE(hull.isInside(Vector2d(1.0, 0.0))); + EXPECT_EQ(20u, hull.nVertices()); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(-0.25, 0.0))); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.49, 0.0))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.5, 0.4))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(1.0, 0.0))); - hull = Polygon::fromCircle(center, radius, nVertices); - EXPECT_EQ(nVertices, hull.nVertices()); - EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0))); - EXPECT_TRUE(hull.isInside(Vector2d(0.49, 0.0))); - EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.4))); - EXPECT_FALSE(hull.isInside(Vector2d(1.0, 0.0))); + hull = grid_map::Polygon::fromCircle(center, radius, nVertices); + EXPECT_EQ(static_cast(nVertices), hull.nVertices()); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(-0.25, 0.0))); + EXPECT_TRUE(hull.isInside(Eigen::Vector2d(0.49, 0.0))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(0.5, 0.4))); + EXPECT_FALSE(hull.isInside(Eigen::Vector2d(1.0, 0.0))); } TEST(convertToInequalityConstraints, triangle1) { - Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.1, -1.1)}); - MatrixXd A; - VectorXd b; + grid_map::Polygon polygon({grid_map::Position(1.0, 1.0), grid_map::Position(0.0, 0.0), + grid_map::Position(1.1, -1.1)}); + Eigen::MatrixXd A; + Eigen::VectorXd b; ASSERT_TRUE(polygon.convertToInequalityConstraints(A, b)); EXPECT_NEAR(-1.3636, A(0, 0), 1e-4); - EXPECT_NEAR( 1.3636, A(0, 1), 1e-4); + EXPECT_NEAR(1.3636, A(0, 1), 1e-4); EXPECT_NEAR(-1.5000, A(1, 0), 1e-4); EXPECT_NEAR(-1.5000, A(1, 1), 1e-4); - EXPECT_NEAR( 2.8636, A(2, 0), 1e-4); - EXPECT_NEAR( 0.1364, A(2, 1), 1e-4); - EXPECT_NEAR( 0.0000, b(0), 1e-4); - EXPECT_NEAR( 0.0000, b(1), 1e-4); - EXPECT_NEAR( 3.0000, b(2), 1e-4); + EXPECT_NEAR(2.8636, A(2, 0), 1e-4); + EXPECT_NEAR(0.1364, A(2, 1), 1e-4); + EXPECT_NEAR(0.0000, b(0), 1e-4); + EXPECT_NEAR(0.0000, b(1), 1e-4); + EXPECT_NEAR(3.0000, b(2), 1e-4); } TEST(convertToInequalityConstraints, triangle2) { - Polygon polygon({Position(-1.0, 0.5), Position(-1.0, -0.5), Position(1.0, -0.5)}); - MatrixXd A; - VectorXd b; + grid_map::Polygon polygon({grid_map::Position(-1.0, 0.5), grid_map::Position(-1.0, -0.5), + grid_map::Position(1.0, -0.5)}); + Eigen::MatrixXd A; + Eigen::VectorXd b; ASSERT_TRUE(polygon.convertToInequalityConstraints(A, b)); EXPECT_NEAR(-1.5000, A(0, 0), 1e-4); - EXPECT_NEAR( 0.0000, A(0, 1), 1e-4); - EXPECT_NEAR( 0.0000, A(1, 0), 1e-4); + EXPECT_NEAR(0.0000, A(0, 1), 1e-4); + EXPECT_NEAR(0.0000, A(1, 0), 1e-4); EXPECT_NEAR(-3.0000, A(1, 1), 1e-4); - EXPECT_NEAR( 1.5000, A(2, 0), 1e-4); - EXPECT_NEAR( 3.0000, A(2, 1), 1e-4); - EXPECT_NEAR( 1.5000, b(0), 1e-4); - EXPECT_NEAR( 1.5000, b(1), 1e-4); - EXPECT_NEAR( 0.0000, b(2), 1e-4); + EXPECT_NEAR(1.5000, A(2, 0), 1e-4); + EXPECT_NEAR(3.0000, A(2, 1), 1e-4); + EXPECT_NEAR(1.5000, b(0), 1e-4); + EXPECT_NEAR(1.5000, b(1), 1e-4); + EXPECT_NEAR(0.0000, b(2), 1e-4); } TEST(offsetInward, triangle) { - Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.0, -1.0)}); + grid_map::Polygon polygon({grid_map::Position(1.0, 1.0), grid_map::Position(0.0, 0.0), + grid_map::Position(1.0, -1.0)}); polygon.offsetInward(0.1); EXPECT_NEAR(0.9, polygon.getVertex(0)(0), 1e-4); EXPECT_NEAR(0.758579, polygon.getVertex(0)(1), 1e-4); @@ -239,10 +242,11 @@ TEST(offsetInward, triangle) TEST(triangulation, triangle) { - Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.0, -1.0)}); - std::vector polygons; + grid_map::Polygon polygon({grid_map::Position(1.0, 1.0), grid_map::Position(0.0, 0.0), + grid_map::Position(1.0, -1.0)}); + std::vector polygons; polygons = polygon.triangulate(); - ASSERT_EQ(1, polygons.size()); + ASSERT_EQ(1u, polygons.size()); EXPECT_EQ(polygon.getVertex(0).x(), polygons[0].getVertex(0).x()); EXPECT_EQ(polygon.getVertex(0).y(), polygons[0].getVertex(0).y()); EXPECT_EQ(polygon.getVertex(1).x(), polygons[0].getVertex(1).x()); @@ -253,13 +257,13 @@ TEST(triangulation, triangle) TEST(triangulation, rectangle) { - Polygon rectangle; - rectangle.addVertex(Vector2d(-2.0, -1.0)); - rectangle.addVertex(Vector2d(-2.0, 2.0)); - rectangle.addVertex(Vector2d(1.0, 2.0)); - rectangle.addVertex(Vector2d(1.0, -1.0)); - std::vector polygons; + grid_map::Polygon rectangle; + rectangle.addVertex(Eigen::Vector2d(-2.0, -1.0)); + rectangle.addVertex(Eigen::Vector2d(-2.0, 2.0)); + rectangle.addVertex(Eigen::Vector2d(1.0, 2.0)); + rectangle.addVertex(Eigen::Vector2d(1.0, -1.0)); + std::vector polygons; polygons = rectangle.triangulate(); - ASSERT_EQ(2, polygons.size()); - // TODO Extend. + ASSERT_EQ(2u, polygons.size()); + // TODO(needs_assignment): Extend. } diff --git a/grid_map_core/test/SlidingWindowIteratorTest.cpp b/grid_map_core/test/SlidingWindowIteratorTest.cpp index b1ff23b01..658ba8994 100644 --- a/grid_map_core/test/SlidingWindowIteratorTest.cpp +++ b/grid_map_core/test/SlidingWindowIteratorTest.cpp @@ -6,25 +6,25 @@ * Institute: ETH Zurich */ -#include "grid_map_core/iterators/SlidingWindowIterator.hpp" -#include "grid_map_core/GridMap.hpp" - -#include #include #include +#include #include -using namespace std; -using namespace grid_map; +#include "grid_map_core/iterators/SlidingWindowIterator.hpp" +#include "grid_map_core/GridMap.hpp" + TEST(SlidingWindowIterator, WindowSize3Cutoff) { - GridMap map; - map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + grid_map::GridMap map; + // bufferSize(8, 5) + map.setGeometry(grid_map::Length(8.1, 5.1), 1.0, grid_map::Position(0.0, 0.0)); map.add("layer"); map["layer"].setRandom(); - SlidingWindowIterator iterator(map, "layer", SlidingWindowIterator::EdgeHandling::CROP, 3); + grid_map::SlidingWindowIterator iterator(map, "layer", + grid_map::SlidingWindowIterator::EdgeHandling::CROP, 3); EXPECT_EQ(iterator.getData().rows(), 2); EXPECT_EQ(iterator.getData().cols(), 2); EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 2, 2))); @@ -41,7 +41,7 @@ TEST(SlidingWindowIterator, WindowSize3Cutoff) for (; !iterator.isPastEnd(); ++iterator) { EXPECT_FALSE(iterator.isPastEnd()); - if ((*iterator == Index(3, 2)).all()) break; + if ((*iterator == grid_map::Index(3, 2)).all()) {break;} } EXPECT_EQ(iterator.getData().rows(), 3); @@ -50,7 +50,7 @@ TEST(SlidingWindowIterator, WindowSize3Cutoff) for (; !iterator.isPastEnd(); ++iterator) { EXPECT_FALSE(iterator.isPastEnd()); - if ((*iterator == Index(7, 4)).all()) break; + if ((*iterator == grid_map::Index(7, 4)).all()) {break;} } EXPECT_EQ(iterator.getData().rows(), 2); @@ -63,12 +63,14 @@ TEST(SlidingWindowIterator, WindowSize3Cutoff) TEST(SlidingWindowIterator, WindowSize5) { - GridMap map; - map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + grid_map::GridMap map; + // bufferSize(8, 5) + map.setGeometry(grid_map::Length(8.1, 5.1), 1.0, grid_map::Position(0.0, 0.0)); map.add("layer"); map["layer"].setRandom(); - SlidingWindowIterator iterator(map, "layer", SlidingWindowIterator::EdgeHandling::CROP, 5); + grid_map::SlidingWindowIterator iterator(map, "layer", + grid_map::SlidingWindowIterator::EdgeHandling::CROP, 5); EXPECT_EQ(iterator.getData().rows(), 3); EXPECT_EQ(iterator.getData().cols(), 3); EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 3, 3))); @@ -85,7 +87,7 @@ TEST(SlidingWindowIterator, WindowSize5) for (; !iterator.isPastEnd(); ++iterator) { EXPECT_FALSE(iterator.isPastEnd()); - if ((*iterator == Index(3, 2)).all()) break; + if ((*iterator == grid_map::Index(3, 2)).all()) {break;} } EXPECT_EQ(iterator.getData().rows(), 5); @@ -94,7 +96,7 @@ TEST(SlidingWindowIterator, WindowSize5) for (; !iterator.isPastEnd(); ++iterator) { EXPECT_FALSE(iterator.isPastEnd()); - if ((*iterator == Index(7, 4)).all()) break; + if ((*iterator == grid_map::Index(7, 4)).all()) {break;} } EXPECT_EQ(iterator.getData().rows(), 3); @@ -107,19 +109,21 @@ TEST(SlidingWindowIterator, WindowSize5) TEST(SlidingWindowIterator, WindowSize3Inside) { - GridMap map; - map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + grid_map::GridMap map; + // bufferSize(8, 5) + map.setGeometry(grid_map::Length(8.1, 5.1), 1.0, grid_map::Position(0.0, 0.0)); map.add("layer"); map["layer"].setRandom(); - SlidingWindowIterator iterator(map, "layer", SlidingWindowIterator::EdgeHandling::INSIDE, 3); + grid_map::SlidingWindowIterator iterator(map, "layer", + grid_map::SlidingWindowIterator::EdgeHandling::INSIDE, 3); EXPECT_EQ(iterator.getData().rows(), 3); EXPECT_EQ(iterator.getData().cols(), 3); EXPECT_TRUE(iterator.getData().isApprox(map["layer"].block(0, 0, 3, 3))); for (; !iterator.isPastEnd(); ++iterator) { EXPECT_FALSE(iterator.isPastEnd()); - if ((*iterator == Index(3, 2)).all()) break; + if ((*iterator == grid_map::Index(3, 2)).all()) {break;} } EXPECT_EQ(iterator.getData().rows(), 3); @@ -128,7 +132,7 @@ TEST(SlidingWindowIterator, WindowSize3Inside) for (; !iterator.isPastEnd(); ++iterator) { EXPECT_FALSE(iterator.isPastEnd()); - if ((*iterator == Index(6, 3)).all()) break; + if ((*iterator == grid_map::Index(6, 3)).all()) {break;} } EXPECT_EQ(iterator.getData().rows(), 3); diff --git a/grid_map_core/test/SpiralIteratorTest.cpp b/grid_map_core/test/SpiralIteratorTest.cpp index a43dd2a8b..2bf5d6215 100644 --- a/grid_map_core/test/SpiralIteratorTest.cpp +++ b/grid_map_core/test/SpiralIteratorTest.cpp @@ -6,9 +6,6 @@ * Institute: University of Hamburg, TAMS */ -#include "grid_map_core/iterators/SpiralIterator.hpp" -#include "grid_map_core/GridMap.hpp" - // Eigen #include @@ -21,20 +18,20 @@ // Vector #include -using namespace std; -using namespace Eigen; -using namespace grid_map; +#include "grid_map_core/iterators/SpiralIterator.hpp" +#include "grid_map_core/GridMap.hpp" + TEST(SpiralIterator, CenterOutOfMap) { - GridMap map( { "types" }); - map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); - Position center(8.0, 0.0); + grid_map::GridMap map({"types"}); + map.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(0.0, 0.0)); + grid_map::Position center(8.0, 0.0); double radius = 5.0; - SpiralIterator iterator(map, center, radius); + grid_map::SpiralIterator iterator(map, center, radius); - Position iterator_position; + grid_map::Position iterator_position; map.getPosition(*iterator, iterator_position); EXPECT_TRUE(map.isInside(iterator_position)); diff --git a/grid_map_core/test/SubmapIteratorTest.cpp b/grid_map_core/test/SubmapIteratorTest.cpp index 4141dec50..bfa594df8 100644 --- a/grid_map_core/test/SubmapIteratorTest.cpp +++ b/grid_map_core/test/SubmapIteratorTest.cpp @@ -6,9 +6,6 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/iterators/SubmapIterator.hpp" -#include "grid_map_core/GridMap.hpp" - // Eigen #include @@ -21,9 +18,11 @@ // Vector #include -using namespace std; -using namespace Eigen; -using namespace grid_map; +#include + +#include "grid_map_core/iterators/SubmapIterator.hpp" +#include "grid_map_core/GridMap.hpp" + TEST(SubmapIterator, Simple) { @@ -32,12 +31,12 @@ TEST(SubmapIterator, Simple) Eigen::Array2i index; Eigen::Array2i submapIndex; - vector types; + std::vector types; types.push_back("type"); - GridMap map(types); - map.setGeometry(Array2d(8.1, 5.1), 1.0, Vector2d(0.0, 0.0)); // bufferSize(8, 5) + grid_map::GridMap map(types); + map.setGeometry(Eigen::Array2d(8.1, 5.1), 1.0, Eigen::Vector2d(0.0, 0.0)); // bufferSize(8, 5) - SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize); + grid_map::SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(submapTopLeftIndex(0), (*iterator)(0)); @@ -95,13 +94,14 @@ TEST(SubmapIterator, CircularBuffer) Eigen::Array2i index; Eigen::Array2i submapIndex; - vector types; + std::vector types; types.push_back("type"); - GridMap map(types); - map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) - map.move(Position(-3.0, -2.0)); // bufferStartIndex(3, 2) + grid_map::GridMap map(types); + // bufferSize(8, 5) + map.setGeometry(grid_map::Length(8.1, 5.1), 1.0, grid_map::Position(0.0, 0.0)); + map.move(grid_map::Position(-3.0, -2.0)); // bufferStartIndex(3, 2) - SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize); + grid_map::SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize); EXPECT_FALSE(iterator.isPastEnd()); EXPECT_EQ(submapTopLeftIndex(0), (*iterator)(0)); diff --git a/grid_map_core/test/test_grid_map_core.cpp b/grid_map_core/test/test_grid_map_core.cpp index 862017d3b..5e08f2233 100644 --- a/grid_map_core/test/test_grid_map_core.cpp +++ b/grid_map_core/test/test_grid_map_core.cpp @@ -10,9 +10,9 @@ #include // Run all the tests that were declared with TEST() -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - srand((int)time(0)); + srand(static_cast(time(0))); return RUN_ALL_TESTS(); } diff --git a/grid_map_core/test/test_helpers.cpp b/grid_map_core/test/test_helpers.cpp index d00dcb49c..ad9352169 100644 --- a/grid_map_core/test/test_helpers.cpp +++ b/grid_map_core/test/test_helpers.cpp @@ -6,36 +6,37 @@ * Institute: ETH Zurich, Robotic Systems Lab */ +// gtest +#include + +#include + #include "test_helpers.hpp" #include "grid_map_core/GridMap.hpp" #include "grid_map_core/iterators/GridMapIterator.hpp" -// gtest -#include - -namespace grid_map_test { - +namespace grid_map_test +{ std::mt19937 rndGenerator; -AnalyticalFunctions createFlatWorld(grid_map::GridMap *map) +AnalyticalFunctions createFlatWorld(grid_map::GridMap * map) { - AnalyticalFunctions func; func.f_ = [](double x, double y) { - return 0.0; - }; + (void)(y); // y param unused, remove when used. + (void)(x); // x param unused, remove when used. + return 0.0; + }; fillGridMap(map, func); return func; - } -AnalyticalFunctions createRationalFunctionWorld(grid_map::GridMap *map) +AnalyticalFunctions createRationalFunctionWorld(grid_map::GridMap * map) { - AnalyticalFunctions func; std::uniform_real_distribution shift(-3.0, 3.0); @@ -44,48 +45,43 @@ AnalyticalFunctions createRationalFunctionWorld(grid_map::GridMap *map) const double y0 = shift(rndGenerator); const double s = scale(rndGenerator); - func.f_ = [x0, y0,s](double x, double y) { - return s / (1 + std::pow(x-x0, 2.0) + std::pow(y-y0, 2.0)); - }; + func.f_ = [x0, y0, s](double x, double y) { + return s / (1 + std::pow(x - x0, 2.0) + std::pow(y - y0, 2.0)); + }; fillGridMap(map, func); return func; - } -AnalyticalFunctions createSecondOrderPolyWorld(grid_map::GridMap *map) +AnalyticalFunctions createSecondOrderPolyWorld(grid_map::GridMap * map) { - AnalyticalFunctions func; - func.f_ = [](double x,double y) { - return (-x*x -y*y +2.0*x*y +x*x*y*y); - }; + func.f_ = [](double x, double y) { + return -x * x - y * y + 2.0 * x * y + x * x * y * y; + }; fillGridMap(map, func); return func; - } -AnalyticalFunctions createSaddleWorld(grid_map::GridMap *map) +AnalyticalFunctions createSaddleWorld(grid_map::GridMap * map) { AnalyticalFunctions func; - func.f_ = [](double x,double y) { - return (x*x-y*y); - }; + func.f_ = [](double x, double y) { + return x * x - y * y; + }; fillGridMap(map, func); return func; - } -AnalyticalFunctions createSineWorld(grid_map::GridMap *map) +AnalyticalFunctions createSineWorld(grid_map::GridMap * map) { - AnalyticalFunctions func; std::uniform_real_distribution Uw(0.1, 4.0); @@ -94,36 +90,34 @@ AnalyticalFunctions createSineWorld(grid_map::GridMap *map) const double w3 = Uw(rndGenerator); const double w4 = Uw(rndGenerator); - func.f_ = [w1,w2,w3,w4](double x,double y) { - return std::cos(w1*x) + std::sin(w2*y) + std::cos(w3*x) + std::sin(w4*y); - }; + func.f_ = [w1, w2, w3, w4](double x, double y) { + return std::cos(w1 * x) + std::sin(w2 * y) + std::cos(w3 * x) + std::sin(w4 * y); + }; fillGridMap(map, func); return func; - } -AnalyticalFunctions createTanhWorld(grid_map::GridMap *map) +AnalyticalFunctions createTanhWorld(grid_map::GridMap * map) { - AnalyticalFunctions func; std::uniform_real_distribution scaling(0.1, 2.0); const double s = scaling(rndGenerator); - func.f_ = [s](double x,double y) { - const double expZ = std::exp(2 *s* x); - return (expZ - 1) / (expZ + 1); - }; + func.f_ = [s](double x, double y) { + (void)(y); // y param unused, remove when used. + const double expZ = std::exp(2 * s * x); + return (expZ - 1) / (expZ + 1); + }; fillGridMap(map, func); return func; } -AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map) +AnalyticalFunctions createGaussianWorld(grid_map::GridMap * map) { - struct Gaussian { double x0, y0; @@ -147,28 +141,29 @@ AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map) g.at(i).s = scale(rndGenerator); } - func.f_ = [g](double x,double y) { - double value = 0.0; - for (int i = 0; i < g.size(); ++i) { - const double x0 = g.at(i).x0; - const double y0 = g.at(i).y0; - const double varX = g.at(i).varX; - const double varY = g.at(i).varY; - const double s = g.at(i).s; - value += s * std::exp(-(x-x0)*(x-x0) / (2.0*varX) - (y-y0)*(y-y0) / (2.0 * varY)); - } - - return value; - }; + func.f_ = [g](double x, double y) { + double value = 0.0; + for (std::size_t i = 0; i < g.size(); ++i) { + const double x0 = g.at(i).x0; + const double y0 = g.at(i).y0; + const double varX = g.at(i).varX; + const double varY = g.at(i).varY; + const double s = g.at(i).s; + value += s * + std::exp(-(x - x0) * (x - x0) / (2.0 * varX) - (y - y0) * (y - y0) / (2.0 * varY)); + } + + return value; + }; fillGridMap(map, func); return func; } -void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions) +void fillGridMap(grid_map::GridMap * map, const AnalyticalFunctions & functions) { - grid_map::Matrix& data = (*map)[testLayer]; + grid_map::Matrix & data = (*map)[testLayer]; for (grid_map::GridMapIterator iterator(*map); !iterator.isPastEnd(); ++iterator) { const grid_map::Index index(*iterator); grid_map::Position pos; @@ -177,8 +172,9 @@ void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions) } } -grid_map::GridMap createMap(const grid_map::Length &length, double resolution, - const grid_map::Position &pos) +grid_map::GridMap createMap( + const grid_map::Length & length, double resolution, + const grid_map::Position & pos) { grid_map::GridMap map; @@ -189,10 +185,10 @@ grid_map::GridMap createMap(const grid_map::Length &length, double resolution, return map; } -std::vector uniformlyDitributedPointsWithinMap(const grid_map::GridMap &map, - unsigned int numPoints) +std::vector uniformlyDitributedPointsWithinMap( + const grid_map::GridMap & map, + unsigned int numPoints) { - // stay away from the edges // on the edges the cubic interp is invalid. Not enough points. const double dimX = map.getLength().x() / 2.0 - 3.0 * map.getResolution(); @@ -201,7 +197,7 @@ std::vector uniformlyDitributedPointsWithinMap(const grid_map::GridMap std::uniform_real_distribution Uy(-dimY, dimY); std::vector points(numPoints); - for (auto &point : points) { + for (auto & point : points) { point.x_ = Ux(rndGenerator); point.y_ = Uy(rndGenerator); } @@ -209,18 +205,19 @@ std::vector uniformlyDitributedPointsWithinMap(const grid_map::GridMap return points; } -void verifyValuesAtQueryPointsAreClose(const grid_map::GridMap &map, const AnalyticalFunctions &trueValues, - const std::vector &queryPoints, - grid_map::InterpolationMethods interpolationMethod){ +void verifyValuesAtQueryPointsAreClose( + const grid_map::GridMap & map, const AnalyticalFunctions & trueValues, + const std::vector & queryPoints, + grid_map::InterpolationMethods interpolationMethod) +{ for (const auto point : queryPoints) { const grid_map::Position p(point.x_, point.y_); const double trueValue = trueValues.f_(p.x(), p.y()); const double interpolatedValue = map.atPosition( - grid_map_test::testLayer, p, interpolationMethod); + grid_map_test::testLayer, p, interpolationMethod); EXPECT_NEAR(trueValue, interpolatedValue, grid_map_test::maxAbsErrorValue); } } -} /* namespace grid_map_test */ - +} // namespace grid_map_test diff --git a/grid_map_core/test/test_helpers.hpp b/grid_map_core/test/test_helpers.hpp index d6cf51ad6..b2ea0b7bf 100644 --- a/grid_map_core/test/test_helpers.hpp +++ b/grid_map_core/test/test_helpers.hpp @@ -6,23 +6,27 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#pragma once -#include "grid_map_core/TypeDefs.hpp" +#ifndef TEST_HELPERS_HPP_ +#define TEST_HELPERS_HPP_ + #include #include #include +#include "grid_map_core/TypeDefs.hpp" -namespace grid_map { - class GridMap; -} +namespace grid_map +{ +class GridMap; +} // namespace grid_map -namespace grid_map_test { +namespace grid_map_test +{ /* * Name of the layer that is used in all tests. * It has no special meaning. */ -static const std::string testLayer = "test"; +static const char testLayer[] = "test"; /* * Class that holds a function pointer to analytical @@ -48,8 +52,9 @@ extern std::mt19937 rndGenerator; // Maximal tolerance when comparing doubles in tests. const double maxAbsErrorValue = 1e-3; -grid_map::GridMap createMap(const grid_map::Length &length, double resolution, - const grid_map::Position &pos); +grid_map::GridMap createMap( + const grid_map::Length & length, double resolution, + const grid_map::Position & pos); /* * Collections of methods that modify the grid map. @@ -64,33 +69,36 @@ grid_map::GridMap createMap(const grid_map::Length &length, double resolution, * function. * Each method returns a structure containing the analytical function. */ -AnalyticalFunctions createFlatWorld(grid_map::GridMap *map); -AnalyticalFunctions createRationalFunctionWorld(grid_map::GridMap *map); -AnalyticalFunctions createSaddleWorld(grid_map::GridMap *map); -AnalyticalFunctions createSecondOrderPolyWorld(grid_map::GridMap *map); -AnalyticalFunctions createSineWorld(grid_map::GridMap *map); -AnalyticalFunctions createTanhWorld(grid_map::GridMap *map); -AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map); +AnalyticalFunctions createFlatWorld(grid_map::GridMap * map); +AnalyticalFunctions createRationalFunctionWorld(grid_map::GridMap * map); +AnalyticalFunctions createSaddleWorld(grid_map::GridMap * map); +AnalyticalFunctions createSecondOrderPolyWorld(grid_map::GridMap * map); +AnalyticalFunctions createSineWorld(grid_map::GridMap * map); +AnalyticalFunctions createTanhWorld(grid_map::GridMap * map); +AnalyticalFunctions createGaussianWorld(grid_map::GridMap * map); /* * Iterates over the grid map and fill it with values. * values are calculated by evaluating analytical function. */ -void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions); +void fillGridMap(grid_map::GridMap * map, const AnalyticalFunctions & functions); /* * Create numPoints uniformly distributed random points that lie within the grid map. */ -std::vector uniformlyDitributedPointsWithinMap(const grid_map::GridMap &map, - unsigned int numPoints); +std::vector uniformlyDitributedPointsWithinMap( + const grid_map::GridMap & map, + unsigned int numPoints); /* * For each point in queryPoints, verify that the interpolated value of the grid map * is close to the ground truth which is contained in Analytical functions structure. * Called inside the tests. Calls macros from gtest. */ -void verifyValuesAtQueryPointsAreClose(const grid_map::GridMap &map, const AnalyticalFunctions &trueValues, - const std::vector &queryPoints, - grid_map::InterpolationMethods interpolationMethod); +void verifyValuesAtQueryPointsAreClose( + const grid_map::GridMap & map, const AnalyticalFunctions & trueValues, + const std::vector & queryPoints, + grid_map::InterpolationMethods interpolationMethod); -} /*grid_map_test*/ +} // namespace grid_map_test +#endif // TEST_HELPERS_HPP_ diff --git a/grid_map_costmap_2d/CHANGELOG.rst b/grid_map_costmap_2d/CHANGELOG.rst index e6192cc91..eb335585f 100644 --- a/grid_map_costmap_2d/CHANGELOG.rst +++ b/grid_map_costmap_2d/CHANGELOG.rst @@ -2,6 +2,34 @@ Changelog for package grid_map_costmap_2d ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#424 `_ from Ryanf55/modern-ament-grid-map-costmap-2d + Use ament_export_targets in grid_map_costmap_2d +* Remove extra whitespace +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Fix whitespace, make INTERFACE type +* Use ament_export_targets + * Link to exported namespace targets when possible +* Contributors: Ryan, Ryan Friedman + +2.1.0 (2022-11-08) +------------------ + +2.0.0 (2022-09-13) +------------------ +* fix: mark Eigen library as SYSTEM +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map_costmap_2d/CMakeLists.txt b/grid_map_costmap_2d/CMakeLists.txt index 38fcdac5f..201c87601 100644 --- a/grid_map_costmap_2d/CMakeLists.txt +++ b/grid_map_costmap_2d/CMakeLists.txt @@ -1,57 +1,66 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.13) project(grid_map_costmap_2d) -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") +## Find ament_cmake macros and libraries +find_package(ament_cmake REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS +find_package(Eigen3 REQUIRED) + +grid_map_package() + +set(dependencies grid_map_core - costmap_2d - tf + geometry_msgs + nav2_costmap_2d + tf2_ros + tf2_geometry_msgs ) -## System dependencies are found with CMake's conventions -#find_package(Eigen3 REQUIRED) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - CATKIN_DEPENDS - grid_map_core - costmap_2d - tf - DEPENDS +add_library(${PROJECT_NAME} INTERFACE + include/grid_map_costmap_2d/costmap_2d_converter.hpp + include/grid_map_costmap_2d/grid_map_costmap_2d.hpp ) -########### -## Build ## -########### +add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) + +target_link_libraries(${PROJECT_NAME} + INTERFACE + grid_map_core::grid_map_core + ${geometry_msgs_TARGETS} + ${tf2_geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + tf2_ros::tf2_ros +) -## Specify additional locations of header files -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} +target_include_directories(${PROJECT_NAME} + INTERFACE + "$" + "$" ) ############# ## Install ## ############# +# Mark library for installation +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + # Mark cpp header files for installation install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} FILES_MATCHING PATTERN "*.hpp" ) @@ -59,17 +68,36 @@ install( ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") - ## Add gtest based cpp test target and link libraries - catkin_add_gtest( - ${PROJECT_NAME}-test - test/test_grid_map_costmap_2d.cpp - test/Costmap2DConverterTest.cpp - ) - add_subdirectory(rostest) -endif() +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() -if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES}) + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright + ) + endif() + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + add_subdirectory(test) endif() + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/grid_map_costmap_2d/include/grid_map_costmap_2d/Costmap2DConverter.hpp b/grid_map_costmap_2d/include/grid_map_costmap_2d/costmap_2d_converter.hpp similarity index 59% rename from grid_map_costmap_2d/include/grid_map_costmap_2d/Costmap2DConverter.hpp rename to grid_map_costmap_2d/include/grid_map_costmap_2d/costmap_2d_converter.hpp index ce3995065..b3e30ca52 100644 --- a/grid_map_costmap_2d/include/grid_map_costmap_2d/Costmap2DConverter.hpp +++ b/grid_map_costmap_2d/include/grid_map_costmap_2d/costmap_2d_converter.hpp @@ -1,5 +1,5 @@ /* - * CostmapConverter.hpp + * costmap_2d_converter.hpp * * Created on: Nov 23, 2016 * Author: Peter Fankhauser, ETH Zurich @@ -8,44 +8,55 @@ * Gabriel Hottiger, ANYbotics */ -#pragma once +#ifndef GRID_MAP_COSTMAP_2D__COSTMAP_2D_CONVERTER_HPP_ +#define GRID_MAP_COSTMAP_2D__COSTMAP_2D_CONVERTER_HPP_ -#include +// STD +#include +#include +#include // ROS -#include -#include -#include +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" -// STD -#include -#include +#include "grid_map_core/grid_map_core.hpp" -namespace grid_map { +namespace grid_map +{ /** * Defines the conversion between grid_map and costmap_2d. * -> There is only one distinct value for no information in the grid map. * -> All values smaller or equal to freeSpace are considered free space (except noInformation). - * -> All values bigger or equal to lethalObstacle are considered a lethal obstacle (except noInformation). - * -> All values between inscribedInflatedObstacle and lethalObstacle are considered an inscribed inflated obstacle (except noInformation). + * -> All values bigger or equal to lethalObstacle are + * considered a lethal obstacle (except noInformation). + * -> All values between inscribedInflatedObstacle and + * lethalObstacle are considered an inscribed inflated obstacle (except noInformation). * -> All other values are interpolated between freeSpace and inscribedInflatedObstacle - * @tparam noInformation Gridmap value that represents no information. - * @tparam lethalObstacle Gridmap value (lower bound) for lethal obstacles. - * @tparam inscribedInflatedObstacle Gridmap value (lower bound) for inscribed inflated obstacles. - * @tparam freeSpace Gridmap value (upper bound) for free space. + * @tparam noInformation Gridmap value that represents no information. + * @tparam lethalObstacle Gridmap value (lower bound) for lethal obstacles. + * @tparam inscribedInflatedObstacle Gridmap value (lower bound) for inscribed inflated obstacles. + * @tparam freeSpace Gridmap value (upper bound) for free space. */ -template -class Costmap2DTranslationTable { +template +class Costmap2DTranslationTable +{ // Check required pre-conditions of template arguments - static_assert(freeSpace < inscribedInflatedObstacle, - "[Costmap2DTranslationTable] Condition violated: freeSpace < inscribedInflatedObstacle."); - static_assert(inscribedInflatedObstacle < lethalObstacle, - "[Costmap2DTranslationTable] Condition violated: inscribedInflatedObstacle < lethalObstacle."); - static_assert(noInformation < freeSpace || noInformation > lethalObstacle, - "[Costmap2DTranslationTable] Condition violated: noInformation < freeSpace || noInformation > lethalObstacle."); - - public: + static_assert( + freeSpace < inscribedInflatedObstacle, + "[Costmap2DTranslationTable] Condition violated: freeSpace < inscribedInflatedObstacle."); + static_assert( + inscribedInflatedObstacle < lethalObstacle, + "[Costmap2DTranslationTable] Condition violated: inscribedInflatedObstacle < lethalObstacle."); + static_assert( + (noInformation < freeSpace) || (noInformation > lethalObstacle), + "[Costmap2DTranslationTable] Condition violated: " + "noInformation < freeSpace || noInformation > lethalObstacle."); + +public: // Only static methods -> delete constructor. Costmap2DTranslationTable() = delete; @@ -54,8 +65,9 @@ class Costmap2DTranslationTable { * @tparam DataType Data type of the grid map. * @param costTranslationTable Translation table mapping from costmap value to grid map value. */ - template - static void create(std::vector& costTranslationTable) { + template + static void create(std::vector & costTranslationTable) + { costTranslationTable.resize(256); for (unsigned int i = 0; i < costTranslationTable.size(); ++i) { costTranslationTable[i] = fromCostmap(static_cast(i)); @@ -68,26 +80,29 @@ class Costmap2DTranslationTable { * @param costmapValue Cost map value. * @return Equivalent grid map value. */ - template - static DataType fromCostmap(const uint8_t costmapValue) { + template + static DataType fromCostmap(const uint8_t costmapValue) + { // Check special cost map values. - if (costmapValue == costmap_2d::FREE_SPACE) { + if (costmapValue == nav2_costmap_2d::FREE_SPACE) { return freeSpace; - } else if (costmapValue == costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { + } else if (costmapValue == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { return inscribedInflatedObstacle; - } else if (costmapValue == costmap_2d::LETHAL_OBSTACLE) { + } else if (costmapValue == nav2_costmap_2d::LETHAL_OBSTACLE) { return lethalObstacle; - } else if (costmapValue == costmap_2d::NO_INFORMATION) { + } else if (costmapValue == nav2_costmap_2d::NO_INFORMATION) { return noInformation; } - // Map costmap map interval to gridmap interval for values between free space and inflated obstacle - constexpr DataType costmapIntervalStart = costmap_2d::FREE_SPACE; - constexpr DataType costmapIntervalWidth = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - costmapIntervalStart; + // Map costmap map interval to gridmap interval + // for values between free space and inflated obstacle + constexpr DataType costmapIntervalStart = nav2_costmap_2d::FREE_SPACE; + constexpr DataType + costmapIntervalWidth = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - costmapIntervalStart; constexpr DataType gridmapIntervalStart = freeSpace; constexpr DataType gridmapIntervalWidth = inscribedInflatedObstacle - gridmapIntervalStart; - const DataType interpolatedValue = - gridmapIntervalStart + (costmapValue - costmapIntervalStart) * gridmapIntervalWidth / costmapIntervalWidth; + const DataType interpolatedValue = gridmapIntervalStart + ( + costmapValue - costmapIntervalStart) * gridmapIntervalWidth / costmapIntervalWidth; return interpolatedValue; } @@ -97,26 +112,28 @@ class Costmap2DTranslationTable { * @param gridmapValue Grid map value. * @return Equivalent cost map value. */ - template - static uint8_t toCostmap(const DataType gridmapValue) { + template + static uint8_t toCostmap(const DataType gridmapValue) + { // Check special grid map values. if (gridmapValue == static_cast(noInformation)) { - return costmap_2d::NO_INFORMATION; + return nav2_costmap_2d::NO_INFORMATION; } else if (gridmapValue <= static_cast(freeSpace)) { - return costmap_2d::FREE_SPACE; + return nav2_costmap_2d::FREE_SPACE; } else if (gridmapValue >= static_cast(lethalObstacle)) { - return costmap_2d::LETHAL_OBSTACLE; + return nav2_costmap_2d::LETHAL_OBSTACLE; } else if (gridmapValue >= static_cast(inscribedInflatedObstacle)) { - return costmap_2d::INSCRIBED_INFLATED_OBSTACLE; + return nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; } // Map gridmap interval to costmap interval for values between free space and inflated obstacle - constexpr DataType costmapIntervalStart = costmap_2d::FREE_SPACE; - constexpr DataType costmapIntervalWidth = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - costmapIntervalStart; + constexpr DataType costmapIntervalStart = nav2_costmap_2d::FREE_SPACE; + constexpr DataType + costmapIntervalWidth = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - costmapIntervalStart; constexpr DataType gridmapIntervalStart = freeSpace; constexpr DataType gridmapIntervalWidth = inscribedInflatedObstacle - gridmapIntervalStart; - const DataType interpolatedValue = - costmapIntervalStart + (gridmapValue - gridmapIntervalStart) * costmapIntervalWidth / gridmapIntervalWidth; + const DataType interpolatedValue = costmapIntervalStart + ( + gridmapValue - gridmapIntervalStart) * costmapIntervalWidth / gridmapIntervalWidth; return std::round(interpolatedValue); } @@ -124,22 +141,33 @@ class Costmap2DTranslationTable { /*! * @brief Direct cost translations. - * This maps the cost directly, simply casting between the underlying unsigned char and float fields. + * This maps the cost directly, simply casting between + * the underlying unsigned char and float fields. */ -using Costmap2DDirectTranslationTable = Costmap2DTranslationTable; +using Costmap2DDirectTranslationTable = Costmap2DTranslationTable; /** * @brief Century cost translations to [0, 100] for costmap_2d -> grid map. - * This maps the cost onto float fields between 0.0 and 100.0 with reserved values allocated for the costmap_2d. + * This maps the cost onto float fields between 0.0 and 100.0 + * with reserved values allocated for the costmap_2d. */ -using Costmap2DCenturyTranslationTable = Costmap2DTranslationTable<-1, 100, 99, costmap_2d::FREE_SPACE>; +using Costmap2DCenturyTranslationTable = + Costmap2DTranslationTable<-1, 100, 99, nav2_costmap_2d::FREE_SPACE>; -template -class Costmap2DDefaultTranslationTable : public Costmap2DDirectTranslationTable {}; +template +class Costmap2DDefaultTranslationTable + : public Costmap2DDirectTranslationTable +{ +}; -template <> -class Costmap2DDefaultTranslationTable : public Costmap2DCenturyTranslationTable {}; +template<> +class Costmap2DDefaultTranslationTable + : public Costmap2DCenturyTranslationTable +{ +}; /***************************************************************************** ** Converter @@ -153,9 +181,11 @@ class Costmap2DDefaultTranslationTable : public Costmap2DCenturyTranslati * * @sa Costmap2DDirectTranslationTable, Costmap2DCenturyTranslationTable */ -template > -class Costmap2DConverter { - public: +template> +class Costmap2DConverter +{ +public: Costmap2DConverter() = default; virtual ~Costmap2DConverter() = default; @@ -164,11 +194,17 @@ class Costmap2DConverter { * @param gridMap Grid map containing size info and properties. * @param outputCostmap Resized costmap. */ - void initializeFromGridMap(const MapType& gridMap, costmap_2d::Costmap2D& outputCostmap) { + void initializeFromGridMap(const MapType & gridMap, nav2_costmap_2d::Costmap2D & outputCostmap) + { // Different origin position const Position position = gridMap.getPosition() - Position(0.5 * gridMap.getLength()); const Size sizeXY = gridMap.getSize(); - outputCostmap.resizeMap(sizeXY(0), sizeXY(1), gridMap.getResolution(), position(0), position(1)); + outputCostmap.resizeMap( + sizeXY(0), + sizeXY(1), + gridMap.getResolution(), + position(0), + position(1)); } /*! @@ -178,7 +214,11 @@ class Costmap2DConverter { * @param outputCostmap to this costmap type object. * @return true if successful, false otherwise. */ - bool setCostmap2DFromGridMap(const MapType& gridMap, const std::string& layer, costmap_2d::Costmap2D& outputCostmap) { + bool setCostmap2DFromGridMap( + const MapType & gridMap, + const std::string & layer, + nav2_costmap_2d::Costmap2D & outputCostmap) + { // Check compliance. Size size(outputCostmap.getSizeInCellsX(), outputCostmap.getSizeInCellsY()); if ((gridMap.getSize() != size).any()) { @@ -194,19 +234,23 @@ class Costmap2DConverter { // between Costmap2D and grid map. const size_t nCells = gridMap.getSize().prod(); for (size_t i = 0, j = nCells - 1; i < nCells; ++i, --j) { - outputCostmap.getCharMap()[j] = TranslationTable::template toCostmap(gridMap.get(layer).data()[i]); + outputCostmap.getCharMap()[j] = + TranslationTable::template toCostmap(gridMap.get(layer).data()[i]); } return true; } - void initializeFromCostmap2D(costmap_2d::Costmap2DROS& costmap2d, MapType& outputMap) { + void initializeFromCostmap2D(nav2_costmap_2d::Costmap2DROS & costmap2d, MapType & outputMap) + { initializeFromCostmap2D(*(costmap2d.getCostmap()), outputMap); outputMap.setFrameId(costmap2d.getGlobalFrameID()); } - void initializeFromCostmap2D(const costmap_2d::Costmap2D& costmap2d, MapType& outputMap) { + void initializeFromCostmap2D(const nav2_costmap_2d::Costmap2D & costmap2d, MapType & outputMap) + { const double resolution = costmap2d.getResolution(); - Length length(costmap2d.getSizeInCellsX() * resolution, costmap2d.getSizeInCellsY() * resolution); + Length + length(costmap2d.getSizeInCellsX() * resolution, costmap2d.getSizeInCellsY() * resolution); Position position(costmap2d.getOriginX(), costmap2d.getOriginY()); // Different conventions. position += Position(0.5 * length); @@ -215,13 +259,18 @@ class Costmap2DConverter { /*! * Load the costmap2d into the specified layer. - * @warning This does not lock the costmap2d object, you should take care to do so from outside this function. + * @warning This does not lock the costmap2d object, + * you should take care to do so from outside this function. * @param costmap2d from this costmap type object. * @param layer the name of the layer to insert. * @param outputMap to this costmap type object. * @return true if successful, false otherwise. */ - bool addLayerFromCostmap2D(const costmap_2d::Costmap2D& costmap2d, const std::string& layer, MapType& outputMap) { + bool addLayerFromCostmap2D( + const nav2_costmap_2d::Costmap2D & costmap2d, + const std::string & layer, + MapType & outputMap) + { // Check compliance. Size size(costmap2d.getSizeInCellsX(), costmap2d.getSizeInCellsY()); if ((outputMap.getSize() != size).any()) { @@ -248,13 +297,18 @@ class Costmap2DConverter { /*! * Load the costmap2d into the specified layer. - * @warning This does not lock the costmap2d object, you should take care to do so from outside this function. + * @warning This does not lock the costmap2d object, + * you should take care to do so from outside this function. * @param costmap2d from this costmap type object. * @param layer the name of the layer to insert. * @param outputMap to this costmap type object. * @return true if successful, false otherwise. */ - bool addLayerFromCostmap2D(costmap_2d::Costmap2DROS& costmap2d, const std::string& layer, MapType& outputMap) { + bool addLayerFromCostmap2D( + nav2_costmap_2d::Costmap2DROS & costmap2d, + const std::string & layer, + MapType & outputMap) + { return addLayerFromCostmap2D(*(costmap2d.getCostmap()), layer, outputMap); } @@ -267,33 +321,30 @@ class Costmap2DConverter { * that here. * * @warning this does not lock the costmap2d object, you should take care to do so from - * outside this function in scope with any addLayerFromCostmap2DAtRobotPose calls you wish to make. + * outside this function in scope with any + * addLayerFromCostmap2DAtRobotPose calls you wish to make. * * @param costmap2d : the underlying Costmap2DROS object * @param geometry: size of the subwindow to snapshot around the robot pose * @param outputMap : initialise the output map with the required parameters */ - bool initializeFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS& costmap2d, const Length& length, MapType& outputMap) { + bool initializeFromCostmap2DAtRobotPose( + nav2_costmap_2d::Costmap2DROS & costmap2d, + const Length & length, + MapType & outputMap) + { const double resolution = costmap2d.getCostmap()->getResolution(); - // Get the Robot Pose Transform. -#if ROS_VERSION_MINIMUM(1, 14, 0) - geometry_msgs::PoseStamped tfPose; + geometry_msgs::msg::PoseStamped tfPose; if (!costmap2d.getRobotPose(tfPose)) { errorMessage_ = "Could not get robot pose, is it actually published?"; return false; } Position robotPosition(tfPose.pose.position.x, tfPose.pose.position.y); -#else - tf::Stamped tfPose; - if (!costmap2d.getRobotPose(tfPose)) { - errorMessage_ = "Could not get robot pose, is it actually published?"; - return false; - } - Position robotPosition(tfPose.getOrigin().x(), tfPose.getOrigin().y()); -#endif + // Determine new costmap origin. - Position rosMapOrigin(costmap2d.getCostmap()->getOriginX(), costmap2d.getCostmap()->getOriginY()); + Position + rosMapOrigin(costmap2d.getCostmap()->getOriginX(), costmap2d.getCostmap()->getOriginY()); Position newCostMapOrigin; // Note: @@ -312,25 +363,27 @@ class Costmap2DConverter { // if there is an even number of cells // centre of the new grid map at the closest vertex between cells // of the current cell - int numberOfCellsX = length.x() / resolution; - int numberOfCellsY = length.y() / resolution; + int numberOfCellsX = static_cast(length.x() / resolution); + int numberOfCellsY = static_cast(length.y() / resolution); if (numberOfCellsX % 2) { // odd - newCostMapOrigin(0) = std::floor(robotCellPosition.x()) * resolution + resolution / 2.0 + rosMapOrigin.x(); + newCostMapOrigin(0) = + std::floor(robotCellPosition.x()) * resolution + resolution / 2.0 + rosMapOrigin.x(); } else { newCostMapOrigin(0) = std::round(robotCellPosition.x()) * resolution + rosMapOrigin.x(); } if (numberOfCellsY % 2) { // odd - newCostMapOrigin(1) = std::floor(robotCellPosition.y()) * resolution + resolution / 2.0 + rosMapOrigin.y(); + newCostMapOrigin(1) = + std::floor(robotCellPosition.y()) * resolution + resolution / 2.0 + rosMapOrigin.y(); } else { newCostMapOrigin(1) = std::round(robotCellPosition.y()) * resolution + rosMapOrigin.y(); } - // TODO check the robot pose is in the window - // TODO check the geometry fits within the costmap2d window + // TODO(tbd): check the robot pose is in the window + // TODO(tbd): check the geometry fits within the costmap2d window // Initialize the output map. outputMap.setFrameId(costmap2d.getGlobalFrameID()); - outputMap.setTimestamp(ros::Time::now().toNSec()); + outputMap.setTimestamp(costmap2d.now().nanoseconds()); outputMap.setGeometry(length, resolution, newCostMapOrigin); return true; } @@ -344,7 +397,11 @@ class Costmap2DConverter { * @param layer the layer name to add. * @param outputMap the initialized and filled in map output map. */ - bool addLayerFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS& costmap2d, const std::string& layer, MapType& outputMap) { + bool addLayerFromCostmap2DAtRobotPose( + nav2_costmap_2d::Costmap2DROS & costmap2d, + const std::string & layer, + MapType & outputMap) + { /**************************************** ** Asserts ****************************************/ @@ -359,20 +416,21 @@ class Costmap2DConverter { // 2) check the geometry fits inside the costmap2d subwindow is done below // Properties. - const double resolution = costmap2d.getCostmap()->getResolution(); const Length geometry = outputMap.getLength(); const Position position = outputMap.getPosition(); // Copy data. - bool isValidWindow = false; - costmap_2d::Costmap2D costmapSubwindow; - // TODO - isValidWindow = costmapSubwindow.copyCostmapWindow(*(costmap2d.getCostmap()), - position.x() - geometry.x() / 2.0, // subwindow_bottom_left_x - position.y() - geometry.y() / 2.0, // subwindow_bottom_left_y - geometry.x(), geometry.y()); + bool isValidWindow; + nav2_costmap_2d::Costmap2D costmapSubwindow; + // TODO(tbd) + isValidWindow = costmapSubwindow.copyCostmapWindow( + *(costmap2d.getCostmap()), + position.x() - geometry.x() / 2.0, // subwindow_bottom_left_x + position.y() - geometry.y() / 2.0, // subwindow_bottom_left_y + geometry.x(), geometry.y()); if (!isValidWindow) { - // handle differently - e.g. copy the internal part only and lethal elsewhere, but other parts would have to handle being outside too + // handle differently - e.g. copy the internal part only and lethal elsewhere, + // but other parts would have to handle being outside too errorMessage_ = "Subwindow landed outside the costmap, aborting."; return false; } @@ -388,10 +446,12 @@ class Costmap2DConverter { * * @return std::string */ - std::string errorMessage() const { return errorMessage_; } + std::string errorMessage() const {return errorMessage_;} - private: +private: std::string errorMessage_; }; } // namespace grid_map + +#endif // GRID_MAP_COSTMAP_2D__COSTMAP_2D_CONVERTER_HPP_ diff --git a/grid_map_costmap_2d/include/grid_map_costmap_2d/grid_map_costmap_2d.hpp b/grid_map_costmap_2d/include/grid_map_costmap_2d/grid_map_costmap_2d.hpp index 089e97694..23420a053 100644 --- a/grid_map_costmap_2d/include/grid_map_costmap_2d/grid_map_costmap_2d.hpp +++ b/grid_map_costmap_2d/include/grid_map_costmap_2d/grid_map_costmap_2d.hpp @@ -1,12 +1,15 @@ /* - * grid_map.hpp + * grid_map_costmap_2d.hpp * * Created on: Jul 14, 2014 * Author: Péter Fankhauser * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_COSTMAP_2D__GRID_MAP_COSTMAP_2D_HPP_ +#define GRID_MAP_COSTMAP_2D__GRID_MAP_COSTMAP_2D_HPP_ #include -#include +#include + +#endif // GRID_MAP_COSTMAP_2D__GRID_MAP_COSTMAP_2D_HPP_ diff --git a/grid_map_costmap_2d/package.xml b/grid_map_costmap_2d/package.xml index 1cd177e3c..a69ed5b82 100644 --- a/grid_map_costmap_2d/package.xml +++ b/grid_map_costmap_2d/package.xml @@ -1,16 +1,31 @@ - + + grid_map_costmap_2d - 1.6.2 + 2.2.2 Interface for grid maps to the costmap_2d format. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Péter Fankhauser - catkin + + ament_cmake + grid_map_cmake_helpers + grid_map_core - costmap_2d - tf + nav2_costmap_2d + geometry_msgs + tf2_ros + tf2_geometry_msgs + + ament_cmake_gtest + ament_lint_common + ament_lint_auto + + + ament_cmake + diff --git a/grid_map_costmap_2d/rostest/CMakeLists.txt b/grid_map_costmap_2d/rostest/CMakeLists.txt deleted file mode 100644 index 6ddcc24a5..000000000 --- a/grid_map_costmap_2d/rostest/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -############################################################################## -# Unit Tests -# -# Only run when CATKIN_ENABLE_TESTING is true. -############################################################################## - -find_package(rostest REQUIRED) - -############################################################################## -# Macro -############################################################################## - -macro(${PROJECT_NAME}_add_rostest test_name) - add_rostest_gtest(test_${test_name} ${test_name}/${test_name}.test ${test_name}/${test_name}.cpp) - if (TARGET test_${test_name}) - add_dependencies(test_${test_name} ${catkin_EXPORTED_TARGETS}) - target_link_libraries(test_${test_name} ${catkin_LIBRARIES}) - endif() -endmacro() - -############################################################################## -# Tests -############################################################################## - -grid_map_costmap_2d_add_rostest(costmap_2d_ros) diff --git a/grid_map_costmap_2d/rostest/Readme.md b/grid_map_costmap_2d/rostest/Readme.md deleted file mode 100644 index 9a1f1c28e..000000000 --- a/grid_map_costmap_2d/rostest/Readme.md +++ /dev/null @@ -1,10 +0,0 @@ - -I can never remember how to run them... - -* http://wiki.ros.org/rostest/Commandline - -``` -rostest --text mytest.test -``` - - diff --git a/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.cpp b/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.cpp deleted file mode 100644 index 298d87a5d..000000000 --- a/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.cpp +++ /dev/null @@ -1,268 +0,0 @@ -/***************************************************************************** -** Includes -*****************************************************************************/ - -#include -#include -#include - -#include -#if ROS_VERSION_MINIMUM(1,14,0) -#include -#include -#else -#include -#endif - -#include "costmap_2d_ros.hpp" - -/***************************************************************************** -** Helpers -*****************************************************************************/ - -void broadcastCostmap2DROSTestSuiteTransforms(TransformBroadcaster& broadcaster) -{ - broadcaster.add("base_link_5x5", tf::Vector3(1.0, 1.0, 0.0), tf::Quaternion(0, 0, 0, 1)); - broadcaster.add("base_link_4x4", tf::Vector3(1.0, -3.0, 0.0), tf::Quaternion(0, 0, 0, 1)); - broadcaster.add("base_link_5x5_3x3_offset", tf::Vector3(-3.7, 2.4, 0.0), tf::Quaternion(0, 0, 0, 1)); - broadcaster.add("base_link_5x5_3x3_centre", tf::Vector3(-3.5, -3.5, 0.0), tf::Quaternion(0, 0, 0, 1)); - broadcaster.add("base_link_5x5_2_5x2_5_offset", tf::Vector3(-9.7, 2.4, 0.0), tf::Quaternion(0, 0, 0, 1)); - broadcaster.startBroadCastingThread(); -} - -/***************************************************************************** -** ROS Costmap Server -*****************************************************************************/ - -ROSCostmapServer::ROSCostmapServer(const std::string& name, - const std::string& baseLinkTransformName, - const grid_map::Position& origin, const double& width, - const double& height) -#if ROS_VERSION_MINIMUM(1,14,0) - : tfBuffer_(ros::Duration(1.0)), - tfListener_(tfBuffer_) -#else - : tfListener_(ros::Duration(1.0)) -#endif -{ - ros::NodeHandle privateNodeHandle("~"); - // lots of parameters here affect the construction ( e.g. rolling window) - // if you don't have any parameters set, then this - // - alot of defaults which get dumped on the ros param server - // - fires up an obstacle layer and an inflation layer - // - creates a publisher for an occupancy grid - privateNodeHandle.setParam(name + "/robot_base_frame", baseLinkTransformName); - privateNodeHandle.setParam(name + "/origin_x", origin.x()); - privateNodeHandle.setParam(name + "/origin_y", origin.y()); - privateNodeHandle.setParam(name + "/width", width); - privateNodeHandle.setParam(name + "/height", height); - privateNodeHandle.setParam(name + "/plugins", std::vector()); - privateNodeHandle.setParam(name + "/resolution", 0.5); - privateNodeHandle.setParam(name + "/robot_radius", 0.03); // clears 1 cell if inside, up to 4 cells on a vertex -#if ROS_VERSION_MINIMUM(1,14,0) - costmap_ = std::make_shared(name, tfBuffer_); -#else - costmap_ = std::make_shared(name, tfListener_); -#endif - - for ( unsigned int index = 0; index < costmap_->getCostmap()->getSizeInCellsY(); ++index ) { - unsigned int dimension = costmap_->getCostmap()->getSizeInCellsX(); - // @todo assert dimension > 1 - // set the first row to costmap_2d::FREE_SPACE? but it shows up invisibly in rviz, so don't bother - for ( unsigned int fill_index = 0; fill_index < dimension - 2; ++fill_index ) - { - double fraction = static_cast(fill_index + 1) / static_cast(costmap_->getCostmap()->getSizeInCellsX()); - costmap_->getCostmap()->setCost(fill_index, index, fraction*costmap_2d::INSCRIBED_INFLATED_OBSTACLE ); - } - costmap_->getCostmap()->setCost(dimension - 2, index, costmap_2d::LETHAL_OBSTACLE); - costmap_->getCostmap()->setCost(dimension - 1, index, costmap_2d::NO_INFORMATION); - } -} - -/***************************************************************************** -** TransformBroadcaster -*****************************************************************************/ - -TransformBroadcaster::~TransformBroadcaster() -{ - broadcastingThread_.join(); -} - -void TransformBroadcaster::shutdown() -{ - shutdownFlag_ = true; -} - -void TransformBroadcaster::add(const std::string& name, tf::Vector3 origin, - const tf::Quaternion& orientation) -{ - tf::Transform transform; - transform.setOrigin(origin); - transform.setRotation(orientation); - transforms_.insert(std::pair(name, transform)); -} - -void TransformBroadcaster::startBroadCastingThread() { - broadcastingThread_ = std::thread(&TransformBroadcaster::broadcast, this); -} - -void TransformBroadcaster::broadcast() -{ -#if ROS_VERSION_MINIMUM(1,14,0) - tf2_ros::TransformBroadcaster tfBroadcaster; -#else - tf::TransformBroadcaster tfBroadcaster; -#endif - while (ros::ok() && !shutdownFlag_) { - for (std::pair p : transforms_) { - tf::StampedTransform transform(p.second, ros::Time::now(), "map", p.first); -#if ROS_VERSION_MINIMUM(1,14,0) - geometry_msgs::TransformStamped transformMsg; - tf::transformStampedTFToMsg(transform, transformMsg); - tfBroadcaster.sendTransform(transformMsg); -#else - tfBroadcaster.sendTransform(transform); -#endif - } - ros::Duration(0.1).sleep(); - } -} - -/***************************************************************************** -** Converter Functions -*****************************************************************************/ - -bool fromCostmap2DROS(costmap_2d::Costmap2DROS& ros_costmap, const std::string& layer_name, - grid_map::GridMap& grid_map) -{ - grid_map::Costmap2DConverter converter; - boost::lock_guard lock(*(ros_costmap.getCostmap()->getMutex())); - converter.initializeFromCostmap2D(ros_costmap, grid_map); - if (!converter.addLayerFromCostmap2D(ros_costmap, layer_name, grid_map)) { - return false; - } - return true; -} - -bool fromCostmap2DROSAtRobotPose(costmap_2d::Costmap2DROS& ros_costmap, - const grid_map::Length& geometry, const std::string& layer_name, - grid_map::GridMap& grid_map) -{ - grid_map::Costmap2DConverter converter; - boost::lock_guard lock(*(ros_costmap.getCostmap()->getMutex())); - if (!converter.initializeFromCostmap2DAtRobotPose(ros_costmap, geometry, grid_map)) { - return false; - } - if (!converter.addLayerFromCostmap2DAtRobotPose(ros_costmap, layer_name, grid_map)) { - return false; - } - return true; -} - -/***************************************************************************** -** Tests -*****************************************************************************/ - -TEST(Costmap2DROSConversion, full_window) -{ - std::cout << std::endl; - ROS_INFO("***********************************************************"); - ROS_INFO(" Copy Full Window"); - ROS_INFO("***********************************************************"); - // preparation - std::string layer_name = "obstacle_costs"; - ROSCostmapServer ros_costmap_5x5("five_by_five", "base_link_5x5", grid_map::Position(0.0, 0.0), 5.0, 5.0); - grid_map::GridMap grid_map_5x5; - fromCostmap2DROS(*(ros_costmap_5x5.getROSCostmap()), layer_name, grid_map_5x5); - // assert map properties - ASSERT_EQ(grid_map_5x5.getFrameId(), ros_costmap_5x5.getROSCostmap()->getGlobalFrameID()); - ASSERT_EQ( - grid_map_5x5.getLength().x(), - ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsX() - * ros_costmap_5x5.getROSCostmap()->getCostmap()->getResolution()); - ASSERT_EQ( - grid_map_5x5.getLength().y(), - ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsY() - * ros_costmap_5x5.getROSCostmap()->getCostmap()->getResolution()); - ASSERT_EQ(grid_map_5x5.getSize()[0], ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsX()); - ASSERT_EQ(grid_map_5x5.getSize()[1], ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsY()); - grid_map::Length position = grid_map_5x5.getPosition() - 0.5 * grid_map_5x5.getLength().matrix(); - ASSERT_EQ(position.x(), ros_costmap_5x5.getROSCostmap()->getCostmap()->getOriginX()); - ASSERT_EQ(position.y(), ros_costmap_5x5.getROSCostmap()->getCostmap()->getOriginY()); - - // assert map data - for (unsigned int i = 0; i < 5; ++i) { - for (unsigned int j = 0; j < 5; ++j) { - std::cout << static_cast(ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(i, j)) - << " "; - } - std::cout << std::endl; - } - for (unsigned int i = 0; i < 5; ++i) { - for (unsigned int j = 0; j < 5; ++j) { - std::cout << static_cast(grid_map_5x5.at(layer_name, grid_map::Index(i, j))) << " "; - } - std::cout << std::endl; - } - // TODO a function which does the index conversion - - std::cout << "Original cost: " << static_cast(ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(8,0)) << std::endl; - std::cout << "New Cost: " << grid_map_5x5.at(layer_name, grid_map::Index(1,9)) << std::endl; - std::vector cost_translation_table; - grid_map::Costmap2DCenturyTranslationTable::create(cost_translation_table); - unsigned char cost = ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(8,0); - ASSERT_EQ(grid_map_5x5.at(layer_name, grid_map::Index(1,9)), cost_translation_table[cost]); - std::cout << std::endl; -} - -TEST(Costmap2DROSConversion, cost_map_centres) -{ - std::cout << std::endl; - ROS_INFO("***********************************************************"); - ROS_INFO(" Check Subwindow Centres"); - ROS_INFO("***********************************************************"); - ROS_INFO("Subwindows are centred as closely as possible to the robot"); - ROS_INFO("pose, though not exactly. They still need to align with"); - ROS_INFO("the underlying ros costmap so that they don't introduce a"); - ROS_INFO("new kind of error. As a result, the centre is shifted from"); - ROS_INFO("the robot pose to the nearest appropriate point which aligns"); - ROS_INFO("the new cost map exactly on top of the original ros costmap."); - std::cout << std::endl; - std::string layer_name = "obstacle_costs"; - ROSCostmapServer ros_costmap_5x5_3x3_offset("five_by_five_three_by_three_offset", "base_link_5x5_3x3_offset", grid_map::Position(-6.0, 0.0), 5.0, 5.0); - ROSCostmapServer ros_costmap_5x5_3x3_centre("five_by_five_three_by_three_centre", "base_link_5x5_3x3_centre", grid_map::Position(-6.0, -6.0), 5.0, 5.0); - ROSCostmapServer ros_costmap_5x5_2_5x2_5_offset("five_by_five_twohalf_by_twohalf_offset", "base_link_5x5_2_5x2_5_offset", grid_map::Position(-12.0, 0.0), 5.0, 5.0); - grid_map::GridMap grid_map_5x5_3x3_offset, grid_map_5x5_3x3_centre, grid_map_5x5_2_5x2_5_offset; - grid_map::Length geometry_3x3(3.0, 3.0); - fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_3x3_offset.getROSCostmap()), geometry_3x3, layer_name, grid_map_5x5_3x3_offset); - fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_3x3_centre.getROSCostmap()), geometry_3x3, layer_name, grid_map_5x5_3x3_centre); - grid_map::Length geometry_2_5x2_5(2.5, 2.5); - fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_2_5x2_5_offset.getROSCostmap()), geometry_2_5x2_5, layer_name, grid_map_5x5_2_5x2_5_offset); - ROS_INFO_STREAM(" grid_map_5x5_3x3_offset : " << grid_map_5x5_3x3_offset.getPosition().transpose()); - ROS_INFO_STREAM(" grid_map_5x5_3x3_offset : " << grid_map_5x5_3x3_centre.getPosition().transpose()); - ROS_INFO_STREAM(" grid_map_5x5_3x3_offset : " << grid_map_5x5_2_5x2_5_offset.getPosition().transpose()); - ASSERT_EQ(-3.5, grid_map_5x5_3x3_offset.getPosition().x()); - ASSERT_EQ(2.5, grid_map_5x5_3x3_offset.getPosition().y()); - ASSERT_EQ(-3.5, grid_map_5x5_3x3_centre.getPosition().x()); - ASSERT_EQ(-3.5, grid_map_5x5_3x3_centre.getPosition().y()); - ASSERT_EQ(-9.75, grid_map_5x5_2_5x2_5_offset.getPosition().x()); - ASSERT_EQ(2.25, grid_map_5x5_2_5x2_5_offset.getPosition().y()); - std::cout << std::endl; -} - -/***************************************************************************** -** Main program -*****************************************************************************/ - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "test_from_ros_costmaps"); - - TransformBroadcaster broadcaster; - broadcastCostmap2DROSTestSuiteTransforms(broadcaster); - - testing::InitGoogleTest(&argc, argv); - int result = RUN_ALL_TESTS(); - broadcaster.shutdown(); - return result; -} diff --git a/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.test b/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.test deleted file mode 100644 index a66b74695..000000000 --- a/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/grid_map_costmap_2d/test/CMakeLists.txt b/grid_map_costmap_2d/test/CMakeLists.txt new file mode 100644 index 000000000..999537413 --- /dev/null +++ b/grid_map_costmap_2d/test/CMakeLists.txt @@ -0,0 +1,17 @@ +ament_add_gtest(${PROJECT_NAME}-test + test_grid_map_costmap_2d.cpp + test_costmap_2d_converter.cpp +) + +target_link_libraries(${PROJECT_NAME}-test + ${PROJECT_NAME}::${PROJECT_NAME} +) + +ament_add_gtest(costmap-2d-ros-test + test_costmap_2d_ros.cpp +) + +target_link_libraries(costmap-2d-ros-test + ${PROJECT_NAME}::${PROJECT_NAME} +) + diff --git a/grid_map_costmap_2d/test/Costmap2DConverterTest.cpp b/grid_map_costmap_2d/test/Costmap2DConverterTest.cpp deleted file mode 100644 index 7ee1a0788..000000000 --- a/grid_map_costmap_2d/test/Costmap2DConverterTest.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/* - * Costmap2DConverterTest.cpp - * - * Created on: Nov 23, 2016 - * Authors: Peter Fankhauser, Gabriel Hottiger - * Institute: ETH Zurich, ANYbotics - */ - -// Grid map -#include -#include - -// Gtest -#include - -// Eigen -#include - -using namespace grid_map; - -template -class TestCostmap2DConversion : public testing::Test { - public: - using TestValue = std::tuple; - - //! Constructor - TestCostmap2DConversion() : costmap2d_(8, 5, 1.0, 2.0, 3.0) { gridMap_.setGeometry(Length(8.0, 5.0), 1.0, Position(6.0, 5.5)); } - - //! Getter of test data - std::vector getTestValues(); - - //! Check that maps have same geometry - void assertMapGeometry(const GridMap& gridMap, const costmap_2d::Costmap2D& costMap) { - // Check map info. - // Different conventions: Costmap2d returns the *centerpoint* of the last cell in the map. - Length length = gridMap.getLength() - Length::Constant(0.5 * gridMap.getResolution()); - Length position = gridMap.getPosition() - 0.5 * gridMap.getLength().matrix(); - EXPECT_EQ(costMap.getSizeInMetersX(), length.x()); - EXPECT_EQ(costMap.getSizeInMetersY(), length.y()); - EXPECT_EQ(costMap.getSizeInCellsX(), gridMap.getSize()[0]); - EXPECT_EQ(costMap.getSizeInCellsY(), gridMap.getSize()[1]); - EXPECT_EQ(costMap.getResolution(), gridMap.getResolution()); - EXPECT_EQ(costMap.getOriginX(), position.x()); - EXPECT_EQ(costMap.getOriginY(), position.y()); - } - - protected: - Costmap2DConverter costmap2dConverter_; - costmap_2d::Costmap2D costmap2d_; - GridMap gridMap_; -}; - -//! Map type that has only mappings for cost map special values. -using Costmap2DSpecialTranslationTable = Costmap2DTranslationTable<3, 2, 1, 0>; -//! Map type that has larger intervals between special values. -using Costmap2DLargeIntervalsTranslationTable = Costmap2DTranslationTable<5, 500, 400, 100>; - -// Test data for special translation -template <> -std::vector::TestValue> -TestCostmap2DConversion::getTestValues() { - std::vector testValues; - testValues.emplace_back(TestValue(Position(3.2, 5.1), costmap_2d::FREE_SPACE, 0.f, true)); - testValues.emplace_back(TestValue(Position(4.2, 4.1), costmap_2d::INSCRIBED_INFLATED_OBSTACLE, 1.f, true)); - testValues.emplace_back(TestValue(Position(6.2, 3.1), costmap_2d::LETHAL_OBSTACLE, 2.f, true)); - testValues.emplace_back(TestValue(Position(5.2, 7.8), costmap_2d::NO_INFORMATION, 3.f, true)); - // Check for grid map to costmap only. - testValues.emplace_back(TestValue(Position(5.4, 6.8), costmap_2d::FREE_SPACE, -1.f, false)); - testValues.emplace_back(TestValue(Position(8.5, 4.5), costmap_2d::LETHAL_OBSTACLE, 4.f, false)); - return testValues; -} - -// Test data for special translation -template <> -std::vector::TestValue> -TestCostmap2DConversion::getTestValues() { - std::vector testValues; - testValues.emplace_back(TestValue(Position(3.2, 5.1), costmap_2d::FREE_SPACE, 100.f, true)); - testValues.emplace_back(TestValue(Position(4.2, 4.1), costmap_2d::INSCRIBED_INFLATED_OBSTACLE, 400.f, true)); - testValues.emplace_back(TestValue(Position(6.2, 3.1), costmap_2d::LETHAL_OBSTACLE, 500.f, true)); - testValues.emplace_back(TestValue(Position(5.2, 7.8), costmap_2d::NO_INFORMATION, 5.f, true)); - testValues.emplace_back(TestValue(Position(3.4, 3.8), 253/11, 100.f + 300.f/11.f, true)); - // Check for grid map to costmap only. - testValues.emplace_back(TestValue(Position(5.4, 6.8), costmap_2d::FREE_SPACE, 83.f, false)); - testValues.emplace_back(TestValue(Position(8.5, 4.5), costmap_2d::LETHAL_OBSTACLE, 504.f, false)); - testValues.emplace_back(TestValue(Position(4.7, 5.8), costmap_2d::INSCRIBED_INFLATED_OBSTACLE, 444.f, false)); - - return testValues; -} - -// Test data for direct translation -template <> -std::vector::TestValue> -TestCostmap2DConversion::getTestValues() { - std::vector testValues; - testValues.emplace_back(TestValue(Position(3.2, 5.1), costmap_2d::FREE_SPACE, costmap_2d::FREE_SPACE, true)); - testValues.emplace_back( - TestValue(Position(4.2, 4.1), costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true)); - testValues.emplace_back(TestValue(Position(6.2, 3.1), costmap_2d::LETHAL_OBSTACLE, costmap_2d::LETHAL_OBSTACLE, true)); - testValues.emplace_back(TestValue(Position(5.2, 7.8), costmap_2d::NO_INFORMATION, costmap_2d::NO_INFORMATION, true)); - testValues.emplace_back(TestValue(Position(5.4, 6.8), 1, 1.0, true)); - testValues.emplace_back(TestValue(Position(8.5, 4.5), 97, 97.0, true)); - // Check for grid map to costmap only. - testValues.emplace_back(TestValue(Position(4.7, 5.8), costmap_2d::FREE_SPACE, -30.f, false)); - testValues.emplace_back(TestValue(Position(3.4, 3.8), costmap_2d::LETHAL_OBSTACLE, 270.f, false)); - return testValues; -} - -// Test data for century translation -template <> -std::vector::TestValue> -TestCostmap2DConversion::getTestValues() { - std::vector testValues; - testValues.emplace_back(TestValue(Position(3.2, 5.1), costmap_2d::FREE_SPACE, 0.f, true)); - testValues.emplace_back(TestValue(Position(4.2, 4.1), costmap_2d::INSCRIBED_INFLATED_OBSTACLE, 99.f, true)); - testValues.emplace_back(TestValue(Position(6.2, 3.1), costmap_2d::LETHAL_OBSTACLE, 100.f, true)); - testValues.emplace_back(TestValue(Position(5.2, 7.8), costmap_2d::NO_INFORMATION, -1.f, true)); - testValues.emplace_back(TestValue(Position(5.4, 6.8), 253/11, 99.f/11.f, true)); - // Check for grid map to costmap only. - testValues.emplace_back(TestValue(Position(4.7, 5.8), costmap_2d::FREE_SPACE, -30.f, false)); - testValues.emplace_back(TestValue(Position(3.4, 3.8), costmap_2d::LETHAL_OBSTACLE, 270.f, false)); - return testValues; -} - -using TranslationTableTestTypes = ::testing::Types; - -TYPED_TEST_CASE(TestCostmap2DConversion, TranslationTableTestTypes); - -TYPED_TEST(TestCostmap2DConversion, initializeFromCostmap2d) { - // Convert to grid map. - GridMap gridMap; - this->costmap2dConverter_.initializeFromCostmap2D(this->costmap2d_, gridMap); - this->assertMapGeometry(gridMap, this->costmap2d_); -} - -TYPED_TEST(TestCostmap2DConversion, initializeFromGridMap) { - // Convert to Costmap2D. - costmap_2d::Costmap2D costmap2d; - this->costmap2dConverter_.initializeFromGridMap(this->gridMap_, costmap2d); - this->assertMapGeometry(this->gridMap_, costmap2d); -} - -TYPED_TEST(TestCostmap2DConversion, addLayerFromCostmap2d) { - // Create grid map. - const std::string layer("layer"); - GridMap gridMap; - this->costmap2dConverter_.initializeFromCostmap2D(this->costmap2d_, gridMap); - - // Fill in test data to Costmap2D. - for (const auto& testValue : this->getTestValues()) { - if (std::get<3>(testValue)) { - unsigned int xIndex, yIndex; - ASSERT_TRUE(this->costmap2d_.worldToMap(std::get<0>(testValue).x(), std::get<0>(testValue).y(), xIndex, yIndex)); - this->costmap2d_.getCharMap()[this->costmap2d_.getIndex(xIndex, yIndex)] = std::get<1>(testValue); - } - } - - // Copy data. - this->costmap2dConverter_.addLayerFromCostmap2D(this->costmap2d_, layer, gridMap); - - // Check data. - for (const auto& testValue : this->getTestValues()) { - if (std::get<3>(testValue)) { - EXPECT_EQ(std::get<2>(testValue), gridMap.atPosition(layer, std::get<0>(testValue))); - } - } -} - -TYPED_TEST(TestCostmap2DConversion, setCostmap2DFromGridMap) { - // Create costmap2d. - costmap_2d::Costmap2D costmap; - this->costmap2dConverter_.initializeFromGridMap(this->gridMap_, costmap); - - // Fill in test data to grid map. - const std::string layer("layer"); - this->gridMap_.add(layer); - for (const auto& testValue : this->getTestValues()) { - Index index; - this->gridMap_.getIndex(std::get<0>(testValue), index); - this->gridMap_.get(layer)(index(0), index(1)) = std::get<2>(testValue); - } - - // Copy data. - this->costmap2dConverter_.setCostmap2DFromGridMap(this->gridMap_, layer, costmap); - - // Check data. - for (const auto& testValue : this->getTestValues()) { - unsigned int xIndex, yIndex; - ASSERT_TRUE(costmap.worldToMap(std::get<0>(testValue).x(), std::get<0>(testValue).y(), xIndex, yIndex)); - costmap.getCharMap()[costmap.getIndex(xIndex, yIndex)] = std::get<1>(testValue); - } -} \ No newline at end of file diff --git a/grid_map_costmap_2d/test/test_costmap_2d_converter.cpp b/grid_map_costmap_2d/test/test_costmap_2d_converter.cpp new file mode 100644 index 000000000..bf037b3d3 --- /dev/null +++ b/grid_map_costmap_2d/test/test_costmap_2d_converter.cpp @@ -0,0 +1,355 @@ +/* + * test_costmap_2d_converter.cpp + * + * Created on: Nov 23, 2016 + * Authors: Peter Fankhauser, Gabriel Hottiger + * Institute: ETH Zurich, ANYbotics + */ + +// Gtest +#include + +#include +#include +#include + +// Grid map +#include "grid_map_core/grid_map_core.hpp" +#include "grid_map_costmap_2d/grid_map_costmap_2d.hpp" + +template +class TestCostmap2DConversion + : public testing::Test +{ +public: + using TestValue = std::tuple; + + //! Constructor + TestCostmap2DConversion() + : costmap2d_(8, 5, 1.0, 2.0, 3.0) + { + gridMap_.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(6.0, 5.5)); + } + + //! Getter of test data + std::vector getTestValues(); + + //! Check that maps have same geometry + void assertMapGeometry( + const grid_map::GridMap & gridMap, + const nav2_costmap_2d::Costmap2D & costMap) + { + // Check map info. + // Different conventions: Costmap2d returns the *centerpoint* of the last cell in the map. + grid_map::Length length = + gridMap.getLength() - grid_map::Length::Constant(0.5 * gridMap.getResolution()); + grid_map::Length position = gridMap.getPosition() - 0.5 * gridMap.getLength().matrix(); + EXPECT_EQ(costMap.getSizeInMetersX(), length.x()); + EXPECT_EQ(costMap.getSizeInMetersY(), length.y()); + EXPECT_EQ(costMap.getSizeInCellsX(), (unsigned int) gridMap.getSize()[0]); + EXPECT_EQ(costMap.getSizeInCellsY(), (unsigned int) gridMap.getSize()[1]); + EXPECT_EQ(costMap.getResolution(), gridMap.getResolution()); + EXPECT_EQ(costMap.getOriginX(), position.x()); + EXPECT_EQ(costMap.getOriginY(), position.y()); + } + +protected: + grid_map::Costmap2DConverter costmap2dConverter_; + nav2_costmap_2d::Costmap2D costmap2d_; + grid_map::GridMap gridMap_; +}; + +//! Map type that has only mappings for cost map special values. +using Costmap2DSpecialTranslationTable = grid_map::Costmap2DTranslationTable<3, 2, 1, 0>; +//! Map type that has larger intervals between special values. +using Costmap2DLargeIntervalsTranslationTable = + grid_map::Costmap2DTranslationTable<5, 500, 400, 100>; + +// Test data for special translation +template<> +std::vector::TestValue> +TestCostmap2DConversion::getTestValues() +{ + std::vector testValues; + testValues.emplace_back( + TestValue( + grid_map::Position(3.2, 5.1), + nav2_costmap_2d::FREE_SPACE, + 0.f, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(4.2, 4.1), + nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, + 1.f, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(6.2, 3.1), + nav2_costmap_2d::LETHAL_OBSTACLE, + 2.f, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(5.2, 7.8), + nav2_costmap_2d::NO_INFORMATION, + 3.f, + true)); + // Check for grid map to costmap only. + testValues.emplace_back( + TestValue( + grid_map::Position(5.4, 6.8), + nav2_costmap_2d::FREE_SPACE, + -1.f, + false)); + testValues.emplace_back( + TestValue( + grid_map::Position(8.5, 4.5), + nav2_costmap_2d::LETHAL_OBSTACLE, + 4.f, + false)); + return testValues; +} + +// Test data for special translation +template<> +std::vector::TestValue> +TestCostmap2DConversion::getTestValues() +{ + std::vector testValues; + testValues.emplace_back( + TestValue( + grid_map::Position(3.2, 5.1), + nav2_costmap_2d::FREE_SPACE, + 100.f, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(4.2, 4.1), + nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, + 400.f, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(6.2, 3.1), + nav2_costmap_2d::LETHAL_OBSTACLE, + 500.f, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(5.2, 7.8), + nav2_costmap_2d::NO_INFORMATION, + 5.f, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(3.4, 3.8), + 253 / 11, + 100.f + 300.f / 11.f, + true)); + // Check for grid map to costmap only. + testValues.emplace_back( + TestValue( + grid_map::Position(5.4, 6.8), + nav2_costmap_2d::FREE_SPACE, + 83.f, + false)); + testValues.emplace_back( + TestValue( + grid_map::Position(8.5, 4.5), + nav2_costmap_2d::LETHAL_OBSTACLE, + 504.f, + false)); + testValues.emplace_back( + TestValue( + grid_map::Position(4.7, 5.8), + nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, + 444.f, + false)); + + return testValues; +} + +// Test data for direct translation +template<> +std::vector::TestValue> +TestCostmap2DConversion::getTestValues() +{ + std::vector testValues; + testValues.emplace_back( + TestValue( + grid_map::Position(3.2, 5.1), + nav2_costmap_2d::FREE_SPACE, + nav2_costmap_2d::FREE_SPACE, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(4.2, 4.1), + nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, + nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(6.2, 3.1), + nav2_costmap_2d::LETHAL_OBSTACLE, + nav2_costmap_2d::LETHAL_OBSTACLE, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(5.2, 7.8), + nav2_costmap_2d::NO_INFORMATION, + nav2_costmap_2d::NO_INFORMATION, + true)); + testValues.emplace_back(TestValue(grid_map::Position(5.4, 6.8), 1, 1.0, true)); + testValues.emplace_back(TestValue(grid_map::Position(8.5, 4.5), 97, 97.0, true)); + // Check for grid map to costmap only. + testValues.emplace_back( + TestValue( + grid_map::Position(4.7, 5.8), + nav2_costmap_2d::FREE_SPACE, + -30.f, + false)); + testValues.emplace_back( + TestValue( + grid_map::Position(3.4, 3.8), + nav2_costmap_2d::LETHAL_OBSTACLE, + 270.f, + false)); + return testValues; +} + +// Test data for century translation +template<> +std::vector::TestValue> +TestCostmap2DConversion::getTestValues() +{ + std::vector testValues; + testValues.emplace_back( + TestValue( + grid_map::Position(3.2, 5.1), + nav2_costmap_2d::FREE_SPACE, + 0.f, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(4.2, 4.1), + nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, + 99.f, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(6.2, 3.1), + nav2_costmap_2d::LETHAL_OBSTACLE, + 100.f, + true)); + testValues.emplace_back( + TestValue( + grid_map::Position(5.2, 7.8), + nav2_costmap_2d::NO_INFORMATION, + -1.f, + true)); + testValues.emplace_back(TestValue(grid_map::Position(5.4, 6.8), 253 / 11, 99.f / 11.f, true)); + // Check for grid map to costmap only. + testValues.emplace_back( + TestValue( + grid_map::Position(4.7, 5.8), + nav2_costmap_2d::FREE_SPACE, + -30.f, + false)); + testValues.emplace_back( + TestValue( + grid_map::Position(3.4, 3.8), + nav2_costmap_2d::LETHAL_OBSTACLE, + 270.f, + false)); + return testValues; +} + +using TranslationTableTestTypes = ::testing::Types< + Costmap2DSpecialTranslationTable, + Costmap2DLargeIntervalsTranslationTable, + grid_map::Costmap2DDirectTranslationTable, + grid_map::Costmap2DCenturyTranslationTable>; + +TYPED_TEST_SUITE( + TestCostmap2DConversion, TranslationTableTestTypes, testing::internal::DefaultNameGenerator); + +TYPED_TEST(TestCostmap2DConversion, initializeFromCostmap2d) +{ + // Convert to grid map. + grid_map::GridMap gridMap; + this->costmap2dConverter_.initializeFromCostmap2D(this->costmap2d_, gridMap); + this->assertMapGeometry(gridMap, this->costmap2d_); +} + +TYPED_TEST(TestCostmap2DConversion, initializeFromGridMap) +{ + // Convert to Costmap2D. + nav2_costmap_2d::Costmap2D costmap2d; + this->costmap2dConverter_.initializeFromGridMap(this->gridMap_, costmap2d); + this->assertMapGeometry(this->gridMap_, costmap2d); +} + +TYPED_TEST(TestCostmap2DConversion, addLayerFromCostmap2d) +{ + // Create grid map. + const std::string layer("layer"); + grid_map::GridMap gridMap; + this->costmap2dConverter_.initializeFromCostmap2D(this->costmap2d_, gridMap); + + // Fill in test data to Costmap2D. + for (const auto & testValue : this->getTestValues()) { + if (std::get<3>(testValue)) { + unsigned int xIndex, yIndex; + ASSERT_TRUE( + this->costmap2d_.worldToMap( + std::get<0>(testValue).x(), + std::get<0>(testValue).y(), + xIndex, + yIndex)); + this->costmap2d_.getCharMap()[this->costmap2d_.getIndex(xIndex, yIndex)] = + std::get<1>(testValue); + } + } + + // Copy data. + this->costmap2dConverter_.addLayerFromCostmap2D(this->costmap2d_, layer, gridMap); + + // Check data. + for (const auto & testValue : this->getTestValues()) { + if (std::get<3>(testValue)) { + EXPECT_EQ(std::get<2>(testValue), gridMap.atPosition(layer, std::get<0>(testValue))); + } + } +} + +TYPED_TEST(TestCostmap2DConversion, setCostmap2DFromGridMap) +{ + // Create costmap2d. + nav2_costmap_2d::Costmap2D costmap; + this->costmap2dConverter_.initializeFromGridMap(this->gridMap_, costmap); + + // Fill in test data to grid map. + const std::string layer("layer"); + this->gridMap_.add(layer); + for (const auto & testValue : this->getTestValues()) { + grid_map::Index index; + this->gridMap_.getIndex(std::get<0>(testValue), index); + this->gridMap_.get(layer)(index(0), index(1)) = std::get<2>(testValue); + } + + // Copy data. + this->costmap2dConverter_.setCostmap2DFromGridMap(this->gridMap_, layer, costmap); + + // Check data. + for (const auto & testValue : this->getTestValues()) { + unsigned int xIndex, yIndex; + ASSERT_TRUE( + costmap.worldToMap( + std::get<0>(testValue).x(), + std::get<0>(testValue).y(), + xIndex, + yIndex)); + costmap.getCharMap()[costmap.getIndex(xIndex, yIndex)] = std::get<1>(testValue); + } +} diff --git a/grid_map_costmap_2d/test/test_costmap_2d_ros.cpp b/grid_map_costmap_2d/test/test_costmap_2d_ros.cpp new file mode 100644 index 000000000..47ac158d0 --- /dev/null +++ b/grid_map_costmap_2d/test/test_costmap_2d_ros.cpp @@ -0,0 +1,339 @@ +/***************************************************************************** +** Includes +*****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "tf2_ros/transform_broadcaster.h" +#include "tf2/transform_datatypes.h" + +#include "test_costmap_2d_ros.hpp" + +/***************************************************************************** +** Global rclcpp::Node +*****************************************************************************/ +std::shared_ptr node_ = nullptr; + +/***************************************************************************** +** Helpers +*****************************************************************************/ + +void broadcastCostmap2DROSTestSuiteTransforms(TransformBroadcaster & broadcaster) +{ + broadcaster.add( + "base_link_5x5", + tf2::Vector3(1.0, 1.0, 0.0), + tf2::Quaternion(0, 0, 0, 1)); + broadcaster.add( + "base_link_4x4", + tf2::Vector3(1.0, -3.0, 0.0), + tf2::Quaternion(0, 0, 0, 1)); + broadcaster.add( + "base_link_5x5_3x3_offset", + tf2::Vector3(-3.7, 2.4, 0.0), + tf2::Quaternion(0, 0, 0, 1)); + broadcaster.add( + "base_link_5x5_3x3_centre", + tf2::Vector3(-3.5, -3.5, 0.0), + tf2::Quaternion(0, 0, 0, 1)); + broadcaster.add( + "base_link_5x5_2_5x2_5_offset", + tf2::Vector3(-9.7, 2.4, 0.0), + tf2::Quaternion(0, 0, 0, 1)); + broadcaster.startBroadCastingThread(); +} + +/***************************************************************************** +** ROS Costmap Server +*****************************************************************************/ + +ROSCostmapServer::ROSCostmapServer( + const std::string & name, + const std::string & baseLinkTransformName, + const grid_map::Position & origin, + const int & width, + const int & height) +: tfBuffer_(node_->get_clock()), + tfListener_(tfBuffer_) +{ + // lots of parameters here affect the construction ( e.g. rolling window) + // if you don't have any parameters set, then this + // - alot of defaults which get dumped on the ros param server + // - fires up an obstacle layer and an inflation layer + // - creates a publisher for an occupancy grid + + costmap_ = std::make_shared(name); + costmap_->set_parameter(rclcpp::Parameter{"plugins", std::vector()}); + costmap_->set_parameter(rclcpp::Parameter{"robot_base_frame", baseLinkTransformName}); + costmap_->set_parameter(rclcpp::Parameter{"origin_x", origin.x()}); + costmap_->set_parameter(rclcpp::Parameter{"origin_y", origin.y()}); + costmap_->set_parameter(rclcpp::Parameter{"width", width}); + costmap_->set_parameter(rclcpp::Parameter{"height", height}); + costmap_->set_parameter(rclcpp::Parameter{"resolution", 0.5}); + // clears 1 cell if inside, up to 4 cells on a vertex + costmap_->set_parameter(rclcpp::Parameter{"robot_radius", 0.03}); + + costmap_->configure(); + costmap_->activate(); + + for (unsigned int index = 0; index < costmap_->getCostmap()->getSizeInCellsY(); ++index) { + unsigned int dimension = costmap_->getCostmap()->getSizeInCellsX(); + // @todo assert dimension > 1 + // set the first row to nav2_costmap_2d::FREE_SPACE? + // but it shows up invisibly in rviz, so don't bother + for (unsigned int fill_index = 0; fill_index < dimension - 2; ++fill_index) { + double fraction = static_cast(fill_index + 1) / + static_cast(costmap_->getCostmap()->getSizeInCellsX()); + costmap_->getCostmap()->setCost( + fill_index, + index, + fraction * nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + } + costmap_->getCostmap()->setCost(dimension - 2, index, nav2_costmap_2d::LETHAL_OBSTACLE); + costmap_->getCostmap()->setCost(dimension - 1, index, nav2_costmap_2d::NO_INFORMATION); + } +} + +/***************************************************************************** +** TransformBroadcaster +*****************************************************************************/ + +TransformBroadcaster::~TransformBroadcaster() +{ + broadcastingThread_.join(); +} + +void TransformBroadcaster::shutdown() +{ + shutdownFlag_ = true; +} + +void TransformBroadcaster::add( + const std::string & name, + tf2::Vector3 origin, + const tf2::Quaternion & orientation) +{ + geometry_msgs::msg::Transform transform; + transform.translation.x = origin.x(); + transform.translation.y = origin.y(); + transform.translation.z = origin.z(); + transform.rotation.x = orientation.x(); + transform.rotation.y = orientation.y(); + transform.rotation.z = orientation.z(); + transform.rotation.w = orientation.w(); + transforms_.insert(std::pair(name, transform)); +} + +void TransformBroadcaster::startBroadCastingThread() +{ + broadcastingThread_ = std::thread(&TransformBroadcaster::broadcast, this); +} + +void TransformBroadcaster::broadcast() +{ + while (rclcpp::ok() && !shutdownFlag_) { + for (std::pair p : transforms_) { + geometry_msgs::msg::TransformStamped transformMsg; + transformMsg.header.stamp = node_->now(); + transformMsg.header.frame_id = "map"; + transformMsg.child_frame_id = p.first; + transformMsg.transform = p.second; + tfBroadcaster_.sendTransform(transformMsg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } +} + +/***************************************************************************** +** Converter Functions +*****************************************************************************/ + +bool fromCostmap2DROS( + nav2_costmap_2d::Costmap2DROS & ros_costmap, + const std::string & layer_name, + grid_map::GridMap & grid_map) +{ + grid_map::Costmap2DConverter converter; + std::lock_guard lock( + *(ros_costmap.getCostmap()->getMutex())); + converter.initializeFromCostmap2D(ros_costmap, grid_map); + return converter.addLayerFromCostmap2D(ros_costmap, layer_name, grid_map); +} + +bool fromCostmap2DROSAtRobotPose( + nav2_costmap_2d::Costmap2DROS & ros_costmap, + const grid_map::Length & geometry, + const std::string & layer_name, + grid_map::GridMap & grid_map) +{ + grid_map::Costmap2DConverter converter; + std::lock_guard lock( + *(ros_costmap.getCostmap()->getMutex())); + if (!converter.initializeFromCostmap2DAtRobotPose(ros_costmap, geometry, grid_map)) { + return false; + } + return converter.addLayerFromCostmap2DAtRobotPose(ros_costmap, layer_name, grid_map); +} + +/***************************************************************************** +** Tests +*****************************************************************************/ + +TEST(Costmap2DROSConversion, full_window) +{ + std::cout << std::endl; + RCLCPP_INFO(node_->get_logger(), "***********************************************************"); + RCLCPP_INFO(node_->get_logger(), " Copy Full Window"); + RCLCPP_INFO(node_->get_logger(), "***********************************************************"); + // preparation + std::string layer_name = "obstacle_costs"; + ROSCostmapServer + ros_costmap_5x5("five_by_five", "base_link_5x5", grid_map::Position(0.0, 0.0), 5.0, 5.0); + grid_map::GridMap grid_map_5x5; + fromCostmap2DROS(*(ros_costmap_5x5.getROSCostmap()), layer_name, grid_map_5x5); + // assert map properties + ASSERT_EQ(grid_map_5x5.getFrameId(), ros_costmap_5x5.getROSCostmap()->getGlobalFrameID()); + ASSERT_EQ( + grid_map_5x5.getLength().x(), + ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsX() * + ros_costmap_5x5.getROSCostmap()->getCostmap()->getResolution()); + ASSERT_EQ( + grid_map_5x5.getLength().y(), + ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsY() * + ros_costmap_5x5.getROSCostmap()->getCostmap()->getResolution()); + ASSERT_EQ( + (unsigned int) grid_map_5x5.getSize()[0], + ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsX()); + ASSERT_EQ( + (unsigned int) grid_map_5x5.getSize()[1], + ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsY()); + grid_map::Length position = grid_map_5x5.getPosition() - 0.5 * grid_map_5x5.getLength().matrix(); + ASSERT_EQ(position.x(), ros_costmap_5x5.getROSCostmap()->getCostmap()->getOriginX()); + ASSERT_EQ(position.y(), ros_costmap_5x5.getROSCostmap()->getCostmap()->getOriginY()); + + // assert map data + for (unsigned int i = 0; i < 5; ++i) { + for (unsigned int j = 0; j < 5; ++j) { + std::cout << + static_cast(ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(i, j)) << " "; + } + std::cout << std::endl; + } + for (unsigned int i = 0; i < 5; ++i) { + for (unsigned int j = 0; j < 5; ++j) { + std::cout << static_cast(grid_map_5x5.at(layer_name, grid_map::Index(i, j))) << " "; + } + std::cout << std::endl; + } + // TODO(tbd): a function which does the index conversion + + std::cout << "Original cost: " << + static_cast(ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(8, 0)) << + std::endl; + std::cout << "New Cost: " << grid_map_5x5.at(layer_name, grid_map::Index(1, 9)) << std::endl; + std::vector cost_translation_table; + grid_map::Costmap2DCenturyTranslationTable::create(cost_translation_table); + unsigned char cost = ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(8, 0); + ASSERT_EQ(grid_map_5x5.at(layer_name, grid_map::Index(1, 9)), cost_translation_table[cost]); + std::cout << std::endl; +} + +TEST(Costmap2DROSConversion, cost_map_centres) +{ + std::cout << std::endl; + RCLCPP_INFO(node_->get_logger(), "***********************************************************"); + RCLCPP_INFO(node_->get_logger(), " Check Subwindow Centres"); + RCLCPP_INFO(node_->get_logger(), "***********************************************************"); + RCLCPP_INFO(node_->get_logger(), "Subwindows are centred as closely as possible to the robot"); + RCLCPP_INFO(node_->get_logger(), "pose, though not exactly. They still need to align with"); + RCLCPP_INFO(node_->get_logger(), "the underlying ros costmap so that they don't introduce a"); + RCLCPP_INFO(node_->get_logger(), "new kind of error. As a result, the centre is shifted from"); + RCLCPP_INFO(node_->get_logger(), "the robot pose to the nearest appropriate point which aligns"); + RCLCPP_INFO(node_->get_logger(), "the new cost map exactly on top of the original ros costmap."); + std::cout << std::endl; + std::string layer_name = "obstacle_costs"; + ROSCostmapServer ros_costmap_5x5_3x3_offset( + "five_by_five_three_by_three_offset", + "base_link_5x5_3x3_offset", + grid_map::Position(-6.0, 0.0), + 5, + 5); + ROSCostmapServer ros_costmap_5x5_3x3_centre( + "five_by_five_three_by_three_centre", + "base_link_5x5_3x3_centre", + grid_map::Position(-6.0, -6.0), + 5, + 5); + ROSCostmapServer ros_costmap_5x5_2_5x2_5_offset( + "five_by_five_twohalf_by_twohalf_offset", + "base_link_5x5_2_5x2_5_offset", + grid_map::Position(-12.0, 0.0), + 5, + 5); + grid_map::GridMap grid_map_5x5_3x3_offset, grid_map_5x5_3x3_centre, grid_map_5x5_2_5x2_5_offset; + grid_map::Length geometry_3x3(3.0, 3.0); + fromCostmap2DROSAtRobotPose( + *(ros_costmap_5x5_3x3_offset.getROSCostmap()), + geometry_3x3, + layer_name, + grid_map_5x5_3x3_offset); + fromCostmap2DROSAtRobotPose( + *(ros_costmap_5x5_3x3_centre.getROSCostmap()), + geometry_3x3, + layer_name, + grid_map_5x5_3x3_centre); + grid_map::Length geometry_2_5x2_5(2.5, 2.5); + fromCostmap2DROSAtRobotPose( + *(ros_costmap_5x5_2_5x2_5_offset.getROSCostmap()), + geometry_2_5x2_5, + layer_name, + grid_map_5x5_2_5x2_5_offset); + RCLCPP_INFO_STREAM( + node_->get_logger(), + " grid_map_5x5_3x3_offset : " << grid_map_5x5_3x3_offset.getPosition().transpose()); + RCLCPP_INFO_STREAM( + node_->get_logger(), + " grid_map_5x5_3x3_offset : " << grid_map_5x5_3x3_centre.getPosition().transpose()); + RCLCPP_INFO_STREAM( + node_->get_logger(), + " grid_map_5x5_3x3_offset : " << grid_map_5x5_2_5x2_5_offset.getPosition().transpose()); + ASSERT_EQ(-3.5, grid_map_5x5_3x3_offset.getPosition().x()); + ASSERT_EQ(2.5, grid_map_5x5_3x3_offset.getPosition().y()); + ASSERT_EQ(-3.5, grid_map_5x5_3x3_centre.getPosition().x()); + ASSERT_EQ(-3.5, grid_map_5x5_3x3_centre.getPosition().y()); + ASSERT_EQ(-9.75, grid_map_5x5_2_5x2_5_offset.getPosition().x()); + ASSERT_EQ(2.25, grid_map_5x5_2_5x2_5_offset.getPosition().y()); + std::cout << std::endl; +} + +/***************************************************************************** +** Main program +*****************************************************************************/ + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + node_ = std::make_shared("test_from_ros_costmaps"); + + TransformBroadcaster broadcaster(node_); + broadcastCostmap2DROSTestSuiteTransforms(broadcaster); + + testing::InitGoogleTest(&argc, argv); + + int test_result = RUN_ALL_TESTS(); + RCLCPP_INFO(node_->get_logger(), "gtest return value: %d", test_result); + + broadcaster.shutdown(); + rclcpp::shutdown(); + + node_.reset(); + + return test_result; +} diff --git a/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.hpp b/grid_map_costmap_2d/test/test_costmap_2d_ros.hpp similarity index 54% rename from grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.hpp rename to grid_map_costmap_2d/test/test_costmap_2d_ros.hpp index 34a01259b..86e58cbb9 100644 --- a/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.hpp +++ b/grid_map_costmap_2d/test/test_costmap_2d_ros.hpp @@ -1,22 +1,28 @@ -#pragma once +#ifndef TEST_COSTMAP_2D_ROS_HPP_ +#define TEST_COSTMAP_2D_ROS_HPP_ /***************************************************************************** ** Includes *****************************************************************************/ -#include -#include -#include -#if ROS_VERSION_MINIMUM(1,14,0) -#include -#else -#include -#endif - #include #include #include #include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "tf2_ros/transform_listener.h" +#include "geometry_msgs/msg/transform.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "geometry_msgs/msg/quaternion.hpp" + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "grid_map_core/grid_map_core.hpp" +#include "grid_map_costmap_2d/grid_map_costmap_2d.hpp" /***************************************************************************** ** Transforms @@ -25,20 +31,28 @@ /** * @brief Broadcast a set of transforms useful for various demos. */ -class TransformBroadcaster { +class TransformBroadcaster +{ public: - TransformBroadcaster() : shutdownFlag_(false) {} + explicit TransformBroadcaster(std::shared_ptr & node) + : shutdownFlag_(false), + tfBroadcaster_(*node) + {} virtual ~TransformBroadcaster(); - void add(const std::string& name, tf::Vector3 origin, const tf::Quaternion& orientation); + void add( + const std::string & name, + tf2::Vector3 origin, + const tf2::Quaternion & orientation); void startBroadCastingThread(); void broadcast(); void shutdown(); private: - std::map transforms_; + std::map transforms_; std::thread broadcastingThread_; std::atomic shutdownFlag_; + tf2_ros::TransformBroadcaster tfBroadcaster_; }; /** @@ -51,24 +65,34 @@ class TransformBroadcaster { * - second last stripe is filled with LETHAL_OBSTACLE cost * - last stripe is filled with NO_INFORMATION cost */ -class ROSCostmapServer { +class ROSCostmapServer +{ public: - typedef costmap_2d::Costmap2DROS ROSCostmap; + typedef nav2_costmap_2d::Costmap2DROS ROSCostmap; typedef std::shared_ptr ROSCostmapPtr; - ROSCostmapServer(const std::string& name, const std::string& baseLinkTransformName, - const grid_map::Position& origin, const double& width, const double& height); + ROSCostmapServer( + const std::string & name, + const std::string & baseLinkTransformName, + const grid_map::Position & origin, + const int & width, + const int & height); + + ~ROSCostmapServer() + { + costmap_->deactivate(); + costmap_->shutdown(); + } - ROSCostmapPtr getROSCostmap() { return costmap_; }; + ROSCostmapPtr getROSCostmap() + { + return costmap_; + } private: ROSCostmapPtr costmap_; -#if ROS_VERSION_MINIMUM(1,14,0) tf2_ros::Buffer tfBuffer_; tf2_ros::TransformListener tfListener_; -#else - tf::TransformListener tfListener_; -#endif }; @@ -82,5 +106,6 @@ class ROSCostmapServer { * * @param[in] broadcaster : uninitialised broadcaster object */ -void broadcastCostmap2DROSTestSuiteTransforms(TransformBroadcaster& broadcaster); +void broadcastCostmap2DROSTestSuiteTransforms(TransformBroadcaster & broadcaster); +#endif // TEST_COSTMAP_2D_ROS_HPP_ diff --git a/grid_map_costmap_2d/test/test_grid_map_costmap_2d.cpp b/grid_map_costmap_2d/test/test_grid_map_costmap_2d.cpp index 862017d3b..bb44eebdd 100644 --- a/grid_map_costmap_2d/test/test_grid_map_costmap_2d.cpp +++ b/grid_map_costmap_2d/test/test_grid_map_costmap_2d.cpp @@ -1,5 +1,5 @@ /* - * test_grid_map.cpp + * test_grid_map_costmap_2d.cpp * * Created on: Feb 10, 2014 * Author: Péter Fankhauser @@ -10,9 +10,9 @@ #include // Run all the tests that were declared with TEST() -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - srand((int)time(0)); + srand(static_cast(time(nullptr))); return RUN_ALL_TESTS(); } diff --git a/grid_map_cv/CHANGELOG.rst b/grid_map_cv/CHANGELOG.rst index 15ea23db1..d92e57121 100644 --- a/grid_map_cv/CHANGELOG.rst +++ b/grid_map_cv/CHANGELOG.rst @@ -2,6 +2,43 @@ Changelog for package grid_map_cv ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#458 `_ from ANYbotics/ci-temp-skip-octomap-server + build: treat several build issues on rolling +* suppress warning due to gcc13 bug +* Merge pull request `#442 `_ from Ryanf55/bugfix-use-old-style-for-filters + Bugfix use old style include directories for filters +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Add link to upstream ticket +* Use old-style variables for filter include directories +* Merge pull request `#404 `_ from Ryanf55/bugfix-403-cmake + grid_map_core: Use ament_export_targets and improve eigen linkage +* fix: use target link libraries instead of ament target deps +* Update ament to latest recommendations + * Fixes include errors in grid_map_geo ros2 port +* Contributors: Ryan, Ryan Friedman, wep21 + +2.1.0 (2022-11-08) +------------------ +* fix: change cv_bridge.h -> .hpp (`#376 `_) +* Contributors: Iván López Broceño + +2.0.0 (2022-09-13) +------------------ +* fix: mark Eigen library as SYSTEM +* fix: build error on jammy +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map_cv/CMakeLists.txt b/grid_map_cv/CMakeLists.txt index f3a9e0bad..a64522fde 100644 --- a/grid_map_cv/CMakeLists.txt +++ b/grid_map_cv/CMakeLists.txt @@ -1,66 +1,62 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(grid_map_cv) -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - grid_map_core - cv_bridge - filters -) +## Find ament packages +find_package(ament_cmake REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(filters REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(OpenCV REQUIRED COMPONENTS - opencv_photo + photo ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - grid_map_core - cv_bridge - filters - DEPENDS - OpenCV -) - -########### -## Build ## -########### +grid_map_package() ## Specify additional locations of header files include_directories( include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} ) +set(dependencies + cv_bridge + filters + grid_map_core + OpenCV + pluginlib + rclcpp + sensor_msgs +) + +########### +## Build ## +########### + ## Declare a cpp library -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/GridMapCvProcessing.cpp src/InpaintFilter.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} +target_link_libraries(${PROJECT_NAME} PUBLIC + cv_bridge::cv_bridge + grid_map_core::grid_map_core + opencv_photo + pluginlib::pluginlib + rclcpp::rclcpp + ${sensor_msgs_TARGETS} ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +# Filters does not expose targets (yet) +# https://github.com/ros/filters/pull/70 +target_include_directories(${PROJECT_NAME} + PUBLIC + ${filters_INCLUDE_DIRS} ) ############# @@ -70,38 +66,69 @@ add_dependencies(${PROJECT_NAME} # Mark executables and/or libraries for installation install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) # Mark cpp header files for installation install( DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} FILES_MATCHING PATTERN "*.hpp" ) # Mark other files for installation. install( FILES filter_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) ############# ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") -## Add gtest based cpp test target and link libraries -catkin_add_gtest(${PROJECT_NAME}-test - test/test_grid_map_cv.cpp - test/GridMapCvTest.cpp - test/GridMapCvProcessingTest.cpp) +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright -build/include_order + ) + endif() + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + ## Add gtest based cpp test target and link libraries + ament_add_gtest(${PROJECT_NAME}-test + test/test_grid_map_cv.cpp + test/GridMapCvTest.cpp + test/GridMapCvProcessingTest.cpp + ) endif() if(TARGET ${PROJECT_NAME}-test) target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) endif() +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(${dependencies}) +pluginlib_export_plugin_description_file(filters filter_plugins.xml) +ament_package() diff --git a/grid_map_cv/filter_plugins.xml b/grid_map_cv/filter_plugins.xml index cdccd5bdd..7f12e21fd 100644 --- a/grid_map_cv/filter_plugins.xml +++ b/grid_map_cv/filter_plugins.xml @@ -1,6 +1,7 @@ - - + + + Use OpenCV to inpaint/fill holes in a layer. diff --git a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp index fc6d0453b..233c28a0d 100644 --- a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp +++ b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp @@ -6,24 +6,37 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CV__GRIDMAPCVCONVERTER_HPP_ +#define GRID_MAP_CV__GRIDMAPCVCONVERTER_HPP_ #include // OpenCV -#include +#include // STD #include -namespace grid_map { +#include +#include + +// GCC 13 has false positive warnings around stringop-overflow. +// Suppress them until this is fixed in upstream gcc. See +// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=114758 for more details. +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstringop-overflow" +#endif + +namespace grid_map +{ /*! * Conversions between grid maps and OpenCV images. */ class GridMapCvConverter { - public: +public: /*! * Initializes the geometry of a grid map from an image. This changes * the geometry of the map and deletes all contents of the layers! @@ -33,8 +46,9 @@ class GridMapCvConverter * @param[in](optional) position the position of the grid map. * @return true if successful, false otherwise. */ - static bool initializeFromImage(const cv::Mat& image, const double resolution, - grid_map::GridMap& gridMap, const grid_map::Position& position) + static bool initializeFromImage( + const cv::Mat & image, const double resolution, + grid_map::GridMap & gridMap, const grid_map::Position & position) { const double lengthX = resolution * image.rows; const double lengthY = resolution * image.cols; @@ -56,9 +70,10 @@ class GridMapCvConverter * @return true if successful, false otherwise. */ template - static bool addLayerFromImage(const cv::Mat& image, const std::string& layer, - grid_map::GridMap& gridMap, const float lowerValue = 0.0, - const float upperValue = 1.0, const double alphaThreshold = 0.5) + static bool addLayerFromImage( + const cv::Mat & image, const std::string & layer, + grid_map::GridMap & gridMap, const float lowerValue = 0.0, + const float upperValue = 1.0, const double alphaThreshold = 0.5) { if (gridMap.getSize()(0) != image.rows || gridMap.getSize()(1) != image.cols) { std::cerr << "Image size does not correspond to grid map size!" << std::endl; @@ -66,16 +81,16 @@ class GridMapCvConverter } bool isColor = false; - if (image.channels() >= 3) isColor = true; + if (image.channels() >= 3) {isColor = true;} bool hasAlpha = false; - if (image.channels() >= 4) hasAlpha = true; + if (image.channels() >= 4) {hasAlpha = true;} cv::Mat imageMono; if (isColor && !hasAlpha) { cv::cvtColor(image, imageMono, CV_BGR2GRAY); } else if (isColor && hasAlpha) { cv::cvtColor(image, imageMono, CV_BGRA2GRAY); - } else if (!isColor && !hasAlpha){ + } else if (!isColor && !hasAlpha) { imageMono = image; } else { std::cerr << "Something went wrong when adding grid map layer form image!" << std::endl; @@ -87,8 +102,8 @@ class GridMapCvConverter float maxImageValue; if (std::is_same::value || std::is_same::value) { maxImageValue = 1.0; - } else if (std::is_same::value || std::is_same::value) { - maxImageValue = (float)std::numeric_limits::max(); + } else if (std::is_same::value || std::is_same::value) { + maxImageValue = static_cast(std::numeric_limits::max()); } else { std::cerr << "This image type is not supported." << std::endl; return false; @@ -97,25 +112,27 @@ class GridMapCvConverter const Type_ alphaTreshold = (Type_)(alphaThreshold * maxImageValue); gridMap.add(layer); - grid_map::Matrix& data = gridMap[layer]; + grid_map::Matrix & data = gridMap[layer]; for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { const Index index(*iterator); // Check for alpha layer. if (hasAlpha) { - const Type_ alpha = image.at>(index(0), index(1))[NChannels_ - 1]; - if (alpha < alphaTreshold) continue; + const Type_ alpha = + image.at>(index(0), index(1))[NChannels_ - 1]; + if (alpha < alphaTreshold) {continue;} } // Compute value. const Type_ imageValue = imageMono.at(index(0), index(1)); - const float mapValue = lowerValue + mapValueDifference * ((float) imageValue / maxImageValue); + const float mapValue = lowerValue + mapValueDifference * + (static_cast(imageValue) / maxImageValue); data(index(0), index(1)) = mapValue; } return true; - }; + } /*! * Adds a color layer with data from an image. @@ -125,8 +142,9 @@ class GridMapCvConverter * @return true if successful, false otherwise. */ template - static bool addColorLayerFromImage(const cv::Mat& image, const std::string& layer, - grid_map::GridMap& gridMap) + static bool addColorLayerFromImage( + const cv::Mat & image, const std::string & layer, + grid_map::GridMap & gridMap) { if (gridMap.getSize()(0) != image.rows || gridMap.getSize()(1) != image.cols) { std::cerr << "Image size does not correspond to grid map size!" << std::endl; @@ -134,7 +152,7 @@ class GridMapCvConverter } bool hasAlpha = false; - if (image.channels() >= 4) hasAlpha = true; + if (image.channels() >= 4) {hasAlpha = true;} cv::Mat imageRGB; if (hasAlpha) { @@ -146,7 +164,7 @@ class GridMapCvConverter gridMap.add(layer); for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { - const auto& cvColor = imageRGB.at>((*iterator)(0), (*iterator)(1)); + const auto & cvColor = imageRGB.at>((*iterator)(0), (*iterator)(1)); Eigen::Vector3i colorVector; colorVector(0) = cvColor[0]; colorVector(1) = cvColor[1]; @@ -170,13 +188,14 @@ class GridMapCvConverter * @return true if successful, false otherwise. */ template - static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer, - const int encoding, cv::Mat& image) + static bool toImage( + const grid_map::GridMap & gridMap, const std::string & layer, + const int encoding, cv::Mat & image) { const float minValue = gridMap.get(layer).minCoeffOfFinites(); const float maxValue = gridMap.get(layer).maxCoeffOfFinites(); return toImage(gridMap, layer, encoding, minValue, maxValue, image); - }; + } /*! * Creates a cv mat from a grid map layer. @@ -189,9 +208,10 @@ class GridMapCvConverter * @return true if successful, false otherwise. */ template - static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer, - const int encoding, const float lowerValue, const float upperValue, - cv::Mat& image) + static bool toImage( + const grid_map::GridMap & gridMap, const std::string & layer, + const int encoding, const float lowerValue, const float upperValue, + cv::Mat & image) { // Initialize image. if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) { @@ -205,7 +225,7 @@ class GridMapCvConverter Type_ imageMax; if (std::is_same::value || std::is_same::value) { imageMax = 1.0; - } else if (std::is_same::value || std::is_same::value) { + } else if (std::is_same::value || std::is_same::value) { imageMax = (Type_)std::numeric_limits::max(); } else { std::cerr << "This image type is not supported." << std::endl; @@ -215,36 +235,41 @@ class GridMapCvConverter // Clamp outliers. grid_map::GridMap map = gridMap; map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp(lowerValue, upperValue)); - const grid_map::Matrix& data = map[layer]; + const grid_map::Matrix & data = map[layer]; // Convert to image. bool isColor = false; - if (image.channels() >= 3) isColor = true; + if (image.channels() >= 3) {isColor = true;} bool hasAlpha = false; - if (image.channels() >= 4) hasAlpha = true; + if (image.channels() >= 4) {hasAlpha = true;} for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { const Index index(*iterator); if (std::isfinite(data(index(0), index(1)))) { - const float& value = data(index(0), index(1)); - const Type_ imageValue = (Type_) (((value - lowerValue) / (upperValue - lowerValue)) * (float) imageMax); + const float & value = data(index(0), index(1)); + const Type_ imageValue = + (Type_) (((value - lowerValue) / (upperValue - lowerValue)) * + static_cast(imageMax)); const Index imageIndex(iterator.getUnwrappedIndex()); unsigned int channel = 0; image.at>(imageIndex(0), imageIndex(1))[channel] = imageValue; if (isColor) { - image.at>(imageIndex(0), imageIndex(1))[++channel] = imageValue; - image.at>(imageIndex(0), imageIndex(1))[++channel] = imageValue; + image.at>( + imageIndex(0), + imageIndex(1))[++channel] = imageValue; + image.at>( + imageIndex(0), + imageIndex(1))[++channel] = imageValue; } if (hasAlpha) { image.at>(imageIndex(0), imageIndex(1))[++channel] = imageMax; } } } - return true; - }; - + } }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_CV__GRIDMAPCVCONVERTER_HPP_ diff --git a/grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp b/grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp index c1f5f9246..137d5c77d 100644 --- a/grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp +++ b/grid_map_cv/include/grid_map_cv/GridMapCvProcessing.hpp @@ -6,21 +6,23 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CV__GRIDMAPCVPROCESSING_HPP_ +#define GRID_MAP_CV__GRIDMAPCVPROCESSING_HPP_ #include // OpenCV -#include +#include -namespace grid_map { +namespace grid_map +{ /*! * Processing of grid maps with OpenCV methods. */ class GridMapCvProcessing { - public: +public: /*! * Default constructor. */ @@ -39,11 +41,12 @@ class GridMapCvProcessing * @param[in](optional) interpolationAlgorithm the interpolation method. * @return true if successful, false otherwise. */ - static bool changeResolution(const GridMap& gridMapSource, - GridMap& gridMapResult, - const double resolution, - const int interpolationAlgorithm = cv::INTER_CUBIC); - + static bool changeResolution( + const grid_map::GridMap & gridMapSource, + grid_map::GridMap & gridMapResult, + const double resolution, + const int interpolationAlgorithm = cv::INTER_CUBIC); }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_CV__GRIDMAPCVPROCESSING_HPP_ diff --git a/grid_map_cv/include/grid_map_cv/InpaintFilter.hpp b/grid_map_cv/include/grid_map_cv/InpaintFilter.hpp index dcf023c11..f4275c535 100644 --- a/grid_map_cv/include/grid_map_cv/InpaintFilter.hpp +++ b/grid_map_cv/include/grid_map_cv/InpaintFilter.hpp @@ -6,26 +6,28 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CV__INPAINTFILTER_HPP_ +#define GRID_MAP_CV__INPAINTFILTER_HPP_ -#include - -//OpenCV -#include "grid_map_cv/grid_map_cv.hpp" +#include #include #include #include -namespace grid_map { +// OpenCV +#include "grid_map_cv/grid_map_cv.hpp" + +namespace grid_map +{ /*! * Uses OpenCV function to inpaint/fill holes in the input layer. */ template -class InpaintFilter : public filters::FilterBase { - - public: +class InpaintFilter : public filters::FilterBase +{ +public: /*! * Constructor */ @@ -48,10 +50,9 @@ class InpaintFilter : public filters::FilterBase { * @param mapIn grid map containing input layer * @param mapOut grid map containing mapIn and inpainted input layer. */ - virtual bool update(const T& mapIn, T& mapOut); - - private: + virtual bool update(const T & mapIn, T & mapOut); +private: //! Inpainting radius. double radius_; @@ -62,4 +63,5 @@ class InpaintFilter : public filters::FilterBase { std::string outputLayer_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_CV__INPAINTFILTER_HPP_ diff --git a/grid_map_cv/include/grid_map_cv/grid_map_cv.hpp b/grid_map_cv/include/grid_map_cv/grid_map_cv.hpp index ed276c13e..27f3df3dd 100644 --- a/grid_map_cv/include/grid_map_cv/grid_map_cv.hpp +++ b/grid_map_cv/include/grid_map_cv/grid_map_cv.hpp @@ -6,8 +6,10 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_CV__GRID_MAP_CV_HPP_ +#define GRID_MAP_CV__GRID_MAP_CV_HPP_ #include #include #include +#endif // GRID_MAP_CV__GRID_MAP_CV_HPP_ diff --git a/grid_map_cv/include/grid_map_cv/utilities.hpp b/grid_map_cv/include/grid_map_cv/utilities.hpp new file mode 100644 index 000000000..ca1e9ea8c --- /dev/null +++ b/grid_map_cv/include/grid_map_cv/utilities.hpp @@ -0,0 +1,131 @@ +#ifndef GRID_MAP_CV__UTILITIES_HPP_ +#define GRID_MAP_CV__UTILITIES_HPP_ + +#include + +#include +#include + +namespace grid_map +{ + +/*! + * This is class is meant as a replacement for the filters::FilterBase::getParam method. + * This class offer declaring and reading paramters. + * It only returns a value if the the user has set the parameter in the parameter + * configuration yaml file and does not return a default value if the parameter + * is not found (unlike the FilterBase method mentioned above). + */ +class ParameterReader +{ +public: + /*! + * Constructor + * @param param_prefix The parameter prefix. + * @param params_interface The node parameters interface to use. + */ + ParameterReader( + std::string param_prefix, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface) + : param_prefix_(param_prefix), + params_interface_(params_interface) {} + + /*! + * Retrives parameter value when defined by user. + * @param name The name of the parameter to read. + * @param value_out The value of the parameter, will only be changed if the parameter is found. + * @param param_type The excpected type of the parameter. + * @return - true if a parameter defined by the user is found. + */ + template + bool get_template(const std::string & name, T & value_out, rclcpp::ParameterType param_type) + { + rclcpp::Parameter param; + + params_interface_->declare_parameter(param_prefix_ + name, param_type); + params_interface_->get_parameter(param_prefix_ + name, param); + + if (param.get_type() != param_type) { + return false; + } else { + value_out = param.get_value(); + return true; + } + } + + /*! + * Reads parameter of type int. + * @param name The name of the parameter to read. + * @param value_out The value of the parameter, will only be changed if the parameter is found. + * @return - true if a parameter defined by the user is found. + */ + bool get(const std::string & name, int & value_out) + { + return get_template(name, value_out, rclcpp::PARAMETER_INTEGER); + } + + /*! + * Reads parameter of type double. + * @param name The name of the parameter to read. + * @param value_out The value of the parameter, will only be changed if the parameter is found. + * @return - true if a parameter defined by the user is found. + */ + bool get(const std::string & name, double & value_out) + { + return get_template(name, value_out, rclcpp::PARAMETER_DOUBLE); + } + + /*! + * Reads parameter of type std::string. + * @param name The name of the parameter to read. + * @param value_out The value of the parameter, will only be changed if the parameter is found. + * @return - true if a parameter defined by the user is found. + */ + bool get(const std::string & name, std::string & value_out) + { + return get_template(name, value_out, rclcpp::PARAMETER_STRING); + } + + /*! + * Reads parameter of type boolean. + * @param name The name of the parameter to read. + * @param value_out The value of the parameter, will only be changed if the parameter is found. + * @return - true if a parameter defined by the user is found. + */ + bool get(const std::string & name, bool & value_out) + { + return get_template(name, value_out, rclcpp::PARAMETER_BOOL); + } + + /*! + * Reads parameter of type array of double. + * @param name The name of the parameter to read. + * @param value_out The value of the parameter, will only be changed if the parameter is found. + * @return - true if a parameter defined by the user is found. + */ + bool get(const std::string & name, std::vector & value_out) + { + return get_template(name, value_out, rclcpp::PARAMETER_DOUBLE_ARRAY); + } + + /*! + * Reads parameter of type array of string. + * @param name The name of the parameter to read. + * @param value_out The value of the parameter, will only be changed if the parameter is found. + * @return - true if a parameter defined by the user is found. + */ + bool get(const std::string & name, std::vector & value_out) + { + return get_template>(name, value_out, rclcpp::PARAMETER_STRING_ARRAY); + } + +private: + //! Layer the threshold should be applied to. + std::string param_prefix_; + + //! Parameters interface + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; +}; +} // namespace grid_map + +#endif // GRID_MAP_CV__UTILITIES_HPP_ diff --git a/grid_map_cv/package.xml b/grid_map_cv/package.xml index 13209229b..5ae04507a 100644 --- a/grid_map_cv/package.xml +++ b/grid_map_cv/package.xml @@ -1,19 +1,32 @@ - + grid_map_cv - 1.6.2 + 2.2.2 Conversions between grid maps and OpenCV images. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Péter Fankhauser - catkin + + ament_cmake + grid_map_cmake_helpers + grid_map_core cv_bridge filters + pluginlib + rclcpp + sensor_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + ament_cmake diff --git a/grid_map_cv/src/GridMapCvProcessing.cpp b/grid_map_cv/src/GridMapCvProcessing.cpp index 10d38d855..746e09196 100644 --- a/grid_map_cv/src/GridMapCvProcessing.cpp +++ b/grid_map_cv/src/GridMapCvProcessing.cpp @@ -9,7 +9,8 @@ #include "grid_map_cv/GridMapCvProcessing.hpp" #include "grid_map_cv/GridMapCvConverter.hpp" -namespace grid_map { +namespace grid_map +{ GridMapCvProcessing::GridMapCvProcessing() { @@ -19,39 +20,58 @@ GridMapCvProcessing::~GridMapCvProcessing() { } -bool GridMapCvProcessing::changeResolution(const GridMap& gridMapSource, - GridMap& gridMapResult, - const double resolution, - const int interpolationAlgorithm) +bool GridMapCvProcessing::changeResolution( + const grid_map::GridMap & gridMapSource, + grid_map::GridMap & gridMapResult, + const double resolution, + const int interpolationAlgorithm) { - GridMap gridMapSourceCopy(gridMapSource); + grid_map::GridMap gridMapSourceCopy(gridMapSource); gridMapSourceCopy.convertToDefaultStartIndex(); const double sizeFactor = gridMapSourceCopy.getResolution() / resolution; bool firstLayer = true; - for (const auto& layer : gridMapSourceCopy.getLayers()) { + for (const auto & layer : gridMapSourceCopy.getLayers()) { cv::Mat imageSource, imageResult; const float minValue = gridMapSourceCopy.get(layer).minCoeffOfFinites(); const float maxValue = gridMapSourceCopy.get(layer).maxCoeffOfFinites(); const bool hasNaN = gridMapSourceCopy.get(layer).hasNaN(); bool result; if (hasNaN) { - result = GridMapCvConverter::toImage(gridMapSourceCopy, layer, CV_16UC4, minValue, maxValue, imageSource); + result = grid_map::GridMapCvConverter::toImage( + gridMapSourceCopy, layer, CV_16UC4, + minValue, maxValue, imageSource); } else { - result = GridMapCvConverter::toImage(gridMapSourceCopy, layer, CV_16UC1, minValue, maxValue, imageSource); + result = grid_map::GridMapCvConverter::toImage( + gridMapSourceCopy, layer, CV_16UC1, + minValue, maxValue, imageSource); } - if (!result) return false; - cv::resize(imageSource, imageResult, cv::Size(0.0, 0.0), sizeFactor, sizeFactor, interpolationAlgorithm); + if (!result) {return false;} + cv::resize( + imageSource, imageResult, cv::Size( + 0.0, + 0.0), sizeFactor, sizeFactor, + interpolationAlgorithm); if (firstLayer) { - if (!GridMapCvConverter::initializeFromImage(imageResult, resolution, gridMapResult, gridMapSourceCopy.getPosition())) + if (!GridMapCvConverter::initializeFromImage( + imageResult, resolution, gridMapResult, + gridMapSourceCopy.getPosition())) + { return false; + } firstLayer = false; } if (hasNaN) { - result = GridMapCvConverter::addLayerFromImage(imageResult, layer, gridMapResult, minValue, maxValue); + result = grid_map::GridMapCvConverter::addLayerFromImage( + imageResult, layer, + gridMapResult, minValue, + maxValue); } else { - result = GridMapCvConverter::addLayerFromImage(imageResult, layer, gridMapResult, minValue, maxValue); + result = grid_map::GridMapCvConverter::addLayerFromImage( + imageResult, layer, + gridMapResult, minValue, + maxValue); } - if (!result) return false; + if (!result) {return false;} } gridMapResult.setFrameId(gridMapSourceCopy.getFrameId()); gridMapResult.setTimestamp(gridMapSourceCopy.getTimestamp()); @@ -59,4 +79,4 @@ bool GridMapCvProcessing::changeResolution(const GridMap& gridMapSource, return true; } -} /* namespace */ +} // namespace grid_map diff --git a/grid_map_cv/src/InpaintFilter.cpp b/grid_map_cv/src/InpaintFilter.cpp index 4bd92b3a5..befc215b2 100644 --- a/grid_map_cv/src/InpaintFilter.cpp +++ b/grid_map_cv/src/InpaintFilter.cpp @@ -7,66 +7,81 @@ */ #include "grid_map_cv/InpaintFilter.hpp" -#include -#include +#include +#include // Grid Map #include -using namespace filters; +#include +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template InpaintFilter::InpaintFilter() - : radius_(5.0) { - +: radius_(5.0) +{ } template -InpaintFilter::~InpaintFilter() { - +InpaintFilter::~InpaintFilter() +{ } template -bool InpaintFilter::configure() { - if (!FilterBase < T > ::getParam(std::string("radius"), radius_)) { - ROS_ERROR("InpaintRadius filter did not find param radius."); +bool InpaintFilter::configure() +{ + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("radius"), radius_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), "InpaintRadius filter did not find param radius."); return false; } if (radius_ < 0.0) { - ROS_ERROR("Radius must be greater than zero."); + RCLCPP_ERROR(this->logging_interface_->get_logger(), "Radius must be greater than zero."); return false; } - ROS_DEBUG("Radius = %f.", radius_); + RCLCPP_DEBUG(this->logging_interface_->get_logger(), "Radius = %f.", radius_); - if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) { - ROS_ERROR("Inpaint filter did not find parameter `input_layer`."); + if (!param_reader.get(std::string("input_layer"), inputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Inpaint filter did not find parameter `input_layer`."); return false; } - ROS_DEBUG("Inpaint input layer is = %s.", inputLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Inpaint input layer is = %s.", inputLayer_.c_str()); - if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) { - ROS_ERROR("Inpaint filter did not find parameter `output_layer`."); + if (!param_reader.get(std::string("output_layer"), outputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Inpaint filter did not find parameter `output_layer`."); return false; } - ROS_DEBUG("Inpaint output layer = %s.", outputLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Inpaint output layer = %s.", outputLayer_.c_str()); return true; } template -bool InpaintFilter::update(const T& mapIn, T& mapOut) { +bool InpaintFilter::update(const T & mapIn, T & mapOut) +{ // Add new layer to the elevation map. mapOut = mapIn; mapOut.add(outputLayer_); - //Convert elevation layer to OpenCV image to fill in holes. - //Get the inpaint mask (nonzero pixels indicate where values need to be filled in). + // Convert elevation layer to OpenCV image to fill in holes. + // Get the inpaint mask (nonzero pixels indicate where values need to be filled in). mapOut.add("inpaint_mask", 0.0); mapOut.setBasicLayers(std::vector()); @@ -81,19 +96,24 @@ bool InpaintFilter::update(const T& mapIn, T& mapOut) { const float minValue = mapOut.get(inputLayer_).minCoeffOfFinites(); const float maxValue = mapOut.get(inputLayer_).maxCoeffOfFinites(); - grid_map::GridMapCvConverter::toImage(mapOut, inputLayer_, CV_8UC3, minValue, maxValue, - originalImage); + grid_map::GridMapCvConverter::toImage( + mapOut, inputLayer_, CV_8UC3, minValue, maxValue, + originalImage); grid_map::GridMapCvConverter::toImage(mapOut, "inpaint_mask", CV_8UC1, mask); const double radiusInPixels = radius_ / mapIn.getResolution(); cv::inpaint(originalImage, mask, filledImage, radiusInPixels, cv::INPAINT_NS); - grid_map::GridMapCvConverter::addLayerFromImage(filledImage, outputLayer_, mapOut, minValue, maxValue); + grid_map::GridMapCvConverter::addLayerFromImage( + filledImage, outputLayer_, + mapOut, minValue, maxValue); mapOut.erase("inpaint_mask"); return true; } -}/* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::InpaintFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::InpaintFilter, + filters::FilterBase) diff --git a/grid_map_cv/test/GridMapCvProcessingTest.cpp b/grid_map_cv/test/GridMapCvProcessingTest.cpp index 2b2b28469..261856ad3 100644 --- a/grid_map_cv/test/GridMapCvProcessingTest.cpp +++ b/grid_map_cv/test/GridMapCvProcessingTest.cpp @@ -5,48 +5,49 @@ * Author: Peter Fankhauser */ -#include "grid_map_cv/grid_map_cv.hpp" - #include #include +// OpenCV +#include + // gtest #include -// OpenCV -#include - -using namespace std; -using namespace grid_map; +#include "grid_map_cv/grid_map_cv.hpp" TEST(GridMapCvProcessing, changeResolution) { // Create grid map. - GridMap mapIn; + grid_map::GridMap mapIn; mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01); mapIn.add("layer", 1.0); - for (grid_map::CircleIterator iterator(mapIn, mapIn.getPosition(), 0.2); !iterator.isPastEnd(); ++iterator) { + for (grid_map::CircleIterator iterator(mapIn, mapIn.getPosition(), 0.2); !iterator.isPastEnd(); + ++iterator) + { mapIn.at("layer", *iterator) = 2.0; } // Change resolution. - GridMap mapOut; - EXPECT_TRUE(GridMapCvProcessing::changeResolution(mapIn, mapOut, 0.1)); + grid_map::GridMap mapOut; + EXPECT_TRUE(grid_map::GridMapCvProcessing::changeResolution(mapIn, mapOut, 0.1)); // Check data. EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); EXPECT_TRUE(mapIn.getPosition() == mapOut.getPosition()); EXPECT_TRUE((mapIn.getSize() == mapOut.getSize() * 10).all()); - EXPECT_EQ(mapIn["layer"](0, 0), mapOut["layer"](0, 0)); // Corner. - EXPECT_EQ(mapIn.atPosition("layer", mapIn.getPosition()), mapOut.atPosition("layer", mapOut.getPosition())); // Center. + EXPECT_EQ(mapIn["layer"](0, 0), mapOut["layer"](0, 0)); // Corner. + EXPECT_EQ( + mapIn.atPosition("layer", mapIn.getPosition()), + mapOut.atPosition("layer", mapOut.getPosition())); // Center. } TEST(GridMapCvProcessing, changeResolutionForMovedMap) { // Create grid map. - GridMap mapIn; + grid_map::GridMap mapIn; mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01); - Position position(0.3, 0.4); + grid_map::Position position(0.3, 0.4); mapIn.move(position); mapIn.add("layer", 1.0); for (grid_map::CircleIterator iterator(mapIn, position, 0.2); !iterator.isPastEnd(); ++iterator) { @@ -54,13 +55,15 @@ TEST(GridMapCvProcessing, changeResolutionForMovedMap) } // Change resolution. - GridMap mapOut; - EXPECT_TRUE(GridMapCvProcessing::changeResolution(mapIn, mapOut, 0.1)); + grid_map::GridMap mapOut; + EXPECT_TRUE(grid_map::GridMapCvProcessing::changeResolution(mapIn, mapOut, 0.1)); // Check data. EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); EXPECT_TRUE(mapIn.getPosition() == mapOut.getPosition()); EXPECT_TRUE((mapIn.getSize() == mapOut.getSize() * 10).all()); - EXPECT_EQ(mapIn["layer"](0, 0), mapOut["layer"](0, 0)); // Corner. - EXPECT_EQ(mapIn.atPosition("layer", mapIn.getPosition()), mapOut.atPosition("layer", mapOut.getPosition())); // Center. + EXPECT_EQ(mapIn["layer"](0, 0), mapOut["layer"](0, 0)); // Corner. + EXPECT_EQ( + mapIn.atPosition("layer", mapIn.getPosition()), + mapOut.atPosition("layer", mapOut.getPosition())); // Center. } diff --git a/grid_map_cv/test/GridMapCvTest.cpp b/grid_map_cv/test/GridMapCvTest.cpp index 16da86bcc..5205d2d1c 100644 --- a/grid_map_cv/test/GridMapCvTest.cpp +++ b/grid_map_cv/test/GridMapCvTest.cpp @@ -6,24 +6,24 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_cv/grid_map_cv.hpp" - #include #include +// OpenCV +#include + // gtest #include -// OpenCV -#include +#include + +#include "grid_map_cv/grid_map_cv.hpp" -using namespace std; -using namespace grid_map; TEST(ImageConversion, roundTrip8UC3) { // Create grid map. - GridMap mapIn({"layer"}); + grid_map::GridMap mapIn({"layer"}); mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01); mapIn["layer"].setRandom(); const float minValue = -1.0; @@ -31,16 +31,21 @@ TEST(ImageConversion, roundTrip8UC3) // Convert to image. cv::Mat image; - GridMapCvConverter::toImage(mapIn, "layer", CV_8UC3, minValue, maxValue, image); + grid_map::GridMapCvConverter::toImage( + mapIn, "layer", CV_8UC3, minValue, + maxValue, image); // Convert back to grid map. - GridMap mapOut(mapIn); + grid_map::GridMap mapOut(mapIn); mapOut["layer"].setConstant(NAN); - GridMapCvConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue); + grid_map::GridMapCvConverter::addLayerFromImage( + image, "layer", mapOut, minValue, + maxValue); // Check data. - const float resolution = (maxValue - minValue) / (float) std::numeric_limits::max(); - expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); + const float resolution = (maxValue - minValue) / + static_cast(std::numeric_limits::max()); + grid_map::expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); } @@ -48,25 +53,30 @@ TEST(ImageConversion, roundTrip8UC3) TEST(ImageConversion, roundTrip8UC4) { // Create grid map. - GridMap mapIn({"layer"}); + grid_map::GridMap mapIn({"layer"}); mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.1); mapIn["layer"].setRandom(); - mapIn["layer"](1, 2) = NAN; // To check for transparnecy/nan handling. + mapIn["layer"](1, 2) = NAN; // To check for transparnecy/nan handling. const float minValue = -1.0; const float maxValue = 1.0; // Convert to image. cv::Mat image; - GridMapCvConverter::toImage(mapIn, "layer", CV_8UC4, minValue, maxValue, image); + grid_map::GridMapCvConverter::toImage( + mapIn, "layer", CV_8UC4, minValue, + maxValue, image); // Convert back to grid map. - GridMap mapOut(mapIn); + grid_map::GridMap mapOut(mapIn); mapOut["layer"].setConstant(NAN); - GridMapCvConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue); + grid_map::GridMapCvConverter::addLayerFromImage( + image, "layer", mapOut, minValue, + maxValue); // Check data. - const float resolution = (maxValue - minValue) / (float) std::numeric_limits::max(); - expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); + const float resolution = (maxValue - minValue) / + static_cast(std::numeric_limits::max()); + grid_map::expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); } @@ -74,7 +84,7 @@ TEST(ImageConversion, roundTrip8UC4) TEST(ImageConversion, roundTrip16UC1) { // Create grid map. - GridMap mapIn({"layer"}); + grid_map::GridMap mapIn({"layer"}); mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01); mapIn["layer"].setRandom(); const float minValue = -1.0; @@ -82,16 +92,21 @@ TEST(ImageConversion, roundTrip16UC1) // Convert to image. cv::Mat image; - GridMapCvConverter::toImage(mapIn, "layer", CV_16UC1, minValue, maxValue, image); + grid_map::GridMapCvConverter::toImage( + mapIn, "layer", CV_16UC1, minValue, maxValue, + image); // Convert back to grid map. - GridMap mapOut(mapIn); + grid_map::GridMap mapOut(mapIn); mapOut["layer"].setConstant(NAN); - GridMapCvConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue); + grid_map::GridMapCvConverter::addLayerFromImage( + image, "layer", mapOut, minValue, + maxValue); // Check data. - const float resolution = (maxValue - minValue) / (float) std::numeric_limits::max(); - expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); + const float resolution = (maxValue - minValue) / + static_cast(std::numeric_limits::max()); + grid_map::expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); } @@ -99,7 +114,7 @@ TEST(ImageConversion, roundTrip16UC1) TEST(ImageConversion, roundTrip32FC1) { // Create grid map. - GridMap mapIn({"layer"}); + grid_map::GridMap mapIn({"layer"}); mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01); mapIn["layer"].setRandom(); const float minValue = -1.0; @@ -107,16 +122,21 @@ TEST(ImageConversion, roundTrip32FC1) // Convert to image. cv::Mat image; - GridMapCvConverter::toImage(mapIn, "layer", CV_32FC1, minValue, maxValue, image); + grid_map::GridMapCvConverter::toImage( + mapIn, "layer", CV_32FC1, minValue, maxValue, + image); // Convert back to grid map. - GridMap mapOut(mapIn); + grid_map::GridMap mapOut(mapIn); mapOut["layer"].setConstant(NAN); - GridMapCvConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue); + grid_map::GridMapCvConverter::addLayerFromImage( + image, "layer", mapOut, minValue, + maxValue); // Check data. - const float resolution = (maxValue - minValue) / (float) std::numeric_limits::max(); - expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); + const float resolution = (maxValue - minValue) / + static_cast(std::numeric_limits::max()); + grid_map::expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); } diff --git a/grid_map_cv/test/test_grid_map_cv.cpp b/grid_map_cv/test/test_grid_map_cv.cpp index d83f356f0..7d7f688d5 100644 --- a/grid_map_cv/test/test_grid_map_cv.cpp +++ b/grid_map_cv/test/test_grid_map_cv.cpp @@ -10,9 +10,9 @@ #include // Run all the tests that were declared with TEST() -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - srand((int)time(0)); + srand(static_cast(time(0))); return RUN_ALL_TESTS(); } diff --git a/grid_map_demos/CHANGELOG.rst b/grid_map_demos/CHANGELOG.rst index 39319eebe..427b5a4ae 100644 --- a/grid_map_demos/CHANGELOG.rst +++ b/grid_map_demos/CHANGELOG.rst @@ -2,6 +2,36 @@ Changelog for package grid_map_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Merge pull request `#419 `_ from Ryanf55/stop-using-CMAKE_COMPILER_IS_GNUCXX + Stop using deprecated CMAKE_COMPILER_IS_GNUCXX +* Stop using deprecated CMAKE_COMPILER_IS_GNUCXX + * Switch to CMAKE_CXX_COMPILER_ID + * https://cmake.org/cmake/help/latest/variable/CMAKE_LANG_COMPILER_ID.html +* Contributors: Ryan, Ryan Friedman + +2.1.0 (2022-11-08) +------------------ +* fix: change cv_bridge.h -> .hpp (`#376 `_) +* Contributors: Iván López Broceño + +2.0.0 (2022-09-13) +------------------ +* fix: mark Eigen library as SYSTEM +* fix: build error on jammy +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map_demos/CMakeLists.txt b/grid_map_demos/CMakeLists.txt index 244109947..e64cf57ee 100644 --- a/grid_map_demos/CMakeLists.txt +++ b/grid_map_demos/CMakeLists.txt @@ -1,26 +1,20 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5.0) project(grid_map_demos) -set(CMAKE_CXX_STANDARD 11) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roscpp - grid_map_core - grid_map_ros - grid_map_cv - grid_map_filters - grid_map_loader - grid_map_msgs - grid_map_octomap - grid_map_rviz_plugin - grid_map_visualization - geometry_msgs - sensor_msgs - cv_bridge - octomap_msgs - filters -) +## Find ament_cmake macros and libraries +find_package(ament_cmake REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(filters REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(grid_map_cv REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(grid_map_octomap REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(OCTOMAP REQUIRED) +find_package(octomap_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(OpenCV REQUIRED COMPONENTS @@ -28,22 +22,12 @@ find_package(OpenCV REQUIRED CONFIG ) -find_package(octomap REQUIRED) +grid_map_package() -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS -# DEPENDS system_lib +set(dependencies + grid_map_msgs + grid_map_ros + rclcpp ) ########### @@ -54,9 +38,8 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} - ${OCTOMAP_INCLUDE_DIR} + SYSTEM + ${EIGEN3_INCLUDE_DIR} ) ## Declare a cpp executable @@ -111,6 +94,9 @@ add_executable( add_executable( filters_demo src/filters_demo_node.cpp +) + +add_library(filters_demo_lib SHARED src/FiltersDemo.cpp ) @@ -126,99 +112,154 @@ add_executable( ) ## Specify libraries to link a library or executable target against -target_link_libraries( - simple_demo - ${catkin_LIBRARIES} +ament_target_dependencies( + simple_demo SYSTEM + ${dependencies} ) -target_link_libraries( - tutorial_demo - ${catkin_LIBRARIES} +ament_target_dependencies( + tutorial_demo SYSTEM + ${dependencies} ) -target_link_libraries( - iterators_demo - ${catkin_LIBRARIES} +ament_target_dependencies( + iterators_demo SYSTEM + ${dependencies} ) -target_link_libraries( - image_to_gridmap_demo - ${catkin_LIBRARIES} +ament_target_dependencies(image_to_gridmap_demo SYSTEM + ${dependencies} ) -target_link_libraries( - octomap_to_gridmap_demo - ${catkin_LIBRARIES} - ${OCTOMAP_LIBRARIES} +ament_target_dependencies( + octomap_to_gridmap_demo SYSTEM + ${dependencies} + grid_map_octomap + octomap_msgs ) -target_link_libraries( - move_demo - ${catkin_LIBRARIES} +ament_target_dependencies( + move_demo SYSTEM + ${dependencies} ) -target_link_libraries( - iterator_benchmark - ${catkin_LIBRARIES} +ament_target_dependencies( + iterator_benchmark SYSTEM + grid_map_core ) -target_link_libraries( - opencv_demo - ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} +ament_target_dependencies( + opencv_demo SYSTEM + ${dependencies} + grid_map_cv ) -target_link_libraries( - resolution_change_demo - ${catkin_LIBRARIES} +ament_target_dependencies( + resolution_change_demo SYSTEM + ${dependencies} ) -target_link_libraries( - filters_demo - ${catkin_LIBRARIES} +target_link_libraries(filters_demo filters_demo_lib) + +ament_target_dependencies( + filters_demo_lib SYSTEM + ${dependencies} + filters ) -target_link_libraries( - normal_filter_comparison_demo - ${catkin_LIBRARIES} +ament_target_dependencies( + filters_demo SYSTEM + ${dependencies} + filters +) + +ament_target_dependencies( + normal_filter_comparison_demo SYSTEM + ${dependencies} + filters ) -target_link_libraries( +ament_target_dependencies( + interpolation_demo SYSTEM + ${dependencies} +) + +set(targets_list + filters_demo + filters_demo_lib + image_to_gridmap_demo interpolation_demo - ${catkin_LIBRARIES} + iterator_benchmark + iterators_demo + move_demo + normal_filter_comparison_demo + octomap_to_gridmap_demo + opencv_demo + resolution_change_demo + simple_demo + tutorial_demo ) +foreach(target ${targets_list}) + if(CMAKE_CXX_COMPILER_ID MATCHES "GNU") + target_compile_options(${target} PRIVATE "SHELL:--param ggc-min-expand=1") + target_compile_options(${target} PRIVATE "SHELL:--param ggc-min-heapsize=32768") + endif() +endforeach() + ############# ## Install ## ############# -catkin_install_python( - PROGRAMS scripts/image_publisher.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - # Mark executables and/or libraries for installation install( - TARGETS - filters_demo - image_to_gridmap_demo - interpolation_demo - iterator_benchmark - iterators_demo - move_demo - normal_filter_comparison_demo - octomap_to_gridmap_demo - opencv_demo - resolution_change_demo - simple_demo - tutorial_demo - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + TARGETS ${targets_list} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) # Mark other files for installation install( - DIRECTORY config data doc launch rviz scripts - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DIRECTORY config data doc launch rviz + DESTINATION share/${PROJECT_NAME} +) + +# Install python scripts +install( + PROGRAMS scripts/image_publisher.py + DESTINATION lib/${PROJECT_NAME} ) + +############# +## Testing ## +############# + +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright -build/include_order + ) + endif() + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include ${EIGEN3_INCLUDE_DIR}) +ament_package() diff --git a/grid_map_demos/config/filters_demo.yaml b/grid_map_demos/config/filters_demo.yaml index 48a2d6cd3..28c7f051f 100644 --- a/grid_map_demos/config/filters_demo.yaml +++ b/grid_map_demos/config/filters_demo.yaml @@ -1,14 +1,16 @@ -image_to_gridmap_demo: - image_topic: "/image_publisher/image" - resolution: 0.02 - map_frame_id: "map" - min_height: -0.5 - max_height: 1.0 +image_to_gridmap: + ros__parameters: + image_topic: "/image" + resolution: 0.02 + map_frame_id: "map" + min_height: -0.5 + max_height: 1.0 grid_map_visualization: - grid_map_topic: /grid_map_filter_demo/filtered_map - grid_map_visualizations: - - name: surface_normals + ros__parameters: + grid_map_topic: /filtered_map + grid_map_visualizations: [surface_normals, traversability_grid] + surface_normals: type: vectors params: layer_prefix: normal_vectors_ @@ -16,7 +18,7 @@ grid_map_visualization: scale: 0.06 line_width: 0.005 color: 15600153 # red - - name: traversability_grid + traversability_grid: type: occupancy_grid params: layer: traversability diff --git a/grid_map_demos/config/filters_demo_filter_chain.yaml b/grid_map_demos/config/filters_demo_filter_chain.yaml index 6cc61718b..2bc93e608 100644 --- a/grid_map_demos/config/filters_demo_filter_chain.yaml +++ b/grid_map_demos/config/filters_demo_filter_chain.yaml @@ -1,120 +1,126 @@ grid_map_filters: + ros__parameters: - - name: buffer_normalizer - type: gridMapFilters/BufferNormalizerFilter - - # # Duplicate layer. - # - name: duplicate - # type: gridMapFilters/DuplicationFilter - # params: - # input_layer: ... - # output_layer: ... - - # Delete color layer. - - name: delete_original_layers - type: gridMapFilters/DeletionFilter - params: - layers: [color] # List of layers. - - # Fill holes in the map with inpainting. - - name: inpaint - type: gridMapCv/InpaintFilter - params: - input_layer: elevation - output_layer: elevation_inpainted - radius: 0.05 - - # Reduce noise with a radial blurring filter. - - name: mean_in_radius - type: gridMapFilters/MeanInRadiusFilter - params: - input_layer: elevation_inpainted - output_layer: elevation_smooth - radius: 0.06 - - # Boxblur as an alternative to the inpaint and radial blurring filter above. - # - name: boxblur - # type: gridMapFilters/SlidingWindowMathExpressionFilter - # params: - # input_layer: elevation - # output_layer: elevation_smooth - # expression: meanOfFinites(elevation) - # compute_empty_cells: true - # edge_handling: crop # options: inside, crop, empty, mean - # window_size: 5 # optional - - # Compute surface normals. - - name: surface_normals - type: gridMapFilters/NormalVectorsFilter - params: - input_layer: elevation_inpainted - output_layers_prefix: normal_vectors_ - radius: 0.05 - normal_vector_positive_axis: z - - # Add a color layer for visualization based on the surface normal. - - name: normal_color_map - type: gridMapFilters/NormalColorMapFilter - params: - input_layers_prefix: normal_vectors_ - output_layer: normal_color - - # Compute slope from surface normal. - - name: slope - type: gridMapFilters/MathExpressionFilter - params: - output_layer: slope - expression: acos(normal_vectors_z) - - # Compute roughness as absolute difference from map to smoothened map. - - name: roughness - type: gridMapFilters/MathExpressionFilter - params: - output_layer: roughness - expression: abs(elevation_inpainted - elevation_smooth) - - # Edge detection by computing the standard deviation from slope. - - name: edge_detection - type: gridMapFilters/SlidingWindowMathExpressionFilter - params: - input_layer: slope - output_layer: edges - expression: sqrt(sumOfFinites(square(slope - meanOfFinites(slope))) ./ numberOfFinites(slope)) # Standard deviation - compute_empty_cells: false - edge_handling: crop # options: inside, crop, empty, mean - window_length: 0.05 - - # # Edge detection on elevation layer with convolution filter as alternative to filter above. - # - name: edge_detection - # type: gridMapFilters/SlidingWindowMathExpressionFilter - # params: - # input_layer: elevation_inpainted - # output_layer: edges - # expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation_inpainted)' # Edge detection. - # # expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation_inpainted)' # Sharpen. - # compute_empty_cells: false - # edge_handling: mean # options: inside, crop, empty, mean - # window_size: 3 # Make sure to make this compatible with the kernel matrix. - - # Compute traversability as normalized weighted sum of slope and roughness. - - name: traversability - type: gridMapFilters/MathExpressionFilter - params: - output_layer: traversability - expression: 0.5 * (1.0 - (slope / 0.6)) + 0.5 * (1.0 - (roughness / 0.1)) - - # Set lower threshold on traversability. - - name: traversability_lower_threshold - type: gridMapFilters/ThresholdFilter - params: - layer: traversability - lower_threshold: 0.0 - set_to: 0.0 - - # Set upper threshold on traversability. - - name: traversability_upper_threshold - type: gridMapFilters/ThresholdFilter - params: - layer: traversability - upper_threshold: 1.0 - set_to: 1.0 # Other uses: .nan, .inf + input_topic: /grid_map + output_topic: /filtered_map + + filters: + filter1: + name: buffer_normalizer + type: gridMapFilters/BufferNormalizerFilter + + # filter2: # Duplicate layer. + # name: duplicate + # type: gridMapFilters/DuplicationFilter + # params: + # input_layer: ... + # output_layer: ... + + filter2: # Delete color layer. + name: delete_original_layers + type: gridMapFilters/DeletionFilter + params: + layers: [color] # List of layers. + + filter3: # Fill holes in the map with inpainting. + name: inpaint + type: gridMapCv/InpaintFilter + params: + input_layer: elevation + output_layer: elevation_inpainted + radius: 0.05 + + filter4: # Reduce noise with a radial blurring filter. + name: mean_in_radius + type: gridMapFilters/MeanInRadiusFilter + params: + input_layer: elevation_inpainted + output_layer: elevation_smooth + radius: 0.06 + + # filter5: # Boxblur as an alternative to the inpaint and radial blurring filter above. + # name: boxblur + # type: gridMapFilters/SlidingWindowMathExpressionFilter + # params: + # input_layer: elevation + # output_layer: elevation_smooth + # expression: meanOfFinites(elevation) + # compute_empty_cells: true + # edge_handling: crop # options: inside, crop, empty, mean + # window_size: 5 # optional + + filter5: # Compute surface normals. + name: surface_normals + type: gridMapFilters/NormalVectorsFilter + params: + input_layer: elevation_inpainted + output_layers_prefix: normal_vectors_ + radius: 0.05 + normal_vector_positive_axis: z + + filter6: # Add a color layer for visualization based on the surface normal. + name: normal_color_map + type: gridMapFilters/NormalColorMapFilter + params: + input_layers_prefix: normal_vectors_ + output_layer: normal_color + + filter7: # Compute slope from surface normal. + name: slope + type: gridMapFilters/MathExpressionFilter + params: + output_layer: slope + expression: acos(normal_vectors_z) + + filter8: # Compute roughness as absolute difference from map to smoothened map. + name: roughness + type: gridMapFilters/MathExpressionFilter + params: + output_layer: roughness + expression: abs(elevation_inpainted - elevation_smooth) + + filter9: # Edge detection by computing the standard deviation from slope. + name: edge_detection + type: gridMapFilters/SlidingWindowMathExpressionFilter + params: + input_layer: slope + output_layer: edges + expression: sqrt(sumOfFinites(square(slope - meanOfFinites(slope))) ./ numberOfFinites(slope)) # Standard deviation + compute_empty_cells: false + edge_handling: crop # options: inside, crop, empty, mean + window_length: 0.05 + + # filter11: # Edge detection on elevation layer with convolution filter as alternative to filter above. + # name: edge_detection + # type: gridMapFilters/SlidingWindowMathExpressionFilter + # params: + # input_layer: elevation_inpainted + # output_layer: edges + # expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation_inpainted)' # Edge detection. + # # expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation_inpainted)' # Sharpen. + # compute_empty_cells: false + # edge_handling: mean # options: inside, crop, empty, mean + # window_size: 3 # Make sure to make this compatible with the kernel matrix. + + filter10: # Compute traversability as normalized weighted sum of slope and roughness. + name: traversability + type: gridMapFilters/MathExpressionFilter + params: + output_layer: traversability + expression: 0.5 * (1.0 - (slope / 0.6)) + 0.5 * (1.0 - (roughness / 0.1)) + + filter11: # Set lower threshold on traversability. + name: traversability_lower_threshold + type: gridMapFilters/ThresholdFilter + params: + layer: traversability + lower_threshold: 0.0 + set_to: 0.0 + + filter12: # Set upper threshold on traversability. + name: traversability_upper_threshold + type: gridMapFilters/ThresholdFilter + params: + layer: traversability + upper_threshold: 1.0 + set_to: 1.0 # Other uses: .nan, .inf diff --git a/grid_map_demos/config/grid_map_loader_demo.yaml b/grid_map_demos/config/grid_map_loader_demo.yaml index 8f87dc40a..ca776c8c4 100644 --- a/grid_map_demos/config/grid_map_loader_demo.yaml +++ b/grid_map_demos/config/grid_map_loader_demo.yaml @@ -1,7 +1,9 @@ -grid_map_topic: /grid_map_loader_demo/grid_map -grid_map_visualizations: - - name: elevation_points - type: point_cloud - params: - layer: elevation - +grid_map_visualization: + ros__parameters: + grid_map_topic: /grid_map + grid_map_visualizations: [elevation_points] + elevation_points: + type: point_cloud + params: + layer: elevation + transient_local: true diff --git a/grid_map_demos/config/image_to_gridmap_demo.yaml b/grid_map_demos/config/image_to_gridmap_demo.yaml index c9a8684b6..c020fb88e 100644 --- a/grid_map_demos/config/image_to_gridmap_demo.yaml +++ b/grid_map_demos/config/image_to_gridmap_demo.yaml @@ -1,24 +1,29 @@ -image_to_gridmap_demo: - image_topic: "/image_publisher/image" - resolution: 0.03 - map_frame_id: "map" - min_height: -0.1 - max_height: 0.1 +image_publisher: + ros__parameters: + image_topic: "/image" + map_frame_id: "map" + +image_to_gridmap: + ros__parameters: + resolution: 0.03 + min_height: -0.1 + max_height: 0.1 grid_map_visualization: - grid_map_topic: /image_to_gridmap_demo/grid_map - grid_map_visualizations: - - name: elevation_points + ros__parameters: + grid_map_topic: /grid_map + grid_map_visualizations: [elevation_points, elevation_grid, flat_grid] + elevation_points: type: point_cloud params: layer: elevation - - name: elevation_grid + elevation_grid: type: occupancy_grid params: layer: elevation data_min: -0.2 data_max: 0.2 - - name: flat_grid + flat_grid: type: flat_point_cloud params: height: 0.0 diff --git a/grid_map_demos/config/interpolation_demo.yaml b/grid_map_demos/config/interpolation_demo.yaml index 784bbb974..14308eb21 100644 --- a/grid_map_demos/config/interpolation_demo.yaml +++ b/grid_map_demos/config/interpolation_demo.yaml @@ -1,19 +1,20 @@ - -world: Sine # options are: Sine, Poly, Gauss, Tanh - -interpolation_type: Nearest # options are: Nearest, Linear, Cubic_convolution, Cubic - -world_size: - length: 5.0 - width: 5.0 - -groundtruth_resolution: 0.02 # resolution in which ground truth is displayed in rviz - -interpolation: - data_resolution: 0.1 # resolution of the data points for the interpolating algorithm - # this number should be higher than groundtruth_resolution - # if you want to see any effect - - interpolated_resolution: 0.02 # resolution requested for the interpolated map - # this number should be lower than data_resolution - # if you want to see any interpolation effect +grid_map_visualization: + ros__parameters: + world: Sine # options are: Sine, Poly, Gauss, Tanh + + interpolation_type: Nearest # options are: Nearest, Linear, Cubic_convolution, Cubic + + world_size: + length: 5.0 + width: 5.0 + + groundtruth_resolution: 0.02 # resolution in which ground truth is displayed in rviz + + interpolation: + data_resolution: 0.1 # resolution of the data points for the interpolating algorithm + # this number should be higher than groundtruth_resolution + # if you want to see any effect + + interpolated_resolution: 0.02 # resolution requested for the interpolated map + # this number should be lower than data_resolution + # if you want to see any interpolation effect diff --git a/grid_map_demos/config/iterators_demo.yaml b/grid_map_demos/config/iterators_demo.yaml index 5398979f5..9cf441d94 100644 --- a/grid_map_demos/config/iterators_demo.yaml +++ b/grid_map_demos/config/iterators_demo.yaml @@ -1,8 +1,10 @@ -grid_map_topic: /grid_map_iterators_demo/grid_map -grid_map_visualizations: - - name: occupancy_grid - type: occupancy_grid - params: - layer: type - data_min: 0.0 - data_max: 1.0 +grid_map_visualization: + ros__parameters: + grid_map_topic: /grid_map + grid_map_visualizations: [occupancy_grid] + occupancy_grid: + type: occupancy_grid + params: + layer: type + data_min: 0.0 + data_max: 1.0 diff --git a/grid_map_demos/config/move_demo.yaml b/grid_map_demos/config/move_demo.yaml index e16bea24e..87f54cb69 100644 --- a/grid_map_demos/config/move_demo.yaml +++ b/grid_map_demos/config/move_demo.yaml @@ -1,8 +1,10 @@ -grid_map_topic: /grid_map_move_demo/grid_map -grid_map_visualizations: - - name: occupancy_grid - type: occupancy_grid - params: - layer: layer - data_min: 0.0 - data_max: 1.0 +grid_map_visualization: + ros__parameters: + grid_map_topic: /grid_map + grid_map_visualizations: [occupancy_grid] + occupancy_grid: + type: occupancy_grid + params: + layer: layer + data_min: 0.0 + data_max: 1.0 diff --git a/grid_map_demos/config/normal_filter_comparison.yaml b/grid_map_demos/config/normal_filter_comparison.yaml index c25b7aa86..39e881ba5 100644 --- a/grid_map_demos/config/normal_filter_comparison.yaml +++ b/grid_map_demos/config/normal_filter_comparison.yaml @@ -1,23 +1,30 @@ -grid_map_filters: - # Compute surface normals. - - name: surface_normals_area - type: gridMapFilters/NormalVectorsFilter - params: - input_layer: elevation_filtered - algorithm: area - output_layers_prefix: normal_area_ - radius: 0.05 - normal_vector_positive_axis: z - parallelization_enabled: true - thread_number: 4 - # thread_number: -1 doesn't limit the number of threads to be used +normal_filter_comparison: + ros__parameters: + noise_on_map: 0.015 # Noise is in meters + outliers_percentage: 0.0 # Max percentage value is 1.0, all outliers - - name: surface_normals_raster - type: gridMapFilters/NormalVectorsFilter - params: - input_layer: elevation_filtered - algorithm: raster - output_layers_prefix: normal_raster_ - normal_vector_positive_axis: z - parallelization_enabled: true - thread_number: 4 \ No newline at end of file + # Compute surface normals. + filters: + filter1: + name: surface_normals_area + type: gridMapFilters/NormalVectorsFilter + params: + input_layer: elevation_filtered + algorithm: area + output_layers_prefix: normal_area_ + radius: 0.05 + normal_vector_positive_axis: z + parallelization_enabled: true + thread_number: 4 + # thread_number: -1 doesn't limit the number of threads to be used + + filter2: + name: surface_normals_raster + type: gridMapFilters/NormalVectorsFilter + params: + input_layer: elevation_filtered + algorithm: raster + output_layers_prefix: normal_raster_ + normal_vector_positive_axis: z + parallelization_enabled: true + thread_number: 4 \ No newline at end of file diff --git a/grid_map_demos/config/normal_filter_comparison_visualization.yaml b/grid_map_demos/config/normal_filter_comparison_visualization.yaml index e00f9446a..c105abdf5 100644 --- a/grid_map_demos/config/normal_filter_comparison_visualization.yaml +++ b/grid_map_demos/config/normal_filter_comparison_visualization.yaml @@ -1,72 +1,84 @@ -grid_map_topic: /normal_filter_comparison_demo/grid_map +grid_map_visualization: + ros__parameters: + grid_map_topic: /grid_map -grid_map_visualizations: + grid_map_visualizations: + - elevation_points + - noisy_points + - filtered_points + - map_region + - elevation_grid + - error_grid + - surface_normals_analytic + - surface_normals_raster + - surface_normals_area + - elevation_cells - - name: elevation_points - type: point_cloud - params: - layer: elevation + elevation_points: + type: point_cloud + params: + layer: elevation - - name: noisy_points - type: point_cloud - params: - layer: elevation_noisy + noisy_points: + type: point_cloud + params: + layer: elevation_noisy - - name: filtered_points - type: point_cloud - params: - layer: elevation_filtered + filtered_points: + type: point_cloud + params: + layer: elevation_filtered - - name: map_region - type: map_region - params: - color: 3289650 - line_width: 0.003 - - - name: elevation_grid - type: occupancy_grid - params: - layer: elevation - data_min: 0.08 - data_max: -0.16 - - - name: error_grid - type: occupancy_grid - params: - layer: error - data_min: -0.15 - data_max: 0.15 - - - name: surface_normals_analytic - type: vectors - params: - layer_prefix: normal_analytic_ - position_layer: elevation - scale: 0.06 - line_width: 0.005 - color: 15600153 # red + map_region: + type: map_region + params: + color: 3289650 + line_width: 0.003 + + elevation_grid: + type: occupancy_grid + params: + layer: elevation + data_min: 0.08 + data_max: -0.16 + + error_grid: + type: occupancy_grid + params: + layer: error + data_min: -0.15 + data_max: 0.15 + + surface_normals_analytic: + type: vectors + params: + layer_prefix: normal_analytic_ + position_layer: elevation + scale: 0.06 + line_width: 0.005 + color: 15600153 # red - - name: surface_normals_raster - type: vectors - params: - layer_prefix: normal_raster_ - position_layer: elevation - scale: 0.06 - line_width: 0.005 - color: 255 # blue + surface_normals_raster: + type: vectors + params: + layer_prefix: normal_raster_ + position_layer: elevation + scale: 0.06 + line_width: 0.005 + color: 255 # blue - - name: surface_normals_area - type: vectors - params: - layer_prefix: normal_area_ - position_layer: elevation - scale: 0.06 - line_width: 0.005 - color: 65280 # green + surface_normals_area: + type: vectors + params: + layer_prefix: normal_area_ + position_layer: elevation + scale: 0.06 + line_width: 0.005 + color: 65280 # green - - name: elevation_cells - type: grid_cells - params: - layer: elevation - lower_threshold: -0.08 - upper_threshold: 0.0 + elevation_cells: + type: grid_cells + params: + layer: elevation + lower_threshold: -0.08 + upper_threshold: 0.0 diff --git a/grid_map_demos/config/octomap_to_gridmap_demo.yaml b/grid_map_demos/config/octomap_to_gridmap_demo.yaml index 394853fd1..dc3bd728b 100644 --- a/grid_map_demos/config/octomap_to_gridmap_demo.yaml +++ b/grid_map_demos/config/octomap_to_gridmap_demo.yaml @@ -1,23 +1,25 @@ octomap_to_gridmap_demo: - octomap_service_topic: "/octomap_binary" - min_z: -2.0 - max_z: 2.0 + ros__parameters: + octomap_service_topic: "/octomap_binary" + min_z: -2.0 + max_z: 2.0 grid_map_visualization: - grid_map_topic: /octomap_to_gridmap_demo/grid_map - grid_map_visualizations: - - name: elevation_points + ros__parameters: + grid_map_topic: /grid_map + grid_map_visualizations: [elevation_points, elevation_grid, map_region] + elevation_points: type: point_cloud params: layer: elevation - - name: elevation_grid + elevation_grid: type: occupancy_grid params: layer: elevation data_min: -2.0 data_max: 2.0 - - name: map_region + map_region: type: map_region params: - color: 3289650 - line_width: 0.1 + color: 3289650 + line_width: 0.1 diff --git a/grid_map_demos/config/opencv_demo.yaml b/grid_map_demos/config/opencv_demo.yaml index c754c69ad..156a22642 100644 --- a/grid_map_demos/config/opencv_demo.yaml +++ b/grid_map_demos/config/opencv_demo.yaml @@ -1,6 +1,8 @@ -grid_map_topic: /grid_map_opencv_demo/grid_map -grid_map_visualizations: - - name: elevation_points - type: point_cloud - params: - layer: elevation \ No newline at end of file +grid_map_visualization: + ros__parameters: + grid_map_topic: /grid_map + grid_map_visualizations: [elevation_points] + elevation_points: + type: point_cloud + params: + layer: elevation diff --git a/grid_map_demos/config/resolution_change_demo.yaml b/grid_map_demos/config/resolution_change_demo.yaml index 2a5409806..c64642399 100644 --- a/grid_map_demos/config/resolution_change_demo.yaml +++ b/grid_map_demos/config/resolution_change_demo.yaml @@ -1,8 +1,10 @@ -grid_map_topic: /grid_map_resolution_change_demo/grid_map -grid_map_visualizations: - - name: occupancy_grid - type: occupancy_grid - params: - layer: elevation - data_min: 0.0 - data_max: 0.3 \ No newline at end of file +grid_map_visualization: + ros__parameters: + grid_map_topic: /grid_map + grid_map_visualizations: [occupancy_grid] + occupancy_grid: + type: occupancy_grid + params: + layer: elevation + data_min: 0.0 + data_max: 0.3 diff --git a/grid_map_demos/config/simple_demo.yaml b/grid_map_demos/config/simple_demo.yaml index 895ecb8f9..15ec03613 100644 --- a/grid_map_demos/config/simple_demo.yaml +++ b/grid_map_demos/config/simple_demo.yaml @@ -1,12 +1,14 @@ -grid_map_topic: /grid_map_simple_demo/grid_map -grid_map_visualizations: - - name: elevation_points - type: point_cloud - params: - layer: elevation - - name: elevation_grid - type: occupancy_grid - params: - layer: elevation - data_min: 0.08 - data_max: -0.16 +grid_map_visualization: + ros__parameters: + grid_map_topic: /grid_map + grid_map_visualizations: [elevation_points, elevation_grid] + elevation_points: + type: point_cloud + params: + layer: elevation + elevation_grid: + type: occupancy_grid + params: + layer: elevation + data_min: 0.08 + data_max: -0.16 diff --git a/grid_map_demos/config/tutorial_demo.yaml b/grid_map_demos/config/tutorial_demo.yaml index 715004f9e..070f71312 100644 --- a/grid_map_demos/config/tutorial_demo.yaml +++ b/grid_map_demos/config/tutorial_demo.yaml @@ -1,54 +1,56 @@ -grid_map_topic: /grid_map_tutorial_demo/grid_map - -grid_map_visualizations: - - - name: elevation_points - type: point_cloud - params: - layer: elevation - - - name: noisy_points - type: point_cloud - params: - layer: elevation_noisy - - - name: filtered_points - type: point_cloud - params: - layer: elevation_filtered - - - name: map_region - type: map_region - params: - color: 3289650 - line_width: 0.003 - - - name: elevation_grid - type: occupancy_grid - params: - layer: elevation - data_min: 0.08 - data_max: -0.16 - - - name: error_grid - type: occupancy_grid - params: - layer: error - data_min: -0.15 - data_max: 0.15 - - - name: surface_normals - type: vectors - params: - layer_prefix: normal_ - position_layer: elevation - scale: 0.06 - line_width: 0.005 - color: 15600153 # red - - - name: elevation_cells - type: grid_cells - params: - layer: elevation - lower_threshold: -0.08 - upper_threshold: 0.0 +grid_map_visualization: + ros__parameters: + grid_map_topic: /grid_map + + grid_map_visualizations: [elevation_points, noisy_points, filtered_points, map_region, elevation_grid, error_grid, surface_normals, elevation_cells] + + elevation_points: + type: point_cloud + params: + layer: elevation + + noisy_points: + type: point_cloud + params: + layer: elevation_noisy + + filtered_points: + type: point_cloud + params: + layer: elevation_filtered + + map_region: + type: map_region + params: + color: 3289650 + line_width: 0.003 + + elevation_grid: + type: occupancy_grid + params: + layer: elevation + data_min: 0.08 + data_max: -0.16 + + error_grid: + type: occupancy_grid + params: + layer: error + data_min: -0.15 + data_max: 0.15 + + surface_normals: + type: vectors + params: + layer_prefix: normal_ + position_layer: elevation + scale: 0.06 + line_width: 0.005 + color: 15600153 # red + + elevation_cells: + type: grid_cells + params: + layer: elevation + lower_threshold: -0.08 + upper_threshold: 0.0 diff --git a/grid_map_demos/data/grid_map.bag b/grid_map_demos/data/grid_map.bag deleted file mode 100644 index 20e1ed486..000000000 Binary files a/grid_map_demos/data/grid_map.bag and /dev/null differ diff --git a/grid_map_demos/data/grid_map_bag/grid_map_bag.db3 b/grid_map_demos/data/grid_map_bag/grid_map_bag.db3 new file mode 100644 index 000000000..170fc7e1c Binary files /dev/null and b/grid_map_demos/data/grid_map_bag/grid_map_bag.db3 differ diff --git a/grid_map_demos/data/grid_map_bag/metadata.yaml b/grid_map_demos/data/grid_map_bag/metadata.yaml new file mode 100644 index 000000000..64d81d88f --- /dev/null +++ b/grid_map_demos/data/grid_map_bag/metadata.yaml @@ -0,0 +1,19 @@ +rosbag2_bagfile_information: + version: 4 + storage_identifier: sqlite3 + relative_file_paths: + - grid_map_bag.db3 + duration: + nanoseconds: 9603163532 + starting_time: + nanoseconds_since_epoch: 1600035091054796171 + message_count: 4 + topics_with_message_count: + - topic_metadata: + name: /grid_map + type: grid_map_msgs/msg/GridMap + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 2147483647\n nsec: 4294967295\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 4 + compression_format: "" + compression_mode: "" \ No newline at end of file diff --git a/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp b/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp index 2a81c25a9..50321beec 100644 --- a/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp +++ b/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp @@ -7,30 +7,29 @@ * */ -#pragma once +#ifndef GRID_MAP_DEMOS__FILTERSDEMO_HPP_ +#define GRID_MAP_DEMOS__FILTERSDEMO_HPP_ #include -#include -#include +#include +#include #include -namespace grid_map_demos { +namespace grid_map_demos +{ /*! * Applies a chain of grid map filters to a topic and * republishes the resulting grid map. */ -class FiltersDemo +class FiltersDemo : public rclcpp::Node { - public: - +public: /*! * Constructor. - * @param nodeHandle the ROS node handle. - * @param success signalizes if filter is configured ok or not. */ - FiltersDemo(ros::NodeHandle& nodeHandle, bool& success); + FiltersDemo(); /*! * Destructor. @@ -47,13 +46,9 @@ class FiltersDemo * Callback method for the incoming grid map message. * @param message the incoming message. */ - void callback(const grid_map_msgs::GridMap& message); - - private: - - //! ROS nodehandle. - ros::NodeHandle& nodeHandle_; + void callback(const grid_map_msgs::msg::GridMap::SharedPtr message); +private: //! Name of the input grid map topic. std::string inputTopic_; @@ -61,10 +56,10 @@ class FiltersDemo std::string outputTopic_; //! Grid map subscriber - ros::Subscriber subscriber_; + rclcpp::Subscription::SharedPtr subscriber_; //! Grid map publisher. - ros::Publisher publisher_; + rclcpp::Publisher::SharedPtr publisher_; //! Filter chain. filters::FilterChain filterChain_; @@ -73,4 +68,5 @@ class FiltersDemo std::string filterChainParametersName_; }; -} /* namespace */ \ No newline at end of file +} // namespace grid_map_demos +#endif // GRID_MAP_DEMOS__FILTERSDEMO_HPP_ diff --git a/grid_map_demos/include/grid_map_demos/ImageToGridmapDemo.hpp b/grid_map_demos/include/grid_map_demos/ImageToGridmapDemo.hpp index 5b25d06ca..e5d0748ff 100644 --- a/grid_map_demos/include/grid_map_demos/ImageToGridmapDemo.hpp +++ b/grid_map_demos/include/grid_map_demos/ImageToGridmapDemo.hpp @@ -7,31 +7,32 @@ * */ -#pragma once +#ifndef GRID_MAP_DEMOS__IMAGETOGRIDMAPDEMO_HPP_ +#define GRID_MAP_DEMOS__IMAGETOGRIDMAPDEMO_HPP_ // ROS -#include -#include +#include +#include #include #include -namespace grid_map_demos { +namespace grid_map_demos +{ /*! * Loads an image and saves it as layer 'elevation' of a grid map. * The grid map is published and can be viewed in Rviz. */ -class ImageToGridmapDemo +class ImageToGridmapDemo : public rclcpp::Node { - public: - +public: /*! * Constructor. * @param nodeHandle the ROS node handle. */ - ImageToGridmapDemo(ros::NodeHandle& nodeHandle); + ImageToGridmapDemo(); /*! * Destructor. @@ -44,28 +45,21 @@ class ImageToGridmapDemo */ bool readParameters(); - void imageCallback(const sensor_msgs::Image& msg); - - private: - - //! ROS nodehandle. - ros::NodeHandle& nodeHandle_; + void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg); +private: //! Grid map publisher. - ros::Publisher gridMapPublisher_; + rclcpp::Publisher::SharedPtr gridMapPublisher_; //! Grid map data. grid_map::GridMap map_; //! Image subscriber - ros::Subscriber imageSubscriber_; + rclcpp::Subscription::SharedPtr imageSubscriber_; //! Name of the grid map topic. std::string imageTopic_; - //! Length of the grid map in x direction. - double mapLengthX_; - //! Resolution of the grid map. double resolution_; @@ -79,4 +73,5 @@ class ImageToGridmapDemo bool mapInitialized_; }; -} /* namespace */ +} // namespace grid_map_demos +#endif // GRID_MAP_DEMOS__IMAGETOGRIDMAPDEMO_HPP_ diff --git a/grid_map_demos/include/grid_map_demos/InterpolationDemo.hpp b/grid_map_demos/include/grid_map_demos/InterpolationDemo.hpp index b94746c0f..f30f442fd 100644 --- a/grid_map_demos/include/grid_map_demos/InterpolationDemo.hpp +++ b/grid_map_demos/include/grid_map_demos/InterpolationDemo.hpp @@ -6,18 +6,22 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#pragma once +#ifndef GRID_MAP_DEMOS__INTERPOLATIONDEMO_HPP_ +#define GRID_MAP_DEMOS__INTERPOLATIONDEMO_HPP_ -#include "grid_map_core/GridMap.hpp" -#include "grid_map_core/TypeDefs.hpp" +#include +#include +#include +#include #include #include #include #include -#include +#include -namespace grid_map_demos { +namespace grid_map_demos +{ /* * This demo visualizes the quality of function approximation @@ -50,7 +54,8 @@ namespace grid_map_demos { * Different analytical functions used * to evaluate the quality of interpolation approximation. */ -enum class Worlds: int { +enum class Worlds : int +{ Poly, GaussMixture, Tanh, @@ -58,14 +63,15 @@ enum class Worlds: int { NUM_WORLDS }; -static const std::string demoLayer = "demo"; +static const std::string demoLayer = "demo"; // NOLINT struct AnalyticalFunctions { std::function f_; }; -struct Error { +struct Error +{ double meanSquare_ = 0.0; double meanAbs_ = 0.0; double max_ = 0.0; @@ -74,21 +80,22 @@ struct Error { /* * Create map geometry and allocate memory */ -grid_map::GridMap createMap(const grid_map::Length &length, double resolution, - const grid_map::Position &pos); +grid_map::GridMap createMap( + const grid_map::Length & length, double resolution, + const grid_map::Position & pos); /* * Fill grid map with values computed from analytical function */ -void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions); +void fillGridMap(grid_map::GridMap * map, const AnalyticalFunctions & functions); /* * Create various analytical functions */ -AnalyticalFunctions createPolyWorld(grid_map::GridMap *map); -AnalyticalFunctions createSineWorld(grid_map::GridMap *map); -AnalyticalFunctions createTanhWorld(grid_map::GridMap *map); -AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map); +AnalyticalFunctions createPolyWorld(grid_map::GridMap * map); +AnalyticalFunctions createSineWorld(grid_map::GridMap * map); +AnalyticalFunctions createTanhWorld(grid_map::GridMap * map); +AnalyticalFunctions createGaussianWorld(grid_map::GridMap * map); /* @@ -96,39 +103,44 @@ AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map); * and create sparse grid map with data points that are used for the * interpolation. */ -AnalyticalFunctions createWorld(Worlds world, double highResolution, double lowResolution, - double length, double width, - grid_map::GridMap *groundTruthHighRes, - grid_map::GridMap *groundTruthLowRes); +AnalyticalFunctions createWorld( + Worlds world, double highResolution, double lowResolution, + double length, double width, + grid_map::GridMap * groundTruthHighRes, + grid_map::GridMap * groundTruthLowRes); /* * Allocate memory and set geometry for the interpolated grid map */ -grid_map::GridMap createInterpolatedMapFromDataMap(const grid_map::GridMap &dataMap, double desiredResolution); +grid_map::GridMap createInterpolatedMapFromDataMap( + const grid_map::GridMap & dataMap, + double desiredResolution); /* * Compute the interpolated values */ -void interpolateInputMap(const grid_map::GridMap &dataMap, - grid_map::InterpolationMethods interpolationMethod, grid_map::GridMap *inteprolatedMap); +void interpolateInputMap( + const grid_map::GridMap & dataMap, + grid_map::InterpolationMethods interpolationMethod, grid_map::GridMap * inteprolatedMap); /* * Compute error measures between the ground truth and the interpolated map */ -Error computeInterpolationError(const AnalyticalFunctions &groundTruth, - const grid_map::GridMap &map); +Error computeInterpolationError( + const AnalyticalFunctions & groundTruth, + const grid_map::GridMap & map); static const std::map worlds = { - { "Sine", Worlds::Sine }, - { "Poly", Worlds::Poly }, - { "Gauss", Worlds::GaussMixture }, - { "Tanh", Worlds::Tanh } }; + {"Sine", Worlds::Sine}, + {"Poly", Worlds::Poly}, + {"Gauss", Worlds::GaussMixture}, + {"Tanh", Worlds::Tanh}}; static const std::map interpolationMethods = { - { "Nearest", grid_map::InterpolationMethods::INTER_NEAREST}, - { "Linear", grid_map::InterpolationMethods::INTER_LINEAR }, - { "Cubic_convolution", grid_map::InterpolationMethods::INTER_CUBIC_CONVOLUTION }, - { "Cubic", grid_map::InterpolationMethods::INTER_CUBIC } }; + {"Nearest", grid_map::InterpolationMethods::INTER_NEAREST}, + {"Linear", grid_map::InterpolationMethods::INTER_LINEAR}, + {"Cubic_convolution", grid_map::InterpolationMethods::INTER_CUBIC_CONVOLUTION}, + {"Cubic", grid_map::InterpolationMethods::INTER_CUBIC}}; /* * Class that actually runs the demo @@ -136,32 +148,34 @@ static const std::map interpolation * computes errors * measures times required for computation */ -class InterpolationDemo +class InterpolationDemo : public rclcpp::Node { +public: + InterpolationDemo(); - public: - InterpolationDemo(ros::NodeHandle *nh); - - private: +private: using clk = std::chrono::steady_clock; using ErrorAndDuration = std::pair; - struct Statistic { + struct Statistic + { Error error_; std::string world_; std::string interpolationMethod_; double duration_ = 0.0; }; - using Statistics = std::map >; + using Statistics = std::map>; void runDemo(); - ErrorAndDuration interpolateAndComputeError(const std::string world, const std::string &method) const; + ErrorAndDuration interpolateAndComputeError( + const std::string world, + const std::string & method) const; Statistics computeStatistics() const; - void printStatistics(const Statistics &stats) const; + void printStatistics(const Statistics & stats) const; void publishGridMaps() const; - ros::Publisher groundTruthMapPub_; - ros::Publisher dataSparseMapPub_; - ros::Publisher interpolatedMapPub_; + rclcpp::Publisher::SharedPtr groundTruthMapPub_; + rclcpp::Publisher::SharedPtr dataSparseMapPub_; + rclcpp::Publisher::SharedPtr interpolatedMapPub_; grid_map::GridMap dataSparseMap_; grid_map::GridMap groundTruthMap_; @@ -169,13 +183,13 @@ class InterpolationDemo std::string world_; std::string interpolationMethod_; - double groundTruthResolution_ = 0.02; // resolution in which ground truth is displayed in rviz - double dataResolution_ = 0.1; // resolution of the data points for the interpolating algorithm - double interpolatedResolution_ = 0.02; // resolution requested for the interpolated map + double groundTruthResolution_ = 0.02; // resolution in which ground truth is displayed in rviz + double dataResolution_ = 0.1; // resolution of the data points for the interpolating algorithm + double interpolatedResolution_ = 0.02; // resolution requested for the interpolated map AnalyticalFunctions groundTruth_; double worldWidth_ = 4.0; double worldLength_ = 4.0; - }; -} /* namespace grid_map_demos */ +} // namespace grid_map_demos +#endif // GRID_MAP_DEMOS__INTERPOLATIONDEMO_HPP_ diff --git a/grid_map_demos/include/grid_map_demos/IteratorsDemo.hpp b/grid_map_demos/include/grid_map_demos/IteratorsDemo.hpp index ceded1195..4abe7a2e4 100644 --- a/grid_map_demos/include/grid_map_demos/IteratorsDemo.hpp +++ b/grid_map_demos/include/grid_map_demos/IteratorsDemo.hpp @@ -7,27 +7,28 @@ * */ -#pragma once +#ifndef GRID_MAP_DEMOS__ITERATORSDEMO_HPP_ +#define GRID_MAP_DEMOS__ITERATORSDEMO_HPP_ #include // ROS -#include +#include +#include -namespace grid_map_demos { +namespace grid_map_demos +{ /*! * Visualizes a grid map by publishing different topics that can be viewed in Rviz. */ -class IteratorsDemo +class IteratorsDemo : public rclcpp::Node { - public: - +public: /*! * Constructor. - * @param nodeHandle the ROS node handle. */ - IteratorsDemo(ros::NodeHandle& nodeHandle); + IteratorsDemo(); /*! * Destructor. @@ -51,19 +52,16 @@ class IteratorsDemo */ void publish(); - private: - - //! ROS nodehandle. - ros::NodeHandle& nodeHandle_; - +private: //! Grid map publisher. - ros::Publisher gridMapPublisher_; + rclcpp::Publisher::SharedPtr gridMapPublisher_; //! Polygon publisher. - ros::Publisher polygonPublisher_; + rclcpp::Publisher::SharedPtr polygonPublisher_; //! Grid map data. grid_map::GridMap map_; }; -} /* namespace */ +} // namespace grid_map_demos +#endif // GRID_MAP_DEMOS__ITERATORSDEMO_HPP_ diff --git a/grid_map_demos/include/grid_map_demos/OctomapToGridmapDemo.hpp b/grid_map_demos/include/grid_map_demos/OctomapToGridmapDemo.hpp index 2a4f7a628..13fa94c8b 100644 --- a/grid_map_demos/include/grid_map_demos/OctomapToGridmapDemo.hpp +++ b/grid_map_demos/include/grid_map_demos/OctomapToGridmapDemo.hpp @@ -6,30 +6,31 @@ * Institute: University of Zürich, Robotics and Perception Group */ -#pragma once - -// ROS -#include +#ifndef GRID_MAP_DEMOS__OCTOMAPTOGRIDMAPDEMO_HPP_ +#define GRID_MAP_DEMOS__OCTOMAPTOGRIDMAPDEMO_HPP_ #include - +#include +#include #include -namespace grid_map_demos { +namespace grid_map_demos +{ /*! * Receives a volumetric OctoMap and converts it to a grid map with an elevation layer. * The grid map is published and can be viewed in Rviz. */ -class OctomapToGridmapDemo +class OctomapToGridmapDemo : public rclcpp::Node { - public: +public: + using GetOctomapSrv = octomap_msgs::srv::GetOctomap; + using OctomapMessage = octomap_msgs::msg::Octomap; /*! * Constructor. - * @param nodeHandle the ROS node handle. */ - OctomapToGridmapDemo(ros::NodeHandle& nodeHandle); + OctomapToGridmapDemo(); /*! * Destructor. @@ -44,16 +45,12 @@ class OctomapToGridmapDemo void convertAndPublishMap(); - private: - - //! ROS nodehandle. - ros::NodeHandle& nodeHandle_; - +private: //! Grid map publisher. - ros::Publisher gridMapPublisher_; + rclcpp::Publisher::SharedPtr gridMapPublisher_; //! Octomap publisher. - ros::Publisher octomapPublisher_; + rclcpp::Publisher::SharedPtr octomapPublisher_; //! Grid map data. grid_map::GridMap map_; @@ -62,7 +59,7 @@ class OctomapToGridmapDemo std::string octomapServiceTopic_; //! Octomap service client - ros::ServiceClient client_; + rclcpp::Client::SharedPtr client_; //! Bounding box of octomap to convert. float minX_; @@ -73,4 +70,5 @@ class OctomapToGridmapDemo float maxZ_; }; -} /* namespace */ +} // namespace grid_map_demos +#endif // GRID_MAP_DEMOS__OCTOMAPTOGRIDMAPDEMO_HPP_ diff --git a/grid_map_demos/launch/filters_demo.launch b/grid_map_demos/launch/filters_demo.launch deleted file mode 100644 index 1d2feafdf..000000000 --- a/grid_map_demos/launch/filters_demo.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/grid_map_demos/launch/filters_demo_launch.py b/grid_map_demos/launch/filters_demo_launch.py new file mode 100644 index 000000000..9f815cbfb --- /dev/null +++ b/grid_map_demos/launch/filters_demo_launch.py @@ -0,0 +1,98 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Find the grid_map_demos package share directory + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + # Declare launch configuration variables that can access the launch arguments values + filters_config_file = LaunchConfiguration('filters_config') + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + # Declare launch arguments + declare_filters_config_file_cmd = DeclareLaunchArgument( + 'filters_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'filters_demo_filter_chain.yaml'), + description='Full path to the filter chain config file to use') + + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'filters_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'filters_demo.rviz'), + description='Full path to the RVIZ config file to use') + + # Declare node actions + grid_map_filter_demo_node = Node( + package='grid_map_demos', + executable='filters_demo', + name='grid_map_filters', + output='screen', + parameters=[filters_config_file] + ) + + image_publisher_node = Node( + package='grid_map_demos', + executable='image_publisher.py', + name='image_publisher', + output='screen', + parameters=[{ + 'image_path': os.path.join(grid_map_demos_dir, 'data', 'terrain.png'), + 'topic': 'image' + }] + ) + + image_to_gridmap_demo_node = Node( + package='grid_map_demos', + executable='image_to_gridmap_demo', + name='image_to_gridmap', + output='screen', + parameters=[visualization_config_file] + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Add launch arguments to the launch description + ld.add_action(declare_filters_config_file_cmd) + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add node actions to the launch description + ld.add_action(grid_map_filter_demo_node) + ld.add_action(image_publisher_node) + ld.add_action(image_to_gridmap_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/grid_map_loader_demo.launch b/grid_map_demos/launch/grid_map_loader_demo.launch deleted file mode 100644 index 77afa1eb8..000000000 --- a/grid_map_demos/launch/grid_map_loader_demo.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/grid_map_demos/launch/grid_map_loader_demo_launch.py b/grid_map_demos/launch/grid_map_loader_demo_launch.py new file mode 100644 index 000000000..f577ede59 --- /dev/null +++ b/grid_map_demos/launch/grid_map_loader_demo_launch.py @@ -0,0 +1,77 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Find the grid_map_demos package share directory + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + # Declare launch configuration variables that can access the launch arguments values + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + # Declare launch arguments + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'grid_map_loader_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'grid_map_demo.rviz'), + description='Full path to the RVIZ config file to use') + + # Declare paramters for the grid_map_loader node + loader_params = [{ + 'file_path': os.path.join(grid_map_demos_dir, 'data', 'grid_map_bag'), + 'bag_topic': '/grid_map', + 'publish_topic': '/grid_map', + 'duration': 10.0 + }] + + # Declare node actions + grid_map_loader_demo_node = Node( + package='grid_map_loader', + executable='grid_map_loader', + name='grid_map_loader_demo', + output='screen', + parameters=loader_params + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Add launch arguments to the launch description + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add node actions to the launch description + ld.add_action(grid_map_loader_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/image_to_gridmap_demo.launch b/grid_map_demos/launch/image_to_gridmap_demo.launch deleted file mode 100644 index a4f0ad2e5..000000000 --- a/grid_map_demos/launch/image_to_gridmap_demo.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/grid_map_demos/launch/image_to_gridmap_demo_launch.py b/grid_map_demos/launch/image_to_gridmap_demo_launch.py new file mode 100644 index 000000000..7e37a2893 --- /dev/null +++ b/grid_map_demos/launch/image_to_gridmap_demo_launch.py @@ -0,0 +1,82 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Find the grid_map_demos package share directory + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + # Declare launch configuration variables that can access the launch arguments values + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + # Declare launch arguments + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'image_to_gridmap_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'grid_map_demo.rviz'), + description='Full path to the RVIZ config file to use') + + # Declare node actions + image_publisher_node = Node( + package='grid_map_demos', + executable='image_publisher.py', + name='image_publisher', + output='screen', + parameters=[ + { + 'image_path': os.path.join(grid_map_demos_dir, 'data', 'eth_logo.png'), + 'topic': 'image' + } + ] + ) + + image_to_gridmap_demo_node = Node( + package='grid_map_demos', + executable='image_to_gridmap_demo', + name='image_to_gridmap', + output='screen' + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Add launch arguments to the launch description + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add node actions to the launch description + ld.add_action(image_publisher_node) + ld.add_action(image_to_gridmap_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/interpolation_demo.launch b/grid_map_demos/launch/interpolation_demo.launch deleted file mode 100644 index b3bcd31de..000000000 --- a/grid_map_demos/launch/interpolation_demo.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - \ No newline at end of file diff --git a/grid_map_demos/launch/interpolation_demo_launch.py b/grid_map_demos/launch/interpolation_demo_launch.py new file mode 100644 index 000000000..19f00c1a7 --- /dev/null +++ b/grid_map_demos/launch/interpolation_demo_launch.py @@ -0,0 +1,54 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + config_file = LaunchConfiguration('param_file') + rviz_config_file = LaunchConfiguration('rviz_config') + + config_file_cmd = DeclareLaunchArgument( + 'param_file', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'interpolation_demo.yaml'), + description='Full path to the parameter config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'interpolation_demo.rviz'), + description='Full path to the RVIZ config file to use') + + interpolation_demo_node = Node( + package='grid_map_demos', + executable='interpolation_demo', + name='grid_map_interpolation_demo', + output='screen', + parameters=[config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + ld.add_action(config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + ld.add_action(interpolation_demo_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/iterators_demo.launch b/grid_map_demos/launch/iterators_demo.launch deleted file mode 100644 index 747cd4e67..000000000 --- a/grid_map_demos/launch/iterators_demo.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/grid_map_demos/launch/iterators_demo_launch.py b/grid_map_demos/launch/iterators_demo_launch.py new file mode 100644 index 000000000..1d23d7150 --- /dev/null +++ b/grid_map_demos/launch/iterators_demo_launch.py @@ -0,0 +1,68 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Find the grid_map_demos package share directory + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + # Declare launch configuration variables that can access the launch arguments values + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + # Declare launch arguments + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'iterators_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'grid_map_iterators_demo.rviz'), + description='Full path to the RVIZ config file to use') + + # Declare node actions + iterators_demo_node = Node( + package='grid_map_demos', + executable='iterators_demo', + name='grid_map_iterators_demo', + output='screen' + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Add launch arguments to the launch description + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add node actions to the launch description + ld.add_action(iterators_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/move_demo.launch b/grid_map_demos/launch/move_demo.launch deleted file mode 100644 index acdd2330a..000000000 --- a/grid_map_demos/launch/move_demo.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/grid_map_demos/launch/move_demo_launch.py b/grid_map_demos/launch/move_demo_launch.py new file mode 100644 index 000000000..4f7b3f05f --- /dev/null +++ b/grid_map_demos/launch/move_demo_launch.py @@ -0,0 +1,68 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Find the grid_map_demos package share directory + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + # Declare launch configuration variables that can access the launch arguments values + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + # Declare launch arguments + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'move_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'grid_map_iterators_demo.rviz'), + description='Full path to the RVIZ config file to use') + + # Declare node actions + move_demo_node = Node( + package='grid_map_demos', + executable='move_demo', + name='grid_map_move_demo', + output='screen' + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Add launch arguments to the launch description + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add node actions to the launch description + ld.add_action(move_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/normal_filter_comparison.launch b/grid_map_demos/launch/normal_filter_comparison.launch deleted file mode 100644 index e3bd00a89..000000000 --- a/grid_map_demos/launch/normal_filter_comparison.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/grid_map_demos/launch/normal_filter_comparison_launch.py b/grid_map_demos/launch/normal_filter_comparison_launch.py new file mode 100644 index 000000000..91d05331a --- /dev/null +++ b/grid_map_demos/launch/normal_filter_comparison_launch.py @@ -0,0 +1,77 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Find the grid_map_demos package share directory + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + # Declare launch configuration variables that can access the launch arguments values + filters_config_file = LaunchConfiguration('normal_filters_config') + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + # Declare launch arguments + declare_filters_config_file_cmd = DeclareLaunchArgument( + 'normal_filters_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'normal_filter_comparison.yaml'), + description='Full path to the normal_filter_comparison_demo config file') + + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'normal_filter_comparison_visualization.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'normal_filter_comparison_demo.rviz'), + description='Full path to the RVIZ config file to use') + + # Declare node actions + normal_filter_comparison_demo_node = Node( + package='grid_map_demos', + executable='normal_filter_comparison_demo', + name='normal_filter_comparison', + output='screen', + parameters=[filters_config_file] + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Add launch arguments to the launch description + ld.add_action(declare_filters_config_file_cmd) + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add node actions to the launch description + ld.add_action(normal_filter_comparison_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/octomap_to_gridmap_demo.launch b/grid_map_demos/launch/octomap_to_gridmap_demo.launch deleted file mode 100644 index c173c22cb..000000000 --- a/grid_map_demos/launch/octomap_to_gridmap_demo.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/grid_map_demos/launch/octomap_to_gridmap_demo_launch.py b/grid_map_demos/launch/octomap_to_gridmap_demo_launch.py new file mode 100644 index 000000000..c5dcf0bf9 --- /dev/null +++ b/grid_map_demos/launch/octomap_to_gridmap_demo_launch.py @@ -0,0 +1,83 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Find the grid_map_demos package share directory + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + # Declare launch configuration variables that can access the launch arguments values + param_file = LaunchConfiguration('param_file') + rviz_config_file = LaunchConfiguration('rviz_config') + + # Declare launch arguments + declare_param_file_cmd = DeclareLaunchArgument( + 'param_file', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'octomap_to_gridmap_demo.yaml'), + description='Full path to the config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'octomap_to_gridmap_demo.rviz'), + description='Full path to the RVIZ config file to use') + + # Declare node actions + + octomap_server_node = Node( + package='octomap_server', + executable='octomap_server_static_node', + name='octomap_server', + output='screen', + parameters=[ + param_file, + { + 'octomap_path': os.path.join(grid_map_demos_dir, 'data', 'freiburg1_360.bt') + } + ], + ) + + octomap_to_gridmap_demo_node = Node( + package='grid_map_demos', + executable='octomap_to_gridmap_demo', + name='octomap_to_gridmap_demo', + output='screen' + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[param_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Add launch arguments to the launch description + ld.add_action(declare_param_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add node actions to the launch description + ld.add_action(octomap_server_node) + ld.add_action(octomap_to_gridmap_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/opencv_demo.launch b/grid_map_demos/launch/opencv_demo.launch deleted file mode 100644 index a33930d1f..000000000 --- a/grid_map_demos/launch/opencv_demo.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/grid_map_demos/launch/opencv_demo_launch.py b/grid_map_demos/launch/opencv_demo_launch.py new file mode 100644 index 000000000..9c51eebc1 --- /dev/null +++ b/grid_map_demos/launch/opencv_demo_launch.py @@ -0,0 +1,68 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Find the grid_map_demos package share directory + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + # Declare launch configuration variables that can access the launch arguments values + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + # Declare launch arguments + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'opencv_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'grid_map_demo.rviz'), + description='Full path to the RVIZ config file to use') + + # Declare node actions + opencv_demo_node = Node( + package='grid_map_demos', + executable='opencv_demo', + name='grid_map_opencv_demo', + output='screen' + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Add launch arguments to the launch description + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add node actions to the launch description + ld.add_action(opencv_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/resolution_change_demo.launch b/grid_map_demos/launch/resolution_change_demo.launch deleted file mode 100644 index aabd2f510..000000000 --- a/grid_map_demos/launch/resolution_change_demo.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/grid_map_demos/launch/resolution_change_demo_launch.py b/grid_map_demos/launch/resolution_change_demo_launch.py new file mode 100644 index 000000000..0b8801bd9 --- /dev/null +++ b/grid_map_demos/launch/resolution_change_demo_launch.py @@ -0,0 +1,61 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'resolution_change_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'grid_map_iterators_demo.rviz'), + description='Full path to the RVIZ config file to use') + + resolution_change_demo_node = Node( + package='grid_map_demos', + executable='resolution_change_demo', + name='grid_map_resolution_change_demo', + output='screen' + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + ld = LaunchDescription() + + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + ld.add_action(resolution_change_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/simple_demo.launch b/grid_map_demos/launch/simple_demo.launch deleted file mode 100644 index 065311feb..000000000 --- a/grid_map_demos/launch/simple_demo.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/grid_map_demos/launch/simple_demo_launch.py b/grid_map_demos/launch/simple_demo_launch.py new file mode 100644 index 000000000..046b3b81d --- /dev/null +++ b/grid_map_demos/launch/simple_demo_launch.py @@ -0,0 +1,68 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Find the grid_map_demos package share directory + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + # Declare launch configuration variables that can access the launch arguments values + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + # Declare launch arguments + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'simple_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'grid_map_demo.rviz'), + description='Full path to the RVIZ config file to use') + + # Declare node actions + simple_demo_node = Node( + package='grid_map_demos', + executable='simple_demo', + name='grid_map_simple_demo', + output='screen' + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Add launch arguments to the launch description + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add node actions to the launch description + ld.add_action(simple_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/launch/tutorial_demo.launch b/grid_map_demos/launch/tutorial_demo.launch deleted file mode 100644 index 616649c34..000000000 --- a/grid_map_demos/launch/tutorial_demo.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/grid_map_demos/launch/tutorial_demo_launch.py b/grid_map_demos/launch/tutorial_demo_launch.py new file mode 100644 index 000000000..326eef0d3 --- /dev/null +++ b/grid_map_demos/launch/tutorial_demo_launch.py @@ -0,0 +1,68 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Find the grid_map_demos package share directory + grid_map_demos_dir = get_package_share_directory('grid_map_demos') + + # Declare launch configuration variables that can access the launch arguments values + visualization_config_file = LaunchConfiguration('visualization_config') + rviz_config_file = LaunchConfiguration('rviz_config') + + # Declare launch arguments + declare_visualization_config_file_cmd = DeclareLaunchArgument( + 'visualization_config', + default_value=os.path.join( + grid_map_demos_dir, 'config', 'tutorial_demo.yaml'), + description='Full path to the Gridmap visualization config file to use') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + grid_map_demos_dir, 'rviz', 'grid_map_demo.rviz'), + description='Full path to the RVIZ config file to use') + + # Declare node actions + tutorial_demo_node = Node( + package='grid_map_demos', + executable='tutorial_demo', + name='grid_map_tutorial_demo', + output='screen' + ) + + grid_map_visualization_node = Node( + package='grid_map_visualization', + executable='grid_map_visualization', + name='grid_map_visualization', + output='screen', + parameters=[visualization_config_file] + ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Add launch arguments to the launch description + ld.add_action(declare_visualization_config_file_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add node actions to the launch description + ld.add_action(tutorial_demo_node) + ld.add_action(grid_map_visualization_node) + ld.add_action(rviz2_node) + + return ld diff --git a/grid_map_demos/package.xml b/grid_map_demos/package.xml index a12734261..bfdefc332 100644 --- a/grid_map_demos/package.xml +++ b/grid_map_demos/package.xml @@ -1,27 +1,46 @@ - + + grid_map_demos - 1.6.2 + 2.2.2 Demo nodes to demonstrate the usage of the grid map library. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Péter Fankhauser - catkin - roscpp + + ament_cmake + grid_map_cmake_helpers + + cv_bridge + geometry_msgs grid_map_core - grid_map_ros grid_map_cv grid_map_filters grid_map_loader grid_map_msgs grid_map_octomap + grid_map_ros grid_map_rviz_plugin grid_map_visualization - geometry_msgs - sensor_msgs - cv_bridge octomap_msgs + rclcpp + sensor_msgs + + octomap_rviz_plugins + octomap_server + python3-opencv + rclpy + rviz2 + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/grid_map_demos/rviz/filters_demo.rviz b/grid_map_demos/rviz/filters_demo.rviz index 0ab6ee78a..6acfe63c6 100644 --- a/grid_map_demos/rviz/filters_demo.rviz +++ b/grid_map_demos/rviz/filters_demo.rviz @@ -1,38 +1,32 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: ~ - Splitter Ratio: 0.611464977 - Tree Height: 374 - - Class: rviz/Selection + Splitter Ratio: 0.6114649772644043 + Tree Height: 443 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" Visualization Manager: Class: "" Displays: - - Class: rviz/Axes + - Class: rviz_default_plugins/Axes Enabled: false - Length: 0.200000003 + Length: 0.20000000298023224 Name: Axes - Radius: 0.00999999978 + Radius: 0.009999999776482582 Reference Frame: Value: false - Alpha: 1 @@ -52,8 +46,13 @@ Visualization Manager: Min Intensity: 0 Name: Elevation Show Grid Lines: true - Topic: /grid_map_filter_demo/filtered_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /filtered_map Use Rainbow: true Value: false - Alpha: 1 @@ -73,8 +72,13 @@ Visualization Manager: Min Intensity: 0 Name: Normal mapping Show Grid Lines: false - Topic: /grid_map_filter_demo/filtered_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /filtered_map Use Rainbow: false Value: true - Alpha: 1 @@ -94,8 +98,13 @@ Visualization Manager: Min Intensity: 0 Name: Edges Show Grid Lines: false - Topic: /grid_map_filter_demo/filtered_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /filtered_map Use Rainbow: false Value: false - Alpha: 1 @@ -110,13 +119,18 @@ Visualization Manager: History Length: 1 Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0.0799999982 + Max Intensity: 0.07999999821186066 Min Color: 0; 0; 0 - Min Intensity: -0.0199999996 + Min Intensity: -0.019999999552965164 Name: Roughness Show Grid Lines: false - Topic: /grid_map_filter_demo/filtered_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /filtered_map Use Rainbow: true Value: false - Alpha: 1 @@ -131,31 +145,52 @@ Visualization Manager: History Length: 1 Invert Rainbow: true Max Color: 255; 255; 255 - Max Intensity: 1.20000005 + Max Intensity: 1.2000000476837158 Min Color: 0; 0; 0 Min Intensity: 0 Name: Traversability Show Grid Lines: false - Topic: /grid_map_filter_demo/filtered_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /filtered_map Use Rainbow: true Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: false - Marker Topic: /grid_map_visualization/surface_normals Name: Surface Normals Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /surface_normals Value: false - Alpha: 1 - Class: rviz/Map + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: false Name: Elevation - Topic: /grid_map_visualization/elevation_grid - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /traversability_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /traversability_grid_updates Use Timestamp: false Value: false Enabled: true @@ -165,41 +200,64 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 17.448988 + Class: rviz_default_plugins/Orbit + Distance: 17.44898796081543 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.806525648 - Y: -0.875660837 - Z: -3.12213802 + X: -0.8065256476402283 + Y: -0.8756608366966248 + Z: -3.122138023376465 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.659796655 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6597966551780701 Target Frame: Value: Orbit (rviz) - Yaw: 0.764933825 + Yaw: 0.7649338245391846 Saved: ~ Window Geometry: Displays: @@ -207,15 +265,13 @@ Window Geometry: Height: 594 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016a000001b7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c000001b7000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ec0000003efc0100000002fb0000000800540069006d00650100000000000003ec0000025800fffffffb0000000800540069006d006501000000000000045000000000000000000000027c000001b700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a000001f8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001f8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ec0000003efc0100000002fb0000000800540069006d00650100000000000003ec0000000000000000fb0000000800540069006d006501000000000000045000000000000000000000027c000001f800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Time: - collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1004 - X: 0 - Y: 0 + X: 72 + Y: 28 diff --git a/grid_map_demos/rviz/grid_map_demo.rviz b/grid_map_demos/rviz/grid_map_demo.rviz index d784c85ff..ab1fa879c 100644 --- a/grid_map_demos/rviz/grid_map_demo.rviz +++ b/grid_map_demos/rviz/grid_map_demo.rviz @@ -1,38 +1,32 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: ~ - Splitter Ratio: 0.611465 - Tree Height: 375 - - Class: rviz/Selection + Splitter Ratio: 0.6114649772644043 + Tree Height: 864 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Elevation Visualization Manager: Class: "" Displays: - - Class: rviz/Axes + - Class: rviz_default_plugins/Axes Enabled: false - Length: 0.2 + Length: 0.20000000298023224 Name: Axes - Radius: 0.01 + Radius: 0.009999999776482582 Reference Frame: Value: false - Alpha: 1 @@ -45,51 +39,66 @@ Visualization Manager: Height Layer: elevation Height Transformer: GridMapLayer History Length: 1 + Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 10 Min Color: 0; 0; 0 Min Intensity: 0 Name: GridMap Show Grid Lines: true - Topic: /grid_map_simple_demo/grid_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /grid_map Use Rainbow: true Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /grid_map_visualization/map_region + - Class: rviz_default_plugins/Marker + Enabled: false Name: Map Region Namespaces: {} - Queue Size: 100 - Value: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_region + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.0849175 - Min Value: -0.165153 + Max Value: 0.08491750061511993 + Min Value: -0.16515299677848816 Value: false Axis: Z Channel Name: z - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0.0768171 + Max Intensity: 0.07679121941328049 Min Color: 0; 85; 127 - Min Intensity: -0.156817 + Min Intensity: -0.15679122507572174 Name: Elevation Position Transformer: XYZ - Queue Size: 10 Selectable: false Size (Pixels): 3 - Size (m): 0.03 + Size (m): 0.029999999329447746 Style: Boxes - Topic: /grid_map_visualization/elevation_points - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_points Use Fixed Frame: true Use rainbow: false Value: true @@ -101,60 +110,99 @@ Visualization Manager: Value: true Axis: Z Channel Name: error - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0.02 + Max Intensity: 0.019999999552965164 Min Color: 0; 85; 127 Min Intensity: 0 Name: Elevation Filtered Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.03 + Size (m): 0.029999999329447746 Style: Boxes - Topic: /grid_map_visualization/filtered_points - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /filtered_points Use Fixed Frame: true Use rainbow: false Value: false - Alpha: 1 - Class: rviz/Map + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: false Name: Elevation - Topic: /grid_map_visualization/elevation_grid - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_grid_updates + Use Timestamp: false Value: false - - Alpha: 0.7 - Class: rviz/Map + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: false Name: Error - Topic: /grid_map_visualization/error_grid - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /error_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /error_grid_updates + Use Timestamp: false Value: false - Alpha: 1 - Class: rviz/GridCells + Class: rviz_default_plugins/GridCells Color: 25; 255; 0 Enabled: false Name: GridCells - Topic: /grid_map_visualization/elevation_cells + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_cells Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: false - Marker Topic: /grid_map_visualization/surface_normals Name: Surface Normals Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /surface_normals Value: false Enabled: true Global Options: @@ -163,51 +211,78 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 2.43931 + Class: rviz_default_plugins/Orbit + Distance: 2.439310073852539 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.00701846 - Y: 0.113553 - Z: -0.107872 + X: 0.007018459960818291 + Y: 0.1135530024766922 + Z: -0.10787200182676315 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.474798 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4747979938983917 Target Frame: Value: Orbit (rviz) - Yaw: 2.39541 + Yaw: 2.3954100608825684 Saved: ~ Window Geometry: Displays: collapsed: false + Height: 1015 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016a000001b8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001b8000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004120000003efc0100000002fb0000000800540069006d0065010000000000000412000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002a2000001b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000039dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004120000003efc0100000002fb0000000800540069006d00650100000000000004120000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005c80000039d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Time: - collapsed: false Tool Properties: collapsed: false Views: collapsed: true + Width: 1848 + X: 72 + Y: 28 diff --git a/grid_map_demos/rviz/grid_map_iterators_demo.rviz b/grid_map_demos/rviz/grid_map_iterators_demo.rviz index d870cec21..e5869a7cb 100644 --- a/grid_map_demos/rviz/grid_map_iterators_demo.rviz +++ b/grid_map_demos/rviz/grid_map_iterators_demo.rviz @@ -1,40 +1,34 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: ~ - Splitter Ratio: 0.642659 - Tree Height: 297 - - Class: rviz/Selection + Splitter Ratio: 0.6426590085029602 + Tree Height: 670 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" Visualization Manager: Class: "" Displays: - Alpha: 0.5 - Cell Size: 0.05 - Class: rviz/Grid + Cell Size: 0.05000000074505806 + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -46,27 +40,46 @@ Visualization Manager: Plane Cell Count: 20 Reference Frame: Value: true - - Class: rviz/Axes + - Class: rviz_default_plugins/Axes Enabled: true - Length: 0.1 + Length: 0.10000000149011612 Name: Axes - Radius: 0.01 + Radius: 0.009999999776482582 Reference Frame: Value: true - - Alpha: 0.7 - Class: rviz/Map + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map - Topic: /grid_map_visualization/occupancy_grid + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /occupancy_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /occupancy_grid_updates + Use Timestamp: false Value: true - Alpha: 0.5 - Class: rviz/Polygon + Class: rviz_default_plugins/Polygon Color: 255; 255; 255 Enabled: true Name: Polygon - Topic: /grid_map_iterators_demo/polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /polygon Value: true Enabled: true Global Options: @@ -75,32 +88,55 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Angle: -1.57079 - Class: rviz/TopDownOrtho + Angle: -1.5707900524139404 + Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Scale: 420.916 + Near Clip Distance: 0.009999999776482582 + Scale: 420.9159851074219 Target Frame: Value: TopDownOrtho (rviz) X: 0 @@ -109,9 +145,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false + Height: 1015 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016b000001b8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001b8000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002a0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004120000003efc0100000002fb0000000800540069006d0065010000000000000412000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002a1000001b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016b00000329fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000329000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002a0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000006efc0100000002fb0000000800540069006d00650100000000000007380000005800fffffffb0000000800540069006d00650100000000000004500000000000000000000005c70000032900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -120,3 +157,6 @@ Window Geometry: collapsed: false Views: collapsed: true + Width: 1848 + X: 72 + Y: 28 diff --git a/grid_map_demos/rviz/interpolation_demo.rviz b/grid_map_demos/rviz/interpolation_demo.rviz index 123967e2c..1d25b1f3c 100644 --- a/grid_map_demos/rviz/interpolation_demo.rviz +++ b/grid_map_demos/rviz/interpolation_demo.rviz @@ -1,45 +1,32 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - - /Status1 - - /Grid1 - - /GridMap1 - - /GridMap2 - Splitter Ratio: 0.33475935459136963 - Tree Height: 728 - - Class: rviz/Selection + - /GridMap ground truth1/Topic1 + - /GridMap interpolated1/Topic1 + Splitter Ratio: 0.6205882430076599 + Tree Height: 786 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 1 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: false Line Style: @@ -55,7 +42,7 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: false - - Class: rviz/Axes + - Class: rviz_default_plugins/Axes Enabled: true Length: 0.10000000149011612 Name: Axes @@ -77,10 +64,15 @@ Visualization Manager: Max Intensity: 10 Min Color: 78; 154; 6 Min Intensity: 0 - Name: GridMap + Name: GridMap ground truth Show Grid Lines: true - Topic: /grid_map_interpolation_demo/ground_truth - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ground_truth Use Rainbow: false Value: true - Alpha: 0.699999988079071 @@ -98,41 +90,64 @@ Visualization Manager: Max Intensity: 10 Min Color: 239; 41; 41 Min Intensity: 0 - Name: GridMap + Name: GridMap interpolated Show Grid Lines: true - Topic: /grid_map_interpolation_demo/interpolated - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /interpolated Use Rainbow: false Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 12.087379455566406 + Class: rviz_default_plugins/Orbit + Distance: 7.29967737197876 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -147,26 +162,24 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.9447981119155884 + Pitch: 1.0647979974746704 Target Frame: Value: Orbit (rviz) Yaw: 5.758227825164795 Saved: ~ Window Geometry: Displays: - collapsed: true - Height: 1025 - Hide Left Dock: true + collapsed: false + Height: 1015 + Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001de00000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002da000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000073d0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001560000039dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002da000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000006efc0100000002fb0000000800540069006d00650100000000000007380000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005dc0000039d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Time: - collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 1853 - X: 67 - Y: 27 + Width: 1848 + X: 72 + Y: 28 diff --git a/grid_map_demos/rviz/normal_filter_comparison_demo.rviz b/grid_map_demos/rviz/normal_filter_comparison_demo.rviz index f14d40291..8cb88f8da 100644 --- a/grid_map_demos/rviz/normal_filter_comparison_demo.rviz +++ b/grid_map_demos/rviz/normal_filter_comparison_demo.rviz @@ -1,38 +1,32 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: ~ - Splitter Ratio: 0.611465 - Tree Height: 375 - - Class: rviz/Selection + Splitter Ratio: 0.6114649772644043 + Tree Height: 380 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Elevation Visualization Manager: Class: "" Displays: - - Class: rviz/Axes + - Class: rviz_default_plugins/Axes Enabled: false - Length: 0.2 + Length: 0.20000000298023224 Name: Axes - Radius: 0.01 + Radius: 0.009999999776482582 Reference Frame: Value: false - Alpha: 1 @@ -45,51 +39,66 @@ Visualization Manager: Height Layer: elevation Height Transformer: GridMapLayer History Length: 1 + Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 10 Min Color: 0; 0; 0 Min Intensity: 0 Name: GridMap Show Grid Lines: true - Topic: /grid_map_simple_demo/grid_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /grid_map Use Rainbow: true Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /grid_map_visualization/map_region Name: Map Region Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: "/map_region" Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.0849175 - Min Value: -0.165153 + Max Value: 0.08491750061511993 + Min Value: -0.16515299677848816 Value: false Axis: Z Channel Name: z - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0.0768171 + Max Intensity: 0.20799855887889862 Min Color: 0; 85; 127 - Min Intensity: -0.156817 + Min Intensity: -0.2879985570907593 Name: Elevation Position Transformer: XYZ - Queue Size: 10 Selectable: false Size (Pixels): 3 - Size (m): 0.03 + Size (m): 0.029999999329447746 Style: Boxes - Topic: /grid_map_visualization/elevation_points - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_points Use Fixed Frame: true Use rainbow: false Value: true @@ -101,76 +110,125 @@ Visualization Manager: Value: true Axis: Z Channel Name: error - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0.02 + Max Intensity: 0.019999999552965164 Min Color: 0; 85; 127 Min Intensity: 0 Name: Elevation Filtered Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.03 + Size (m): 0.029999999329447746 Style: Boxes - Topic: /grid_map_visualization/filtered_points - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /filtered_points Use Fixed Frame: true Use rainbow: false Value: false - Alpha: 1 - Class: rviz/Map + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: false Name: Elevation - Topic: /grid_map_visualization/elevation_grid - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_grid_updates + Use Timestamp: false Value: false - - Alpha: 0.7 - Class: rviz/Map + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: false Name: Error - Topic: /grid_map_visualization/error_grid - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /error_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /error_grid_updates + Use Timestamp: false Value: false - Alpha: 1 - Class: rviz/GridCells + Class: rviz_default_plugins/GridCells Color: 25; 255; 0 Enabled: false Name: GridCells - Topic: /grid_map_visualization/elevation_cells + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_cells Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: false - Marker Topic: /grid_map_visualization/surface_normals_analytic Name: Surface Normals Analytic Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /surface_normals_analytic Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: false - Marker Topic: /grid_map_visualization/surface_normals_area Name: Surface Normals Area Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /surface_normals_area Value: false - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: false - Marker Topic: /grid_map_visualization/surface_normals_raster Name: Surface Normals Raster Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /surface_normals_raster Value: false Enabled: true Global Options: @@ -179,51 +237,78 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 2.43931 + Class: rviz_default_plugins/Orbit + Distance: 2.439310073852539 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.00701846 - Y: 0.113553 - Z: -0.107872 + X: 0.007018459960818291 + Y: 0.1135530024766922 + Z: -0.10787200182676315 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.474798 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4747979938983917 Target Frame: Value: Orbit (rviz) - Yaw: 2.39541 + Yaw: 2.3954100608825684 Saved: ~ Window Geometry: Displays: collapsed: false + Height: 531 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016a000001b8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001b8000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004120000003efc0100000002fb0000000800540069006d0065010000000000000412000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002a2000001b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a000001b9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004120000003efc0100000002fb0000000800540069006d00650100000000000004120000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000246000001b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Time: - collapsed: false Tool Properties: collapsed: false Views: collapsed: true + Width: 950 + X: 110 + Y: 66 diff --git a/grid_map_demos/rviz/octomap_to_gridmap_demo.rviz b/grid_map_demos/rviz/octomap_to_gridmap_demo.rviz index ed7ddbf85..3e025c206 100644 --- a/grid_map_demos/rviz/octomap_to_gridmap_demo.rviz +++ b/grid_map_demos/rviz/octomap_to_gridmap_demo.rviz @@ -1,30 +1,25 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: ~ Splitter Ratio: 0.611464977 Tree Height: 956 - - Class: rviz/Selection + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679016 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" Visualization Manager: Class: "" Displays: @@ -45,35 +40,62 @@ Visualization Manager: Min Intensity: 0 Name: GridMap Show Grid Lines: true - Topic: /octomap_to_gridmap_demo/grid_map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /grid_map Use Rainbow: true Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /grid_map_visualization/map_region Name: Map Region Namespaces: map_region: true - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_region Value: true - Alpha: 1 - Class: rviz/Map + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: true Name: Elevation - Topic: /grid_map_visualization/elevation_grid - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /elevation_grid_updates + Use Timestamp: false Value: true - - Class: octomap_rviz_plugin/OccupancyGrid + - Class: octomap_rviz_plugins/OccupancyGrid Enabled: false Max. Height Display: inf Max. Octree Depth: 16 Min. Height Display: -inf Name: OccupancyGrid - Octomap Topic: /octomap_to_gridmap_demo/octomap - Queue Size: 5 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /octomap Value: false Voxel Alpha: 1 Voxel Coloring: Z-Axis @@ -85,23 +107,23 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + - Class: rviz_default_plugins/SetInitialPose Topic: /initialpose - - Class: rviz/SetGoal + - Class: rviz_default_plugins/SetGoal Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: - Class: rviz/Orbit + Class: rviz_default_plugins/Orbit Distance: 8.02025986 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 diff --git a/grid_map_demos/scripts/image_publisher.py b/grid_map_demos/scripts/image_publisher.py index 9c1745db3..94fa71216 100755 --- a/grid_map_demos/scripts/image_publisher.py +++ b/grid_map_demos/scripts/image_publisher.py @@ -1,64 +1,74 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # simple script to publish a image from a file. -import rospy -import rospkg -import time import cv2 -import sensor_msgs.msg - -def callback(self): - """ Convert a image to a ROS compatible message - (sensor_msgs.Image). - """ - global publisher, imagePath - - img = cv2.imread(imagePath, cv2.IMREAD_UNCHANGED) - -# print img.shape -# print img.size -# print img.dtype.itemsize - - rosimage = sensor_msgs.msg.Image() - if img.dtype.itemsize == 2: - if len(img.shape) == 3: - if img.shape[2] == 3: - rosimage.encoding = 'bgr16' - if img.shape[2] == 4: - rosimage.encoding = 'bgra16' - else: - rosimage.encoding = 'mono16' - if img.dtype.itemsize == 1: - if len(img.shape) == 3: - if img.shape[2] == 3: - rosimage.encoding = 'bgr8' - if img.shape[2] == 4: - rosimage.encoding = 'bgra8' - else: - rosimage.encoding = 'mono8' -# print "Encoding: ", rosimage.encoding - - rosimage.width = img.shape[1] - rosimage.height = img.shape[0] - rosimage.step = img.strides[0] - rosimage.data = img.tostring() - rosimage.header.stamp = rospy.Time.now() - rosimage.header.frame_id = 'map' - - publisher.publish(rosimage) - - -#Main function initializes node and subscribers and starts the ROS loop -def main_program(): - global publisher, imagePath - rospack = rospkg.RosPack() - rospy.init_node('image_publisher') - imagePath = rospy.get_param('~image_path') - topicName = rospy.get_param('~topic') - publisher = rospy.Publisher(topicName, sensor_msgs.msg.Image, queue_size=10) - rospy.Timer(rospy.Duration(2), callback) - rospy.spin() +import rclpy + +from rclpy.node import Node +from sensor_msgs.msg import Image + + +class ImagePublisher(Node): + + def __init__(self): + super().__init__('image_publisher') + + self.declare_parameter('image_path') + self.declare_parameter('topic') + + self.imagePath_ = self.get_parameter( + 'image_path').get_parameter_value().string_value + topicName = self.get_parameter( + 'topic').get_parameter_value().string_value + + self.publisher_ = self.create_publisher(Image, topicName, 10) + timer_period = 2 # seconds + self.timer = self.create_timer(timer_period, self.callback) + + def callback(self): + """Convert a image to a ROS compatible message (sensor_msgs.Image).""" + img = cv2.imread(self.imagePath_, cv2.IMREAD_UNCHANGED) + + # print img.shape + # print img.size + # print img.dtype.itemsize + + rosimage = Image() + + if img.dtype.itemsize == 2: + if len(img.shape) == 3: + if img.shape[2] == 3: + rosimage.encoding = 'bgr16' + if img.shape[2] == 4: + rosimage.encoding = 'bgra16' + else: + rosimage.encoding = 'mono16' + if img.dtype.itemsize == 1: + if len(img.shape) == 3: + if img.shape[2] == 3: + rosimage.encoding = 'bgr8' + if img.shape[2] == 4: + rosimage.encoding = 'bgra8' + else: + rosimage.encoding = 'mono8' + # print('Encoding: ', rosimage.encoding) + + rosimage.width = img.shape[1] + rosimage.height = img.shape[0] + rosimage.step = img.strides[0] + rosimage.data = img.tostring() + rosimage.header.stamp = self.get_clock().now().to_msg() + rosimage.header.frame_id = 'map' + + self.publisher_.publish(rosimage) + + +# Main function initializes node and subscribers and starts the ROS loop +def main_program(args=None): + rclpy.init(args=args) + image_publisher = ImagePublisher() + rclpy.spin(image_publisher) + rclpy.shutdown() + if __name__ == '__main__': - try: - main_program() - except rospy.ROSInterruptException: pass + main_program() diff --git a/grid_map_demos/src/FiltersDemo.cpp b/grid_map_demos/src/FiltersDemo.cpp index 666580952..58ebd627f 100644 --- a/grid_map_demos/src/FiltersDemo.cpp +++ b/grid_map_demos/src/FiltersDemo.cpp @@ -7,32 +7,43 @@ * */ -#include "grid_map_demos/FiltersDemo.hpp" +#include +#include +#include -using namespace grid_map; +#include "grid_map_demos/FiltersDemo.hpp" -namespace grid_map_demos { +namespace grid_map_demos +{ -FiltersDemo::FiltersDemo(ros::NodeHandle& nodeHandle, bool& success) - : nodeHandle_(nodeHandle), - filterChain_("grid_map::GridMap") +FiltersDemo::FiltersDemo() +: Node("grid_map_filters_demo"), + filterChain_("grid_map::GridMap") { if (!readParameters()) { - success = false; return; } - subscriber_ = nodeHandle_.subscribe(inputTopic_, 1, &FiltersDemo::callback, this); - publisher_ = nodeHandle_.advertise(outputTopic_, 1, true); + subscriber_ = this->create_subscription( + inputTopic_, 1, + std::bind(&FiltersDemo::callback, this, std::placeholders::_1)); + + publisher_ = this->create_publisher( + outputTopic_, + rclcpp::QoS(1).transient_local()); + // Setup filter chain. - if (!filterChain_.configure(filterChainParametersName_, nodeHandle)) { - ROS_ERROR("Could not configure the filter chain!"); - success = false; + if (filterChain_.configure( + filterChainParametersName_, this->get_node_logging_interface(), + this->get_node_parameters_interface())) + { + RCLCPP_INFO(this->get_logger(), "Filter chain configured."); + } else { + RCLCPP_ERROR(this->get_logger(), "Could not configure the filter chain!"); + rclcpp::shutdown(); return; } - - success = true; } FiltersDemo::~FiltersDemo() @@ -41,33 +52,38 @@ FiltersDemo::~FiltersDemo() bool FiltersDemo::readParameters() { - if (!nodeHandle_.getParam("input_topic", inputTopic_)) { - ROS_ERROR("Could not read parameter `input_topic`."); + this->declare_parameter("input_topic"); + this->declare_parameter("output_topic", std::string("output")); + this->declare_parameter("filter_chain_parameter_name", std::string("filters")); + + if (!this->get_parameter("input_topic", inputTopic_)) { + RCLCPP_ERROR(this->get_logger(), "Could not read parameter `input_topic`."); return false; } - nodeHandle_.param("output_topic", outputTopic_, std::string("output")); - nodeHandle_.param("filter_chain_parameter_name", filterChainParametersName_, std::string("grid_map_filters")); + + this->get_parameter("output_topic", outputTopic_); + this->get_parameter("filter_chain_parameter_name", filterChainParametersName_); return true; } -void FiltersDemo::callback(const grid_map_msgs::GridMap& message) +void FiltersDemo::callback(const grid_map_msgs::msg::GridMap::SharedPtr message) { // Convert message to map. - GridMap inputMap; - GridMapRosConverter::fromMessage(message, inputMap); + grid_map::GridMap inputMap; + grid_map::GridMapRosConverter::fromMessage(*message, inputMap); // Apply filter chain. grid_map::GridMap outputMap; if (!filterChain_.update(inputMap, outputMap)) { - ROS_ERROR("Could not update the grid map filter chain!"); + RCLCPP_ERROR(this->get_logger(), "Could not update the grid map filter chain!"); return; } - ROS_INFO("PUBLISH"); + RCLCPP_INFO(this->get_logger(), "PUBLISH"); // Publish filtered output grid map. - grid_map_msgs::GridMap outputMessage; - GridMapRosConverter::toMessage(outputMap, outputMessage); - publisher_.publish(outputMessage); + std::unique_ptr outputMessage; + outputMessage = grid_map::GridMapRosConverter::toMessage(outputMap); + publisher_->publish(std::move(outputMessage)); } -} /* namespace */ +} // namespace grid_map_demos diff --git a/grid_map_demos/src/ImageToGridmapDemo.cpp b/grid_map_demos/src/ImageToGridmapDemo.cpp index 295b08023..a0dcb35ee 100644 --- a/grid_map_demos/src/ImageToGridmapDemo.cpp +++ b/grid_map_demos/src/ImageToGridmapDemo.cpp @@ -6,19 +6,28 @@ * Institute: ETH Zurich, ANYbotics */ +#include +#include + #include "grid_map_demos/ImageToGridmapDemo.hpp" -namespace grid_map_demos { +namespace grid_map_demos +{ -ImageToGridmapDemo::ImageToGridmapDemo(ros::NodeHandle& nodeHandle) - : nodeHandle_(nodeHandle), - map_(grid_map::GridMap({"elevation"})), - mapInitialized_(false) +ImageToGridmapDemo::ImageToGridmapDemo() +: Node("image_to_gridmap_demo"), + map_(grid_map::GridMap({"elevation"})), + mapInitialized_(false) { readParameters(); map_.setBasicLayers({"elevation"}); - imageSubscriber_ = nodeHandle_.subscribe(imageTopic_, 1, &ImageToGridmapDemo::imageCallback, this); - gridMapPublisher_ = nodeHandle_.advertise("grid_map", 1, true); + imageSubscriber_ = + this->create_subscription( + imageTopic_, 1, + std::bind(&ImageToGridmapDemo::imageCallback, this, std::placeholders::_1)); + + gridMapPublisher_ = this->create_publisher( + "grid_map", rclcpp::QoS(1).transient_local()); } ImageToGridmapDemo::~ImageToGridmapDemo() @@ -27,28 +36,34 @@ ImageToGridmapDemo::~ImageToGridmapDemo() bool ImageToGridmapDemo::readParameters() { - nodeHandle_.param("image_topic", imageTopic_, std::string("/image")); - nodeHandle_.param("resolution", resolution_, 0.03); - nodeHandle_.param("min_height", minHeight_, 0.0); - nodeHandle_.param("max_height", maxHeight_, 1.0); + this->declare_parameter("image_topic", std::string("/image")); + this->declare_parameter("resolution", rclcpp::ParameterValue(0.03)); + this->declare_parameter("min_height", rclcpp::ParameterValue(0.0)); + this->declare_parameter("max_height", rclcpp::ParameterValue(1.0)); + + this->get_parameter("image_topic", imageTopic_); + this->get_parameter("resolution", resolution_); + this->get_parameter("min_height", minHeight_); + this->get_parameter("max_height", maxHeight_); return true; } -void ImageToGridmapDemo::imageCallback(const sensor_msgs::Image& msg) +void ImageToGridmapDemo::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) { if (!mapInitialized_) { - grid_map::GridMapRosConverter::initializeFromImage(msg, resolution_, map_); - ROS_INFO("Initialized map with size %f x %f m (%i x %i cells).", map_.getLength().x(), - map_.getLength().y(), map_.getSize()(0), map_.getSize()(1)); + grid_map::GridMapRosConverter::initializeFromImage(*msg, resolution_, map_); + RCLCPP_INFO( + this->get_logger(), + "Initialized map with size %f x %f m (%i x %i cells).", map_.getLength().x(), + map_.getLength().y(), map_.getSize()(0), map_.getSize()(1)); mapInitialized_ = true; } - grid_map::GridMapRosConverter::addLayerFromImage(msg, "elevation", map_, minHeight_, maxHeight_); - grid_map::GridMapRosConverter::addColorLayerFromImage(msg, "color", map_); + grid_map::GridMapRosConverter::addLayerFromImage(*msg, "elevation", map_, minHeight_, maxHeight_); + grid_map::GridMapRosConverter::addColorLayerFromImage(*msg, "color", map_); // Publish as grid map. - grid_map_msgs::GridMap mapMessage; - grid_map::GridMapRosConverter::toMessage(map_, mapMessage); - gridMapPublisher_.publish(mapMessage); + auto message = grid_map::GridMapRosConverter::toMessage(map_); + gridMapPublisher_->publish(std::move(message)); } -} /* namespace */ +} // namespace grid_map_demos diff --git a/grid_map_demos/src/InterpolationDemo.cpp b/grid_map_demos/src/InterpolationDemo.cpp index 5f4d3678b..4fe071ef5 100644 --- a/grid_map_demos/src/InterpolationDemo.cpp +++ b/grid_map_demos/src/InterpolationDemo.cpp @@ -8,44 +8,48 @@ #include "grid_map_demos/InterpolationDemo.hpp" -#include "grid_map_core/iterators/GridMapIterator.hpp" -#include "grid_map_msgs/GridMap.h" -#include "grid_map_ros/GridMapRosConverter.hpp" +#include +#include +#include -namespace grid_map_demos { +#include +#include +#include -AnalyticalFunctions createWorld(Worlds world, double highResolution, double lowResolution, - double length, double width, grid_map::GridMap *groundTruthHighRes, - grid_map::GridMap *groundTruthLowRes) +namespace grid_map_demos { +AnalyticalFunctions createWorld( + Worlds world, double highResolution, double lowResolution, + double length, double width, grid_map::GridMap * groundTruthHighRes, + grid_map::GridMap * groundTruthLowRes) +{ const grid_map::Length mapLength(length, width); const grid_map::Position mapPosition(0.0, 0.0); *groundTruthHighRes = createMap(mapLength, highResolution, mapPosition); *groundTruthLowRes = createMap(mapLength, lowResolution, mapPosition); AnalyticalFunctions groundTruth; switch (world) { - case Worlds::Sine: { - groundTruth = createSineWorld(groundTruthHighRes); - createSineWorld(groundTruthLowRes); - break; - } + groundTruth = createSineWorld(groundTruthHighRes); + createSineWorld(groundTruthLowRes); + break; + } case Worlds::Tanh: { - groundTruth = createTanhWorld(groundTruthHighRes); - createTanhWorld(groundTruthLowRes); - break; - } + groundTruth = createTanhWorld(groundTruthHighRes); + createTanhWorld(groundTruthLowRes); + break; + } case Worlds::GaussMixture: { - groundTruth = createGaussianWorld(groundTruthHighRes); - createGaussianWorld(groundTruthLowRes); - break; - } + groundTruth = createGaussianWorld(groundTruthHighRes); + createGaussianWorld(groundTruthLowRes); + break; + } case Worlds::Poly: { - groundTruth = createPolyWorld(groundTruthHighRes); - createPolyWorld(groundTruthLowRes); - break; - } + groundTruth = createPolyWorld(groundTruthHighRes); + createPolyWorld(groundTruthLowRes); + break; + } default: throw std::runtime_error("Interpolation demo: Unknown world requested."); @@ -54,24 +58,21 @@ AnalyticalFunctions createWorld(Worlds world, double highResolution, double lowR return groundTruth; } -AnalyticalFunctions createPolyWorld(grid_map::GridMap *map) +AnalyticalFunctions createPolyWorld(grid_map::GridMap * map) { - AnalyticalFunctions func; - func.f_ = [](double x,double y) { - return (-x*x -y*y +2.0*x*y +x*x*y*y); - }; + func.f_ = [](double x, double y) { + return -x * x - y * y + 2.0 * x * y + x * x * y * y; + }; fillGridMap(map, func); return func; - } -AnalyticalFunctions createSineWorld(grid_map::GridMap *map) +AnalyticalFunctions createSineWorld(grid_map::GridMap * map) { - AnalyticalFunctions func; const double w1 = 1.0; @@ -79,38 +80,35 @@ AnalyticalFunctions createSineWorld(grid_map::GridMap *map) const double w3 = 3.0; const double w4 = 4.0; - func.f_ = [w1,w2,w3,w4](double x,double y) { - return std::cos(w1*x) + std::sin(w2*y) + std::cos(w3*x) + std::sin(w4*y); - }; + func.f_ = [w1, w2, w3, w4](double x, double y) { + return std::cos(w1 * x) + std::sin(w2 * y) + std::cos(w3 * x) + std::sin(w4 * y); + }; fillGridMap(map, func); return func; - } -AnalyticalFunctions createTanhWorld(grid_map::GridMap *map) +AnalyticalFunctions createTanhWorld(grid_map::GridMap * map) { - AnalyticalFunctions func; const double s = 5.0; - func.f_ = [s](double x,double y) { - const double expZx = std::exp(2 *s* x); - const double tanX = (expZx - 1) / (expZx + 1); - const double expZy = std::exp(2 *s* y); - const double tanY = (expZy - 1) / (expZy + 1); - return tanX + tanY; - }; + func.f_ = [s](double x, double y) { + const double expZx = std::exp(2 * s * x); + const double tanX = (expZx - 1) / (expZx + 1); + const double expZy = std::exp(2 * s * y); + const double tanY = (expZy - 1) / (expZy + 1); + return tanX + tanY; + }; fillGridMap(map, func); return func; } -AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map) +AnalyticalFunctions createGaussianWorld(grid_map::GridMap * map) { - struct Gaussian { double x0, y0; @@ -120,11 +118,11 @@ AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map) AnalyticalFunctions func; constexpr int numGaussians = 7; - std::array, numGaussians> vars = { { { 1.0, 0.3 }, { 0.25, 0.25 }, { - 0.1, 0.1 }, { 0.1, 0.1 }, { 0.1, 0.1 }, { 0.1, 0.05 }, { 0.05, 0.05 } } }; - std::array, numGaussians> means = { { { 1, -1 }, { 1, 1.7 }, - { -1, 1.6 }, { -1.8, -1.8 }, { -1, 1.8 }, { 0, 0 }, { -1.2, 0 } } }; - std::array scales = { -2.0, -1.0, 2.0, 1.0, 3.0, 4.0, 1.0 }; + std::array, numGaussians> vars = {{{1.0, 0.3}, {0.25, 0.25}, { + 0.1, 0.1}, {0.1, 0.1}, {0.1, 0.1}, {0.1, 0.05}, {0.05, 0.05}}}; + std::array, numGaussians> means = {{{1, -1}, {1, 1.7}, + {-1, 1.6}, {-1.8, -1.8}, {-1, 1.8}, {0, 0}, {-1.2, 0}}}; + std::array scales = {-2.0, -1.0, 2.0, 1.0, 3.0, 4.0, 1.0}; std::array g; @@ -136,28 +134,29 @@ AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map) g.at(i).s = scales.at(i); } - func.f_ = [g](double x,double y) { - double value = 0.0; - for (int i = 0; i < g.size(); ++i) { - const double x0 = g.at(i).x0; - const double y0 = g.at(i).y0; - const double varX = g.at(i).varX; - const double varY = g.at(i).varY; - const double s = g.at(i).s; - value += s * std::exp(-(x-x0)*(x-x0) / (2.0*varX) - (y-y0)*(y-y0) / (2.0 * varY)); - } - - return value; - }; + func.f_ = [g](double x, double y) { + double value = 0.0; + for (std::size_t i = 0; i < g.size(); ++i) { + const double x0 = g.at(i).x0; + const double y0 = g.at(i).y0; + const double varX = g.at(i).varX; + const double varY = g.at(i).varY; + const double s = g.at(i).s; + value += s * + std::exp(-(x - x0) * (x - x0) / (2.0 * varX) - (y - y0) * (y - y0) / (2.0 * varY)); + } + + return value; + }; fillGridMap(map, func); return func; } -void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions) +void fillGridMap(grid_map::GridMap * map, const AnalyticalFunctions & functions) { - grid_map::Matrix& data = (*map)[demoLayer]; + grid_map::Matrix & data = (*map)[demoLayer]; for (grid_map::GridMapIterator iterator(*map); !iterator.isPastEnd(); ++iterator) { const grid_map::Index index(*iterator); grid_map::Position pos; @@ -166,8 +165,9 @@ void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions) } } -grid_map::GridMap createMap(const grid_map::Length &length, double resolution, - const grid_map::Position &pos) +grid_map::GridMap createMap( + const grid_map::Length & length, double resolution, + const grid_map::Position & pos) { grid_map::GridMap map; @@ -178,21 +178,23 @@ grid_map::GridMap createMap(const grid_map::Length &length, double resolution, return map; } -grid_map::GridMap createInterpolatedMapFromDataMap(const grid_map::GridMap &dataMap, - double desiredResolution) +grid_map::GridMap createInterpolatedMapFromDataMap( + const grid_map::GridMap & dataMap, + double desiredResolution) { grid_map::GridMap interpolatedMap; interpolatedMap.setGeometry(dataMap.getLength(), desiredResolution, dataMap.getPosition()); - const std::string &layer = demoLayer; + const std::string & layer = demoLayer; interpolatedMap.add(layer, 0.0); interpolatedMap.setFrameId(dataMap.getFrameId()); return interpolatedMap; } -void interpolateInputMap(const grid_map::GridMap &dataMap, - grid_map::InterpolationMethods interpolationMethod, - grid_map::GridMap *interpolatedMap) +void interpolateInputMap( + const grid_map::GridMap & dataMap, + grid_map::InterpolationMethods interpolationMethod, + grid_map::GridMap * interpolatedMap) { for (grid_map::GridMapIterator iterator(*interpolatedMap); !iterator.isPastEnd(); ++iterator) { const grid_map::Index index(*iterator); @@ -203,13 +205,10 @@ void interpolateInputMap(const grid_map::GridMap &dataMap, } } -Error computeInterpolationError(const AnalyticalFunctions &groundTruth, - const grid_map::GridMap &map) +Error computeInterpolationError( + const AnalyticalFunctions & groundTruth, + const grid_map::GridMap & map) { - const double r = map.getResolution(); - const double dimX = map.getLength().x() / 2.0 - 3.0 * r; - const double dimY = map.getLength().y() / 2.0 - 3.0 * r; - unsigned int count = 0; Error error; const int nRow = map.getSize().x(); @@ -217,7 +216,7 @@ Error computeInterpolationError(const AnalyticalFunctions &groundTruth, for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { const auto row = (*iterator).x(); const auto col = (*iterator).y(); - const bool skipEvaluation = row < 2 || col < 2 || col > (nCol - 3) || row > (nRow - 3); + const bool skipEvaluation = row<2 || col<2 || col>(nCol - 3) || row>(nRow - 3); if (skipEvaluation) { continue; } @@ -236,37 +235,51 @@ Error computeInterpolationError(const AnalyticalFunctions &groundTruth, error.meanSquare_ /= count; error.meanAbs_ /= count; return error; - } -InterpolationDemo::InterpolationDemo(ros::NodeHandle *nh) +InterpolationDemo::InterpolationDemo() +: Node("grid_map_interpolation_demo") { - - nh->param("interpolation_type", interpolationMethod_, "Nearest"); - nh->param("world", world_, "Sine"); - nh->param("groundtruth_resolution", groundTruthResolution_, 0.02); - nh->param("interpolation/data_resolution", dataResolution_, 0.1); - nh->param("interpolation/interpolated_resolution", interpolatedResolution_, 0.02); - nh->param("world_size/length", worldLength_, 4.0); - nh->param("world_size/width", worldWidth_, 4.0); - - groundTruthMapPub_ = nh->advertise("ground_truth", 1, true); - dataSparseMapPub_ = nh->advertise("data_sparse", 1, true); - interpolatedMapPub_ = nh->advertise("interpolated", 1, true); + this->declare_parameter("interpolation_type", std::string("Nearest")); + this->declare_parameter("world", std::string("Sine")); + this->declare_parameter("groundtruth_resolution", rclcpp::ParameterValue(0.02)); + this->declare_parameter("interpolation.data_resolution", rclcpp::ParameterValue(0.1)); + this->declare_parameter("interpolation.interpolated_resolution", rclcpp::ParameterValue(0.02)); + this->declare_parameter("world_size.length", rclcpp::ParameterValue(4.0)); + this->declare_parameter("world_size.width", rclcpp::ParameterValue(4.0)); + + this->get_parameter("interpolation_type", interpolationMethod_); + this->get_parameter("world", world_); + this->get_parameter("groundtruth_resolution", groundTruthResolution_); + this->get_parameter("interpolation/data_resolution", dataResolution_); + this->get_parameter("interpolation/interpolated_resolution", interpolatedResolution_); + this->get_parameter("world_size.length", worldLength_); + this->get_parameter("world_size.width", worldWidth_); + + groundTruthMapPub_ = this->create_publisher( + "ground_truth", rclcpp::QoS( + 1).transient_local()); + dataSparseMapPub_ = this->create_publisher( + "data_sparse", rclcpp::QoS( + 1).transient_local()); + interpolatedMapPub_ = this->create_publisher( + "interpolated", rclcpp::QoS( + 1).transient_local()); runDemo(); } void InterpolationDemo::runDemo() { - // visualize stuff - groundTruth_ = createWorld(worlds.at(world_), groundTruthResolution_, dataResolution_, - worldLength_, worldWidth_, &groundTruthMap_, &dataSparseMap_); + groundTruth_ = createWorld( + worlds.at(world_), groundTruthResolution_, dataResolution_, + worldLength_, worldWidth_, &groundTruthMap_, &dataSparseMap_); interpolatedMap_ = createInterpolatedMapFromDataMap(dataSparseMap_, interpolatedResolution_); - interpolateInputMap(dataSparseMap_, interpolationMethods.at(interpolationMethod_), - &interpolatedMap_); + interpolateInputMap( + dataSparseMap_, interpolationMethods.at(interpolationMethod_), + &interpolatedMap_); publishGridMaps(); std::cout << "\n \n visualized the world: " << world_ << std::endl; @@ -274,7 +287,6 @@ void InterpolationDemo::runDemo() // print some info const auto statistics = computeStatistics(); printStatistics(statistics); - } InterpolationDemo::Statistics InterpolationDemo::computeStatistics() const @@ -283,27 +295,29 @@ InterpolationDemo::Statistics InterpolationDemo::computeStatistics() const for (auto world = worlds.cbegin(); world != worlds.cend(); ++world) { std::map methodsStats; for (auto method = interpolationMethods.cbegin(); method != interpolationMethods.cend(); - ++method) { + ++method) + { const auto errorAndDuration = interpolateAndComputeError(world->first, method->first); Statistic statistic; statistic.duration_ = errorAndDuration.second; statistic.error_ = errorAndDuration.first; statistic.interpolationMethod_ = method->first; statistic.world_ = world->first; - methodsStats.insert( { method->first, statistic }); + methodsStats.insert({method->first, statistic}); } - stats.insert( { world->first, methodsStats }); + stats.insert({world->first, methodsStats}); } - return std::move(stats); + return stats; } InterpolationDemo::ErrorAndDuration InterpolationDemo::interpolateAndComputeError( - const std::string world, const std::string &method) const + const std::string world, const std::string & method) const { grid_map::GridMap highResMap, dataMap; - auto groundTruth = createWorld(worlds.at(world), groundTruthResolution_, dataResolution_, - worldLength_, worldWidth_, &highResMap, &dataMap); + auto groundTruth = createWorld( + worlds.at(world), groundTruthResolution_, dataResolution_, + worldLength_, worldWidth_, &highResMap, &dataMap); auto interpolatedMap = createInterpolatedMapFromDataMap(dataMap, interpolatedResolution_); const auto start = clk::now(); @@ -318,22 +332,26 @@ InterpolationDemo::ErrorAndDuration InterpolationDemo::interpolateAndComputeErro return errorAndDuration; } -void InterpolationDemo::printStatistics(const Statistics &stats) const +void InterpolationDemo::printStatistics(const Statistics & stats) const { std::cout << " \n \n ================================== \n"; - printf("Interpolated map of size: %f x %f \n", interpolatedMap_.getLength().x(), - interpolatedMap_.getLength().y()); - printf("Resolution of the data: %f, resolution of the interpolated map: %f, \n \n", - dataSparseMap_.getResolution(), interpolatedMap_.getResolution()); + printf( + "Interpolated map of size: %f x %f \n", interpolatedMap_.getLength().x(), + interpolatedMap_.getLength().y()); + printf( + "Resolution of the data: %f, resolution of the interpolated map: %f, \n \n", + dataSparseMap_.getResolution(), interpolatedMap_.getResolution()); for (auto world = worlds.cbegin(); world != worlds.cend(); ++world) { std::cout << "Stats for the world: " << world->first << std::endl; - for (auto method = interpolationMethods.cbegin(); method != interpolationMethods.cend(); - ++method) { + for (auto method = interpolationMethods.cbegin(); + method != interpolationMethods.cend(); ++method) + { Statistic s = stats.at(world->first).at(method->first); printf( - "Method: %-20s Mean absolute error: %-10f max error: %-10f avg query duration: %-10f microsec \n", - method->first.c_str(), s.error_.meanAbs_, s.error_.max_, s.duration_); + "Method: %-20s Mean absolute error: %-10f " + "max error: %-10f avg query duration: %-10f microsec \n", + method->first.c_str(), s.error_.meanAbs_, s.error_.max_, s.duration_); } std::cout << std::endl; } @@ -341,14 +359,13 @@ void InterpolationDemo::printStatistics(const Statistics &stats) const void InterpolationDemo::publishGridMaps() const { - grid_map_msgs::GridMap highResMsg, lowResMsg, interpolatedMsg; - grid_map::GridMapRosConverter::toMessage(groundTruthMap_, highResMsg); - grid_map::GridMapRosConverter::toMessage(dataSparseMap_, lowResMsg); - grid_map::GridMapRosConverter::toMessage(interpolatedMap_, interpolatedMsg); - groundTruthMapPub_.publish(highResMsg); - dataSparseMapPub_.publish(lowResMsg); - interpolatedMapPub_.publish(interpolatedMsg); + auto highResMsg = grid_map::GridMapRosConverter::toMessage(groundTruthMap_); + auto lowResMsg = grid_map::GridMapRosConverter::toMessage(dataSparseMap_); + auto interpolatedMsg = grid_map::GridMapRosConverter::toMessage(interpolatedMap_); + std::cout << "LOOOL" << std::endl; + groundTruthMapPub_->publish(std::move(highResMsg)); + dataSparseMapPub_->publish(std::move(lowResMsg)); + interpolatedMapPub_->publish(std::move(interpolatedMsg)); } -} /* namespace grid_map_demos */ - +} // namespace grid_map_demos diff --git a/grid_map_demos/src/IteratorsDemo.cpp b/grid_map_demos/src/IteratorsDemo.cpp index abc0b2e7b..dea065a46 100644 --- a/grid_map_demos/src/IteratorsDemo.cpp +++ b/grid_map_demos/src/IteratorsDemo.cpp @@ -8,30 +8,30 @@ #include "grid_map_demos/IteratorsDemo.hpp" -// ROS -#include +#include +#include +#include +#include -using namespace std; -using namespace ros; -using namespace grid_map; - -namespace grid_map_demos { +namespace grid_map_demos +{ -IteratorsDemo::IteratorsDemo(ros::NodeHandle& nodeHandle) - : nodeHandle_(nodeHandle), - map_(vector({"type"})) +IteratorsDemo::IteratorsDemo() +: Node("grid_map_iterators_demo"), + map_(std::vector({"type"})) { - ROS_INFO("Grid map iterators demo node started."); - gridMapPublisher_ = nodeHandle_.advertise("grid_map", 1, true); - polygonPublisher_ = nodeHandle_.advertise("polygon", 1, true); + RCLCPP_INFO(this->get_logger(), "Grid map iterators demo node started."); + gridMapPublisher_ = this->create_publisher( + "grid_map", rclcpp::QoS(1).transient_local()); + polygonPublisher_ = this->create_publisher( + "polygon", rclcpp::QoS(1).transient_local()); // Setting up map. - map_.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0)); + map_.setGeometry(grid_map::Length(1.0, 1.0), 0.05, grid_map::Position(0.0, 0.0)); map_.setFrameId("map"); publish(); - ros::Duration duration(2.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(2)); demoGridMapIterator(); demoSubmapIterator(); @@ -47,212 +47,208 @@ IteratorsDemo::~IteratorsDemo() {} void IteratorsDemo::demoGridMapIterator() { - ROS_INFO("Running grid map iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running grid map iterator demo."); map_.clearAll(); publish(); // Note: In this example we locally store a reference to the map data // for improved performance. See `iterator_benchmark.cpp` and the // README.md for a comparison and more information. - grid_map::Matrix& data = map_["type"]; + grid_map::Matrix & data = map_["type"]; for (grid_map::GridMapIterator iterator(map_); !iterator.isPastEnd(); ++iterator) { const int i = iterator.getLinearIndex(); data(i) = 1.0; publish(); - ros::Duration duration(0.01); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(10)); } - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } void IteratorsDemo::demoSubmapIterator() { - ROS_INFO("Running submap iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running submap iterator demo."); map_.clearAll(); publish(); - Index submapStartIndex(3, 5); - Index submapBufferSize(12, 7); + grid_map::Index submapStartIndex(3, 5); + grid_map::Index submapBufferSize(12, 7); for (grid_map::SubmapIterator iterator(map_, submapStartIndex, submapBufferSize); - !iterator.isPastEnd(); ++iterator) { + !iterator.isPastEnd(); ++iterator) + { map_.at("type", *iterator) = 1.0; publish(); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } void IteratorsDemo::demoCircleIterator() { - ROS_INFO("Running circle iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running circle iterator demo."); map_.clearAll(); publish(); - Position center(0.0, -0.15); + grid_map::Position center(0.0, -0.15); double radius = 0.4; for (grid_map::CircleIterator iterator(map_, center, radius); - !iterator.isPastEnd(); ++iterator) { + !iterator.isPastEnd(); ++iterator) + { map_.at("type", *iterator) = 1.0; publish(); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } void IteratorsDemo::demoEllipseIterator() { - ROS_INFO("Running ellipse iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running ellipse iterator demo."); map_.clearAll(); publish(); - Position center(0.0, -0.15); - Length length(0.45, 0.9); + grid_map::Position center(0.0, -0.15); + grid_map::Length length(0.45, 0.9); for (grid_map::EllipseIterator iterator(map_, center, length, M_PI_4); - !iterator.isPastEnd(); ++iterator) { + !iterator.isPastEnd(); ++iterator) + { map_.at("type", *iterator) = 1.0; publish(); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } void IteratorsDemo::demoSpiralIterator() { - ROS_INFO("Running spiral iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running spiral iterator demo."); map_.clearAll(); publish(); - Position center(0.0, -0.15); + grid_map::Position center(0.0, -0.15); double radius = 0.4; for (grid_map::SpiralIterator iterator(map_, center, radius); - !iterator.isPastEnd(); ++iterator) { + !iterator.isPastEnd(); ++iterator) + { map_.at("type", *iterator) = 1.0; publish(); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } void IteratorsDemo::demoLineIterator() { - ROS_INFO("Running line iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running line iterator demo."); map_.clearAll(); publish(); - Index start(18, 2); - Index end(2, 13); + grid_map::Index start(18, 2); + grid_map::Index end(2, 13); for (grid_map::LineIterator iterator(map_, start, end); - !iterator.isPastEnd(); ++iterator) { + !iterator.isPastEnd(); ++iterator) + { map_.at("type", *iterator) = 1.0; publish(); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } void IteratorsDemo::demoPolygonIterator(const bool prepareForOtherDemos) { - ROS_INFO("Running polygon iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running polygon iterator demo."); map_.clearAll(); - if (prepareForOtherDemos) map_["type"].setZero(); + if (prepareForOtherDemos) {map_["type"].setZero();} publish(); grid_map::Polygon polygon; polygon.setFrameId(map_.getFrameId()); - polygon.addVertex(Position( 0.480, 0.000)); - polygon.addVertex(Position( 0.164, 0.155)); - polygon.addVertex(Position( 0.116, 0.500)); - polygon.addVertex(Position(-0.133, 0.250)); - polygon.addVertex(Position(-0.480, 0.399)); - polygon.addVertex(Position(-0.316, 0.000)); - polygon.addVertex(Position(-0.480, -0.399)); - polygon.addVertex(Position(-0.133, -0.250)); - polygon.addVertex(Position( 0.116, -0.500)); - polygon.addVertex(Position( 0.164, -0.155)); - polygon.addVertex(Position( 0.480, 0.000)); - - geometry_msgs::PolygonStamped message; + polygon.addVertex(grid_map::Position(0.480, 0.000)); + polygon.addVertex(grid_map::Position(0.164, 0.155)); + polygon.addVertex(grid_map::Position(0.116, 0.500)); + polygon.addVertex(grid_map::Position(-0.133, 0.250)); + polygon.addVertex(grid_map::Position(-0.480, 0.399)); + polygon.addVertex(grid_map::Position(-0.316, 0.000)); + polygon.addVertex(grid_map::Position(-0.480, -0.399)); + polygon.addVertex(grid_map::Position(-0.133, -0.250)); + polygon.addVertex(grid_map::Position(0.116, -0.500)); + polygon.addVertex(grid_map::Position(0.164, -0.155)); + polygon.addVertex(grid_map::Position(0.480, 0.000)); + + geometry_msgs::msg::PolygonStamped message; grid_map::PolygonRosConverter::toMessage(polygon, message); - polygonPublisher_.publish(message); + polygonPublisher_->publish(message); for (grid_map::PolygonIterator iterator(map_, polygon); - !iterator.isPastEnd(); ++iterator) { + !iterator.isPastEnd(); ++iterator) + { map_.at("type", *iterator) = 1.0; if (!prepareForOtherDemos) { publish(); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } } if (!prepareForOtherDemos) { - ros::Duration duration(1.0); - duration.sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); } } void IteratorsDemo::demoSlidingWindowIterator() { - ROS_INFO("Running sliding window iterator demo."); + RCLCPP_INFO(this->get_logger(), "Running sliding window iterator demo."); demoPolygonIterator(true); publish(); const size_t windowSize = 3; - const grid_map::SlidingWindowIterator::EdgeHandling edgeHandling = grid_map::SlidingWindowIterator::EdgeHandling::CROP; + const grid_map::SlidingWindowIterator::EdgeHandling edgeHandling = + grid_map::SlidingWindowIterator::EdgeHandling::CROP; map_.add("copy", map_["type"]); for (grid_map::SlidingWindowIterator iterator(map_, "copy", edgeHandling, windowSize); - !iterator.isPastEnd(); ++iterator) { - map_.at("type", *iterator) = iterator.getData().meanOfFinites(); // Blurring. + !iterator.isPastEnd(); ++iterator) + { + map_.at("type", *iterator) = iterator.getData().meanOfFinites(); // Blurring. publish(); // Visualize sliding window as polygon. grid_map::Polygon polygon; - Position center; + grid_map::Position center; map_.getPosition(*iterator, center); - const Length windowHalfLength(Length::Constant(0.5 * (double) windowSize * map_.getResolution())); - polygon.addVertex(center + (Eigen::Array2d(-1.0,-1.0) * windowHalfLength).matrix()); + const grid_map::Length windowHalfLength(grid_map::Length::Constant( + 0.5 * static_cast(windowSize) * map_.getResolution())); + polygon.addVertex(center + (Eigen::Array2d(-1.0, -1.0) * windowHalfLength).matrix()); polygon.addVertex(center + (Eigen::Array2d(-1.0, 1.0) * windowHalfLength).matrix()); - polygon.addVertex(center + (Eigen::Array2d( 1.0, 1.0) * windowHalfLength).matrix()); - polygon.addVertex(center + (Eigen::Array2d( 1.0,-1.0) * windowHalfLength).matrix()); + polygon.addVertex(center + (Eigen::Array2d(1.0, 1.0) * windowHalfLength).matrix()); + polygon.addVertex(center + (Eigen::Array2d(1.0, -1.0) * windowHalfLength).matrix()); polygon.setFrameId(map_.getFrameId()); - geometry_msgs::PolygonStamped message; + geometry_msgs::msg::PolygonStamped message; grid_map::PolygonRosConverter::toMessage(polygon, message); - polygonPublisher_.publish(message); + polygonPublisher_->publish(message); - ros::Duration duration(0.02); - duration.sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(20)); } } void IteratorsDemo::publish() { - map_.setTimestamp(ros::Time::now().toNSec()); - grid_map_msgs::GridMap message; - grid_map::GridMapRosConverter::toMessage(map_, message); - gridMapPublisher_.publish(message); - ROS_DEBUG("Grid map (timestamp %f) published.", message.info.header.stamp.toSec()); + map_.setTimestamp(this->now().nanoseconds()); + std::unique_ptr message; + message = grid_map::GridMapRosConverter::toMessage(map_); + RCLCPP_DEBUG( + this->get_logger(), "Publishing grid map (timestamp %f).", + rclcpp::Time(message->header.stamp).nanoseconds() * 1e-9); + gridMapPublisher_->publish(std::move(message)); } -} /* namespace */ +} // namespace grid_map_demos diff --git a/grid_map_demos/src/OctomapToGridmapDemo.cpp b/grid_map_demos/src/OctomapToGridmapDemo.cpp index 4f614a385..7d3208f1e 100644 --- a/grid_map_demos/src/OctomapToGridmapDemo.cpp +++ b/grid_map_demos/src/OctomapToGridmapDemo.cpp @@ -10,22 +10,32 @@ #include -#include #include #include -#include +#include +#include -namespace grid_map_demos { +#include +#include +#include +#include -OctomapToGridmapDemo::OctomapToGridmapDemo(ros::NodeHandle& nodeHandle) - : nodeHandle_(nodeHandle), - map_(grid_map::GridMap({"elevation"})) +namespace grid_map_demos +{ + +OctomapToGridmapDemo::OctomapToGridmapDemo() +: Node("octomap_to_gridmap_demo"), + map_(grid_map::GridMap({"elevation"})) { readParameters(); - client_ = nodeHandle_.serviceClient(octomapServiceTopic_); + client_ = this->create_client(octomapServiceTopic_); map_.setBasicLayers({"elevation"}); - gridMapPublisher_ = nodeHandle_.advertise("grid_map", 1, true); - octomapPublisher_ = nodeHandle_.advertise("octomap", 1, true); + gridMapPublisher_ = this->create_publisher( + "grid_map", rclcpp::QoS( + 1).transient_local()); + octomapPublisher_ = this->create_publisher( + "octomap", rclcpp::QoS( + 1).transient_local()); } OctomapToGridmapDemo::~OctomapToGridmapDemo() @@ -34,31 +44,53 @@ OctomapToGridmapDemo::~OctomapToGridmapDemo() bool OctomapToGridmapDemo::readParameters() { - nodeHandle_.param("octomap_service_topic", octomapServiceTopic_, std::string("/octomap_binary")); - nodeHandle_.param("min_x", minX_, NAN); - nodeHandle_.param("max_x", maxX_, NAN); - nodeHandle_.param("min_y", minY_, NAN); - nodeHandle_.param("max_y", maxY_, NAN); - nodeHandle_.param("min_z", minZ_, NAN); - nodeHandle_.param("max_z", maxZ_, NAN); + this->declare_parameter("octomap_service_topic", std::string("/octomap_binary")); + this->declare_parameter("min_x", NAN); + this->declare_parameter("max_x", NAN); + this->declare_parameter("min_y", NAN); + this->declare_parameter("max_y", NAN); + this->declare_parameter("min_z", NAN); + this->declare_parameter("max_z", NAN); + + this->get_parameter("octomap_service_topic", octomapServiceTopic_); + this->get_parameter("min_x", minX_); + this->get_parameter("max_x", maxX_); + this->get_parameter("min_y", minY_); + this->get_parameter("max_y", maxY_); + this->get_parameter("min_z", minZ_); + this->get_parameter("max_z", maxZ_); return true; } void OctomapToGridmapDemo::convertAndPublishMap() { - octomap_msgs::GetOctomap srv; - if (!client_.call(srv)) { - ROS_ERROR_STREAM("Failed to call service: " << octomapServiceTopic_); + rclcpp::Clock clock; + + while (!client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "client interrupted while waiting for service to appear."); + return; + } + RCLCPP_INFO_THROTTLE(this->get_logger(), clock, 1000, "waiting for service to appear..."); + } + + auto request = std::make_shared(); + auto result_future = client_->async_send_request(request); + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to call service: " << octomapServiceTopic_); return; } + auto response = result_future.get(); // creating octree - octomap::OcTree* octomap = nullptr; - octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(srv.response.map); + octomap::OcTree * octomap = nullptr; + octomap::AbstractOcTree * tree = octomap_msgs::msgToMap(response->map); if (tree) { - octomap = dynamic_cast(tree); + octomap = dynamic_cast(tree); } else { - ROS_ERROR("Failed to call convert Octomap."); + RCLCPP_ERROR(this->get_logger(), "Failed to call convert Octomap."); return; } @@ -66,35 +98,45 @@ void OctomapToGridmapDemo::convertAndPublishMap() grid_map::Position3 max_bound; octomap->getMetricMin(min_bound(0), min_bound(1), min_bound(2)); octomap->getMetricMax(max_bound(0), max_bound(1), max_bound(2)); - if(!std::isnan(minX_)) + if (!std::isnan(minX_)) { min_bound(0) = minX_; - if(!std::isnan(maxX_)) + } + if (!std::isnan(maxX_)) { max_bound(0) = maxX_; - if(!std::isnan(minY_)) + } + if (!std::isnan(minY_)) { min_bound(1) = minY_; - if(!std::isnan(maxY_)) + } + if (!std::isnan(maxY_)) { max_bound(1) = maxY_; - if(!std::isnan(minZ_)) + } + if (!std::isnan(minZ_)) { min_bound(2) = minZ_; - if(!std::isnan(maxZ_)) + } + if (!std::isnan(maxZ_)) { max_bound(2) = maxZ_; - bool res = grid_map::GridMapOctomapConverter::fromOctomap(*octomap, "elevation", map_, &min_bound, &max_bound); + } + bool res = grid_map::GridMapOctomapConverter::fromOctomap( + *octomap, "elevation", map_, &min_bound, + &max_bound); if (!res) { - ROS_ERROR("Failed to call convert Octomap."); + RCLCPP_ERROR(this->get_logger(), "Failed to call convert Octomap."); return; } - map_.setFrameId(srv.response.map.header.frame_id); + map_.setFrameId(response->map.header.frame_id); // Publish as grid map. - grid_map_msgs::GridMap gridMapMessage; - grid_map::GridMapRosConverter::toMessage(map_, gridMapMessage); - gridMapPublisher_.publish(gridMapMessage); + auto gridMapMessage = grid_map::GridMapRosConverter::toMessage(map_); + gridMapPublisher_->publish(std::move(gridMapMessage)); // Also publish as an octomap msg for visualization - octomap_msgs::Octomap octomapMessage; + OctomapMessage octomapMessage; octomap_msgs::fullMapToMsg(*octomap, octomapMessage); octomapMessage.header.frame_id = map_.getFrameId(); - octomapPublisher_.publish(octomapMessage); + + std::unique_ptr octomapMessagePtr(new + OctomapMessage(octomapMessage)); + octomapPublisher_->publish(std::move(octomapMessagePtr)); } -} /* namespace */ +} // namespace grid_map_demos diff --git a/grid_map_demos/src/filters_demo_node.cpp b/grid_map_demos/src/filters_demo_node.cpp index b8b142a8c..9a07b392d 100644 --- a/grid_map_demos/src/filters_demo_node.cpp +++ b/grid_map_demos/src/filters_demo_node.cpp @@ -6,100 +6,14 @@ * Institute: ETH Zurich, ANYbotics */ +#include +#include #include "grid_map_demos/FiltersDemo.hpp" -#include - -int main(int argc, char** argv) +int main(int argc, char ** argv) { - ros::init(argc, argv, "grid_map_filters_demo"); - ros::NodeHandle nodeHandle("~"); - bool success; - grid_map_demos::FiltersDemo filtersDemo(nodeHandle, success); - if (success) ros::spin(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } - - - - - -///* -// * filters_demo_node.cpp -// * -// * Created on: Aug 14, 2017 -// * Author: Peter Fankhauser -// * Institute: ETH Zurich, ANYbotics -// */ -// -//#include -//#include -//#include -//#include -//#include -// -//using namespace grid_map; -// -//class GridMapFilterChain -// -// -//(const grid_map_msgs::GridMapPtr& message) -//{ -// GridMap inputMap; -// GridMapRosConverter::fromMessage(message, inputMap); -// ROS_INFO("I heard: [%s]", msg->data.c_str()); -//} -// -//int main(int argc, char** argv) -//{ -// ros::init(argc, argv, "grid_map_filters_demo"); -// ros::NodeHandle nodeHandle("~"); -// ros::Subscriber subscriber = nodeHandle.subscribe("grid_map_in", 1000, chatterCallback); -// ros::Publisher publisher = nodeHandle.advertise("grid_map_out", 1, true); -// -// // Setup filter chain. -// filters::FilterChain filterChain("grid_map::GridMap"); -// if (!filterChain.configure("grid_map_filters", nodeHandle)) { -// ROS_ERROR("Could not configure the filter chain!"); -// return false; -// } -// -// // Create input grid map. -// GridMap inputMap({"elevation"}); -// inputMap.setFrameId("map"); -// inputMap.setGeometry(Length(0.7, 0.7), 0.01, Position(0.0, 0.0)); -// ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.", -// inputMap.getLength().x(), inputMap.getLength().y(), -// inputMap.getSize()(0), inputMap.getSize()(1), -// inputMap.getPosition().x(), inputMap.getPosition().y(), inputMap.getFrameId().c_str()); -// inputMap["elevation"].setRandom(); -// inputMap["elevation"] *= 0.01; -// for (grid_map::CircleIterator iterator(inputMap, Position(0.0, 0.0), 0.15); !iterator.isPastEnd(); ++iterator) { -// inputMap.at("elevation", *iterator) = 0.1; -// } -// -// while (nodeHandle.ok()) { -// -// // Publish original grid map. -// inputMap.setTimestamp(ros::Time::now().toNSec()); -// grid_map_msgs::GridMap message; -// GridMapRosConverter::toMessage(inputMap, message); -// publisher.publish(message); -// ros::Duration(1.0).sleep(); -// -// // Apply filter chain. -// GridMap outputMap; -// if (!filterChain.update(inputMap, outputMap)) { -// ROS_ERROR("Could not update the grid map filter chain!"); -// break; -// } -// -// // Publish filtered output grid map. -// outputMap.setTimestamp(ros::Time::now().toNSec()); -// GridMapRosConverter::toMessage(outputMap, message); -// publisher.publish(message); -// ros::Duration(1.0).sleep(); -// } -// -// return 0; -//} diff --git a/grid_map_demos/src/image_to_gridmap_demo_node.cpp b/grid_map_demos/src/image_to_gridmap_demo_node.cpp index 0b7349592..80a6697c0 100644 --- a/grid_map_demos/src/image_to_gridmap_demo_node.cpp +++ b/grid_map_demos/src/image_to_gridmap_demo_node.cpp @@ -6,16 +6,14 @@ * Institute: ETH Zurich, ANYbotics */ -#include +#include +#include #include "grid_map_demos/ImageToGridmapDemo.hpp" -int main(int argc, char** argv) +int main(int argc, char ** argv) { - // Initialize node and publisher. - ros::init(argc, argv, "image_to_gridmap_demo"); - ros::NodeHandle nh("~"); - grid_map_demos::ImageToGridmapDemo imageToGridmapDemo(nh); - - ros::spin(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } diff --git a/grid_map_demos/src/interpolation_demo_node.cpp b/grid_map_demos/src/interpolation_demo_node.cpp index 873e8761f..47aa13fa9 100644 --- a/grid_map_demos/src/interpolation_demo_node.cpp +++ b/grid_map_demos/src/interpolation_demo_node.cpp @@ -5,14 +5,14 @@ * Author: jelavice */ +#include +#include #include "grid_map_demos/InterpolationDemo.hpp" -#include -int main(int argc, char** argv) +int main(int argc, char ** argv) { - ros::init(argc, argv, "grid_map_interpolation_demo"); - ros::NodeHandle nodeHandle("~"); - grid_map_demos::InterpolationDemo interpolationDemo(&nodeHandle); - ros::spin(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } diff --git a/grid_map_demos/src/iterator_benchmark.cpp b/grid_map_demos/src/iterator_benchmark.cpp index ec09714e4..1052413a4 100644 --- a/grid_map_demos/src/iterator_benchmark.cpp +++ b/grid_map_demos/src/iterator_benchmark.cpp @@ -8,22 +8,21 @@ #include #include #include +#include -using namespace std; -using namespace std::chrono; -using namespace grid_map; - -#define duration(a) duration_cast(a).count() -typedef high_resolution_clock clk; +#define duration(a) std::chrono::duration_cast(a).count() +typedef std::chrono::high_resolution_clock clk; /*! * Convenient use of iterator. */ -void runGridMapIteratorVersion1(GridMap& map, const string& layer_from, const string& layer_to) +void runGridMapIteratorVersion1( + grid_map::GridMap & map, const std::string & layer_from, + const std::string & layer_to) { - for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { + for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { const float value_from = map.at(layer_from, *iterator); - float& value_to = map.at(layer_to, *iterator); + float & value_to = map.at(layer_to, *iterator); value_to = value_to > value_from ? value_to : value_from; } } @@ -31,29 +30,33 @@ void runGridMapIteratorVersion1(GridMap& map, const string& layer_from, const st /*! * Improved efficiency by storing direct access to data layers. */ -void runGridMapIteratorVersion2(GridMap& map, const string& layer_from, const string& layer_to) +void runGridMapIteratorVersion2( + grid_map::GridMap & map, const std::string & layer_from, + const std::string & layer_to) { - const auto& data_from = map[layer_from]; - auto& data_to = map[layer_to]; - for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { - const Index index(*iterator); + const auto & data_from = map[layer_from]; + auto & data_to = map[layer_to]; + for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index index(*iterator); const float value_from = data_from(index(0), index(1)); - float& value_to = data_to(index(0), index(1)); + float & value_to = data_to(index(0), index(1)); value_to = value_to > value_from ? value_to : value_from; } } /*! - * Improved efficiency by using linear index. + * Improved efficiency by using linear grid_map::Index. */ -void runGridMapIteratorVersion3(GridMap& map, const string& layer_from, const string& layer_to) +void runGridMapIteratorVersion3( + grid_map::GridMap & map, const std::string & layer_from, + const std::string & layer_to) { - const auto& data_from = map[layer_from]; - auto& data_to = map[layer_to]; - for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { - const size_t i = iterator.getLinearIndex(); + const auto & data_from = map[layer_from]; + auto & data_to = map[layer_to]; + for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { + const std::size_t i = iterator.getLinearIndex(); const float value_from = data_from(i); - float& value_to = data_to(i); + float & value_to = data_to(i); value_to = value_to > value_from ? value_to : value_from; } } @@ -62,7 +65,9 @@ void runGridMapIteratorVersion3(GridMap& map, const string& layer_from, const st * Whenever possible, make use of the Eigen methods for maximum efficiency * and readability. */ -void runEigenFunction(GridMap& map, const string& layer_from, const string& layer_to) +void runEigenFunction( + grid_map::GridMap & map, const std::string & layer_from, + const std::string & layer_to) { map[layer_to] = map[layer_to].cwiseMax(map[layer_from]); } @@ -70,14 +75,16 @@ void runEigenFunction(GridMap& map, const string& layer_from, const string& laye /*! * For comparison. */ -void runCustomIndexIteration(GridMap& map, const string& layer_from, const string& layer_to) +void runCustomIndexIteration( + grid_map::GridMap & map, const std::string & layer_from, + const std::string & layer_to) { - const auto& data_from = map[layer_from]; - auto& data_to = map[layer_to]; - for (size_t j = 0; j < data_to.cols(); ++j) { - for (size_t i = 0; i < data_to.rows(); ++i) { + const auto & data_from = map[layer_from]; + auto & data_to = map[layer_to]; + for (int64_t j = 0; j < data_to.cols(); ++j) { + for (int64_t i = 0; i < data_to.rows(); ++i) { const float value_from = data_from(i, j); - float& value_to = data_to(i, j); + float & value_to = data_to(i, j); value_to = value_to > value_from ? value_to : value_from; } } @@ -86,19 +93,21 @@ void runCustomIndexIteration(GridMap& map, const string& layer_from, const strin /*! * For comparison. */ -void runCustomLinearIndexIteration(GridMap& map, const string& layer_from, const string& layer_to) +void runCustomLinearIndexIteration( + grid_map::GridMap & map, const std::string & layer_from, + const std::string & layer_to) { - const auto& data_from = map[layer_from]; - auto& data_to = map[layer_to]; - for (size_t i = 0; i < data_to.size(); ++i) { + const auto & data_from = map[layer_from]; + auto & data_to = map[layer_to]; + for (int64_t i = 0; i < data_to.size(); ++i) { data_to(i) = data_to(i) > data_from(i) ? data_to(i) : data_from(i); } } -int main(int argc, char* argv[]) +int main() { - GridMap map; - map.setGeometry(Length(20.0, 20.0), 0.004, Position(0.0, 0.0)); + grid_map::GridMap map; + map.setGeometry(grid_map::Length(20.0, 20.0), 0.004, grid_map::Position(0.0, 0.0)); map.add("random"); map["random"].setRandom(); map.add("layer1", 0.0); @@ -108,38 +117,45 @@ int main(int argc, char* argv[]) map.add("layer5", 0.0); map.add("layer6", 0.0); - cout << "Results for iteration over " << map.getSize()(0) << " x " << map.getSize()(1) << " (" << map.getSize().prod() << ") grid cells." << endl; - cout << "=========================================" << endl; + std::cout << "Results for iteration over " << map.getSize()(0) << " x " << map.getSize()(1) << + " (" << + map.getSize().prod() << ") grid cells." << std::endl; + std::cout << "=========================================" << std::endl; clk::time_point t1 = clk::now(); runGridMapIteratorVersion1(map, "random", "layer1"); clk::time_point t2 = clk::now(); - cout << "Duration grid map iterator (convenient use): " << duration(t2 - t1) << " ms" << endl; + std::cout << "Duration grid map iterator (convenient use): " << duration(t2 - t1) << " ms" << + std::endl; t1 = clk::now(); runGridMapIteratorVersion2(map, "random", "layer2"); t2 = clk::now(); - cout << "Duration grid map iterator (direct access to data layers): " << duration(t2 - t1) << " ms" << endl; + std::cout << "Duration grid map iterator (direct access to data layers): " << duration(t2 - t1) << + " ms" << std::endl; t1 = clk::now(); runGridMapIteratorVersion3(map, "random", "layer3"); t2 = clk::now(); - cout << "Duration grid map iterator (linear index): " << duration(t2 - t1) << " ms" << endl; + std::cout << "Duration grid map iterator (linear grid_map::Index): " << duration(t2 - t1) << + " ms" << std::endl; t1 = clk::now(); runEigenFunction(map, "random", "layer4"); t2 = clk::now(); - cout << "Duration Eigen function: " << duration(t2 - t1) << " ms" << endl; + std::cout << "Duration Eigen function: " << duration(t2 - t1) << " ms" << std::endl; t1 = clk::now(); runCustomIndexIteration(map, "random", "layer5"); t2 = clk::now(); - cout << "Duration custom index iteration: " << duration(t2 - t1) << " ms" << endl; + std::cout << "Duration custom grid_map::Index iteration: " << duration(t2 - t1) << " ms" << + std::endl; t1 = clk::now(); runCustomLinearIndexIteration(map, "random", "layer6"); t2 = clk::now(); - cout << "Duration custom linear index iteration: " << duration(t2 - t1) << " ms" << endl; + std::cout << "Duration custom linear grid_map::Index iteration: " << duration(t2 - t1) << " ms" << + std::endl; return 0; } diff --git a/grid_map_demos/src/iterators_demo_node.cpp b/grid_map_demos/src/iterators_demo_node.cpp index dc4c50096..42b721cb0 100644 --- a/grid_map_demos/src/iterators_demo_node.cpp +++ b/grid_map_demos/src/iterators_demo_node.cpp @@ -6,16 +6,14 @@ * Institute: ETH Zurich, ANYbotics */ -#include +#include +#include #include "grid_map_demos/IteratorsDemo.hpp" -int main(int argc, char** argv) +int main(int argc, char ** argv) { - ros::init(argc, argv, "grid_map_iterators_demo"); - - ros::NodeHandle nodeHandle("~"); - grid_map_demos::IteratorsDemo iteratorsDemo(nodeHandle); - - ros::requestShutdown(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } diff --git a/grid_map_demos/src/move_demo_node.cpp b/grid_map_demos/src/move_demo_node.cpp index 5f379eac5..080a443a1 100644 --- a/grid_map_demos/src/move_demo_node.cpp +++ b/grid_map_demos/src/move_demo_node.cpp @@ -1,48 +1,54 @@ -#include +#include #include +#include +#include -using namespace grid_map; -using namespace ros; +using namespace std::chrono_literals; -int main(int argc, char** argv) +int main(int argc, char ** argv) { // Initialize node and publisher. - init(argc, argv, "move_demo"); - NodeHandle nh("~"); - Publisher publisher = nh.advertise("grid_map", 1, true); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = std::make_shared("move_demo"); + + auto publisher = node->create_publisher( + "grid_map", rclcpp::QoS( + 1).transient_local()); // Create grid map. - GridMap map({"layer"}); + grid_map::GridMap map({"layer"}); map.setFrameId("map"); - map.setGeometry(Length(0.7, 0.7), 0.01, Position(0.0, 0.0)); - ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.", + map.setGeometry(grid_map::Length(0.7, 0.7), 0.01, grid_map::Position(0.0, 0.0)); + RCLCPP_INFO( + node->get_logger(), + "Created map with size %f x %f m (%i x %i cells).\n" + " The center of the map is located at (%f, %f) in the %s frame.", map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1), map.getPosition().x(), map.getPosition().y(), map.getFrameId().c_str()); map["layer"].setRandom(); bool useMoveMethod = true; - while (nh.ok()) { - + while (rclcpp::ok()) { if (useMoveMethod) { - ROS_INFO("Using the `move(...)` method."); + RCLCPP_INFO(node->get_logger(), "Using the `move(...)` method."); } else { - ROS_INFO("Using the `setPosition(...)` method."); + RCLCPP_INFO(node->get_logger(), "Using the `setPosition(...)` method."); } // Work with temporary map in a loop. - GridMap tempMap(map); - Rate rate(10.0); - ros::Time startTime = ros::Time::now(); - ros::Duration duration(0.0); + grid_map::GridMap tempMap(map); + rclcpp::Rate rate(10.0); + rclcpp::Time startTime = node->now(); + rclcpp::Duration duration(0.0ns); - while (duration <= ros::Duration(10.0)) { - ros::Time time = ros::Time::now(); + while (duration <= rclcpp::Duration::from_seconds(10.0)) { + rclcpp::Time time = node->now(); duration = time - startTime; // Change position of the map with either the `move` or `setPosition` method. - const double t = duration.toSec(); - Position newPosition = 0.03 * t * Position(cos(t), sin(t)); + const double t = duration.seconds(); + grid_map::Position newPosition = 0.03 * t * grid_map::Position(cos(t), sin(t)); if (useMoveMethod) { tempMap.move(newPosition); @@ -51,12 +57,13 @@ int main(int argc, char** argv) } // Publish grid map. - tempMap.setTimestamp(time.toNSec()); - grid_map_msgs::GridMap message; - GridMapRosConverter::toMessage(tempMap, message); - publisher.publish(message); - ROS_DEBUG("Grid map (duration %f) published with new position [%f, %f].", - duration.toSec(), tempMap.getPosition().x(), tempMap.getPosition().y()); + tempMap.setTimestamp(time.nanoseconds()); + auto message = grid_map::GridMapRosConverter::toMessage(tempMap); + publisher->publish(std::move(message)); + RCLCPP_DEBUG( + node->get_logger(), + "Grid map (duration %f) published with new position [%f, %f].", + duration.seconds(), tempMap.getPosition().x(), tempMap.getPosition().y()); rate.sleep(); } diff --git a/grid_map_demos/src/normal_filter_comparison_node.cpp b/grid_map_demos/src/normal_filter_comparison_node.cpp index bb78e1ca8..e5970a113 100644 --- a/grid_map_demos/src/normal_filter_comparison_node.cpp +++ b/grid_map_demos/src/normal_filter_comparison_node.cpp @@ -4,17 +4,24 @@ * @brief Node for comparing different normal filters performances. */ -#include -#include -#include -#include -#include +#include +#include #include #include +#include +#include + +#include +#include +#include +#include +#include + // Function headers -namespace grid_map { +namespace grid_map +{ /*! * Function to calculate the normal vector computation error.Error is defined as sum over the map of absolute cosines * between analytical and computed normals. The cosines are calculated using the dot product between two unitary vectors. @@ -25,8 +32,9 @@ namespace grid_map { * @param directionalErrorAreaSum: Average of the summed error over the map for area method. * @param directionalErrorRasterSum: Average of the summed error over the map for raster method. */ -void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridMapSize, double& directionalErrorAreaSum, - double& directionalErrorRasterSum); +void normalsErrorCalculation( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, double & directionalErrorAreaSum, + double & directionalErrorRasterSum); /*! * Function to add noise to the elevation map. Noise added is white noise from a uniform distribution [-1:1] multiplied by @@ -36,7 +44,9 @@ void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridM * @param gridMapSize: Dimensions of grid map, passed as parameter to not being calculated every time. * @param noise_on_map: Amount of noise wanted in meters, can be set as an argument in the roslaunch phase. */ -void mapAddNoise(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double& noise_on_map); +void mapAddNoise( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, + const double & noise_on_map); /*! * Function to add outliers to the elevation map. Outliers are point where the elevation value is set to infinity. @@ -46,7 +56,9 @@ void mapAddNoise(grid_map::GridMap& map, const grid_map::Size& gridMapSize, cons * @param gridMapSize: Dimensions of grid map, passed as parameter to not being calculated every time. * @param outlierPercentage: Amount of outliers wanted percentage, can be set as an argument in the roslaunch phase. */ -void mapAddOutliers(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double outlierPercentage); +void mapAddOutliers( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, + const double outlierPercentage); /*! * Function to add outliers to the elevation map. Outliers are point where the elevation value is set to infinity. @@ -56,36 +68,48 @@ void mapAddOutliers(grid_map::GridMap& map, const grid_map::Size& gridMapSize, c * @param gridMapSize: Dimensions of grid map, passed as parameter to not being calculated every time. * @param filterRadius: Radius of the wanted shifting average filter. */ -void mapAverageFiltering(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double filterRadius); +void mapAverageFiltering( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, + const double filterRadius); } // namespace grid_map -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ // Initialize node and publisher. - ros::init(argc, argv, "normal_filter_comparison_demo"); - ros::NodeHandle nh("~"); - ros::Publisher publisher = nh.advertise("grid_map", 1, true); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = std::make_shared("normal_filter_comparison_demo"); + + auto publisher = node->create_publisher( + "grid_map", rclcpp::QoS(1).transient_local()); // Start time. - double begin = ros::Time::now().toSec(); + double begin = node->now().seconds(); // Load filter chain, defined in grid_map_demos/config/normal_filter_comparison.yaml. filters::FilterChain filterChain_("grid_map::GridMap"); + + node->declare_parameter("filter_chain_parameter_name", "filters"); + node->declare_parameter("noise_on_map", 0.015); + node->declare_parameter("outliers_percentage", 0.0); + std::string filterChainParametersName_; - nh.param("filter_chain_parameter_name", filterChainParametersName_, std::string("grid_map_filters")); + node->get_parameter("filter_chain_parameter_name", filterChainParametersName_); // Read noise amount, in meters, from parameters server. double noise_on_map; - nh.param("noise_on_map", noise_on_map, 0.015); - ROS_INFO("noise_on_map = %f", noise_on_map); + node->get_parameter("noise_on_map", noise_on_map); + RCLCPP_INFO(node->get_logger(), "noise_on_map = %f", noise_on_map); double outliers_percentage; - ; - nh.param("outliers_percentage", outliers_percentage, 0.0); - ROS_INFO("outliers_percentage = %f", outliers_percentage); + node->get_parameter("outliers_percentage", outliers_percentage); + RCLCPP_INFO(node->get_logger(), "outliers_percentage = %f", outliers_percentage); // Configuration of chain filter. - if (!filterChain_.configure(filterChainParametersName_, nh)) { - ROS_ERROR("Could not configure the filter chain!"); + if (!filterChain_.configure( + filterChainParametersName_, node->get_node_logging_interface(), + node->get_node_parameters_interface())) + { + RCLCPP_ERROR(node->get_logger(), "Could not configure the filter chain!"); } // Parameters for grid map dimensions. @@ -94,13 +118,20 @@ int main(int argc, char** argv) { const double mapResolution = 0.02; // Grid map object creation. - grid_map::GridMap map({"elevation", "normal_analytic_x", "normal_analytic_y", "normal_analytic_z"}); + grid_map::GridMap map({"elevation", "normal_analytic_x", "normal_analytic_y", + "normal_analytic_z"}); map.setFrameId("map"); - map.setGeometry(grid_map::Length(mapLength, mapWidth), mapResolution, grid_map::Position(0.0, 0.0)); + map.setGeometry( + grid_map::Length(mapLength, mapWidth), mapResolution, + grid_map::Position(0.0, 0.0)); const grid_map::Size gridMapSize = map.getSize(); - ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.", - map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1), map.getPosition().x(), map.getPosition().y(), - map.getFrameId().c_str()); + RCLCPP_INFO( + node->get_logger(), + "Created map with size %f x %f m (%i x %i cells).\n" + " The center of the map is located at (%f, %f) in the %s frame.", + map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1), + map.getPosition().x(), map.getPosition().y(), + map.getFrameId().c_str()); // Initialize variables for normal quality comparison. double directionalErrorAreaSum = 0; @@ -112,28 +143,32 @@ int main(int argc, char** argv) { const double surfaceBias = -0.04; const double wavePeriod = 5.0; - // Work with grid map in a loop. Grid map and analytic normals are continuously generated using exact functions. - ros::Rate rate(10.0); - while (nh.ok()) { - ros::Time time = ros::Time::now(); + // Work with grid map in a loop. + // Grid map and analytic normals are continuously generated using exact functions. + rclcpp::Rate rate(10.0); + while (rclcpp::ok()) { + rclcpp::Time time = node->now(); // Calculate wave shaped elevation and analytic surface normal. for (grid_map::GridMapIterator it(map); !it.isPastEnd(); ++it) { grid_map::Position position; map.getPosition(*it, position); map.at("elevation", *it) = - surfaceBias + surfaceSlope * std::sin(surfaceSpeed * time.toSec() + wavePeriod * position.y()) * position.x(); + surfaceBias + surfaceSlope * std::sin( + surfaceSpeed * time.seconds() + wavePeriod * position.y()) * position.x(); // Analytic normals computation. - grid_map::Vector3 normalAnalytic(-surfaceSlope * std::sin(surfaceSpeed * time.toSec() + wavePeriod * position.y()), - -position.x() * std::cos(surfaceSpeed * time.toSec() + wavePeriod * position.y()), 1.0); + grid_map::Vector3 normalAnalytic(-surfaceSlope * std::sin( + surfaceSpeed * time.seconds() + wavePeriod * position.y()), + -position.x() * std::cos(surfaceSpeed * time.seconds() + wavePeriod * position.y()), 1.0); normalAnalytic.normalize(); map.at("normal_analytic_x", *it) = normalAnalytic.x(); map.at("normal_analytic_y", *it) = normalAnalytic.y(); map.at("normal_analytic_z", *it) = normalAnalytic.z(); } - // elevation_filtered layer is used for visualization, initialize it here and if there is noise and filtering it will be updated. + // elevation_filtered layer is used for visualization, + // initialize it here and if there is noise and filtering it will be updated. map.add("elevation_filtered", map.get("elevation")); // Perturb and then filter map only if noise != 0. @@ -149,20 +184,25 @@ int main(int argc, char** argv) { // Computation of normals using filterChain_.update function. if (!filterChain_.update(map, map)) { - ROS_ERROR("Could not update the grid map filter chain!"); + RCLCPP_ERROR(node->get_logger(), "Could not update the grid map filter chain!"); } // Normals error computation - grid_map::normalsErrorCalculation(map, gridMapSize, directionalErrorAreaSum, directionalErrorRasterSum); - ROS_INFO_THROTTLE(2.0, "directionalErrorArea = %f", directionalErrorAreaSum); - ROS_INFO_THROTTLE(2.0, "directionalErrorRaster = %f", directionalErrorRasterSum); + grid_map::normalsErrorCalculation( + map, gridMapSize, directionalErrorAreaSum, + directionalErrorRasterSum); + rclcpp::Clock clock; + RCLCPP_INFO_THROTTLE( + node->get_logger(), clock, 2000, "directionalErrorArea = %f", directionalErrorAreaSum); + RCLCPP_INFO_THROTTLE( + node->get_logger(), clock, 2000, "directionalErrorRaster = %f", directionalErrorRasterSum); // Publish grid map. - map.setTimestamp(time.toNSec()); - grid_map_msgs::GridMap message; - grid_map::GridMapRosConverter::toMessage(map, message); - publisher.publish(message); - double end = ros::Time::now().toSec(); + map.setTimestamp(time.nanoseconds()); + std::unique_ptr message; + message = grid_map::GridMapRosConverter::toMessage(map); + publisher->publish(std::move(message)); + double end = node->now().seconds(); // Limit simulation length to 1 minute. if ((end - begin) > 60) { @@ -173,9 +213,12 @@ int main(int argc, char** argv) { return 0; } -namespace grid_map { -void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridMapSize, double& directionalErrorAreaSum, - double& directionalErrorRasterSum) { +namespace grid_map +{ +void normalsErrorCalculation( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, double & directionalErrorAreaSum, + double & directionalErrorRasterSum) +{ // If layers saved as matrices accessing values is faster. const grid_map::Matrix mapNormalAnalyticX = map["normal_analytic_x"]; const grid_map::Matrix mapNormalAnalyticY = map["normal_analytic_y"]; @@ -199,7 +242,9 @@ void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridM // Raster normals not defined for outermost layer of cells. const grid_map::Index submapStartIndex(1, 1); const grid_map::Index submapBufferSize(gridMapSize(0) - 2, gridMapSize(1) - 2); - for (grid_map::SubmapIterator iterator(map, submapStartIndex, submapBufferSize); !iterator.isPastEnd(); ++iterator) { + for (grid_map::SubmapIterator iterator(map, submapStartIndex, submapBufferSize); + !iterator.isPastEnd(); ++iterator) + { const grid_map::Index index(*iterator); normalVectorAnalytic(0) = mapNormalAnalyticX(index(0), index(1)); @@ -214,13 +259,19 @@ void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridM normalVectorRaster(1) = mapNormalRasterY(index(0), index(1)); normalVectorRaster(2) = mapNormalRasterZ(index(0), index(1)); - // Error(alpha) = 1.0 - abs(cos(alpha)), where alpha = angle(normalVectorAnalytic, normalVectorArea) + // Error(alpha) = 1.0 - abs(cos(alpha)), + // where alpha = angle(normalVectorAnalytic, normalVectorArea) // Error of perfect normals will be 0.0. - map.at("directionalErrorArea", *iterator) = 1.0 - std::abs(normalVectorAnalytic.dot(normalVectorArea)); - map.at("directionalErrorRaster", *iterator) = 1.0 - std::abs(normalVectorAnalytic.dot(normalVectorRaster)); + map.at( + "directionalErrorArea", + *iterator) = 1.0 - std::abs(normalVectorAnalytic.dot(normalVectorArea)); + map.at( + "directionalErrorRaster", + *iterator) = 1.0 - std::abs(normalVectorAnalytic.dot(normalVectorRaster)); } - // Directional error defined as sum of absolute cosines of normal calculated with different methods + // Directional error defined as sum of + // absolute cosines of normal calculated with different methods const double directionalErrorArea = map["directionalErrorArea"].array().sum(); const double directionalErrorRaster = map["directionalErrorRaster"].array().sum(); @@ -229,13 +280,19 @@ void normalsErrorCalculation(grid_map::GridMap& map, const grid_map::Size& gridM directionalErrorRasterSum = (directionalErrorRasterSum * 19.0 + directionalErrorRaster) / 20.0; } -void mapAddNoise(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double& noise_on_map) { +void mapAddNoise( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, + const double & noise_on_map) +{ // Add noise (using Eigen operators). map.add("noise", noise_on_map * Matrix::Random(gridMapSize(0), gridMapSize(1))); map.add("elevation_noisy", map.get("elevation") + map["noise"]); } -void mapAddOutliers(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double outlierPercentage) { +void mapAddOutliers( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, + const double outlierPercentage) +{ // Adding outliers at infinite height (accessing cell by position). const double numberInfPoints = outlierPercentage * gridMapSize(0) * gridMapSize(1); @@ -247,7 +304,10 @@ void mapAddOutliers(grid_map::GridMap& map, const grid_map::Size& gridMapSize, c } } -void mapAverageFiltering(grid_map::GridMap& map, const grid_map::Size& gridMapSize, const double filterRadius) { +void mapAverageFiltering( + grid_map::GridMap & map, const grid_map::Size & gridMapSize, + const double filterRadius) +{ grid_map::Index startIndex(0, 0); grid_map::SubmapIterator it(map, startIndex, gridMapSize); // Iterate over whole map. @@ -258,7 +318,9 @@ void mapAverageFiltering(grid_map::GridMap& map, const grid_map::Size& gridMapSi double sumOfWeights = 0.0; // Compute weighted mean. - for (grid_map::CircleIterator circleIt(map, currentPosition, filterRadius); !circleIt.isPastEnd(); ++circleIt) { + for (grid_map::CircleIterator circleIt(map, currentPosition, filterRadius); + !circleIt.isPastEnd(); ++circleIt) + { if (!map.isValid(*circleIt, "elevation_noisy")) { continue; } @@ -271,11 +333,11 @@ void mapAverageFiltering(grid_map::GridMap& map, const grid_map::Size& gridMapSi mean += weight * map.at("elevation_noisy", *circleIt); sumOfWeights += weight; } - if (sumOfWeights!=0) { + if (sumOfWeights != 0) { map.at("elevation_filtered", *it) = mean / sumOfWeights; } else { map.at("elevation_filtered", *it) = std::numeric_limits::infinity(); } } } -} // namespace grid_map \ No newline at end of file +} // namespace grid_map diff --git a/grid_map_demos/src/octomap_to_gridmap_demo_node.cpp b/grid_map_demos/src/octomap_to_gridmap_demo_node.cpp index c976320f8..594eedd07 100644 --- a/grid_map_demos/src/octomap_to_gridmap_demo_node.cpp +++ b/grid_map_demos/src/octomap_to_gridmap_demo_node.cpp @@ -6,22 +6,23 @@ * Institute: University of Zürich, Robotics and Perception Group */ -#include +#include +#include #include "grid_map_demos/OctomapToGridmapDemo.hpp" -int main(int argc, char** argv) +int main(int argc, char ** argv) { // Initialize node and publisher. - ros::init(argc, argv, "octomap_to_gridmap_demo"); - ros::NodeHandle nh("~"); - grid_map_demos::OctomapToGridmapDemo octomapToGridmapDemo(nh); - ros::Duration(2.0).sleep(); + rclcpp::init(argc, argv); - ros::Rate r(0.1); // 1 hz - while (ros::ok()) - { - octomapToGridmapDemo.convertAndPublishMap(); - ros::spinOnce(); + auto octomapToGridmapDemo = std::make_shared(); + + rclcpp::sleep_for(std::chrono::seconds(2)); + + rclcpp::Rate r(0.1); // 1 hz + while (rclcpp::ok()) { + octomapToGridmapDemo->convertAndPublishMap(); + rclcpp::spin_some(octomapToGridmapDemo); r.sleep(); } return 0; diff --git a/grid_map_demos/src/opencv_demo_node.cpp b/grid_map_demos/src/opencv_demo_node.cpp index bfdc4a58c..c5b42c118 100644 --- a/grid_map_demos/src/opencv_demo_node.cpp +++ b/grid_map_demos/src/opencv_demo_node.cpp @@ -1,43 +1,46 @@ -#include +#include #include #include -#include +#include #include +#include +#include -using namespace grid_map; -using namespace ros; - -int main(int argc, char** argv) +int main(int argc, char ** argv) { // Initialize node and publisher. - init(argc, argv, "opencv_demo"); - NodeHandle nodeHandle("~"); - Publisher publisher = nodeHandle.advertise("grid_map", 1, true); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = std::make_shared("opencv_demo"); + auto publisher = node->create_publisher( + "grid_map", rclcpp::QoS( + 1).transient_local()); const bool useTransparency = false; // Create grid map. - GridMap map({"original", "elevation"}); + grid_map::GridMap map({"original", "elevation"}); map.setFrameId("map"); - map.setGeometry(Length(1.2, 2.0), 0.01); - ROS_INFO("Created map with size %f x %f m (%i x %i cells).", + map.setGeometry(grid_map::Length(1.2, 2.0), 0.01); + RCLCPP_INFO( + node->get_logger(), + "Created map with size %f x %f m (%i x %i cells).", map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1)); // Add data. - if (!useTransparency) map["original"].setZero(); + if (!useTransparency) {map["original"].setZero();} grid_map::Polygon polygon; polygon.setFrameId(map.getFrameId()); - polygon.addVertex(Position( 0.480, 0.000)); - polygon.addVertex(Position( 0.164, 0.155)); - polygon.addVertex(Position( 0.116, 0.500)); - polygon.addVertex(Position(-0.133, 0.250)); - polygon.addVertex(Position(-0.480, 0.399)); - polygon.addVertex(Position(-0.316, 0.000)); - polygon.addVertex(Position(-0.480, -0.399)); - polygon.addVertex(Position(-0.133, -0.250)); - polygon.addVertex(Position( 0.116, -0.500)); - polygon.addVertex(Position( 0.164, -0.155)); - polygon.addVertex(Position( 0.480, 0.000)); + polygon.addVertex(grid_map::Position(0.480, 0.000)); + polygon.addVertex(grid_map::Position(0.164, 0.155)); + polygon.addVertex(grid_map::Position(0.116, 0.500)); + polygon.addVertex(grid_map::Position(-0.133, 0.250)); + polygon.addVertex(grid_map::Position(-0.480, 0.399)); + polygon.addVertex(grid_map::Position(-0.316, 0.000)); + polygon.addVertex(grid_map::Position(-0.480, -0.399)); + polygon.addVertex(grid_map::Position(-0.133, -0.250)); + polygon.addVertex(grid_map::Position(0.116, -0.500)); + polygon.addVertex(grid_map::Position(0.164, -0.155)); + polygon.addVertex(grid_map::Position(0.480, 0.000)); for (grid_map::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); ++iterator) { map.at("original", *iterator) = 0.3; @@ -48,22 +51,25 @@ int main(int argc, char** argv) if (useTransparency) { // Note: The template parameters have to be set based on your encoding // of the image. For 8-bit images use `unsigned char`. - GridMapCvConverter::toImage(map, "original", CV_16UC4, 0.0, 0.3, originalImage); + grid_map::GridMapCvConverter::toImage( + map, "original", CV_16UC4, 0.0, 0.3, + originalImage); } else { - GridMapCvConverter::toImage(map, "original", CV_16UC1, 0.0, 0.3, originalImage); + grid_map::GridMapCvConverter::toImage( + map, "original", CV_16UC1, 0.0, 0.3, + originalImage); } // Create OpenCV window. cv::namedWindow("OpenCV Demo"); // Work with copy of image in a loop. - while (nodeHandle.ok()) { - + while (rclcpp::ok()) { // Initialize. - ros::Time time = ros::Time::now(); - map.setTimestamp(time.toNSec()); + rclcpp::Time time = node->now(); + map.setTimestamp(time.nanoseconds()); cv::Mat modifiedImage; - int blurRadius = 200 - abs((int)(200.0 * sin(time.toSec()))); + int blurRadius = 200 - abs(static_cast(200.0 * sin(time.seconds()))); blurRadius = blurRadius - (blurRadius % 2) + 1; // Apply Gaussian blur. @@ -75,16 +81,20 @@ int main(int argc, char** argv) // Convert resulting image to a grid map. if (useTransparency) { - GridMapCvConverter::addLayerFromImage(modifiedImage, "elevation", map, 0.0, 0.3, 0.3); + grid_map::GridMapCvConverter::addLayerFromImage( + modifiedImage, "elevation", map, 0.0, + 0.3, 0.3); } else { - GridMapCvConverter::addLayerFromImage(modifiedImage, "elevation", map, 0.0, 0.3); + grid_map::GridMapCvConverter::addLayerFromImage( + modifiedImage, "elevation", map, 0.0, + 0.3); } // Publish grid map. - grid_map_msgs::GridMap message; - GridMapRosConverter::toMessage(map, message); - publisher.publish(message); - ROS_INFO_STREAM("Published image and grid map with blur radius " << blurRadius << "."); + auto message = grid_map::GridMapRosConverter::toMessage(map); + publisher->publish(std::move(message)); + RCLCPP_INFO_STREAM( + node->get_logger(), "Published image and grid map with blur radius " << blurRadius << "."); } return 0; diff --git a/grid_map_demos/src/resolution_change_demo_node.cpp b/grid_map_demos/src/resolution_change_demo_node.cpp index ea0084ebb..cf1f24af2 100644 --- a/grid_map_demos/src/resolution_change_demo_node.cpp +++ b/grid_map_demos/src/resolution_change_demo_node.cpp @@ -1,63 +1,65 @@ -#include +#include #include #include +#include +#include -using namespace grid_map; -using namespace ros; - -int main(int argc, char** argv) +int main(int argc, char ** argv) { // Initialize node and publisher. - init(argc, argv, "resolution_change_demo"); - NodeHandle nodeHandle("~"); - Publisher publisher = nodeHandle.advertise("grid_map", 1, true); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = std::make_shared("resolution_change_demo"); + auto publisher = node->create_publisher( + "grid_map", rclcpp::QoS( + 1).transient_local()); // Create grid map. - GridMap map({"elevation"}); + grid_map::GridMap map({"elevation"}); map.setFrameId("map"); - map.setGeometry(Length(1.2, 2.0), 0.03); - ROS_INFO("Created map with size %f x %f m (%i x %i cells).", + map.setGeometry(grid_map::Length(1.2, 2.0), 0.03); + RCLCPP_INFO( + node->get_logger(), + "Created map with size %f x %f m (%i x %i cells).", map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1)); // Add data. - map["elevation"].setZero(); // Un/comment this line to try with and without transparency. + map["elevation"].setZero(); // Un/comment this line to try with and without transparency. grid_map::Polygon polygon; polygon.setFrameId(map.getFrameId()); - polygon.addVertex(Position( 0.480, 0.000)); - polygon.addVertex(Position( 0.164, 0.155)); - polygon.addVertex(Position( 0.116, 0.500)); - polygon.addVertex(Position(-0.133, 0.250)); - polygon.addVertex(Position(-0.480, 0.399)); - polygon.addVertex(Position(-0.316, 0.000)); - polygon.addVertex(Position(-0.480, -0.399)); - polygon.addVertex(Position(-0.133, -0.250)); - polygon.addVertex(Position( 0.116, -0.500)); - polygon.addVertex(Position( 0.164, -0.155)); - polygon.addVertex(Position( 0.480, 0.000)); + polygon.addVertex(grid_map::Position(0.480, 0.000)); + polygon.addVertex(grid_map::Position(0.164, 0.155)); + polygon.addVertex(grid_map::Position(0.116, 0.500)); + polygon.addVertex(grid_map::Position(-0.133, 0.250)); + polygon.addVertex(grid_map::Position(-0.480, 0.399)); + polygon.addVertex(grid_map::Position(-0.316, 0.000)); + polygon.addVertex(grid_map::Position(-0.480, -0.399)); + polygon.addVertex(grid_map::Position(-0.133, -0.250)); + polygon.addVertex(grid_map::Position(0.116, -0.500)); + polygon.addVertex(grid_map::Position(0.164, -0.155)); + polygon.addVertex(grid_map::Position(0.480, 0.000)); for (grid_map::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); ++iterator) { map.at("elevation", *iterator) = 0.3; } - Rate rate(10.0); + rclcpp::Rate rate(10.0); // Work in a loop. - while (nodeHandle.ok()) { - + while (rclcpp::ok()) { // Initialize. - ros::Time time = ros::Time::now(); - const double resolution = 0.05 + 0.04 * sin(time.toSec()); + rclcpp::Time time = node->now(); + const double resolution = 0.05 + 0.04 * sin(time.seconds()); // Change resoltion of grid map. - GridMap modifiedMap; - GridMapCvProcessing::changeResolution(map, modifiedMap, resolution); + grid_map::GridMap modifiedMap; + grid_map::GridMapCvProcessing::changeResolution(map, modifiedMap, resolution); // Publish grid map. - grid_map_msgs::GridMap message; - GridMapRosConverter::toMessage(modifiedMap, message); - publisher.publish(message); - ROS_INFO_STREAM("Published grid map with " << resolution << " m/cell resolution."); + auto message = grid_map::GridMapRosConverter::toMessage(modifiedMap); + publisher->publish(std::move(message)); + RCLCPP_INFO_STREAM( + node->get_logger(), "Published grid map with " << resolution << " m/cell resolution."); rate.sleep(); } diff --git a/grid_map_demos/src/simple_demo_node.cpp b/grid_map_demos/src/simple_demo_node.cpp index 253e05760..0fdd0cdc6 100644 --- a/grid_map_demos/src/simple_demo_node.cpp +++ b/grid_map_demos/src/simple_demo_node.cpp @@ -1,43 +1,48 @@ -#include +#include #include -#include +#include #include +#include +#include -using namespace grid_map; - -int main(int argc, char** argv) +int main(int argc, char ** argv) { // Initialize node and publisher. - ros::init(argc, argv, "grid_map_simple_demo"); - ros::NodeHandle nh("~"); - ros::Publisher publisher = nh.advertise("grid_map", 1, true); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = std::make_shared("grid_map_simple_demo"); + auto publisher = node->create_publisher( + "grid_map", rclcpp::QoS(1).transient_local()); // Create grid map. - GridMap map({"elevation"}); + grid_map::GridMap map({"elevation"}); map.setFrameId("map"); - map.setGeometry(Length(1.2, 2.0), 0.03); - ROS_INFO("Created map with size %f x %f m (%i x %i cells).", + map.setGeometry(grid_map::Length(1.2, 2.0), 0.03); + RCLCPP_INFO( + node->get_logger(), + "Created map with size %f x %f m (%i x %i cells).", map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1)); // Work with grid map in a loop. - ros::Rate rate(30.0); - while (nh.ok()) { - + rclcpp::Rate rate(30.0); + rclcpp::Clock clock; + while (rclcpp::ok()) { // Add data to grid map. - ros::Time time = ros::Time::now(); - for (GridMapIterator it(map); !it.isPastEnd(); ++it) { - Position position; + rclcpp::Time time = node->now(); + for (grid_map::GridMapIterator it(map); !it.isPastEnd(); ++it) { + grid_map::Position position; map.getPosition(*it, position); - map.at("elevation", *it) = -0.04 + 0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()) * position.x(); + map.at( + "elevation", + *it) = -0.04 + 0.2 * std::sin(3.0 * time.seconds() + 5.0 * position.y()) * position.x(); } // Publish grid map. - map.setTimestamp(time.toNSec()); - grid_map_msgs::GridMap message; - GridMapRosConverter::toMessage(map, message); - publisher.publish(message); - ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", message.info.header.stamp.toSec()); + map.setTimestamp(time.nanoseconds()); + std::unique_ptr message; + message = grid_map::GridMapRosConverter::toMessage(map); + publisher->publish(std::move(message)); + RCLCPP_INFO_THROTTLE(node->get_logger(), clock, 1000, "Grid map published."); // Wait for next cycle. rate.sleep(); diff --git a/grid_map_demos/src/tutorial_demo_node.cpp b/grid_map_demos/src/tutorial_demo_node.cpp index 343dea3d5..35459f84f 100644 --- a/grid_map_demos/src/tutorial_demo_node.cpp +++ b/grid_map_demos/src/tutorial_demo_node.cpp @@ -1,40 +1,47 @@ -#include +#include #include -#include -#include #include #include +#include +#include +#include +#include -using namespace grid_map; - -int main(int argc, char** argv) +int main(int argc, char ** argv) { // Initialize node and publisher. - ros::init(argc, argv, "grid_map_tutorial_demo"); - ros::NodeHandle nh("~"); - ros::Publisher publisher = nh.advertise("grid_map", 1, true); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = std::make_shared("grid_map_tutorial_demo"); + auto publisher = node->create_publisher( + "grid_map", rclcpp::QoS(1).transient_local()); // Create grid map. - GridMap map({"elevation", "normal_x", "normal_y", "normal_z"}); + grid_map::GridMap map({"elevation", "normal_x", "normal_y", "normal_z"}); map.setFrameId("map"); - map.setGeometry(Length(1.2, 2.0), 0.03, Position(0.0, -0.1)); - ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.", + map.setGeometry(grid_map::Length(1.2, 2.0), 0.03, grid_map::Position(0.0, -0.1)); + RCLCPP_INFO( + node->get_logger(), + "Created map with size %f x %f m (%i x %i cells).\n" + " The center of the map is located at (%f, %f) in the %s frame.", map.getLength().x(), map.getLength().y(), map.getSize()(0), map.getSize()(1), map.getPosition().x(), map.getPosition().y(), map.getFrameId().c_str()); // Work with grid map in a loop. - ros::Rate rate(30.0); - while (nh.ok()) { - ros::Time time = ros::Time::now(); + rclcpp::Rate rate(30.0); + rclcpp::Clock clock; + while (rclcpp::ok()) { + rclcpp::Time time = node->now(); // Add elevation and surface normal (iterating through grid map and adding data). - for (GridMapIterator it(map); !it.isPastEnd(); ++it) { - Position position; + for (grid_map::GridMapIterator it(map); !it.isPastEnd(); ++it) { + grid_map::Position position; map.getPosition(*it, position); - map.at("elevation", *it) = -0.04 + 0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()) * position.x(); - Eigen::Vector3d normal(-0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()), - -position.x() * std::cos(3.0 * time.toSec() + 5.0 * position.y()), 1.0); + map.at( + "elevation", + *it) = -0.04 + 0.2 * std::sin(3.0 * time.seconds() + 5.0 * position.y()) * position.x(); + Eigen::Vector3d normal(-0.2 * std::sin(3.0 * time.seconds() + 5.0 * position.y()), + -position.x() * std::cos(3.0 * time.seconds() + 5.0 * position.y()), 1.0); normal.normalize(); map.at("normal_x", *it) = normal.x(); map.at("normal_y", *it) = normal.y(); @@ -42,38 +49,43 @@ int main(int argc, char** argv) } // Add noise (using Eigen operators). - map.add("noise", 0.015 * Matrix::Random(map.getSize()(0), map.getSize()(1))); + map.add("noise", 0.015 * grid_map::Matrix::Random(map.getSize()(0), map.getSize()(1))); map.add("elevation_noisy", map.get("elevation") + map["noise"]); // Adding outliers (accessing cell by position). for (unsigned int i = 0; i < 500; ++i) { - Position randomPosition = Position::Random(); - if (map.isInside(randomPosition)) + grid_map::Position randomPosition = grid_map::Position::Random(); + if (map.isInside(randomPosition)) { map.atPosition("elevation_noisy", randomPosition) = std::numeric_limits::infinity(); + } } // Filter values for submap (iterators). map.add("elevation_filtered", map.get("elevation_noisy")); - Position topLeftCorner(1.0, 0.4); - boundPositionToRange(topLeftCorner, map.getLength(), map.getPosition()); - Index startIndex; + grid_map::Position topLeftCorner(1.0, 0.4); + grid_map::boundPositionToRange(topLeftCorner, map.getLength(), map.getPosition()); + grid_map::Index startIndex; map.getIndex(topLeftCorner, startIndex); - ROS_INFO_ONCE("Top left corner was limited from (1.0, 0.2) to (%f, %f) and corresponds to index (%i, %i).", + RCLCPP_INFO_ONCE( + node->get_logger(), + "Top left corner was limited from (1.0, 0.2) to (%f, %f) and corresponds to index (%i, %i).", topLeftCorner.x(), topLeftCorner.y(), startIndex(0), startIndex(1)); - Size size = (Length(1.2, 0.8) / map.getResolution()).cast(); - SubmapIterator it(map, startIndex, size); + grid_map::Size size = (grid_map::Length(1.2, 0.8) / map.getResolution()).cast(); + grid_map::SubmapIterator it(map, startIndex, size); for (; !it.isPastEnd(); ++it) { - Position currentPosition; + grid_map::Position currentPosition; map.getPosition(*it, currentPosition); double radius = 0.1; double mean = 0.0; double sumOfWeights = 0.0; // Compute weighted mean. - for (CircleIterator circleIt(map, currentPosition, radius); !circleIt.isPastEnd(); ++circleIt) { - if (!map.isValid(*circleIt, "elevation_noisy")) continue; - Position currentPositionInCircle; + for (grid_map::CircleIterator circleIt(map, currentPosition, radius); !circleIt.isPastEnd(); + ++circleIt) + { + if (!map.isValid(*circleIt, "elevation_noisy")) {continue;} + grid_map::Position currentPositionInCircle; map.getPosition(*circleIt, currentPositionInCircle); // Computed weighted mean based on Euclidian distance. @@ -88,19 +100,17 @@ int main(int argc, char** argv) // Show absolute difference and compute mean squared error. map.add("error", (map.get("elevation_filtered") - map.get("elevation")).cwiseAbs()); - unsigned int nCells = map.getSize().prod(); - // cppcheck-suppress unreadVariable - double rootMeanSquaredError = sqrt((map["error"].array().pow(2).sum()) / nCells); // Publish grid map. - map.setTimestamp(time.toNSec()); - grid_map_msgs::GridMap message; - GridMapRosConverter::toMessage(map, message); - publisher.publish(message); - ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", message.info.header.stamp.toSec()); + map.setTimestamp(time.nanoseconds()); + std::unique_ptr message; + message = grid_map::GridMapRosConverter::toMessage(map); + publisher->publish(std::move(message)); + RCLCPP_INFO_THROTTLE(node->get_logger(), clock, 1000, "Grid map published."); + rclcpp::spin_some(node->get_node_base_interface()); rate.sleep(); } return 0; -} \ No newline at end of file +} diff --git a/grid_map_filters/CHANGELOG.rst b/grid_map_filters/CHANGELOG.rst index e30da8999..b0f1fdab2 100644 --- a/grid_map_filters/CHANGELOG.rst +++ b/grid_map_filters/CHANGELOG.rst @@ -2,6 +2,37 @@ Changelog for package grid_map_filters ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#458 `_ from ANYbotics/ci-temp-skip-octomap-server + build: treat several build issues on rolling +* apply uncrustify except for EigenLab.hpp +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Merge pull request `#419 `_ from Ryanf55/stop-using-CMAKE_COMPILER_IS_GNUCXX + Stop using deprecated CMAKE_COMPILER_IS_GNUCXX +* Stop using deprecated CMAKE_COMPILER_IS_GNUCXX + * Switch to CMAKE_CXX_COMPILER_ID + * https://cmake.org/cmake/help/latest/variable/CMAKE_LANG_COMPILER_ID.html +* Contributors: Ryan, Ryan Friedman, wep21 + +2.1.0 (2022-11-08) +------------------ + +2.0.0 (2022-09-13) +------------------ +* fix: mark Eigen library as SYSTEM +* fix: build error on jammy +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ * Update MinInRadiusFilter.cpp diff --git a/grid_map_filters/CMakeLists.txt b/grid_map_filters/CMakeLists.txt index 16da8c9d6..711302f25 100644 --- a/grid_map_filters/CMakeLists.txt +++ b/grid_map_filters/CMakeLists.txt @@ -1,59 +1,56 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(grid_map_filters) -# Better with parallelized algorithms. -#set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS} -ffast-math") +## Find ament_cmake macros and libraries +find_package(ament_cmake REQUIRED) +find_package(filters REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(pluginlib REQUIRED) -# Better with serial algorithms. -set(CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ffast-math") +## System dependencies are found with CMake's conventions +find_package(PkgConfig REQUIRED) + +pkg_check_modules(TBB "tbb") +if(TBB_FOUND) +add_definitions( + ${TBB_CFLAGS} + ${TBB_CFLAGS_OTHER} +) +else() +message([FATAL_ERROR] "tbb module not found") +endif() -# Other possible options. -#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native -mtune=native -ffast-math") -#set(TARGET_ARCHITECTURE "kaby-lake") +grid_map_package() -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS +set(dependencies + TBB + filters grid_map_core - grid_map_ros grid_map_msgs - filters + grid_map_ros + pluginlib ) -## System dependencies are found with CMake's conventions -find_package(PkgConfig REQUIRED) - -pkg_check_modules(TBB "tbb") -if (TBB_FOUND) - add_definitions( - ${TBB_CFLAGS} - ${TBB_CFLAGS_OTHER} - ) -else() - message([FATAL_ERROR] "tbb module not found") -endif () - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS - include - ${TBB_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - grid_map_ros - grid_map_core - grid_map_msgs - filters - DEPENDS +set(filter_libs + threshold_filter + min_in_radius_filter + mean_in_radius_filter + normal_vectors_filter + curvature_filter + normal_color_map_filter + light_intensity_filter + math_expression_filter + sliding_window_math_expression_filter + duplication_filter + deletion_filter + color_fill_filter + color_map_filter + color_blending_filter + set_basic_layers_filter + buffer_normalizer_filter ) ########### @@ -64,34 +61,37 @@ catkin_package( include_directories( include SYSTEM - ${catkin_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} ) -## Declare a cpp library -add_library(${PROJECT_NAME} - src/ThresholdFilter.cpp - src/MinInRadiusFilter.cpp - src/MeanInRadiusFilter.cpp - src/NormalVectorsFilter.cpp - src/CurvatureFilter.cpp - src/NormalColorMapFilter.cpp - src/LightIntensityFilter.cpp - src/MathExpressionFilter.cpp - src/SlidingWindowMathExpressionFilter.cpp - src/DuplicationFilter.cpp - src/DeletionFilter.cpp - src/ColorFillFilter.cpp - src/ColorMapFilter.cpp - src/ColorBlendingFilter.cpp - src/SetBasicLayersFilter.cpp - src/BufferNormalizerFilter.cpp -) +## Declare cpp libraries +add_library(threshold_filter SHARED src/ThresholdFilter.cpp) +add_library(min_in_radius_filter SHARED src/MinInRadiusFilter.cpp) +add_library(mean_in_radius_filter SHARED src/MeanInRadiusFilter.cpp) +add_library(normal_vectors_filter SHARED src/NormalVectorsFilter.cpp) +add_library(curvature_filter SHARED src/CurvatureFilter.cpp) +add_library(normal_color_map_filter SHARED src/NormalColorMapFilter.cpp) +add_library(light_intensity_filter SHARED src/LightIntensityFilter.cpp) +add_library(math_expression_filter SHARED src/MathExpressionFilter.cpp) +add_library(sliding_window_math_expression_filter SHARED src/SlidingWindowMathExpressionFilter.cpp) +add_library(duplication_filter SHARED src/DuplicationFilter.cpp) +add_library(deletion_filter SHARED src/DeletionFilter.cpp) +add_library(color_fill_filter SHARED src/ColorFillFilter.cpp) +add_library(color_map_filter SHARED src/ColorMapFilter.cpp) +add_library(color_blending_filter SHARED src/ColorBlendingFilter.cpp) +add_library(set_basic_layers_filter SHARED src/SetBasicLayersFilter.cpp) +add_library(buffer_normalizer_filter SHARED src/BufferNormalizerFilter.cpp) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${TBB_LIBRARIES} -) +foreach(lib_name ${filter_libs}) + ament_target_dependencies(${lib_name} SYSTEM + ${dependencies} + ) + + if(CMAKE_CXX_COMPILER_ID MATCHES "GNU") + target_compile_options(${lib_name} PRIVATE "SHELL:--param ggc-min-expand=1") + target_compile_options(${lib_name} PRIVATE "SHELL:--param ggc-min-heapsize=32768") + endif() +endforeach() ############# ## Install ## @@ -99,21 +99,72 @@ target_link_libraries(${PROJECT_NAME} # Mark executables and/or libraries for installation install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + TARGETS ${filter_libs} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) # Mark cpp header files for installation install( DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} FILES_MATCHING PATTERN "*.hpp" ) # Mark other files for installation install( FILES filter_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} +) + +############# +## Testing ## +############# + +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ament_cmake_uncrustify + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright -build/include_order + ) + + # run uncrustify except for EigenLab.hpp + find_package(ament_cmake_uncrustify) + set( + _linter_excludes + include/EigenLab/EigenLab.hpp + ) + ament_uncrustify( + EXCLUDE ${_linter_excludes} + LANGUAGE c++ + ) + endif() + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories( + include + ${TBB_INCLUDE_DIRS} ) +ament_export_dependencies(${dependencies}) +ament_export_libraries(${filter_libs}) +pluginlib_export_plugin_description_file(filters filter_plugins.xml) +ament_package() diff --git a/grid_map_filters/filter_plugins.xml b/grid_map_filters/filter_plugins.xml index 3e0ba2dcd..f78abc36e 100644 --- a/grid_map_filters/filter_plugins.xml +++ b/grid_map_filters/filter_plugins.xml @@ -1,81 +1,127 @@ - - + + + Set values below/above a threshold to a specified value. - + + + + Compute for each cell of a layer the minimal value inside a radius. - + + + + Compute for each cell of a layer the mean value inside a radius. - + + + + Compute the normal vectors of a layer in a map. - + + + + Compute the curvature (second derivative) of a layer in the map. - + + + + Compute a new color layer based on normal vectors layers. - + + + + Compute the diffuse lighting of a surface as new black and white color layer. - + + + + Parse and evaluate a mathematical matrix expression with layers of a grid map. - + + + + Parse and evaluate a mathematical matrix expression within a sliding window on a layer of a grid map. - + + + + Duplicate a layer of a grid map. - + + + + Delete layers from a grid map. - + + + + Creates a new color layer. - + + + + Creates a new color layer with the color mapped between min. and max. value. - + + + + Blend two color layers. - + + + + Set specified layers of a grid map as basic layers. - + + + + Normalizes the buffer of a map such that it has default (zero) start index. diff --git a/grid_map_filters/include/EigenLab/EigenLab.h b/grid_map_filters/include/EigenLab/EigenLab.h deleted file mode 100644 index 61e3a8127..000000000 --- a/grid_map_filters/include/EigenLab/EigenLab.h +++ /dev/null @@ -1,2358 +0,0 @@ -// --*- Mode: C++; c-basic-offset:4; indent-tabs-mode:t; tab-width:4 -*-- -// EigenLab -// Version: 1.0.0 -// Author: Dr. Marcel Paz Goldschen-Ohm -// Email: marcel.goldschen@gmail.com -// Copyright (c) 2015 by Dr. Marcel Paz Goldschen-Ohm. -// Licence: MIT -//---------------------------------------- - -#ifndef EigenLab_H -#define EigenLab_H - -#include -#include -#include -#include -#include -#include -#include -#include - -// Define both DEBUG and EIGENLAB_DEBUG for step-by-step equation parsing printouts. -#ifndef DEBUG -//# define DEBUG -#endif - -#ifndef EIGENLAB_DEBUG -//# define EIGENLAB_DEBUG -#endif - -#ifdef DEBUG -# include -#endif - -namespace EigenLab -{ - //---------------------------------------- - // A wrapper for a matrix whose data is either stored locally or shared. - // - // Template typename Derived can be any dynamically sized matrix type supported by Eigen. - // - // !!! matrix() promises to ALWAYS return a map to the matrix data whether it's - // stored locally or externally in some shared memory. - // - // !!! local() provides direct access to the local data, but this data is - // ONLY valid when isLocal() is true. In most cases, you're best off - // accessing the matrix data via matrix() instead. - //---------------------------------------- - template - class Value - { - private: - // Local matrix data. - Derived mLocal; - - // Map to shared matrix data (map points to mLocal if the data is local). - // !!! This map promises to ALWAYS point to the matrix data whether it's - // stored locally in mLocal or externally in some shared memory. - Eigen::Map mShared; - - // Flag indicating whether the local data is being used. - bool mIsLocal; - - public: - // Access mapped data (whether its local or shared). - // !!! matrix() promises to ALWAYS return a map to the matrix data whether it's - // stored locally in mLocal or externally in some shared memory. - inline Eigen::Map & matrix() { return mShared; } - inline const Eigen::Map & matrix() const { return mShared; } - - // Access local data. - // !!! WARNING! This data is ONLY valid if isLocal() is true. - // !!! WARNING! If you change the local data via this method, you MUST call mapLocal() immediately afterwards. - // In most cases, you're best off accessing the matrix data via matrix() instead. - inline Derived & local() { return mLocal; } - inline const Derived & local() const { return mLocal; } - - // Is mapped data local? - inline bool isLocal() const { return mIsLocal; } - - // Set mapped data to point to local data. - inline void mapLocal() { new (& mShared) Eigen::Map(mLocal.data(), mLocal.rows(), mLocal.cols()); mIsLocal = true; } - - // Copy shared data to local data (if needed). - inline void copySharedToLocal() { if(!isLocal()) { mLocal = mShared; mapLocal(); } } - - // Set local data. - Value() : mLocal(1, 1), mShared(mLocal.data(), mLocal.rows(), mLocal.cols()), mIsLocal(true) {} - Value(const typename Derived::Scalar s) : mLocal(Derived::Constant(1, 1, s)), mShared(mLocal.data(), mLocal.rows(), mLocal.cols()), mIsLocal(true) {} - Value(const Derived & mat) : mLocal(mat), mShared(mLocal.data(), mLocal.rows(), mLocal.cols()), mIsLocal(true) {} - inline void setLocal(const typename Derived::Scalar s) { mLocal = Derived::Constant(1, 1, s); mapLocal(); } - inline void setLocal(const Eigen::MatrixBase & mat) { mLocal = mat; mapLocal(); } - inline void setLocal(const Value & val) { mLocal = val.matrix(); mapLocal(); } - inline void setLocal(const typename Derived::Scalar * data, size_t rows = 1, size_t cols = 1) { setShared(data, rows, cols); copySharedToLocal(); } - inline Value & operator = (const typename Derived::Scalar s) { setLocal(s); return (* this); } - inline Value & operator = (const Derived & mat) { setLocal(mat); return (* this); } - - // Set shared data. - Value(const typename Derived::Scalar * data, size_t rows = 1, size_t cols = 1) : mShared(const_cast(data), rows, cols), mIsLocal(false) {} - inline void setShared(const typename Derived::Scalar * data, size_t rows = 1, size_t cols = 1) { new (& mShared) Eigen::Map(const_cast(data), rows, cols); mIsLocal = false; } - inline void setShared(const Derived & mat) { setShared(mat.data(), mat.rows(), mat.cols()); } - inline void setShared(const Value & val) { setShared(val.matrix().data(), val.matrix().rows(), val.matrix().cols()); } - - // Set to local or shared data dependent on whether val maps its own local data or some other shared data. - Value(const Value & val) : mLocal(1, 1), mShared(mLocal.data(), mLocal.rows(), mLocal.cols()) { (* this) = val; } - inline Value & operator = (const Value & val) { if(val.isLocal()) { setLocal(val); } else { setShared(val); } return (* this); } - }; - typedef Value ValueXd; - typedef Value ValueXf; - typedef Value ValueXi; - - // check if a class has a comparison operator (ie. std::complex does not) - template - struct has_operator_lt_impl - { - template - static auto test(U*) -> decltype(std::declval() < std::declval()); - template - static auto test(...) -> std::false_type; - using type = typename std::is_same(0))>::type; - }; - template - struct has_operator_lt : has_operator_lt_impl::type {}; - - //---------------------------------------- - // Equation parser. - // - // Template typename Derived can be any dynamically sized matrix type supported by Eigen. - //---------------------------------------- - template - class Parser - { - public: - // A map to hold named values. - typedef std::map > ValueMap; - - private: - // Variables are stored in a map of named values. - ValueMap mVariables; - - // Operator symbols and function names used by the parser. - std::string mOperators1, mOperators2; - std::vector mFunctions; - - // Expressions are parsed by first splitting them into chunks. - struct Chunk { - std::string field; - int type; - Value value; - int row0, col0, rows, cols; - Chunk(const std::string & str = "", int t = -1, const Value & val = Value()) : field(str), type(t), value(val), row0(-1), col0(-1), rows(-1), cols(-1) {} - }; - enum ChunkType { VALUE = 0, VARIABLE, OPERATOR, FUNCTION }; - typedef std::vector ChunkArray; - typedef typename Derived::Index Index; - bool mCacheChunkedExpressions; - std::map mCachedChunkedExpressions; - - public: - // Access to named variables. - // !!! WARNING! var(name) will create the variable name if it does not already exist. - inline ValueMap & vars() { return mVariables; } - inline Value & var(const std::string & name) { return mVariables[name]; } - - // Check if a variable exists. - inline bool hasVar(const std::string & name) { return isVariable(name); } - - // Delete a variable. - inline void clearVar(const std::string & name) { typename ValueMap::iterator it = mVariables.find(name); if(it != mVariables.end()) mVariables.erase(it); } - - // Expression chunk caching. - inline bool cacheExpressions() const { return mCacheChunkedExpressions; } - inline void setCacheExpressions(bool b) { mCacheChunkedExpressions = b; } - inline void clearCachedExpressions() { mCachedChunkedExpressions.clear(); } - - Parser(); - ~Parser() { clearCachedExpressions(); } - - // Evaluate an expression and return the result in a value wrapper. - Value eval(const std::string & expression); - - private: - void splitEquationIntoChunks(const std::string & expression, ChunkArray & chunks, std::string & code); - std::string::const_iterator findClosingBracket(const std::string & str, const std::string::const_iterator openingBracket, const char closingBracket) const; - std::vector splitArguments(const std::string & str, const char delimeter) const; - void evalIndexRange(const std::string & str, int * first, int * last, int numIndices); - void evalMatrixExpression(const std::string & str, Value & mat); - void evalFunction(const std::string & name, std::vector & args, Value & result); - bool evalFunction_1_lt(const std::string & name, Value & arg, Value & result, std::false_type); - bool evalFunction_1_lt(const std::string & name, Value & arg, Value & result, std::true_type); - bool evalFunction_2_lt(const std::string & name, Value & arg0, Value & arg1, Value & result, std::false_type); - bool evalFunction_2_lt(const std::string & name, Value & arg0, Value & arg1, Value & result, std::true_type); - - void evalNumericRange(const std::string & str, Value & mat); - inline bool isVariable(const std::string & name) const { return mVariables.count(name) > 0; } - inline bool isOperator(const char c) const { return (std::find(mOperators1.begin(), mOperators1.end(), c) != mOperators1.end()); } - bool isOperator(const std::string & str) const; - inline bool isFunction(const std::string & str) const { return (std::find(mFunctions.begin(), mFunctions.end(), str) != mFunctions.end()); } - void evalIndices(ChunkArray & chunks); - void evalNegations(ChunkArray & chunks); - void evalPowers(ChunkArray & chunks); - void evalMultiplication(ChunkArray & chunks); - void evalAddition(ChunkArray & chunks); - void evalAssignment(ChunkArray & chunks); -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - void printChunks(ChunkArray & chunks, size_t maxRows = 2, size_t maxCols = 2, int precision = 0); - void printVars(size_t maxRows = 2, size_t maxCols = 2, int precision = 0); - std::string textRepresentation(Value & val, size_t maxRows = 2, size_t maxCols = 2, int precision = 0); -# endif -#endif - - public: - static std::string trim(const std::string & str); - static std::vector split(const std::string & str, const char delimeter); - template static bool isNumber(const std::string & str, T * num = 0); - template static T stringToNumber(const std::string & str); - template static std::string numberToString(T num, int precision = 0); -#ifdef DEBUG - void test_w_lt(size_t & numFails, typename Derived::Scalar & s, Derived & a34, Derived & b34, Derived & c43, Derived & v, std::true_type); - void test_w_lt(size_t & numFails, typename Derived::Scalar & s, Derived & a34, Derived & b34, Derived & c43, Derived & v, std::false_type); - size_t test(); -#endif - }; - typedef Parser ParserXd; - typedef Parser ParserXf; - typedef Parser ParserXi; - - //---------------------------------------- - // Function definitions. - //---------------------------------------- - template - Parser::Parser() : - mOperators1("+-*/^()[]="), - mOperators2(".+.-.*./.^"), - mCacheChunkedExpressions(false) - { - // Coefficient-wise operations. - mFunctions.push_back("abs"); - mFunctions.push_back("sqrt"); - mFunctions.push_back("square"); - mFunctions.push_back("exp"); - mFunctions.push_back("log"); - mFunctions.push_back("log10"); - mFunctions.push_back("sin"); - mFunctions.push_back("cos"); - mFunctions.push_back("tan"); - mFunctions.push_back("asin"); - mFunctions.push_back("acos"); - - // Matrix reduction operations. - mFunctions.push_back("trace"); - mFunctions.push_back("norm"); - mFunctions.push_back("size"); - if (has_operator_lt::value) { - mFunctions.push_back("min"); - mFunctions.push_back("minOfFinites"); - mFunctions.push_back("max"); - mFunctions.push_back("maxOfFinites"); - mFunctions.push_back("absmax"); - mFunctions.push_back("cwiseMin"); - mFunctions.push_back("cwiseMax"); - } - mFunctions.push_back("mean"); - mFunctions.push_back("meanOfFinites"); - mFunctions.push_back("sum"); - mFunctions.push_back("sumOfFinites"); - mFunctions.push_back("prod"); - mFunctions.push_back("numberOfFinites"); - - // Matrix operations. - mFunctions.push_back("transpose"); - mFunctions.push_back("conjugate"); - mFunctions.push_back("adjoint"); - - // Matrix initializers. - mFunctions.push_back("zeros"); - mFunctions.push_back("ones"); - mFunctions.push_back("eye"); - } - - template - Value Parser::eval(const std::string & expression) - { -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - std::cout << "---" << std::endl; - std::cout << "EXPRESSION: " << expression << std::endl; -# endif -#endif - ChunkArray chunks; - std::string code; - splitEquationIntoChunks(trim(expression), chunks, code); - evalIndices(chunks); - evalNegations(chunks); - evalPowers(chunks); - evalMultiplication(chunks); - evalAddition(chunks); - evalAssignment(chunks); - if(chunks.size() != 1) - throw std::runtime_error("Failed to reduce expression '" + expression + "' to a single value."); -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - std::cout << "---" << std::endl; -# endif -#endif - if(chunks[0].type == VARIABLE) { - if(!isVariable(chunks[0].field)) - throw std::runtime_error("Unknown variable '" + chunks[0].field + "'."); - return mVariables[chunks[0].field]; - } - return chunks[0].value; - } - - template - void Parser::splitEquationIntoChunks(const std::string & expression, ChunkArray & chunks, std::string & code) - { - if(cacheExpressions()) { - if(mCachedChunkedExpressions.count(expression) > 0) { - chunks = mCachedChunkedExpressions.at(expression); -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - std::cout << "CACHED CHUNKS: "; printChunks(chunks); std::cout << std::endl; -# endif -#endif - return; - } - } - - for(std::string::const_iterator it = expression.begin(); it != expression.end();) - { - int prevType = (chunks.size() ? chunks.back().type : -1); - char ci = (* it); - if(isspace(ci)) { - // Ignore whitespace. - it++; - } else if(ci == '(' && (prevType == VALUE || prevType == VARIABLE)) { - // Index group. - std::string::const_iterator jt = findClosingBracket(expression, it, ')'); - if(jt == expression.end()) - throw std::runtime_error("Missing closing bracket for '" + std::string(it, jt) + "'."); - std::string field = std::string(it + 1, jt); // Outer brackets stripped. - if(prevType == VARIABLE) { - if(!isVariable(chunks.back().field)) - throw std::runtime_error("Unknown variable '" + chunks.back().field + "'."); - chunks.back().value.setShared(var(chunks.back().field)); - } - int first, last; - int rows = int(chunks.back().value.matrix().rows()); - int cols = int(chunks.back().value.matrix().cols()); - std::vector args = splitArguments(field, ','); - if(args.size() == 1) { - if(cols == 1) { - evalIndexRange(args[0], & first, & last, rows); - chunks.back().row0 = first; - chunks.back().col0 = 0; - chunks.back().rows = last + 1 - first;//(last == -1 ? int(chunks.back().value.matrix().rows()) : last + 1) - first; - chunks.back().cols = 1; - } else if(rows == 1) { - evalIndexRange(args[0], & first, & last, cols); - chunks.back().row0 = 0; - chunks.back().col0 = first; - chunks.back().rows = 1; - chunks.back().cols = last + 1 - first;//(last == -1 ? int(chunks.back().value.matrix().cols()) : last + 1) - first; - } else { - throw std::runtime_error("Missing row or column indices for '(" + chunks.back().field + "(" + field + ")'."); - } - } else if(args.size() == 2) { - evalIndexRange(args[0], & first, & last, rows); - chunks.back().row0 = first; - chunks.back().rows = last + 1 - first;//(last == -1 ? int(chunks.back().value.matrix().rows()) : last + 1) - first; - evalIndexRange(args[1], & first, & last, cols); - chunks.back().col0 = first; - chunks.back().cols = last + 1 - first;//(last == -1 ? int(chunks.back().value.matrix().cols()) : last + 1) - first; - } else { - throw std::runtime_error("Invalid index expression '" + chunks.back().field + "(" + field + ")'."); - } - code += "i"; - it = jt + 1; - } else if(ci == '(' && prevType == FUNCTION) { - // Function argument group. - std::string::const_iterator jt = findClosingBracket(expression, it, ')'); - if(jt == expression.end()) - throw std::runtime_error("Missing closing bracket for '" + std::string(it, jt) + "'."); - std::string field = std::string(it + 1, jt); // Outer brackets stripped. - std::vector args = splitArguments(field, ','); - evalFunction(chunks.back().field, args, chunks.back().value); - chunks.back().field += "(" + field + ")"; - chunks.back().type = VALUE; - code += (chunks.back().value.matrix().size() == 1 ? "n" : "M"); - it = jt + 1; - } else if(ci == '(') { - // Recursively evaluate group to a single value. - std::string::const_iterator jt = findClosingBracket(expression, it, ')'); - if(jt == expression.end()) - throw std::runtime_error("Missing closing bracket for '" + std::string(it, jt) + "'."); - std::string field = std::string(it + 1, jt); // Outer brackets stripped. - chunks.push_back(Chunk(field, prevType = VALUE, eval(field))); - code += (chunks.back().value.matrix().size() == 1 ? "n" : "M"); - it = jt + 1; - } else if(ci == '[') { - // Evaluate matrix. - if(prevType == VALUE || prevType == VARIABLE) - throw std::runtime_error("Invalid operation '" + chunks.back().field + std::string(1, ci) + "'."); - std::string::const_iterator jt = findClosingBracket(expression, it, ']'); - if(jt == expression.end()) - throw std::runtime_error("Missing closing bracket for '" + std::string(it, jt) + "'."); - std::string field = std::string(it + 1, jt); // Outer brackets stripped. - chunks.push_back(Chunk("[" + field + "]", prevType = VALUE)); - evalMatrixExpression(field, chunks.back().value); - code += (chunks.back().value.matrix().size() == 1 ? "n" : "M"); - it = jt + 1; - } else if(it + 1 != expression.end() && isOperator(std::string(it, it + 2))) { - // Double character operator. - std::string field = std::string(it, it + 2); - chunks.push_back(Chunk(field, prevType = OPERATOR)); - code += field; - it += 2; - } else if(isOperator(ci)) { - // Single character operator. - std::string field = std::string(1, ci); - chunks.push_back(Chunk(field, prevType = OPERATOR)); - code += field; - it++; - } else { - // Non-operator: value range, number, function, or variable name. - std::string::const_iterator jt = it + 1; - // accept fp-strings, ie [+-] - unsigned char state = 1; - for(std::string::const_iterator kt = it; state && kt != expression.end(); kt++) { - unsigned char token; - if (*kt == ' ') token = 0; - else if (*kt == '+' || *kt == '-') token = 1; - else if (isdigit(*kt)) token = 2; - else if (*kt == '.') token = 3; - else if (*kt == 'e' || *kt == 'E') token = 4; - else break; - const static char nextstate[9][5] = {{0}, - {1, 2, 3, 4, 0}, - {0, 0, 3, 4, 0}, - {0, 0, 3, 5, 6}, - {0, 0, 5, 0, 0}, - {0, 0, 5, 0, 6}, - {0, 7, 8, 0, 0}, - {0, 0, 8, 0, 0}, - {0, 0, 8, 0, 0}}; - //WARN("state=" << (int)state << " token(" << *kt << ")=" << (int)token - // << " nextstate = " << (int)nextstate[state][token] << "\n"); - state = nextstate[state][token]; - if (state == 8) jt = kt; - } - for(; jt != expression.end(); jt++) { - if(isOperator(*jt) || (jt + 1 != expression.end() && isOperator(std::string(jt, jt + 2)))) - break; - } - std::string field = trim(std::string(it, jt)); - if(prevType == VALUE || prevType == VARIABLE) - throw std::runtime_error("Invalid operation '" + chunks.back().field + field + "'."); - double num; - if(field.find(":") != std::string::npos) { - // Numeric range. - chunks.push_back(Chunk(field, prevType = VALUE)); - evalNumericRange(field, chunks.back().value); - code += (chunks.back().value.matrix().size() == 1 ? "n" : "M"); - } else if(isNumber(field, & num)) { - // Number. - chunks.push_back(Chunk(field, prevType = VALUE, Value(num))); - code += "n"; - } else if(isVariable(field)) { - // Local variable. - chunks.push_back(Chunk(field, prevType = VARIABLE)); - code += (mVariables[field].matrix().size() == 1 ? "vn" : "vM"); - } else if(isFunction(field)) { - // Function. - chunks.push_back(Chunk(field, prevType = FUNCTION)); - } else { - // New undefined variable. - chunks.push_back(Chunk(field, prevType = VARIABLE)); - code += "vU"; - } - it = jt; - } - } // it -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - std::cout << "CHUNKS: "; printChunks(chunks); std::cout << std::endl; - std::cout << "CODE: " << code << std::endl; -# endif -#endif - if(cacheExpressions()) - mCachedChunkedExpressions[expression] = chunks; - } - - template - std::string::const_iterator Parser::findClosingBracket(const std::string & str, const std::string::const_iterator openingBracket, const char closingBracket) const - { - int depth = 1; - std::string::const_iterator it = openingBracket + 1; - for(; it != str.end(); it++) { - if((*it) == (*openingBracket)) depth++; - else if((*it) == closingBracket) depth--; - if(depth == 0) return it; - } - return str.end(); - } - - template - std::vector Parser::splitArguments(const std::string & str, const char delimeter) const - { - std::vector args; - std::string::const_iterator i0 = str.begin(); - for(std::string::const_iterator it = str.begin(); it != str.end(); it++) { - if((*it) == '(') it = findClosingBracket(str, it, ')'); - else if((*it) == '[') it = findClosingBracket(str, it, ']'); - else if((*it) == delimeter) { - args.push_back(trim(std::string(i0, it))); - i0 = it + 1; - } - } - args.push_back(std::string(i0, str.end())); - return args; - } - - template - void Parser::evalIndexRange(const std::string & str, int * first, int * last, int numIndices) - { - if(str.empty()) - throw std::runtime_error("Empty index range."); - ValueXi valuei; - ParserXi parseri; - size_t pos; - for(std::string::const_iterator it = str.begin(); it != str.end(); it++) { - if((*it) == ':') { - std::string firstStr = trim(std::string(str.begin(), it)); - std::string lastStr = trim(std::string(it + 1, str.end())); - if(firstStr.empty() && lastStr.empty()) { - (* first) = 0; - (* last) = numIndices - 1; - return; - } - if(firstStr.empty() || lastStr.empty()) - throw std::runtime_error("Missing indices for '" + str + "'."); - - pos = firstStr.find("end"); - if(pos != std::string::npos) { - firstStr = firstStr.substr(0, pos) + numberToString(numIndices - 1) + firstStr.substr(pos + 3); - } - pos = lastStr.find("end"); - if(pos != std::string::npos) { - lastStr = lastStr.substr(0, pos) + numberToString(numIndices - 1) + lastStr.substr(pos + 3); - } - - valuei = parseri.eval(firstStr); - if(valuei.matrix().size() != 1) - throw std::runtime_error("Invalid indices '" + str + "'."); - (* first) = valuei.matrix()(0, 0); - - valuei = parseri.eval(lastStr); - if(valuei.matrix().size() != 1) - throw std::runtime_error("Invalid indices '" + str + "'."); - (* last) = valuei.matrix()(0, 0); - - return; - } - } - std::string firstStr = str; - - pos = firstStr.find("end"); - if(pos != std::string::npos) { - firstStr = firstStr.substr(0, pos) + numberToString(numIndices - 1) + firstStr.substr(pos + 3); - } - - valuei = parseri.eval(firstStr); - if(valuei.matrix().size() != 1) - throw std::runtime_error("Invalid index '" + str + "'."); - (* first) = valuei.matrix()(0, 0); - (* last) = (* first); - } - - template - void Parser::evalMatrixExpression(const std::string & str, Value & mat) - { - // !!! Expression may NOT include outer brackets, although brackets for individual rows are OK. - std::vector rows = splitArguments(str, ';'); - std::vector > temp; - Value submatrix; - size_t row0 = 0, col0 = 0, nrows = 0, ncols = 0; - size_t pos; - for(size_t i = 0; i < rows.size(); i++) { - // Strip row brackets if they exist. - if(rows[i][0] == '[' && rows[i].back() == ']') rows[i] = rows[i].substr(1, int(rows[i].size()) - 2); - std::vector cols = splitArguments(rows[i], ','); - col0 = 0; - ncols = 0; - for(size_t j = 0; j < cols.size(); j++) { - pos = cols[j].find(":"); - if(pos != std::string::npos) { - std::string firstStr = cols[j].substr(0, pos); - std::string lastStr = cols[j].substr(pos + 1); - pos = lastStr.find(":"); - if(pos != std::string::npos) { - std::string stepStr = lastStr.substr(0, pos); - lastStr = lastStr.substr(pos + 1); - if(lastStr.find(":") != std::string::npos) - throw std::runtime_error("Invalid matrix definition '[" + str + "]'. Invalid range '" + cols[j] + "'."); - // first:step:last - Value first = eval(firstStr); - Value step = eval(stepStr); - Value last = eval(lastStr); - if(first.matrix().size() != 1 || step.matrix().size() != 1 || last.matrix().size() != 1) - throw std::runtime_error("Invalid matrix definition '[" + str + "]'. Invalid range '" + cols[j] + "'."); - typename Derived::RealScalar sfirst = std::real(first.matrix()(0)); - typename Derived::RealScalar sstep = std::real(step.matrix()(0)); - typename Derived::RealScalar slast = std::real(last.matrix()(0)); - if(sfirst == slast) { - submatrix.local().setConstant(1, 1, sfirst); - submatrix.mapLocal(); - } else if((slast - sfirst >= 0 && sstep > 0) || (slast - sfirst <= 0 && sstep < 0)) { - int n = floor((slast - sfirst) / sstep) + 1; - submatrix.local().resize(1, n); - for(int k = 0; k < n; ++k) - submatrix.local()(0, k) = sfirst + k * sstep; - submatrix.mapLocal(); - } else { - throw std::runtime_error("Invalid matrix definition '[" + str + "]'. Invalid range '" + cols[j] + "'."); - } - } else { - // first:last => first:1:last - Value first = eval(firstStr); - Value last = eval(lastStr); - if(first.matrix().size() != 1 || last.matrix().size() != 1) - throw std::runtime_error("Invalid matrix definition '[" + str + "]'. Invalid range '" + cols[j] + "'."); - typename Derived::RealScalar sfirst = std::real(first.matrix()(0)); - typename Derived::RealScalar slast = std::real(last.matrix()(0)); - if(sfirst == slast) { - submatrix.local().setConstant(1, 1, sfirst); - submatrix.mapLocal(); - } else if(slast - sfirst >= 0) { - int n = floor(slast - sfirst) + 1; - submatrix.local().resize(1, n); - for(int k = 0; k < n; ++k) - submatrix.local()(0, k) = sfirst + k; - submatrix.mapLocal(); - } else { - throw std::runtime_error("Invalid matrix definition '[" + str + "]'. Invalid range '" + cols[j] + "'."); - } - } - } else { - submatrix = eval(cols[j]); - } - if(j > 0 && size_t(submatrix.matrix().cols()) != nrows) - throw std::runtime_error("Invalid matrix definition '[" + str + "]'. Successive column entries '" + cols[int(j) - 1] + "' and '" + cols[j] + "' do not have the same number of rows."); - nrows = submatrix.matrix().rows(); - ncols += submatrix.matrix().cols(); - temp.resize(row0 + submatrix.matrix().rows()); - for(size_t row = 0; row < size_t(submatrix.matrix().rows()); row++) { - temp[row0 + row].resize(col0 + submatrix.matrix().cols()); - for(size_t col = 0; col < size_t(submatrix.matrix().cols()); col++) - temp[row0 + row][col0 + col] = submatrix.matrix()(row, col); - } - col0 += submatrix.matrix().cols(); - } - if(row0 > 0 && ncols != temp[int(row0) - 1].size()) - throw std::runtime_error("Invalid matrix definition '[" + str + "]'. Successive row entries '" + rows[int(i) - 1] + "' and '" + rows[i] + "' do not have the same number of columns."); - row0 += nrows; - } - nrows = temp.size(); - if(nrows == 0) return; - ncols = temp[0].size(); - mat.setLocal(Derived(nrows, ncols)); - for(size_t row = 0; row < nrows; row++) { - for(size_t col = 0; col < ncols; col++) - mat.matrix()(row, col) = temp[row][col]; - } - mat.mapLocal(); - } - - template - bool Parser::evalFunction_1_lt(const std::string & name, Value & arg, Value & result, std::true_type) - { - if(name == "min") { - result.setLocal(arg.matrix().minCoeff()); - return true; - } else if(name == "minOfFinites") { - result.setLocal(arg.matrix().minCoeffOfFinites()); - return true; - } else if(name == "max") { - result.setLocal(arg.matrix().maxCoeff()); - return true; - } else if(name == "maxOfFinites") { - result.setLocal(arg.matrix().maxCoeffOfFinites()); - return true; - } else if(name == "absmax") { - typename Derived::Scalar minimum = arg.matrix().minCoeff(); - typename Derived::Scalar maximum = arg.matrix().maxCoeff(); - result.setLocal(std::abs(maximum) >= std::abs(minimum) ? maximum : minimum); - return true; - } - return false; - } - - template - bool Parser::evalFunction_1_lt(const std::string &/*name*/, Value &/*arg0*/, Value &/*result*/, std::false_type) - { - return false; - } - - template - bool Parser::evalFunction_2_lt(const std::string & name, Value & arg0, Value & arg1, Value & result, std::true_type) - { - if(name == "min") { - if(arg1.matrix().size() != 1) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); - int dim = floor(std::real(arg1.matrix()(0, 0))); - if((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); - if(dim == 0) { - result.local() = arg0.matrix().colwise().minCoeff(); - result.mapLocal(); - return true; - } else if(dim == 1) { - result.local() = arg0.matrix().rowwise().minCoeff(); - result.mapLocal(); - return true; - } - } else if(name == "max") { - if(arg1.matrix().size() != 1) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); - int dim = floor(std::real(arg1.matrix()(0, 0))); - if((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); - if(dim == 0) { - result.local() = arg0.matrix().colwise().maxCoeff(); - result.mapLocal(); - return true; - } else if(dim == 1) { - result.local() = arg0.matrix().rowwise().maxCoeff(); - result.mapLocal(); - return true; - } - } else if(name == "absmax") { - if(arg1.matrix().size() != 1) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); - int dim = floor(std::real(arg1.matrix()(0, 0))); - if((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); - if(dim == 0) { - result.local() = arg0.matrix().colwise().maxCoeff(); - result.mapLocal(); - Derived minimum = arg0.matrix().colwise().minCoeff(); - for(size_t i = 0; i < size_t(result.matrix().size()); i++) { - if(std::abs(result.matrix()(i)) < std::abs(minimum(i))) - result.matrix()(i) = minimum(i); - } - return true; - } else if(dim == 1) { - result.local() = arg0.matrix().rowwise().maxCoeff(); - result.mapLocal(); - Derived minimum = arg0.matrix().rowwise().minCoeff(); - for(size_t i = 0; i < size_t(result.matrix().size()); i++) { - if(std::abs(result.matrix()(i)) < std::abs(minimum(i))) - result.matrix()(i) = minimum(i); - } - return true; - } - } else if (name == "cwiseMin") { - if (arg1.matrix().size() == 1) { - typename Derived::Scalar arg1scalar = arg1.matrix()(0, 0); - Derived arg1matrix = Derived::Constant(arg0.matrix().rows(), arg0.matrix().cols(), arg1scalar); - result.local() = arg0.matrix().cwiseMin(arg1matrix); - result.mapLocal(); - return true; - } else if (arg0.matrix().cols() == arg1.matrix().cols() && arg0.matrix().rows() == arg1.matrix().rows()) { - result.local() = arg0.matrix().cwiseMin(arg1.matrix()); - result.mapLocal(); - return true; - } else { - throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); - } - } else if (name == "cwiseMax") { - if (arg1.matrix().size() == 1) { - typename Derived::Scalar arg1scalar = arg1.matrix()(0, 0); - Derived arg1matrix = Derived::Constant(arg0.matrix().rows(), arg0.matrix().cols(), arg1scalar); - result.local() = arg0.matrix().cwiseMax(arg1matrix); - result.mapLocal(); - return true; - } else if (arg0.matrix().cols() == arg1.matrix().cols() && arg0.matrix().rows() == arg1.matrix().rows()) { - result.local() = arg0.matrix().cwiseMax(arg1.matrix()); - result.mapLocal(); - return true; - } else { - throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); - } - } - return false; - } - - template - bool Parser::evalFunction_2_lt(const std::string &/*name*/, Value &/*arg0*/, Value &/*arg1*/, Value &/*result*/, std::false_type) - { - return false; - } - - template - void Parser::evalFunction(const std::string & name, std::vector & args, Value & result) - { - if(args.size() == 1) { - Value arg = eval(args[0]); - if(name == "abs") { - result.local() = arg.matrix().array().abs().template cast(); - result.mapLocal(); - return; - } else if(name == "sqrt") { - result.local() = arg.matrix().array().sqrt(); - result.mapLocal(); - return; - } else if(name == "square") { - result.local() = arg.matrix().array().square(); - result.mapLocal(); - return; - } else if(name == "exp") { - result.local() = arg.matrix().array().exp(); - result.mapLocal(); - return; - } else if(name == "log") { - result.local() = arg.matrix().array().log(); - result.mapLocal(); - return; - } else if(name == "log10") { - result.local() = arg.matrix().array().log(); - result.local() *= (1.0 / log(10)); - result.mapLocal(); - return; - } else if(name == "sin") { - result.local() = arg.matrix().array().sin(); - result.mapLocal(); - return; - } else if(name == "cos") { - result.local() = arg.matrix().array().cos(); - result.mapLocal(); - return; - } else if(name == "tan") { - result.local() = arg.matrix().array().tan(); - result.mapLocal(); - return; - } else if(name == "acos") { - result.local() = arg.matrix().array().acos(); - result.mapLocal(); - return; - } else if(name == "asin") { - result.local() = arg.matrix().array().asin(); - result.mapLocal(); - return; - } else if(name == "trace") { - result.setLocal(arg.matrix().trace()); - return; - } else if(name == "norm") { - result.setLocal(arg.matrix().norm()); - return; - } else if (evalFunction_1_lt(name, arg, result, has_operator_lt())) { - return; - } else if(name == "mean") { - result.setLocal(arg.matrix().mean()); - return; - } else if(name == "meanOfFinites") { - result.setLocal(arg.matrix().meanOfFinites()); - return; - } else if(name == "sum") { - result.setLocal(arg.matrix().sum()); - return; - } else if(name == "sumOfFinites") { - result.setLocal(arg.matrix().sumOfFinites()); - return; - } else if(name == "prod") { - result.setLocal(arg.matrix().prod()); - return; - } else if(name == "numberOfFinites") { - result.setLocal(arg.matrix().numberOfFinites()); - return; - } else if(name == "transpose") { - result.local() = arg.matrix().transpose(); - result.mapLocal(); - return; - } else if(name == "conjugate") { - result.local() = arg.matrix().conjugate(); - result.mapLocal(); - return; - } else if(name == "adjoint") { - result.local() = arg.matrix().adjoint(); - result.mapLocal(); - return; - } else if(name == "zeros") { - if(arg.matrix().size() != 1) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + ")'."); - int N = floor(std::real(arg.matrix()(0, 0))); - if(N <= 0 || N != std::real(arg.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + ")'."); - result.local() = Derived::Zero(N, N); - result.mapLocal(); - return; - } else if(name == "ones") { - if(arg.matrix().size() != 1) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + ")'."); - int N = floor(std::real(arg.matrix()(0, 0))); - if(N <= 0 || N != std::real(arg.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + ")'."); - result.local() = Derived::Ones(N, N); - result.mapLocal(); - return; - } else if(name == "eye") { - if(arg.matrix().size() != 1) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + ")'."); - int N = floor(std::real(arg.matrix()(0, 0))); - if(N <= 0 || N != std::real(arg.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + ")'."); - result.local() = Derived::Identity(N, N); - result.mapLocal(); - return; - } - else { - throw std::runtime_error("Invalid function '" + name + "(" + args[0] + ")'."); - } - } else if(args.size() == 2) { - Value arg0 = eval(args[0]); - Value arg1 = eval(args[1]); - if(name == "size") { - if(arg1.matrix().size() != 1) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - int dim = floor(std::real(arg1.matrix()(0, 0))); - if((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - if(dim == 0) { - result.setLocal((typename Derived::Scalar) arg0.matrix().rows()); - return; - } else if(dim == 1) { - result.setLocal((typename Derived::Scalar) arg0.matrix().cols()); - return; - } - } else if (evalFunction_2_lt(name, arg0, arg1, result, has_operator_lt())) { - return; - } else if(name == "mean") { - if(arg1.matrix().size() != 1) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - int dim = floor(std::real(arg1.matrix()(0, 0))); - if((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - if(dim == 0) { - result.local() = arg0.matrix().colwise().mean(); - result.mapLocal(); - return; - } else if(dim == 1) { - result.local() = arg0.matrix().rowwise().mean(); - result.mapLocal(); - return; - } - } else if(name == "sum") { - if(arg1.matrix().size() != 1) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - int dim = floor(std::real(arg1.matrix()(0, 0))); - if((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - if(dim == 0) { - result.local() = arg0.matrix().colwise().sum(); - result.mapLocal(); - return; - } else if(dim == 1) { - result.local() = arg0.matrix().rowwise().sum(); - result.mapLocal(); - return; - } - } else if(name == "prod") { - if(arg1.matrix().size() != 1) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - int dim = floor(std::real(arg1.matrix()(0, 0))); - if((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension argument for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - if(dim == 0) { - result.local() = arg0.matrix().colwise().prod(); - result.mapLocal(); - return; - } else if(dim == 1) { - result.local() = arg0.matrix().rowwise().prod(); - result.mapLocal(); - return; - } - } else if(name == "zeros") { - if((arg0.matrix().size() != 1) || (arg1.matrix().size() != 1)) - throw std::runtime_error("Invalid dimension arguments for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - int rows = floor(std::real(arg0.matrix()(0, 0))); - int cols = floor(std::real(arg1.matrix()(0, 0))); - if(rows <= 0 || cols <= 0 || rows != std::real(arg0.matrix()(0, 0)) || cols != std::real(arg1.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension arguments for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - result.local() = Derived::Zero(rows, cols); - result.mapLocal(); - return; - } else if(name == "ones") { - if((arg0.matrix().size() != 1) || (arg1.matrix().size() != 1)) - throw std::runtime_error("Invalid dimension arguments for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - int rows = floor(std::real(arg0.matrix()(0, 0))); - int cols = floor(std::real(arg1.matrix()(0, 0))); - if(rows <= 0 || cols <= 0 || rows != std::real(arg0.matrix()(0, 0)) || cols != std::real(arg1.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension arguments for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - result.local() = Derived::Ones(rows, cols); - result.mapLocal(); - return; - } else if(name == "eye") { - if((arg0.matrix().size() != 1) || (arg1.matrix().size() != 1)) - throw std::runtime_error("Invalid dimension arguments for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - int rows = floor(std::real(arg0.matrix()(0, 0))); - int cols = floor(std::real(arg1.matrix()(0, 0))); - if(rows <= 0 || cols <= 0 || rows != std::real(arg0.matrix()(0, 0)) || cols != std::real(arg1.matrix()(0, 0))) - throw std::runtime_error("Invalid dimension arguments for function '" + name + "(" + args[0] + "," + args[1] + ")'."); - result.local() = Derived::Identity(rows, cols); - result.mapLocal(); - return; - } else { - throw std::runtime_error("Invalid function '" + name + "(" + args[0] + "," + args[1] + ")'."); - } - } - std::string argsStr = "("; - for(size_t i = 0; i < args.size(); i++) { - if(i > 0) argsStr += ","; - argsStr += args[i]; - } - argsStr += ")"; - throw std::runtime_error("Invalid function/arguments for '" + name + argsStr + "'."); - } - - template - void Parser::evalNumericRange(const std::string & str, Value & mat) - { - size_t pos = str.find(":"); - if(pos == std::string::npos) - throw std::runtime_error("Invalid numeric range '" + str + "'."); - size_t pos2 = str.substr(pos + 1).find(":"); - if(pos2 == std::string::npos) { - // first:last - std::string firstStr = str.substr(0, pos); - std::string lastStr = str.substr(pos + 1); - Value first = eval(firstStr); - Value last = eval(lastStr); - if(first.matrix().size() != 1 || last.matrix().size() != 1) - throw std::runtime_error("Invalid numeric range '" + str + "'."); - typename Derived::RealScalar sfirst = std::real(first.matrix()(0,0)); - typename Derived::RealScalar slast = std::real(last.matrix()(0,0)); - if(sfirst > slast) - throw std::runtime_error("Invalid numeric range '" + str + "'. Must not reverse."); - int n = 1 + floor(slast - sfirst); - mat.local().resize(1, n); - for(int i = 0; i < n; i++) - mat.local()(0, i) = sfirst + i; - mat.mapLocal(); - } else { - // first:step:last - pos2 += pos + 1; - std::string firstStr = str.substr(0, pos); - std::string stepStr = str.substr(pos + 1, pos2 - pos - 1); - std::string lastStr = str.substr(pos2 + 1); - Value first = eval(firstStr); - Value step = eval(stepStr); - Value last = eval(lastStr); - if(first.matrix().size() != 1 || step.matrix().size() != 1 || last.matrix().size() != 1) - throw std::runtime_error("Invalid numeric range '" + str + "'."); - typename Derived::RealScalar sfirst = std::real(first.matrix()(0, 0)); - typename Derived::RealScalar sstep = std::real(step.matrix()(0, 0)); - typename Derived::RealScalar slast = std::real(last.matrix()(0, 0)); - if(sfirst == slast) { - mat = sfirst; - } else if(sfirst < slast && sstep > 0) { - int n = 1 + floor((slast - sfirst) / sstep); - mat.local().resize(1, n); - for(int i = 0; i < n; i++) - mat.local()(0, i) = sfirst + i * sstep; - mat.mapLocal(); - } else if(sfirst > slast && sstep < 0) { - int n = 1 + floor((slast - sfirst) / sstep); - mat.local().resize(1, n); - for(int i = 0; i < n; i++) - mat.local()(0, i) = sfirst + i * sstep; - mat.mapLocal(); - } else { - throw std::runtime_error("Invalid numeric range '" + str + "'."); - } - } - } - - template - bool Parser::isOperator(const std::string & str) const - { - if(str.size() == 1) - return isOperator(str[0]); - else if(str.size() == 2) { - size_t pos = mOperators2.find(str); - return (pos != std::string::npos && pos % 2 == 0); - } - return false; - } - - template - void Parser::evalIndices(ChunkArray & chunks) - { -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - bool operationPerformed = false; -# endif -#endif - for(typename ChunkArray::iterator it = chunks.begin(); it != chunks.end(); it++) { - if(it->row0 != -1 && (it->type == VALUE || (it->type == VARIABLE && (it + 1 == chunks.end() || (it + 1)->type != OPERATOR || (it + 1)->field != "=")))) { - if(it->type == VALUE) { - Derived temp = it->value.local().block(it->row0, it->col0, it->rows, it->cols); - it->value.local() = temp; - it->value.mapLocal(); - } else { //if(it->type == VARIABLE) { - if(!isVariable(it->field)) - throw std::runtime_error("Attempted indexing into uninitialized variable '" + it->field + "'."); - it->value.local() = mVariables[it->field].matrix().block(it->row0, it->col0, it->rows, it->cols); - it->value.mapLocal(); - it->type = VALUE; - } - it->row0 = -1; - it->col0 = -1; - it->rows = -1; - it->cols = -1; -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - operationPerformed = true; -# endif -#endif - } - } -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - if(operationPerformed) { std::cout << "i: "; printChunks(chunks); std::cout << std::endl; } -# endif -#endif - } - - template - void Parser::evalNegations(ChunkArray & chunks) - { -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - bool operationPerformed = false; -# endif -#endif - if(chunks.size() < 2) return; - typename ChunkArray::iterator lhs = chunks.begin(), op = chunks.begin(), rhs = op + 1; - for(; lhs != chunks.end() && op != chunks.end() && rhs != chunks.end();) { - if(op->type == OPERATOR && op->field == "-" && (op == chunks.begin() || (lhs->type != VALUE && lhs->type != VARIABLE)) && (rhs->type == VALUE || rhs->type == VARIABLE)) { - if(rhs->type == VALUE) - rhs->value.matrix().array() *= -1; - else if(rhs->type == VARIABLE) { - if(!isVariable(rhs->field)) - throw std::runtime_error("Attempted operation '" + op->field + rhs->field + "' on uninitialized variable '" + rhs->field + "'."); - rhs->value.local() = mVariables[rhs->field].matrix().array() * -1; - rhs->value.mapLocal(); - rhs->type = VALUE; - } - lhs = chunks.erase(op); - op = (lhs != chunks.end()) ? lhs + 1 : lhs; - rhs = (op != chunks.end()) ? op + 1 : op; -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - operationPerformed = true; -# endif -#endif - } else { - lhs = op; - op = rhs; - rhs++; - } - } -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - if(operationPerformed) { std::cout << "-: "; printChunks(chunks); std::cout << std::endl; } -# endif -#endif - } - - template - void Parser::evalPowers(ChunkArray & chunks) - { -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - bool operationPerformed = false; -# endif -#endif - if(chunks.size() < 3) return; - typename ChunkArray::iterator lhs = chunks.begin(), op = lhs + 1, rhs = op + 1; - for(; lhs != chunks.end() && op != chunks.end() && rhs != chunks.end();) { - if((op->type == OPERATOR) && (op->field == "^" || op->field == ".^")) { - if(lhs->type == VARIABLE) { - if(!isVariable(lhs->field)) - throw std::runtime_error("Attempted operation '" + lhs->field + op->field + rhs->field + "' on uninitialized variable '" + lhs->field + "'."); - lhs->value.setShared(mVariables[lhs->field]); - } - if(rhs->type == VARIABLE) { - if(!isVariable(rhs->field)) - throw std::runtime_error("Attempted operation '" + lhs->field + op->field + rhs->field + "' on uninitialized variable '" + rhs->field + "'."); - rhs->value.setShared(mVariables[rhs->field]); - } - if(rhs->value.matrix().size() == 1) { - lhs->value.local() = lhs->value.matrix().array().pow(rhs->value.matrix()(0, 0)); - lhs->value.mapLocal(); - lhs->type = VALUE; - } else if(lhs->value.matrix().size() == 1) { - typename Derived::Scalar temp = lhs->value.matrix()(0, 0); - lhs->value.local().resize(rhs->value.matrix().rows(), rhs->value.matrix().cols()); - for(size_t row = 0; row < size_t(rhs->value.matrix().rows()); row++) { - for(size_t col = 0; col < size_t(rhs->value.matrix().cols()); col++) - lhs->value.local()(row, col) = pow(temp, rhs->value.matrix()(row, col)); - } - lhs->value.mapLocal(); - lhs->type = VALUE; - } else if(op->field == ".^" && lhs->value.matrix().rows() == rhs->value.matrix().rows() && lhs->value.matrix().cols() == rhs->value.matrix().cols()) { - lhs->value.local().resize(rhs->value.matrix().rows(), rhs->value.matrix().cols()); - for(size_t row = 0; row < size_t(rhs->value.matrix().rows()); row++) { - for(size_t col = 0; col < size_t(rhs->value.matrix().cols()); col++) - lhs->value.local()(row, col) = pow(lhs->value.matrix()(row, col), rhs->value.matrix()(row, col)); - } - lhs->value.mapLocal(); - lhs->type = VALUE; - } else { - throw std::runtime_error("Invalid operand dimensions for operation '" + lhs->field + op->field + rhs->field + "'."); - } - chunks.erase(op, rhs + 1); - op = lhs + 1; - rhs = (op != chunks.end()) ? op + 1 : op; -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - operationPerformed = true; -# endif -#endif - } else { - lhs = op; - op = rhs; - rhs++; - } - } -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - if(operationPerformed) { std::cout << "^: "; printChunks(chunks); std::cout << std::endl; } -# endif -#endif - } - - template - void Parser::evalMultiplication(ChunkArray & chunks) - { -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - bool operationPerformed = false; -# endif -#endif - if(chunks.size() < 3) return; - typename ChunkArray::iterator lhs = chunks.begin(), op = lhs + 1, rhs = op + 1; - for(; lhs != chunks.end() && op != chunks.end() && rhs != chunks.end();) { - if((op->type == OPERATOR) && (op->field == "*" || op->field == "/" || op->field == ".*" || op->field == "./")) { - if(lhs->type == VARIABLE) { - if(!isVariable(lhs->field)) - throw std::runtime_error("Attempted operation '" + lhs->field + op->field + rhs->field + "' on uninitialized variable '" + lhs->field + "'."); - lhs->value.setShared(mVariables[lhs->field]); - } - if(rhs->type == VARIABLE) { - if(!isVariable(rhs->field)) - throw std::runtime_error("Attempted operation '" + lhs->field + op->field + rhs->field + "' on uninitialized variable '" + rhs->field + "'."); - rhs->value.setShared(mVariables[rhs->field]); - } - if(rhs->value.matrix().size() == 1) { - if(lhs->value.isLocal()) { - if(op->field == "*" || op->field == ".*") - lhs->value.local().array() *= rhs->value.matrix()(0, 0); - else // if(op->field == "/" || op->field == "./") - lhs->value.local().array() /= rhs->value.matrix()(0, 0); - } else { - if(op->field == "*" || op->field == ".*") - lhs->value.local() = lhs->value.matrix().array() * rhs->value.matrix()(0, 0); - else // if(op->field == "/" || op->field == "./") - lhs->value.local() = lhs->value.matrix().array() / rhs->value.matrix()(0, 0); - lhs->value.mapLocal(); - lhs->type = VALUE; - } - } else if(lhs->value.matrix().size() == 1) { - typename Derived::Scalar temp = lhs->value.matrix()(0, 0); - if(op->field == "*" || op->field == ".*") - lhs->value.local() = rhs->value.matrix().array() * temp; - else // if(op->field == "/" || op->field == "./") - lhs->value.local() = Derived::Constant(rhs->value.matrix().rows(), rhs->value.matrix().cols(), temp).array() / rhs->value.matrix().array(); - lhs->value.mapLocal(); - lhs->type = VALUE; - } else if((op->field == ".*" || op->field == "./") && lhs->value.matrix().rows() == rhs->value.matrix().rows() && lhs->value.matrix().cols() == rhs->value.matrix().cols()) { - if(lhs->value.isLocal()) { - if(op->field == ".*") - lhs->value.local().array() *= rhs->value.matrix().array(); - else // if(op->field == "./") - lhs->value.local().array() /= rhs->value.matrix().array(); - } else { - if(op->field == ".*") - lhs->value.local() = lhs->value.matrix().array() * rhs->value.matrix().array(); - else // if(op->field == "./") - lhs->value.local() = lhs->value.matrix().array() / rhs->value.matrix().array(); - lhs->value.mapLocal(); - lhs->type = VALUE; - } - } else if(op->field == "*" && lhs->value.matrix().cols() == rhs->value.matrix().rows()) { - if(lhs->value.isLocal()) { - lhs->value.local() *= rhs->value.matrix(); - lhs->value.mapLocal(); - } else { - lhs->value.local() = lhs->value.matrix() * rhs->value.matrix(); - lhs->value.mapLocal(); - lhs->type = VALUE; - } - } else { - throw std::runtime_error("Invalid operand dimensions for operation '" + lhs->field + op->field + rhs->field + "'."); - } - chunks.erase(op, rhs + 1); - op = lhs + 1; - rhs = (op != chunks.end()) ? op + 1 : op; -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - operationPerformed = true; -# endif -#endif - } else { - lhs = op; - op = rhs; - rhs++; - } - } -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - if(operationPerformed) { std::cout << "*: "; printChunks(chunks); std::cout << std::endl; } -# endif -#endif - } - - template - void Parser::evalAddition(ChunkArray & chunks) - { -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - bool operationPerformed = false; -# endif -#endif - if(chunks.size() < 3) return; - typename ChunkArray::iterator lhs = chunks.begin(), op = lhs + 1, rhs = op + 1; - for(; lhs != chunks.end() && op != chunks.end() && rhs != chunks.end();) { - if((op->type == OPERATOR) && (op->field == "+" || op->field == "-" || op->field == ".+" || op->field == ".-")) { - if(lhs->type == VARIABLE) { - if(!isVariable(lhs->field)) - throw std::runtime_error("Attempted operation '" + lhs->field + op->field + rhs->field + "' on uninitialized variable '" + lhs->field + "'."); - lhs->value.setShared(mVariables[lhs->field]); - } - if(rhs->type == VARIABLE) { - if(!isVariable(rhs->field)) - throw std::runtime_error("Attempted operation '" + lhs->field + op->field + rhs->field + "' on uninitialized variable '" + rhs->field + "'."); - rhs->value.setShared(mVariables[rhs->field]); - } - if(rhs->value.matrix().size() == 1) { - if(lhs->value.isLocal()) { - if(op->field == "+" || op->field == ".+") - lhs->value.local().array() += rhs->value.matrix()(0, 0); - else // if(op->field == "-" || op->field == ".-") - lhs->value.local().array() -= rhs->value.matrix()(0, 0); - } else { - if(op->field == "+" || op->field == ".+") - lhs->value.local() = lhs->value.matrix().array() + rhs->value.matrix()(0, 0); - else // if(op->field == "-" || op->field == ".-") - lhs->value.local() = lhs->value.matrix().array() - rhs->value.matrix()(0, 0); - lhs->value.mapLocal(); - lhs->type = VALUE; - } - } else if(lhs->value.matrix().size() == 1) { - typename Derived::Scalar temp = lhs->value.matrix()(0, 0); - if(op->field == "+" || op->field == ".+") - lhs->value.local() = rhs->value.matrix().array() + temp; - else // if(op->field == "-" || op->field == ".-") - lhs->value.local() = Derived::Constant(rhs->value.matrix().rows(), rhs->value.matrix().cols(), temp).array() - rhs->value.matrix().array(); - lhs->value.mapLocal(); - lhs->type = VALUE; - } else if(lhs->value.matrix().rows() == rhs->value.matrix().rows() && lhs->value.matrix().cols() == rhs->value.matrix().cols()) { - if(lhs->value.isLocal()) { - if(op->field == "+" || op->field == ".+") - lhs->value.local().array() += rhs->value.matrix().array(); - else // if(op->field == "-" || op->field == ".-") - lhs->value.local().array() -= rhs->value.matrix().array(); - } else { - if(op->field == "+" || op->field == ".+") - lhs->value.local() = lhs->value.matrix().array() + rhs->value.matrix().array(); - else // if(op->field == "-" || op->field == ".-") - lhs->value.local() = lhs->value.matrix().array() - rhs->value.matrix().array(); - lhs->value.mapLocal(); - lhs->type = VALUE; - } - } else { - throw std::runtime_error("Invalid operand dimensions for operation '" + lhs->field + op->field + rhs->field + "'."); - } - chunks.erase(op, rhs + 1); - op = lhs + 1; - rhs = (op != chunks.end()) ? op + 1 : op; -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - operationPerformed = true; -# endif -#endif - } else { - lhs = op; - op = rhs; - rhs++; - } - } -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - if(operationPerformed) { std::cout << "+: "; printChunks(chunks); std::cout << std::endl; } -# endif -#endif - } - - template - void Parser::evalAssignment(ChunkArray & chunks) - { -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - bool operationPerformed = false; -# endif -#endif - if(chunks.size() < 3) return; - typename ChunkArray::iterator rhs = chunks.end() - 1, op = rhs - 1, lhs = op - 1; - for(; op != chunks.begin() && rhs != chunks.begin();) { - if(op->type == OPERATOR && op->field == "=" && (lhs->type == VALUE || lhs->type == VARIABLE) && (rhs->type == VALUE || rhs->type == VARIABLE)) { - if(rhs->type == VARIABLE) { - if(!isVariable(rhs->field)) - throw std::runtime_error("Attempted operation '" + lhs->field + op->field + rhs->field + "' on uninitialized variable '" + rhs->field + "'."); - rhs->value.setShared(mVariables[rhs->field]); - } - if(lhs->type == VALUE) { - lhs->value.local() = rhs->value.matrix(); - lhs->value.mapLocal(); - } else { //if(lhs->type == VARIABLE) { - if(isVariable(lhs->field)) { - lhs->value.setShared(mVariables[lhs->field]); - if(lhs->row0 == -1) { - if(lhs->value.matrix().rows() == rhs->value.matrix().rows() && lhs->value.matrix().cols() == rhs->value.matrix().cols()) { - lhs->value.matrix() = rhs->value.matrix(); - } else { - mVariables[lhs->field].local() = rhs->value.matrix(); - mVariables[lhs->field].mapLocal(); - } - } else { //if(lhs->row0 != -1) { - lhs->value.matrix().block(lhs->row0, lhs->col0, lhs->rows, lhs->cols) = rhs->value.matrix(); - } - } else { - mVariables[lhs->field].local() = rhs->value.matrix(); - mVariables[lhs->field].mapLocal(); - } - } - rhs = chunks.erase(op, rhs + 1); - op = (rhs != chunks.begin()) ? rhs - 1 : rhs; - if (op != chunks.begin()) lhs = op - 1; -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - operationPerformed = true; -# endif -#endif - } else { - rhs = op; - op = lhs; - lhs--; - } - } -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - if(operationPerformed) { std::cout << "=: "; printChunks(chunks); std::cout << std::endl; } -# endif -#endif - } - -#ifdef DEBUG -# ifdef EIGENLAB_DEBUG - template - void Parser::printChunks(ChunkArray & chunks, size_t maxRows, size_t maxCols, int precision) - { - std::cout << "__"; - for(typename ChunkArray::iterator it = chunks.begin(); it != chunks.end(); it++) { - switch(it->type) { - case VALUE: - std::cout << textRepresentation(it->value, maxRows, maxCols, precision); - if(it->row0 != -1) - std::cout << "(" << it->row0 << ":" << it->row0 + it->rows - 1 << "," << it->col0 << ":" << it->col0 + it->cols - 1 << ")"; - break; - case VARIABLE: - std::cout << it->field;// << "=" << textRepresentation(mVariables[it->field], maxRows, maxCols, precision); - if(it->row0 != -1) - std::cout << "(" << it->row0 << ":" << it->row0 + it->rows - 1 << "," << it->col0 << ":" << it->col0 + it->cols - 1 << ")"; - break; - case OPERATOR: - std::cout << it->field; - break; - case FUNCTION: - std::cout << "f()=" << it->field; - break; - } - std::cout << "__"; - } - } - - template - void Parser::printVars(size_t maxRows, size_t maxCols, int precision) - { - for(typename ValueMap::iterator it = mVariables.begin(); it != mVariables.end(); it++) - std::cout << it->first << " (" << it->second.matrix().rows() << "x" << it->second.matrix().cols() << ") = " << textRepresentation(it->second, maxRows, maxCols, precision) << std::endl; - } - - template - std::string Parser::textRepresentation(Value & val, size_t maxRows, size_t maxCols, int precision) - { - if(val.matrix().size() == 1) - return numberToString(val.matrix()(0, 0), precision); - else { - std::string str = "["; - for(size_t row = 0; row < val.matrix().rows() && row < maxRows; row++) { - str += (row > 0 ? ";[" : "["); - for(size_t col = 0; col < val.matrix().cols() && col < maxCols; col++) { - if(col > 0) str += ","; - str += numberToString(val.matrix()(row, col), precision); - } - str += (val.matrix().cols() > maxCols ? "...]" : "]"); - } - str += (val.matrix().rows() > maxRows ? "...]" : "]"); - return str; - } - } -# endif // #ifdef EIGENLAB_DEBUG -#endif // #ifdef DEBUG - - template - std::string Parser::trim(const std::string & str) - { - if(str.size() == 0) return str; - std::string::const_iterator first = str.begin(); - std::string::const_iterator last = str.end() - 1; - while(first < last && isspace(*first)) first++; - while(first < last && isspace(*last)) last--; - return std::string(first, last + 1); - } - - template - std::vector Parser::split(const std::string & str, const char delimeter) - { - std::vector args; - std::string::const_iterator i0 = str.begin(); - for(std::string::const_iterator it = str.begin(); it != str.end(); it++) { - if((*it) == delimeter) { - args.push_back(trim(std::string(i0, it))); - i0 = it + 1; - } - } - args.push_back(std::string(i0, str.end())); - return args; - } - - template - template - bool Parser::isNumber(const std::string & str, T * num) - { - std::istringstream iss(str); - if(num) - iss >> (* num); - else { - T number; - iss >> number; - } - return (!iss.fail() && !iss.bad() && iss.eof()); - } - - template - template - T Parser::stringToNumber(const std::string & str) - { - std::istringstream iss(str); - T number; - iss >> number; - if(iss.fail() || iss.bad() || !iss.eof()) - throw std::runtime_error("Failed to convert " + str + " to a number."); - return number; - } - - template - template - std::string Parser::numberToString(T num, int precision) - { - std::ostringstream oss; - if(precision) - oss << std::setprecision(precision) << num; - else - oss << num; - return oss.str(); - } - -#ifdef DEBUG - template - void - Parser::test_w_lt(size_t & numFails, - typename Derived::Scalar & /* s */, - Derived & a34, - Derived & b34, - Derived & /* c43 */, - Derived & /* v */, std::true_type) - { - // - // tests that only work if Derived::Scalar has operator< - // - Value resultValue; - Derived resultMatrix; - Derived temp; - std::cout << "Test min(a): "; - resultValue = eval("min(a)"); - resultMatrix.setConstant(1, 1, a34.minCoeff()); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test min(a, 0): "; - resultValue = eval("min(a, 0)"); - resultMatrix = a34.colwise().minCoeff(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test min(a, 1): "; - resultValue = eval("min(a, 1)"); - resultMatrix = a34.rowwise().minCoeff(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test max(a): "; - resultValue = eval("max(a)"); - resultMatrix.setConstant(1, 1, a34.maxCoeff()); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test max(a, 0): "; - resultValue = eval("max(a, 0)"); - resultMatrix = a34.colwise().maxCoeff(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test max(a, 1): "; - resultValue = eval("max(a, 1)"); - resultMatrix = a34.rowwise().maxCoeff(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test absmax(a): "; - resultValue = eval("absmax(a)"); - resultMatrix.setConstant(1, 1, std::abs(a34.maxCoeff()) >= std::abs(a34.minCoeff()) ? a34.maxCoeff() : a34.minCoeff()); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test absmax(a, 0): "; - resultValue = eval("absmax(a, 0)"); - resultMatrix = a34.colwise().maxCoeff(); - temp = a34.colwise().minCoeff(); - for(Index i = 0; i < resultMatrix.size(); ++i) { - if(std::abs(resultMatrix(i)) < std::abs(temp(i))) - resultMatrix(i) = temp(i); - } - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test absmax(a, 1): "; - resultValue = eval("absmax(a, 1)"); - resultMatrix = a34.rowwise().maxCoeff(); - temp = a34.rowwise().minCoeff(); - for(Index i = 0; i < resultMatrix.size(); ++i) { - if(std::abs(resultMatrix(i)) < std::abs(temp(i))) - resultMatrix(i) = temp(i); - } - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test cwiseMin(a, b): "; - resultValue = eval("cwiseMin(a, b)"); - resultMatrix = a34.cwiseMin(b34); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test cwiseMax(a, b): "; - resultValue = eval("cwiseMax(a, b)"); - resultMatrix = a34.cwiseMax(b34); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - } - - template - void - Parser::test_w_lt(size_t & /* numFails */, - typename Derived::Scalar & /* s */, - Derived & /* a34 */, - Derived & /* b34 */, - Derived & /* c43 */, - Derived & /* v */, std::false_type) - { - // do nothing - } - - template - size_t Parser::test() - { - std::cout << std::endl; - std::cout << "BEGIN unit test for EigenLab..." << std::endl; - std::cout << "Make sure this function completes successfuly and prints the message " - "'Successfully completed unit test for EigenLab with no failures.'" << std::endl; - std::cout << std::endl; - - size_t numFails = 0; - Value resultValue; - Derived resultMatrix; - Derived temp; - typename Derived::Scalar s = 2; - - Derived a34 = Derived::Random(3, 4); - Derived b34 = Derived::Random(3, 4); - Derived c43 = Derived::Random(4, 3); - Derived v = Derived::Random(1, 10); - //std::cout << "a34=" << std::endl << a34 << std::endl << std::endl; - //std::cout << "b34=" << std::endl << b34 << std::endl << std::endl; - //std::cout << "c43=" << std::endl << c43 << std::endl << std::endl; - //std::cout << "v=" << std::endl << v << std::endl << std::endl; - - var("a").setShared(a34); - var("b").setShared(b34); - var("c").setShared(c43); - var("v").setShared(v); - var("s").setShared(& s); - - //////////////////////////////////////// - std::cout << "Testing basic operations..." << std::endl << std::endl; - //////////////////////////////////////// - - std::cout << "Test matrix addition a + b: "; - resultValue = eval("a + b"); - resultMatrix = a34 + b34; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix/scalar addition a + s: "; - resultValue = eval("a + s"); - resultMatrix = a34.array() + s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test scalar/matrix addition s + b: "; - resultValue = eval("s + b"); - resultMatrix = b34.array() + s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix addition a .+ b: "; - resultValue = eval("a .+ b"); - resultMatrix = a34 + b34; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix/scalar addition a .+ s: "; - resultValue = eval("a .+ s"); - resultMatrix = a34.array() + s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test scalar/matrix addition s .+ b: "; - resultValue = eval("s .+ b"); - resultMatrix = b34.array() + s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix subtraction a - b: "; - resultValue = eval("a - b"); - resultMatrix = a34 - b34; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix/scalar subtraction a - s: "; - resultValue = eval("a - s"); - resultMatrix = a34.array() - s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test scalar/matrix subtraction s - b: "; - resultValue = eval("s - b"); - resultMatrix = (-b34.array()) + s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix subtraction a .- b: "; - resultValue = eval("a .- b"); - resultMatrix = a34 - b34; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix/scalar subtraction a .- s: "; - resultValue = eval("a .- s"); - resultMatrix = a34.array() - s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test scalar/matrix subtraction s .- b: "; - resultValue = eval("s .- b"); - resultMatrix = (-b34.array()) + s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix negation -a: "; - resultValue = eval("-a"); - resultMatrix = -a34; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test scalar negation -s: "; - resultValue = eval("-s"); - resultMatrix.setConstant(1, 1, -s); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix coefficient-wise multiplication a .* b: "; - resultValue = eval("a .* b"); - resultMatrix = a34.array() * b34.array(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix/scalar coefficient-wise multiplication a * s: "; - resultValue = eval("a * s"); - resultMatrix = a34.array() * s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test scalar/matrix coefficient-wise multiplication s * b: "; - resultValue = eval("s * b"); - resultMatrix = b34.array() * s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix/scalar coefficient-wise multiplication a .* s: "; - resultValue = eval("a .* s"); - resultMatrix = a34.array() * s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test scalar/matrix coefficient-wise multiplication s .* b: "; - resultValue = eval("s .* b"); - resultMatrix = b34.array() * s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix coefficient-wise division a ./ b: "; - resultValue = eval("a ./ b"); - resultMatrix = a34.array() / b34.array(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix/scalar coefficient-wise division a / s: "; - resultValue = eval("a / s"); - resultMatrix = a34.array() / s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test scalar/matrix coefficient-wise division s / b: "; - resultValue = eval("s / b"); - resultMatrix = Derived::Constant(b34.rows(), b34.cols(), s).array() / b34.array(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix/scalar coefficient-wise division a ./ s: "; - resultValue = eval("a ./ s"); - resultMatrix = a34.array() / s; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test scalar/matrix coefficient-wise division s ./ b: "; - resultValue = eval("s ./ b"); - resultMatrix = Derived::Constant(b34.rows(), b34.cols(), s).array() / b34.array(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix coefficient-wise power a .^ b: "; - resultValue = eval("abs(a) .^ b"); - resultMatrix = a34; - for(Index i = 0; i < a34.size(); ++i) - resultMatrix(i) = pow(std::abs(a34(i)), b34(i)); - // std::cout << std::endl; - // std::cout << "a=" << std::endl << a34 << std::endl << std::endl; - // std::cout << "b=" << std::endl << b34 << std::endl << std::endl; - // std::cout << "val=" << std::endl << resultValue.matrix() << std::endl << std::endl; - // std::cout << "mat=" << std::endl << resultMatrix << std::endl << std::endl; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix/scalar coefficient-wise power a ^ s: "; - resultValue = eval("abs(a) ^ s"); - resultMatrix = a34; - for(Index i = 0; i < a34.size(); ++i) - resultMatrix(i) = pow(std::abs(a34(i)), s); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test scalar/matrix coefficient-wise power s ^ b: "; - resultValue = eval("s ^ b"); - resultMatrix = b34; - for(Index i = 0; i < b34.size(); ++i) - resultMatrix(i) = pow(s, b34(i)); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix/scalar coefficient-wise power a .^ s: "; - resultValue = eval("abs(a) .^ s"); - resultMatrix = a34; - for(Index i = 0; i < a34.size(); ++i) - resultMatrix(i) = pow(std::abs(a34(i)), s); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test scalar/matrix coefficient-wise power s .^ b: "; - resultValue = eval("s .^ b"); - resultMatrix = b34; - for(Index i = 0; i < b34.size(); ++i) - resultMatrix(i) = pow(s, b34(i)); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix multiplication a * b: "; - resultValue = eval("a * c"); - resultMatrix = a34 * c43; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test grouped subexpression (a + b) * c: "; - resultValue = eval("(a + b) * c"); - resultMatrix = (a34 + b34) * c43; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test nested groups ((a + (b * 3 + 1)) * c).^2: "; - resultValue = eval("((a + (b * 3 + 1)) * c).^2"); - resultMatrix = ((a34.array() + (b34.array() * 3 + 1)).matrix() * c43).array().pow(2); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - //////////////////////////////////////// - std::cout << std::endl << "Testing coefficient and submatrix block access..." << std::endl << std::endl; - //////////////////////////////////////// - - std::cout << "Test matrix coefficient access a(i,j): "; - resultValue = eval("a(1,2)"); - resultMatrix.setConstant(1, 1, a34(1,2)); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test submatrix block access a(i:p,j:q): "; - resultValue = eval("a(1:2,2:3)"); - resultMatrix = a34.block(1, 2, 2, 2); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test submatrix block access using 'end' and ':' identifiers a(i:end,:): "; - resultValue = eval("a(1:end,:)"); - resultMatrix = a34.block(1, 0, a34.rows() - 1, a34.cols()); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test submatrix block access using subexpressions: "; - resultValue = eval("a(2-1:2-1,0+1:3-1)"); - resultMatrix = a34.block(1, 1, 1, 2); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test submatrix block access using subexpressions with 'end' keyword: "; - resultValue = eval("a(2-1:end-1,0+1:end-1)"); - resultMatrix = a34.block(1, 1, a34.rows() - 2, a34.cols() - 2); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test vector coefficient access v(i): "; - resultValue = eval("v(5)"); - resultMatrix.setConstant(1, 1, v(5)); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test subvector segment access v(i:j): "; - resultValue = eval("v(3:6)"); - resultMatrix = v.block(0, 3, 1, 4); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test subvector segment access using 'end' identifier v(i:end): "; - resultValue = eval("v(5:end)"); - resultMatrix = v.block(0, 5, 1, v.cols() - 5); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test subvector segment access using ':' identifier v(:): "; - resultValue = eval("v(:)"); - resultMatrix = v; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test subvector segment access using subexpressions: "; - resultValue = eval("v(3-1:5+2)"); - resultMatrix = v.block(0, 2, 1, 6); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test subvector segment access using subexpressions with 'end' keyword: "; - resultValue = eval("v((end-8)*2:end-3)"); - resultMatrix = v.block(0, 2, 1, 5); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - //////////////////////////////////////// - std::cout << std::endl << "Testing vector/matrix expressions..." << std::endl << std::endl; - //////////////////////////////////////// - - std::cout << "Test numeric range expression [i:j]: "; - resultValue = eval("[2:5]"); - resultMatrix.resize(1, 4); - resultMatrix << 2, 3, 4, 5; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test numeric range expression [i:s:j]: "; - resultValue = eval("[2:2:10]"); - resultMatrix.resize(1, 5); - resultMatrix << 2, 4, 6, 8, 10; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test numeric range with subexpressions: "; - resultValue = eval("[6-2:5*2-3]"); - std::cout << "val=" << std::endl << resultValue.matrix() << std::endl << std::endl; - resultMatrix.resize(1, 4); - resultMatrix << 4, 5, 6, 7; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix expression [[1, 2]; [3, 4]]: "; - resultValue = eval("[[1, 2]; [3, 4]]"); - resultMatrix.resize(2, 2); - resultMatrix << 1, 2, 3, 4; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test matrix expression [ 1, 2, 3; 4:6 ]: "; - resultValue = eval("[1, 2, 3; 4:6]"); - resultMatrix.resize(2, 3); - resultMatrix << 1, 2, 3, 4, 5, 6; - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - //////////////////////////////////////// - std::cout << std::endl << "Testing coefficient-wise functions..." << std::endl << std::endl; - //////////////////////////////////////// - - std::cout << "Test coefficient-wise abs(a): "; - resultValue = eval("abs(a)"); - resultMatrix.resize(3, 4); - resultMatrix.real() = a34.array().abs(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test coefficient-wise sqrt(a): "; - resultValue = eval("sqrt(abs(a))"); - resultMatrix.real() = a34.array().abs().sqrt(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test coefficient-wise exp(a): "; - resultValue = eval("exp(abs(a) + 0.001)"); - resultMatrix.real() = (a34.array().abs() + 0.001).exp(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test coefficient-wise log(a): "; - resultValue = eval("log(abs(a) + 0.001)"); - resultMatrix.real() = (a34.array().abs() + 0.001).log(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test coefficient-wise log10(a): "; - resultValue = eval("log10(abs(a) + 0.001)"); - resultMatrix.real() = (a34.array().abs() + 0.001).log() * (1.0 / log(10)); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test coefficient-wise sin(a): "; - resultValue = eval("sin(a)"); - resultMatrix = a34.array().sin(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test coefficient-wise cos(a): "; - resultValue = eval("cos(a)"); - resultMatrix = a34.array().cos(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test coefficient-wise tan(a): "; - resultValue = eval("tan(a)"); - resultMatrix = a34.array().tan(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test coefficient-wise asin(a): "; - resultValue = eval("asin(a)"); - resultMatrix = a34.array().asin(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test coefficient-wise acos(a): "; - resultValue = eval("acos(a)"); - resultMatrix = a34.array().acos(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - //////////////////////////////////////// - std::cout << std::endl << "Testing matrix reduction functions..." << std::endl << std::endl; - //////////////////////////////////////// - - std::cout << "Test trace(a): "; - resultValue = eval("trace(a)"); - resultMatrix.setConstant(1, 1, a34.trace()); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test norm(a): "; - resultValue = eval("norm(a)"); - resultMatrix.setConstant(1, 1, a34.norm()); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test size(a, 0): "; - resultValue = eval("size(a, 0)"); - resultMatrix.setConstant(1, 1, a34.rows()); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test size(a, 1): "; - resultValue = eval("size(a, 1)"); - resultMatrix.setConstant(1, 1, a34.cols()); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - test_w_lt(numFails, s, a34, b34, c43, v, has_operator_lt()); - - std::cout << "Test mean(a): "; - resultValue = eval("mean(a)"); - resultMatrix.setConstant(1, 1, a34.mean()); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test mean(a, 0): "; - resultValue = eval("mean(a, 0)"); - resultMatrix = a34.colwise().mean(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test mean(a, 1): "; - resultValue = eval("mean(a, 1)"); - resultMatrix = a34.rowwise().mean(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test sum(a): "; - resultValue = eval("sum(a)"); - resultMatrix.setConstant(1, 1, a34.sum()); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test sum(a, 0): "; - resultValue = eval("sum(a, 0)"); - resultMatrix = a34.colwise().sum(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test sum(a, 1): "; - resultValue = eval("sum(a, 1)"); - resultMatrix = a34.rowwise().sum(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test prod(a): "; - resultValue = eval("prod(a)"); - resultMatrix.setConstant(1, 1, a34.prod()); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test prod(a, 0): "; - resultValue = eval("prod(a, 0)"); - resultMatrix = a34.colwise().prod(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test prod(a, 1): "; - resultValue = eval("prod(a, 1)"); - resultMatrix = a34.rowwise().prod(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - //////////////////////////////////////// - std::cout << std::endl << "Testing matrix functions..." << std::endl << std::endl; - //////////////////////////////////////// - - std::cout << "Test transpose(a): "; - resultValue = eval("transpose(a)"); - resultMatrix = a34.transpose(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test conjugate(a): "; - resultValue = eval("conjugate(a)"); - resultMatrix = a34.conjugate(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test adjoint(a): "; - resultValue = eval("adjoint(a)"); - resultMatrix = a34.adjoint(); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - //////////////////////////////////////// - std::cout << std::endl << "Testing matrix initializers..." << std::endl << std::endl; - //////////////////////////////////////// - - std::cout << "Test zeros(5): "; - resultValue = eval("zeros(5)"); - resultMatrix = Derived::Zero(5, 5); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test ones(5): "; - resultValue = eval("ones(5)"); - resultMatrix = Derived::Ones(5, 5); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test eye(5): "; - resultValue = eval("eye(5)"); - resultMatrix = Derived::Identity(5, 5); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - try { - std::cout << "Test zeros(5.2): "; - resultValue = eval("zeros(5.2)"); // <-- Should NOT succeed!!! - std::cout << "FAIL" << std::endl; ++numFails; - } catch(std::runtime_error &err) { - std::cout << err.what() << std::endl; - std::cout << "Exception caught, so we're OK" << std::endl; - } - - try { - std::cout << "Test eye(-3): "; - resultValue = eval("eye(-3)"); // <-- Should NOT succeed!!! - std::cout << "FAIL" << std::endl; ++numFails; - } catch(std::runtime_error &err) { - std::cout << err.what() << std::endl; - std::cout << "Exception caught, so we're OK" << std::endl; - } - - std::cout << "Test zeros(4,7): "; - resultValue = eval("zeros(4,7)"); - resultMatrix = Derived::Zero(4, 7); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test ones(4,7): "; - resultValue = eval("ones(4,7)"); - resultMatrix = Derived::Ones(4, 7); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test eye(4,7): "; - resultValue = eval("eye(4,7)"); - resultMatrix = Derived::Identity(4, 7); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - //////////////////////////////////////// - std::cout << std::endl << "Testing variable assignment..." << std::endl << std::endl; - //////////////////////////////////////// - - std::cout << "Test assigning to a variable with the same dimensions a = b: "; - eval("a = b"); - if(a34.isApprox(b34)) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test assigning to a variable with different dimensions a = c: "; - eval("a = c"); - if(var("a").matrix().isApprox(c43) && a34.isApprox(b34)) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - var("a").setShared(a34); - - std::cout << "Test creating a new variable x = [1,2;3,4]: "; - resultValue = eval("x = [1,2;3,4]"); - if(var("x").matrix().isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test assigning to a variable coefficient a(i,j) = s: "; - eval("a(1, 2) = s"); - if(a34(1, 2) == s) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test assigning to a variable submatrix block a(0:1,1:2) = x: "; - resultValue = eval("a(0:1,1:2) = x"); - if(a34.block(0,1,2,2).isApprox(var("x").matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - try { - std::cout << "Test bad function call: "; - resultValue = eval("foobar(-3)"); // <-- Should NOT succeed!!! - std::cout << "FAIL" << std::endl; ++numFails; - } catch(std::runtime_error &err) { - std::cout << err.what() << std::endl; - std::cout << "Exception caught, so we're OK" << std::endl; - } - //////////////////////////////////////// - std::cout << std::endl << "Testing fp parsing..." << std::endl << std::endl; - //////////////////////////////////////// - - std::cout << "Test assigning 1.2e-3: "; - resultValue = eval("s = 1.2e-3"); - resultMatrix.setConstant(1, 1, typename Derived::Scalar(1.2e-3)); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test assigning 1.e3: "; - resultValue = eval("s = 1.e3"); - resultMatrix.setConstant(1, 1, typename Derived::Scalar(1000)); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << "Test assigning 12.34e05: "; - resultValue = eval("s = 12.34e05"); - resultMatrix.setConstant(1, 1, typename Derived::Scalar(123.4e4)); - if(resultMatrix.isApprox(resultValue.matrix())) std::cout << "OK" << std::endl; - else { std::cout << "FAIL" << std::endl; ++numFails; } - - std::cout << std::endl; - if(numFails == 0) - std::cout << "Successfully completed unit test for EigenLab with no failures." << std::endl; - else - std::cout << "Completed unit test for EigenLab with " << numFails << " failures (see above)." << std::endl; - std::cout << std::endl; - return numFails; - } -#endif // #ifdef DEBUG - -} // namespace EigenLab - -#endif // #ifndef EigenLab_H diff --git a/grid_map_filters/include/EigenLab/EigenLab.hpp b/grid_map_filters/include/EigenLab/EigenLab.hpp new file mode 100644 index 000000000..63bcec665 --- /dev/null +++ b/grid_map_filters/include/EigenLab/EigenLab.hpp @@ -0,0 +1,2848 @@ +// --*- Mode: C++; c-basic-offset:4; indent-tabs-mode:t; tab-width:4 -*-- +// EigenLab +// Version: 1.0.0 +// Author: Dr. Marcel Paz Goldschen-Ohm +// Email: marcel.goldschen@gmail.com +// Copyright (c) 2015 by Dr. Marcel Paz Goldschen-Ohm. +// Licence: MIT +//---------------------------------------- + +#ifndef EIGENLAB__EIGENLAB_HPP_ +#define EIGENLAB__EIGENLAB_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +// Define both DEBUG and EIGENLAB_DEBUG for step-by-step equation parsing printouts. +#ifndef DEBUG +// #define DEBUG +#endif + +#ifndef EIGENLAB_DEBUG +// #define EIGENLAB_DEBUG +#endif + +#ifdef DEBUG +# include +#endif + +namespace EigenLab +{ +//---------------------------------------- +// A wrapper for a matrix whose data is either stored locally or shared. +// +// Template typename Derived can be any dynamically sized matrix type supported by Eigen. +// +// !!! matrix() promises to ALWAYS return a map to the matrix data whether it's +// stored locally or externally in some shared memory. +// +// !!! local() provides direct access to the local data, but this data is +// ONLY valid when isLocal() is true. In most cases, you're best off +// accessing the matrix data via matrix() instead. +//---------------------------------------- +template +class Value +{ +private: + // Local matrix data. + Derived mLocal; + + // Map to shared matrix data (map points to mLocal if the data is local). + // !!! This map promises to ALWAYS point to the matrix data whether it's + // stored locally in mLocal or externally in some shared memory. + Eigen::Map mShared; + + // Flag indicating whether the local data is being used. + bool mIsLocal; + +public: + // Access mapped data (whether its local or shared). + // !!! matrix() promises to ALWAYS return a map to the matrix data whether it's + // stored locally in mLocal or externally in some shared memory. + inline Eigen::Map & matrix() + { + return mShared; + } + inline const Eigen::Map & matrix() const {return mShared;} + + // Access local data. + // !!! WARNING! This data is ONLY valid if isLocal() is true. + // !!! WARNING! If you change the local data via this method, + // you MUST call mapLocal() immediately afterwards. + // In most cases, you're best off accessing the matrix data via matrix() instead. + inline Derived & local() {return mLocal;} + inline const Derived & local() const {return mLocal;} + + // Is mapped data local? + inline bool isLocal() const {return mIsLocal;} + + // Set mapped data to point to local data. + inline void mapLocal() + { + new(&mShared) Eigen::Map(mLocal.data(), mLocal.rows(), mLocal.cols()); + mIsLocal = true; + } + + // Copy shared data to local data (if needed). + inline void copySharedToLocal() {if (!isLocal()) {mLocal = mShared; mapLocal();}} + + // Set local data. + Value() + : mLocal(1, 1), mShared(mLocal.data(), mLocal.rows(), mLocal.cols()), mIsLocal(true) + { + } + explicit Value(const typename Derived::Scalar s) + : mLocal(Derived::Constant(1, 1, s)), mShared( + mLocal.data(), mLocal.rows(), mLocal.cols()), mIsLocal(true) + { + } + explicit Value(const Derived & mat) + : mLocal(mat), mShared(mLocal.data(), mLocal.rows(), mLocal.cols()), + mIsLocal(true) + { + } + inline void setLocal(const typename Derived::Scalar s) + { + mLocal = Derived::Constant(1, 1, s); mapLocal(); + } + inline void setLocal(const Eigen::MatrixBase & mat) {mLocal = mat; mapLocal();} + inline void setLocal(const Value & val) {mLocal = val.matrix(); mapLocal();} + inline void setLocal(const typename Derived::Scalar * data, size_t rows = 1, size_t cols = 1) + { + setShared(data, rows, cols); copySharedToLocal(); + } + inline Value & operator=(const typename Derived::Scalar s) {setLocal(s); return *this;} + inline Value & operator=(const Derived & mat) {setLocal(mat); return *this;} + + // Set shared data. + explicit Value(const typename Derived::Scalar * data, size_t rows = 1, size_t cols = 1) + : mShared( + const_cast(data), rows, cols), mIsLocal(false) + { + } + inline void setShared(const typename Derived::Scalar * data, size_t rows = 1, size_t cols = 1) + { + new(&mShared) Eigen::Map(const_cast(data), rows, cols); + mIsLocal = false; + } + inline void setShared(const Derived & mat) {setShared(mat.data(), mat.rows(), mat.cols());} + inline void setShared(const Value & val) + { + setShared(val.matrix().data(), val.matrix().rows(), val.matrix().cols()); + } + + // Set to local or shared data dependent on whether val + // maps its own local data or some other shared data. + Value(const Value & val) + : mLocal(1, 1), mShared(mLocal.data(), mLocal.rows(), mLocal.cols()) + { + (*this) = val; + } + inline Value & operator=(const Value & val) + { + if (val.isLocal()) { + setLocal(val); + } else { + setShared(val); + } return *this; + } +}; +typedef Value ValueXd; +typedef Value ValueXf; +typedef Value ValueXi; + +// check if a class has a comparison operator (ie. std::complex does not) +template +struct has_operator_lt_impl +{ + template + // deepcode ignore CopyPasteError: We will not change third party code yet. + static auto test(U *)->decltype(std::declval() < std::declval()); + template + static auto test(...)->std::false_type; + using type = typename std::is_same(0))>::type; +}; +template +struct has_operator_lt : has_operator_lt_impl::type {}; + +//---------------------------------------- +// Equation parser. +// +// Template typename Derived can be any dynamically sized matrix type supported by Eigen. +//---------------------------------------- +template +class Parser +{ +public: + // A map to hold named values. + typedef std::map> ValueMap; + +private: + // Variables are stored in a map of named values. + ValueMap mVariables; + + // Operator symbols and function names used by the parser. + std::string mOperators1, mOperators2; + std::vector mFunctions; + + // Expressions are parsed by first splitting them into chunks. + struct Chunk + { + std::string field; + int type; + Value value; + int row0, col0, rows, cols; + Chunk( + const std::string & str = "", int t = -1, + const Value & val = Value()) + : field(str), type(t), value(val), + row0(-1), col0(-1), rows(-1), cols(-1) + { + } + }; + enum ChunkType { VALUE = 0, VARIABLE, OPERATOR, FUNCTION }; + typedef std::vector ChunkArray; + typedef typename Derived::Index Index; + bool mCacheChunkedExpressions; + std::map mCachedChunkedExpressions; + +public: + // Access to named variables. + // !!! WARNING! var(name) will create the variable name if it does not already exist. + inline ValueMap & vars() {return mVariables;} + inline Value & var(const std::string & name) + { + return mVariables[name]; + } + + // Check if a variable exists. + inline bool hasVar(const std::string & name) {return isVariable(name);} + + // Delete a variable. + inline void clearVar(const std::string & name) + { + typename ValueMap::iterator it = mVariables.find(name); if (it != mVariables.end()) { + mVariables.erase(it); + } + } + + // Expression chunk caching. + inline bool cacheExpressions() const {return mCacheChunkedExpressions;} + inline void setCacheExpressions(bool b) {mCacheChunkedExpressions = b;} + inline void clearCachedExpressions() {mCachedChunkedExpressions.clear();} + + Parser(); + ~Parser() + { + clearCachedExpressions(); + } + + // Evaluate an expression and return the result in a value wrapper. + Value eval(const std::string & expression); + +private: + void splitEquationIntoChunks( + const std::string & expression, ChunkArray & chunks, + std::string & code); + std::string::const_iterator findClosingBracket( + const std::string & str, + const std::string::const_iterator openingBracket, + const char closingBracket) const; + std::vector splitArguments(const std::string & str, const char delimeter) const; + void evalIndexRange(const std::string & str, int * first, int * last, int numIndices); + void evalMatrixExpression(const std::string & str, Value & mat); + void evalFunction( + const std::string & name, std::vector & args, + Value & result); + bool evalFunction_1_lt( + const std::string & name, Value & arg, + Value & result, std::false_type); + bool evalFunction_1_lt( + const std::string & name, Value & arg, + Value & result, std::true_type); + bool evalFunction_2_lt( + const std::string & name, Value & arg0, + Value & arg1, Value & result, std::false_type); + bool evalFunction_2_lt( + const std::string & name, Value & arg0, + Value & arg1, Value & result, std::true_type); + + void evalNumericRange(const std::string & str, Value & mat); + inline bool isVariable(const std::string & name) const {return mVariables.count(name) > 0;} + inline bool isOperator(const char c) const + { + return std::find(mOperators1.begin(), mOperators1.end(), c) != mOperators1.end(); + } + bool isOperator(const std::string & str) const; + inline bool isFunction(const std::string & str) const + { + return std::find(mFunctions.begin(), mFunctions.end(), str) != mFunctions.end(); + } + void evalIndices(ChunkArray & chunks); + void evalNegations(ChunkArray & chunks); + void evalPowers(ChunkArray & chunks); + void evalMultiplication(ChunkArray & chunks); + void evalAddition(ChunkArray & chunks); + void evalAssignment(ChunkArray & chunks); +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + void printChunks( + ChunkArray & chunks, size_t maxRows = 2, size_t maxCols = 2, + int precision = 0); + void printVars(size_t maxRows = 2, size_t maxCols = 2, int precision = 0); + std::string textRepresentation( + Value & val, size_t maxRows = 2, size_t maxCols = 2, + int precision = 0); +# endif +#endif + +public: + static std::string trim(const std::string & str); + static std::vector split(const std::string & str, const char delimeter); + template + static bool isNumber(const std::string & str, T * num = 0); + template + static T stringToNumber(const std::string & str); + template + static std::string numberToString(T num, int precision = 0); +#ifdef DEBUG + void test_w_lt( + size_t & numFails, typename Derived::Scalar & s, Derived & a34, Derived & b34, + Derived & c43, Derived & v, std::true_type); + void test_w_lt( + size_t & numFails, typename Derived::Scalar & s, Derived & a34, Derived & b34, + Derived & c43, Derived & v, std::false_type); + size_t test(); +#endif +}; +typedef Parser ParserXd; +typedef Parser ParserXf; +typedef Parser ParserXi; + +//---------------------------------------- +// Function definitions. +//---------------------------------------- +template +Parser::Parser() +: mOperators1("+-*/^()[]="), + mOperators2(".+.-.*./.^"), + mCacheChunkedExpressions(false) +{ + // Coefficient-wise operations. + mFunctions.push_back("abs"); + mFunctions.push_back("sqrt"); + mFunctions.push_back("square"); + mFunctions.push_back("exp"); + mFunctions.push_back("log"); + mFunctions.push_back("log10"); + mFunctions.push_back("sin"); + mFunctions.push_back("cos"); + mFunctions.push_back("tan"); + mFunctions.push_back("asin"); + mFunctions.push_back("acos"); + + // Matrix reduction operations. + mFunctions.push_back("trace"); + mFunctions.push_back("norm"); + mFunctions.push_back("size"); + if (has_operator_lt::value) { + mFunctions.push_back("min"); + mFunctions.push_back("minOfFinites"); + mFunctions.push_back("max"); + mFunctions.push_back("maxOfFinites"); + mFunctions.push_back("absmax"); + mFunctions.push_back("cwiseMin"); + mFunctions.push_back("cwiseMax"); + } + mFunctions.push_back("mean"); + mFunctions.push_back("meanOfFinites"); + mFunctions.push_back("sum"); + mFunctions.push_back("sumOfFinites"); + mFunctions.push_back("prod"); + mFunctions.push_back("numberOfFinites"); + + // Matrix operations. + mFunctions.push_back("transpose"); + mFunctions.push_back("conjugate"); + mFunctions.push_back("adjoint"); + + // Matrix initializers. + mFunctions.push_back("zeros"); + mFunctions.push_back("ones"); + mFunctions.push_back("eye"); +} + +template +Value Parser::eval(const std::string & expression) +{ +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + std::cout << "---" << std::endl; + std::cout << "EXPRESSION: " << expression << std::endl; +# endif +#endif + ChunkArray chunks; + std::string code; + splitEquationIntoChunks(trim(expression), chunks, code); + evalIndices(chunks); + evalNegations(chunks); + evalPowers(chunks); + evalMultiplication(chunks); + evalAddition(chunks); + evalAssignment(chunks); + if (chunks.size() != 1) { + throw std::runtime_error( + "Failed to reduce expression '" + expression + "' to a single value."); + } +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + std::cout << "---" << std::endl; +# endif +#endif + if (chunks[0].type == VARIABLE) { + if (!isVariable(chunks[0].field)) { + throw std::runtime_error("Unknown variable '" + chunks[0].field + "'."); + } + return mVariables[chunks[0].field]; + } + return chunks[0].value; +} + +template +void Parser::splitEquationIntoChunks( + const std::string & expression, + ChunkArray & chunks, std::string & code) +{ + if (cacheExpressions()) { + if (mCachedChunkedExpressions.count(expression) > 0) { + chunks = mCachedChunkedExpressions.at(expression); +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + std::cout << "CACHED CHUNKS: "; printChunks(chunks); std::cout << std::endl; +# endif +#endif + return; + } + } + + for (std::string::const_iterator it = expression.begin(); it != expression.end(); ) { + int prevType = (chunks.size() ? chunks.back().type : -1); + char ci = (*it); + if (isspace(ci)) { + // Ignore whitespace. + it++; + } else if (ci == '(' && (prevType == VALUE || prevType == VARIABLE)) { + // Index group. + std::string::const_iterator jt = findClosingBracket(expression, it, ')'); + if (jt == expression.end()) { + throw std::runtime_error("Missing closing bracket for '" + std::string(it, jt) + "'."); + } + std::string field = std::string(it + 1, jt); // Outer brackets stripped. + if (prevType == VARIABLE) { + if (!isVariable(chunks.back().field)) { + throw std::runtime_error("Unknown variable '" + chunks.back().field + "'."); + } + chunks.back().value.setShared(var(chunks.back().field)); + } + int first, last; + int rows = static_cast(chunks.back().value.matrix().rows()); + int cols = static_cast(chunks.back().value.matrix().cols()); + std::vector args = splitArguments(field, ','); + if (args.size() == 1) { + if (cols == 1) { + evalIndexRange(args[0], &first, &last, rows); + chunks.back().row0 = first; + chunks.back().col0 = 0; + // (last == -1 ? int(chunks.back().value.matrix().rows()) : last + 1) - first; + chunks.back().rows = last + 1 - first; + chunks.back().cols = 1; + } else if (rows == 1) { + evalIndexRange(args[0], &first, &last, cols); + chunks.back().row0 = 0; + chunks.back().col0 = first; + chunks.back().rows = 1; + // (last == -1 ? int(chunks.back().value.matrix().cols()) : last + 1) - first; + chunks.back().cols = last + 1 - first; + } else { + throw std::runtime_error( + "Missing row or column indices for '(" + chunks.back().field + "(" + field + + ")'."); + } + } else if (args.size() == 2) { + evalIndexRange(args[0], &first, &last, rows); + chunks.back().row0 = first; + // (last == -1 ? int(chunks.back().value.matrix().rows()) : last + 1) - first; + chunks.back().rows = last + 1 - first; + evalIndexRange(args[1], &first, &last, cols); + chunks.back().col0 = first; + // (last == -1 ? int(chunks.back().value.matrix().cols()) : last + 1) - first; + chunks.back().cols = last + 1 - first; + } else { + throw std::runtime_error( + "Invalid index expression '" + chunks.back().field + "(" + field + ")'."); + } + code += "i"; + it = jt + 1; + } else if (ci == '(' && prevType == FUNCTION) { + // Function argument group. + std::string::const_iterator jt = findClosingBracket(expression, it, ')'); + if (jt == expression.end()) { + throw std::runtime_error("Missing closing bracket for '" + std::string(it, jt) + "'."); + } + std::string field = std::string(it + 1, jt); // Outer brackets stripped. + std::vector args = splitArguments(field, ','); + evalFunction(chunks.back().field, args, chunks.back().value); + chunks.back().field += "(" + field + ")"; + chunks.back().type = VALUE; + code += (chunks.back().value.matrix().size() == 1 ? "n" : "M"); + it = jt + 1; + } else if (ci == '(') { + // Recursively evaluate group to a single value. + std::string::const_iterator jt = findClosingBracket(expression, it, ')'); + if (jt == expression.end()) { + throw std::runtime_error("Missing closing bracket for '" + std::string(it, jt) + "'."); + } + std::string field = std::string(it + 1, jt); // Outer brackets stripped. + chunks.push_back(Chunk(field, prevType = VALUE, eval(field))); + code += (chunks.back().value.matrix().size() == 1 ? "n" : "M"); + it = jt + 1; + } else if (ci == '[') { + // Evaluate matrix. + if (prevType == VALUE || prevType == VARIABLE) { + throw std::runtime_error( + "Invalid operation '" + chunks.back().field + std::string(1, ci) + "'."); + } + std::string::const_iterator jt = findClosingBracket(expression, it, ']'); + if (jt == expression.end()) { + throw std::runtime_error("Missing closing bracket for '" + std::string(it, jt) + "'."); + } + std::string field = std::string(it + 1, jt); // Outer brackets stripped. + chunks.push_back(Chunk("[" + field + "]", prevType = VALUE)); + evalMatrixExpression(field, chunks.back().value); + code += (chunks.back().value.matrix().size() == 1 ? "n" : "M"); + it = jt + 1; + } else if (it + 1 != expression.end() && isOperator(std::string(it, it + 2))) { + // Double character operator. + std::string field = std::string(it, it + 2); + chunks.push_back(Chunk(field, prevType = OPERATOR)); + code += field; + it += 2; + } else if (isOperator(ci)) { + // Single character operator. + std::string field = std::string(1, ci); + chunks.push_back(Chunk(field, prevType = OPERATOR)); + code += field; + it++; + } else { + // Non-operator: value range, number, function, or variable name. + std::string::const_iterator jt = it + 1; + // accept fp-strings, ie [+-] + unsigned char state = 1; + for (std::string::const_iterator kt = it; state && kt != expression.end(); kt++) { + unsigned char token; + if (*kt == ' ') {token = 0;} else if (*kt == '+' || *kt == '-') { + token = 1; + } else if (isdigit(*kt)) {token = 2;} else if (*kt == '.') { + token = 3; + } else if (*kt == 'e' || *kt == 'E') {token = 4;} else {break;} + static const char nextstate[9][5] = {{0}, + {1, 2, 3, 4, 0}, + {0, 0, 3, 4, 0}, + {0, 0, 3, 5, 6}, + {0, 0, 5, 0, 0}, + {0, 0, 5, 0, 6}, + {0, 7, 8, 0, 0}, + {0, 0, 8, 0, 0}, + {0, 0, 8, 0, 0}}; + // WARN("state=" << (int)state << " token(" << *kt << ")=" << (int)token + // << " nextstate = " << (int)nextstate[state][token] << "\n"); + state = nextstate[state][token]; + if (state == 8) {jt = kt;} + } + for (; jt != expression.end(); jt++) { + if (isOperator(*jt) || + (jt + 1 != expression.end() && isOperator(std::string(jt, jt + 2)))) + { + break; + } + } + std::string field = trim(std::string(it, jt)); + if (prevType == VALUE || prevType == VARIABLE) { + throw std::runtime_error("Invalid operation '" + chunks.back().field + field + "'."); + } + double num; + if (field.find(":") != std::string::npos) { + // Numeric range. + chunks.push_back(Chunk(field, prevType = VALUE)); + evalNumericRange(field, chunks.back().value); + code += (chunks.back().value.matrix().size() == 1 ? "n" : "M"); + } else if (isNumber(field, &num)) { + // Number. + chunks.push_back(Chunk(field, prevType = VALUE, Value(num))); + code += "n"; + } else if (isVariable(field)) { + // Local variable. + chunks.push_back(Chunk(field, prevType = VARIABLE)); + code += (mVariables[field].matrix().size() == 1 ? "vn" : "vM"); + } else if (isFunction(field)) { + // Function. + chunks.push_back(Chunk(field, prevType = FUNCTION)); + } else { + // New undefined variable. + chunks.push_back(Chunk(field, prevType = VARIABLE)); + code += "vU"; + } + it = jt; + } + } // it +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + std::cout << "CHUNKS: "; printChunks(chunks); std::cout << std::endl; + std::cout << "CODE: " << code << std::endl; +# endif +#endif + if (cacheExpressions()) { + mCachedChunkedExpressions[expression] = chunks; + } +} + +template +std::string::const_iterator Parser::findClosingBracket( + const std::string & str, + const std::string::const_iterator openingBracket, + const char closingBracket) const +{ + int depth = 1; + std::string::const_iterator it = openingBracket + 1; + for (; it != str.end(); it++) { + if ((*it) == (*openingBracket)) {depth++;} else if ((*it) == closingBracket) {depth--;} + if (depth == 0) {return it;} + } + return str.end(); +} + +template +std::vector Parser::splitArguments( + const std::string & str, + const char delimeter) const +{ + std::vector args; + std::string::const_iterator i0 = str.begin(); + for (std::string::const_iterator it = str.begin(); it != str.end(); it++) { + if ((*it) == '(') {it = findClosingBracket(str, it, ')');} else if ((*it) == '[') { + it = findClosingBracket(str, it, ']'); + } else if ((*it) == delimeter) { + args.push_back(trim(std::string(i0, it))); + i0 = it + 1; + } + } + args.push_back(std::string(i0, str.end())); + return args; +} + +template +void Parser::evalIndexRange( + const std::string & str, int * first, int * last, + int numIndices) +{ + if (str.empty()) { + throw std::runtime_error("Empty index range."); + } + ValueXi valuei; + ParserXi parseri; + size_t pos; + for (std::string::const_iterator it = str.begin(); it != str.end(); it++) { + if ((*it) == ':') { + std::string firstStr = trim(std::string(str.begin(), it)); + std::string lastStr = trim(std::string(it + 1, str.end())); + if (firstStr.empty() && lastStr.empty()) { + (*first) = 0; + (*last) = numIndices - 1; + return; + } + if (firstStr.empty() || lastStr.empty()) { + throw std::runtime_error("Missing indices for '" + str + "'."); + } + + pos = firstStr.find("end"); + if (pos != std::string::npos) { + firstStr = + firstStr.substr(0, pos) + numberToString(numIndices - 1) + firstStr.substr( + pos + 3); + } + pos = lastStr.find("end"); + if (pos != std::string::npos) { + lastStr = + lastStr.substr(0, pos) + numberToString(numIndices - 1) + lastStr.substr( + pos + 3); + } + + valuei = parseri.eval(firstStr); + if (valuei.matrix().size() != 1) { + throw std::runtime_error("Invalid indices '" + str + "'."); + } + (*first) = valuei.matrix()(0, 0); + + valuei = parseri.eval(lastStr); + if (valuei.matrix().size() != 1) { + throw std::runtime_error("Invalid indices '" + str + "'."); + } + (*last) = valuei.matrix()(0, 0); + + return; + } + } + std::string firstStr = str; + + pos = firstStr.find("end"); + if (pos != std::string::npos) { + firstStr = + firstStr.substr( + 0, + pos) + numberToString(numIndices - 1) + firstStr.substr(pos + 3); + } + + valuei = parseri.eval(firstStr); + if (valuei.matrix().size() != 1) { + throw std::runtime_error("Invalid index '" + str + "'."); + } + (*first) = valuei.matrix()(0, 0); + (*last) = (*first); +} + +template +void Parser::evalMatrixExpression(const std::string & str, Value & mat) +{ + // !!! Expression may NOT include outer brackets, although brackets for individual rows are OK. + std::vector rows = splitArguments(str, ';'); + std::vector> temp; + Value submatrix; + size_t row0 = 0, col0 = 0, nrows = 0, ncols = 0; + size_t pos; + for (size_t i = 0; i < rows.size(); i++) { + // Strip row brackets if they exist. + if (rows[i][0] == '[' && rows[i].back() == ']') { + rows[i] = rows[i].substr(1, static_cast(rows[i].size()) - 2); + } + std::vector cols = splitArguments(rows[i], ','); + col0 = 0; + ncols = 0; + for (size_t j = 0; j < cols.size(); j++) { + pos = cols[j].find(":"); + if (pos != std::string::npos) { + std::string firstStr = cols[j].substr(0, pos); + std::string lastStr = cols[j].substr(pos + 1); + pos = lastStr.find(":"); + if (pos != std::string::npos) { + std::string stepStr = lastStr.substr(0, pos); + lastStr = lastStr.substr(pos + 1); + if (lastStr.find(":") != std::string::npos) { + throw std::runtime_error( + "Invalid matrix definition '[" + str + "]'. Invalid range '" + cols[j] + "'."); + } + // first:step:last + Value first = eval(firstStr); + Value step = eval(stepStr); + Value last = eval(lastStr); + if (first.matrix().size() != 1 || step.matrix().size() != 1 || + last.matrix().size() != 1) + { + throw std::runtime_error( + "Invalid matrix definition '[" + str + "]'. Invalid range '" + cols[j] + "'."); + } + typename Derived::RealScalar sfirst = std::real(first.matrix()(0)); + typename Derived::RealScalar sstep = std::real(step.matrix()(0)); + typename Derived::RealScalar slast = std::real(last.matrix()(0)); + if (sfirst == slast) { + submatrix.local().setConstant(1, 1, sfirst); + submatrix.mapLocal(); + } else if ((slast - sfirst >= 0 && sstep > 0) || (slast - sfirst <= 0 && sstep < 0)) { + int n = floor((slast - sfirst) / sstep) + 1; + submatrix.local().resize(1, n); + for (int k = 0; k < n; ++k) { + submatrix.local()(0, k) = sfirst + k * sstep; + } + submatrix.mapLocal(); + } else { + throw std::runtime_error( + "Invalid matrix definition '[" + str + "]'. Invalid range '" + cols[j] + "'."); + } + } else { + // first:last => first:1:last + Value first = eval(firstStr); + Value last = eval(lastStr); + if (first.matrix().size() != 1 || last.matrix().size() != 1) { + throw std::runtime_error( + "Invalid matrix definition '[" + str + "]'. Invalid range '" + cols[j] + "'."); + } + typename Derived::RealScalar sfirst = std::real(first.matrix()(0)); + typename Derived::RealScalar slast = std::real(last.matrix()(0)); + if (sfirst == slast) { + submatrix.local().setConstant(1, 1, sfirst); + submatrix.mapLocal(); + } else if (slast - sfirst >= 0) { + int n = floor(slast - sfirst) + 1; + submatrix.local().resize(1, n); + for (int k = 0; k < n; ++k) { + submatrix.local()(0, k) = sfirst + k; + } + submatrix.mapLocal(); + } else { + throw std::runtime_error( + "Invalid matrix definition '[" + str + "]'. Invalid range '" + cols[j] + "'."); + } + } + } else { + submatrix = eval(cols[j]); + } + if (j > 0 && size_t(submatrix.matrix().cols()) != nrows) { + throw std::runtime_error( + "Invalid matrix definition '[" + str + "]'. Successive column entries '" + + cols[static_cast(j) - 1] + "' and '" + cols[j] + + "' do not have the same number of rows."); + } + nrows = submatrix.matrix().rows(); + ncols += submatrix.matrix().cols(); + temp.resize(row0 + submatrix.matrix().rows()); + for (size_t row = 0; row < size_t(submatrix.matrix().rows()); row++) { + temp[row0 + row].resize(col0 + submatrix.matrix().cols()); + for (size_t col = 0; col < size_t(submatrix.matrix().cols()); col++) { + temp[row0 + row][col0 + col] = submatrix.matrix()(row, col); + } + } + col0 += submatrix.matrix().cols(); + } + if (row0 > 0 && ncols != temp[static_cast(row0) - 1].size()) { + throw std::runtime_error( + "Invalid matrix definition '[" + str + "]'. Successive row entries '" + + rows[static_cast(i) - 1] + "' and '" + rows[i] + + "' do not have the same number of columns."); + } + row0 += nrows; + } + if (temp.empty()) {return;} + nrows = temp.size(); + ncols = temp[0].size(); + mat.setLocal(Derived(nrows, ncols)); + for (size_t row = 0; row < nrows; row++) { + for (size_t col = 0; col < ncols; col++) { + mat.matrix()(row, col) = temp[row][col]; + } + } + mat.mapLocal(); +} + +template +bool Parser::evalFunction_1_lt( + const std::string & name, Value & arg, + Value & result, std::true_type) +{ + if (name == "min") { + result.setLocal(arg.matrix().minCoeff()); + return true; + } else if (name == "minOfFinites") { + result.setLocal(arg.matrix().minCoeffOfFinites()); + return true; + } else if (name == "max") { + result.setLocal(arg.matrix().maxCoeff()); + return true; + } else if (name == "maxOfFinites") { + result.setLocal(arg.matrix().maxCoeffOfFinites()); + return true; + } else if (name == "absmax") { + typename Derived::Scalar minimum = arg.matrix().minCoeff(); + typename Derived::Scalar maximum = arg.matrix().maxCoeff(); + result.setLocal(std::abs(maximum) >= std::abs(minimum) ? maximum : minimum); + return true; + } + return false; +} + +template +bool Parser::evalFunction_1_lt( + const std::string & /*name*/, + Value & /*arg0*/, + Value & /*result*/, std::false_type) +{ + return false; +} + +template +bool Parser::evalFunction_2_lt( + const std::string & name, Value & arg0, + Value & arg1, Value & result, + std::true_type) +{ + if (name == "min") { + if (arg1.matrix().size() != 1) { + throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); + } + int dim = floor(std::real(arg1.matrix()(0, 0))); + if ((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) { + throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); + } + if (dim == 0) { + result.local() = arg0.matrix().colwise().minCoeff(); + result.mapLocal(); + return true; + } else if (dim == 1) { + result.local() = arg0.matrix().rowwise().minCoeff(); + result.mapLocal(); + return true; + } + } else if (name == "max") { + if (arg1.matrix().size() != 1) { + throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); + } + int dim = floor(std::real(arg1.matrix()(0, 0))); + if ((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) { + throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); + } + if (dim == 0) { + result.local() = arg0.matrix().colwise().maxCoeff(); + result.mapLocal(); + return true; + } else if (dim == 1) { + result.local() = arg0.matrix().rowwise().maxCoeff(); + result.mapLocal(); + return true; + } + } else if (name == "absmax") { + if (arg1.matrix().size() != 1) { + throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); + } + int dim = floor(std::real(arg1.matrix()(0, 0))); + if ((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) { + throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); + } + if (dim == 0) { + result.local() = arg0.matrix().colwise().maxCoeff(); + result.mapLocal(); + Derived minimum = arg0.matrix().colwise().minCoeff(); + for (size_t i = 0; i < size_t(result.matrix().size()); i++) { + if (std::abs(result.matrix()(i)) < std::abs(minimum(i))) { + result.matrix()(i) = minimum(i); + } + } + return true; + } else if (dim == 1) { + result.local() = arg0.matrix().rowwise().maxCoeff(); + result.mapLocal(); + Derived minimum = arg0.matrix().rowwise().minCoeff(); + for (size_t i = 0; i < size_t(result.matrix().size()); i++) { + if (std::abs(result.matrix()(i)) < std::abs(minimum(i))) { + result.matrix()(i) = minimum(i); + } + } + return true; + } + } else if (name == "cwiseMin") { + if (arg1.matrix().size() == 1) { + typename Derived::Scalar arg1scalar = arg1.matrix()(0, 0); + Derived arg1matrix = Derived::Constant( + arg0.matrix().rows(), + arg0.matrix().cols(), arg1scalar); + result.local() = arg0.matrix().cwiseMin(arg1matrix); + result.mapLocal(); + return true; + } else if ( // NOLINT + arg0.matrix().cols() == arg1.matrix().cols() && + arg0.matrix().rows() == arg1.matrix().rows()) + { + result.local() = arg0.matrix().cwiseMin(arg1.matrix()); + result.mapLocal(); + return true; + } else { + throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); + } + } else if (name == "cwiseMax") { + if (arg1.matrix().size() == 1) { + typename Derived::Scalar arg1scalar = arg1.matrix()(0, 0); + Derived arg1matrix = Derived::Constant( + arg0.matrix().rows(), + arg0.matrix().cols(), arg1scalar); + result.local() = arg0.matrix().cwiseMax(arg1matrix); + result.mapLocal(); + return true; + } else if ( // NOLINT + arg0.matrix().cols() == arg1.matrix().cols() && + arg0.matrix().rows() == arg1.matrix().rows()) + { + result.local() = arg0.matrix().cwiseMax(arg1.matrix()); + result.mapLocal(); + return true; + } else { + throw std::runtime_error("Invalid dimension argument for function '" + name + "(...)'."); + } + } + return false; +} + +template +bool Parser::evalFunction_2_lt( + const std::string & /*name*/, + Value & /*arg0*/, + Value & /*arg1*/, + Value & /*result*/, std::false_type) +{ + return false; +} + +template +void Parser::evalFunction( + const std::string & name, + std::vector & args, + Value & result) +{ + if (args.size() == 1) { + Value arg = eval(args[0]); + if (name == "abs") { + result.local() = arg.matrix().array().abs().template cast(); + result.mapLocal(); + return; + } else if (name == "sqrt") { + result.local() = arg.matrix().array().sqrt(); + result.mapLocal(); + return; + } else if (name == "square") { + result.local() = arg.matrix().array().square(); + result.mapLocal(); + return; + } else if (name == "exp") { + result.local() = arg.matrix().array().exp(); + result.mapLocal(); + return; + } else if (name == "log") { + result.local() = arg.matrix().array().log(); + result.mapLocal(); + return; + } else if (name == "log10") { + result.local() = arg.matrix().array().log(); + result.local() *= (1.0 / log(10)); + result.mapLocal(); + return; + } else if (name == "sin") { + result.local() = arg.matrix().array().sin(); + result.mapLocal(); + return; + } else if (name == "cos") { + result.local() = arg.matrix().array().cos(); + result.mapLocal(); + return; + } else if (name == "tan") { + result.local() = arg.matrix().array().tan(); + result.mapLocal(); + return; + } else if (name == "acos") { + result.local() = arg.matrix().array().acos(); + result.mapLocal(); + return; + } else if (name == "asin") { + result.local() = arg.matrix().array().asin(); + result.mapLocal(); + return; + } else if (name == "trace") { + result.setLocal(arg.matrix().trace()); + return; + } else if (name == "norm") { + result.setLocal(arg.matrix().norm()); + return; + } else if ( // NOLINT + evalFunction_1_lt( + name, arg, result, + has_operator_lt())) + { + return; + } else if (name == "mean") { + result.setLocal(arg.matrix().mean()); + return; + } else if (name == "meanOfFinites") { + result.setLocal(arg.matrix().meanOfFinites()); + return; + } else if (name == "sum") { + result.setLocal(arg.matrix().sum()); + return; + } else if (name == "sumOfFinites") { + result.setLocal(arg.matrix().sumOfFinites()); + return; + } else if (name == "prod") { + result.setLocal(arg.matrix().prod()); + return; + } else if (name == "numberOfFinites") { + result.setLocal(arg.matrix().numberOfFinites()); + return; + } else if (name == "transpose") { + result.local() = arg.matrix().transpose(); + result.mapLocal(); + return; + } else if (name == "conjugate") { + result.local() = arg.matrix().conjugate(); + result.mapLocal(); + return; + } else if (name == "adjoint") { + result.local() = arg.matrix().adjoint(); + result.mapLocal(); + return; + } else if (name == "zeros") { + if (arg.matrix().size() != 1) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + ")'."); + } + int N = floor(std::real(arg.matrix()(0, 0))); + if (N <= 0 || N != std::real(arg.matrix()(0, 0))) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + ")'."); + } + result.local() = Derived::Zero(N, N); + result.mapLocal(); + return; + } else if (name == "ones") { + if (arg.matrix().size() != 1) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + ")'."); + } + int N = floor(std::real(arg.matrix()(0, 0))); + if (N <= 0 || N != std::real(arg.matrix()(0, 0))) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + ")'."); + } + result.local() = Derived::Ones(N, N); + result.mapLocal(); + return; + } else if (name == "eye") { + if (arg.matrix().size() != 1) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + ")'."); + } + int N = floor(std::real(arg.matrix()(0, 0))); + if (N <= 0 || N != std::real(arg.matrix()(0, 0))) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + ")'."); + } + result.local() = Derived::Identity(N, N); + result.mapLocal(); + return; + } else { + throw std::runtime_error( + "Invalid function '" + + name + "(" + args[0] + ")'."); + } + } else if (args.size() == 2) { + Value arg0 = eval(args[0]); + Value arg1 = eval(args[1]); + if (name == "size") { + if (arg1.matrix().size() != 1) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + int dim = floor(std::real(arg1.matrix()(0, 0))); + if ((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + if (dim == 0) { + result.setLocal((typename Derived::Scalar)arg0.matrix().rows()); + return; + } else if (dim == 1) { + result.setLocal((typename Derived::Scalar)arg0.matrix().cols()); + return; + } + } else if ( // NOLINT + evalFunction_2_lt( + name, arg0, arg1, result, + has_operator_lt())) + { + return; + } else if (name == "mean") { + if (arg1.matrix().size() != 1) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + int dim = floor(std::real(arg1.matrix()(0, 0))); + if ((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + if (dim == 0) { + result.local() = arg0.matrix().colwise().mean(); + result.mapLocal(); + return; + } else if (dim == 1) { + result.local() = arg0.matrix().rowwise().mean(); + result.mapLocal(); + return; + } + } else if (name == "sum") { + if (arg1.matrix().size() != 1) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + int dim = floor(std::real(arg1.matrix()(0, 0))); + if ((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + if (dim == 0) { + result.local() = arg0.matrix().colwise().sum(); + result.mapLocal(); + return; + } else if (dim == 1) { + result.local() = arg0.matrix().rowwise().sum(); + result.mapLocal(); + return; + } + } else if (name == "prod") { + if (arg1.matrix().size() != 1) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + int dim = floor(std::real(arg1.matrix()(0, 0))); + if ((dim != 0 && dim != 1) || dim != std::real(arg1.matrix()(0, 0))) { + throw std::runtime_error( + "Invalid dimension argument for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + if (dim == 0) { + result.local() = arg0.matrix().colwise().prod(); + result.mapLocal(); + return; + } else if (dim == 1) { + result.local() = arg0.matrix().rowwise().prod(); + result.mapLocal(); + return; + } + } else if (name == "zeros") { + if ((arg0.matrix().size() != 1) || (arg1.matrix().size() != 1)) { + throw std::runtime_error( + "Invalid dimension arguments for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + int rows = floor(std::real(arg0.matrix()(0, 0))); + int cols = floor(std::real(arg1.matrix()(0, 0))); + if (rows <= 0 || cols <= 0 || + rows != std::real(arg0.matrix()(0, 0)) || cols != std::real(arg1.matrix()(0, 0))) + { + throw std::runtime_error( + "Invalid dimension arguments for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + result.local() = Derived::Zero(rows, cols); + result.mapLocal(); + return; + } else if (name == "ones") { + if ((arg0.matrix().size() != 1) || (arg1.matrix().size() != 1)) { + throw std::runtime_error( + "Invalid dimension arguments for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + int rows = floor(std::real(arg0.matrix()(0, 0))); + int cols = floor(std::real(arg1.matrix()(0, 0))); + if (rows <= 0 || cols <= 0 || + rows != std::real(arg0.matrix()(0, 0)) || cols != std::real(arg1.matrix()(0, 0))) + { + throw std::runtime_error( + "Invalid dimension arguments for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + result.local() = Derived::Ones(rows, cols); + result.mapLocal(); + return; + } else if (name == "eye") { + if ((arg0.matrix().size() != 1) || (arg1.matrix().size() != 1)) { + throw std::runtime_error( + "Invalid dimension arguments for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + int rows = floor(std::real(arg0.matrix()(0, 0))); + int cols = floor(std::real(arg1.matrix()(0, 0))); + if (rows <= 0 || cols <= 0 || + rows != std::real(arg0.matrix()(0, 0)) || cols != std::real(arg1.matrix()(0, 0))) + { + throw std::runtime_error( + "Invalid dimension arguments for function '" + + name + "(" + args[0] + "," + args[1] + ")'."); + } + result.local() = Derived::Identity(rows, cols); + result.mapLocal(); + return; + } else { + throw std::runtime_error( + "Invalid function '" + name + "(" + args[0] + "," + args[1] + ")'."); + } + } + std::string argsStr = "("; + for (size_t i = 0; i < args.size(); i++) { + if (i > 0) {argsStr += ",";} + argsStr += args[i]; + } + argsStr += ")"; + throw std::runtime_error("Invalid function/arguments for '" + name + argsStr + "'."); +} + +template +void Parser::evalNumericRange(const std::string & str, Value & mat) +{ + size_t pos = str.find(":"); + if (pos == std::string::npos) { + throw std::runtime_error("Invalid numeric range '" + str + "'."); + } + size_t pos2 = str.substr(pos + 1).find(":"); + if (pos2 == std::string::npos) { + // first:last + std::string firstStr = str.substr(0, pos); + std::string lastStr = str.substr(pos + 1); + Value first = eval(firstStr); + Value last = eval(lastStr); + if (first.matrix().size() != 1 || last.matrix().size() != 1) { + throw std::runtime_error("Invalid numeric range '" + str + "'."); + } + typename Derived::RealScalar sfirst = std::real(first.matrix()(0, 0)); + typename Derived::RealScalar slast = std::real(last.matrix()(0, 0)); + if (sfirst > slast) { + throw std::runtime_error("Invalid numeric range '" + str + "'. Must not reverse."); + } + int n = 1 + floor(slast - sfirst); + mat.local().resize(1, n); + for (int i = 0; i < n; i++) { + mat.local()(0, i) = sfirst + i; + } + mat.mapLocal(); + } else { + // first:step:last + pos2 += pos + 1; + std::string firstStr = str.substr(0, pos); + std::string stepStr = str.substr(pos + 1, pos2 - pos - 1); + std::string lastStr = str.substr(pos2 + 1); + Value first = eval(firstStr); + Value step = eval(stepStr); + Value last = eval(lastStr); + if (first.matrix().size() != 1 || step.matrix().size() != 1 || last.matrix().size() != 1) { + throw std::runtime_error("Invalid numeric range '" + str + "'."); + } + typename Derived::RealScalar sfirst = std::real(first.matrix()(0, 0)); + typename Derived::RealScalar sstep = std::real(step.matrix()(0, 0)); + typename Derived::RealScalar slast = std::real(last.matrix()(0, 0)); + if (sfirst == slast) { + mat = sfirst; + } else if (sfirst < slast && sstep > 0) { + int n = 1 + floor((slast - sfirst) / sstep); + mat.local().resize(1, n); + for (int i = 0; i < n; i++) { + mat.local()(0, i) = sfirst + i * sstep; + } + mat.mapLocal(); + } else if (sfirst > slast && sstep < 0) { + int n = 1 + floor((slast - sfirst) / sstep); + mat.local().resize(1, n); + for (int i = 0; i < n; i++) { + mat.local()(0, i) = sfirst + i * sstep; + } + mat.mapLocal(); + } else { + throw std::runtime_error("Invalid numeric range '" + str + "'."); + } + } +} + +template +bool Parser::isOperator(const std::string & str) const +{ + if (str.size() == 1) { + return isOperator(str[0]); + } else if (str.size() == 2) { + size_t pos = mOperators2.find(str); + return pos != std::string::npos && pos % 2 == 0; + } + return false; +} + +template +void Parser::evalIndices(ChunkArray & chunks) +{ +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + bool operationPerformed = false; +# endif +#endif + for (typename ChunkArray::iterator it = chunks.begin(); it != chunks.end(); it++) { + if (it->row0 != -1 && + (it->type == VALUE || + (it->type == VARIABLE && + (it + 1 == chunks.end() || (it + 1)->type != OPERATOR || (it + 1)->field != "=")))) + { + if (it->type == VALUE) { + Derived temp = it->value.local().block(it->row0, it->col0, it->rows, it->cols); + it->value.local() = temp; + it->value.mapLocal(); + } else { // if(it->type == VARIABLE) { + if (!isVariable(it->field)) { + throw std::runtime_error( + "Attempted indexing into uninitialized variable '" + it->field + "'."); + } + it->value.local() = mVariables[it->field].matrix().block( + it->row0, it->col0, it->rows, + it->cols); + it->value.mapLocal(); + it->type = VALUE; + } + it->row0 = -1; + it->col0 = -1; + it->rows = -1; + it->cols = -1; +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + operationPerformed = true; +# endif +#endif + } + } +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + if (operationPerformed) {std::cout << "i: "; printChunks(chunks); std::cout << std::endl;} +# endif +#endif +} + +template +void Parser::evalNegations(ChunkArray & chunks) +{ +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + bool operationPerformed = false; +# endif +#endif + if (chunks.size() < 2) {return;} + typename ChunkArray::iterator lhs = chunks.begin(), op = chunks.begin(), rhs = op + 1; + for (; lhs != chunks.end() && op != chunks.end() && rhs != chunks.end(); ) { + if (op->type == OPERATOR && op->field == "-" && + (op == chunks.begin() || (lhs->type != VALUE && lhs->type != VARIABLE)) && + (rhs->type == VALUE || rhs->type == VARIABLE)) + { + if (rhs->type == VALUE) { + rhs->value.matrix().array() *= -1; + } else if (rhs->type == VARIABLE) { + if (!isVariable(rhs->field)) { + throw std::runtime_error( + "Attempted operation '" + op->field + rhs->field + + "' on uninitialized variable '" + rhs->field + "'."); + } + rhs->value.local() = mVariables[rhs->field].matrix().array() * -1; + rhs->value.mapLocal(); + rhs->type = VALUE; + } + lhs = chunks.erase(op); + op = (lhs != chunks.end()) ? lhs + 1 : lhs; + rhs = (op != chunks.end()) ? op + 1 : op; +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + operationPerformed = true; +# endif +#endif + } else { + lhs = op; + op = rhs; + rhs++; + } + } +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + if (operationPerformed) {std::cout << "-: "; printChunks(chunks); std::cout << std::endl;} +# endif +#endif +} + +template +void Parser::evalPowers(ChunkArray & chunks) +{ +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + bool operationPerformed = false; +# endif +#endif + if (chunks.size() < 3) {return;} + typename ChunkArray::iterator lhs = chunks.begin(), op = lhs + 1, rhs = op + 1; + for (; lhs != chunks.end() && op != chunks.end() && rhs != chunks.end(); ) { + if ((op->type == OPERATOR) && (op->field == "^" || op->field == ".^")) { + if (lhs->type == VARIABLE) { + if (!isVariable(lhs->field)) { + throw std::runtime_error( + "Attempted operation '" + lhs->field + op->field + rhs->field + + "' on uninitialized variable '" + lhs->field + "'."); + } + lhs->value.setShared(mVariables[lhs->field]); + } + if (rhs->type == VARIABLE) { + if (!isVariable(rhs->field)) { + throw std::runtime_error( + "Attempted operation '" + lhs->field + op->field + rhs->field + + "' on uninitialized variable '" + rhs->field + "'."); + } + rhs->value.setShared(mVariables[rhs->field]); + } + if (rhs->value.matrix().size() == 1) { + lhs->value.local() = lhs->value.matrix().array().pow(rhs->value.matrix()(0, 0)); + lhs->value.mapLocal(); + lhs->type = VALUE; + } else if (lhs->value.matrix().size() == 1) { + typename Derived::Scalar temp = lhs->value.matrix()(0, 0); + lhs->value.local().resize(rhs->value.matrix().rows(), rhs->value.matrix().cols()); + for (size_t row = 0; row < size_t(rhs->value.matrix().rows()); row++) { + for (size_t col = 0; col < size_t(rhs->value.matrix().cols()); col++) { + lhs->value.local()(row, col) = pow(temp, rhs->value.matrix()(row, col)); + } + } + lhs->value.mapLocal(); + lhs->type = VALUE; + } else if ( // NOLINT + op->field == ".^" && lhs->value.matrix().rows() == rhs->value.matrix().rows() && + lhs->value.matrix().cols() == rhs->value.matrix().cols()) + { + lhs->value.local().resize(rhs->value.matrix().rows(), rhs->value.matrix().cols()); + for (size_t row = 0; row < size_t(rhs->value.matrix().rows()); row++) { + for (size_t col = 0; col < size_t(rhs->value.matrix().cols()); col++) { + lhs->value.local()( + row, + col) = pow(lhs->value.matrix()(row, col), rhs->value.matrix()(row, col)); + } + } + lhs->value.mapLocal(); + lhs->type = VALUE; + } else { + throw std::runtime_error( + "Invalid operand dimensions for operation '" + + lhs->field + op->field + rhs->field + "'."); + } + chunks.erase(op, rhs + 1); + op = lhs + 1; + rhs = (op != chunks.end()) ? op + 1 : op; +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + operationPerformed = true; +# endif +#endif + } else { + lhs = op; + op = rhs; + rhs++; + } + } +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + if (operationPerformed) {std::cout << "^: "; printChunks(chunks); std::cout << std::endl;} +# endif +#endif +} + +template +void Parser::evalMultiplication(ChunkArray & chunks) +{ +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + bool operationPerformed = false; +# endif +#endif + if (chunks.size() < 3) {return;} + typename ChunkArray::iterator lhs = chunks.begin(), op = lhs + 1, rhs = op + 1; + for (; lhs != chunks.end() && op != chunks.end() && rhs != chunks.end(); ) { + if ((op->type == OPERATOR) && + (op->field == "*" || op->field == "/" || op->field == ".*" || op->field == "./")) + { + if (lhs->type == VARIABLE) { + if (!isVariable(lhs->field)) { + throw std::runtime_error( + "Attempted operation '" + lhs->field + op->field + rhs->field + + "' on uninitialized variable '" + lhs->field + "'."); + } + lhs->value.setShared(mVariables[lhs->field]); + } + if (rhs->type == VARIABLE) { + if (!isVariable(rhs->field)) { + throw std::runtime_error( + "Attempted operation '" + lhs->field + op->field + rhs->field + + "' on uninitialized variable '" + rhs->field + "'."); + } + rhs->value.setShared(mVariables[rhs->field]); + } + if (rhs->value.matrix().size() == 1) { + if (lhs->value.isLocal()) { + if (op->field == "*" || op->field == ".*") { + lhs->value.local().array() *= rhs->value.matrix()(0, 0); + } else { // if(op->field == "/" || op->field == "./") + lhs->value.local().array() /= rhs->value.matrix()(0, 0); + } + } else { + if (op->field == "*" || op->field == ".*") { + lhs->value.local() = lhs->value.matrix().array() * rhs->value.matrix()(0, 0); + } else { // if(op->field == "/" || op->field == "./") + lhs->value.local() = lhs->value.matrix().array() / rhs->value.matrix()(0, 0); + } + lhs->value.mapLocal(); + lhs->type = VALUE; + } + } else if (lhs->value.matrix().size() == 1) { + typename Derived::Scalar temp = lhs->value.matrix()(0, 0); + if (op->field == "*" || op->field == ".*") { + lhs->value.local() = rhs->value.matrix().array() * temp; + } else { // if(op->field == "/" || op->field == "./") + lhs->value.local() = Derived::Constant( + rhs->value.matrix().rows(), + rhs->value.matrix().cols(), temp).array() / rhs->value.matrix().array(); + } + lhs->value.mapLocal(); + lhs->type = VALUE; + } else if ( // NOLINT + (op->field == ".*" || op->field == "./") && + lhs->value.matrix().rows() == rhs->value.matrix().rows() && + lhs->value.matrix().cols() == rhs->value.matrix().cols()) + { + if (lhs->value.isLocal()) { + if (op->field == ".*") { + lhs->value.local().array() *= rhs->value.matrix().array(); + } else { // if(op->field == "./") + lhs->value.local().array() /= rhs->value.matrix().array(); + } + } else { + if (op->field == ".*") { + lhs->value.local() = lhs->value.matrix().array() * rhs->value.matrix().array(); + } else { // if(op->field == "./") + lhs->value.local() = lhs->value.matrix().array() / rhs->value.matrix().array(); + } + lhs->value.mapLocal(); + lhs->type = VALUE; + } + } else if (op->field == "*" && lhs->value.matrix().cols() == rhs->value.matrix().rows()) { + if (lhs->value.isLocal()) { + lhs->value.local() *= rhs->value.matrix(); + lhs->value.mapLocal(); + } else { + lhs->value.local() = lhs->value.matrix() * rhs->value.matrix(); + lhs->value.mapLocal(); + lhs->type = VALUE; + } + } else { + throw std::runtime_error( + "Invalid operand dimensions for operation '" + + lhs->field + op->field + rhs->field + "'."); + } + chunks.erase(op, rhs + 1); + op = lhs + 1; + rhs = (op != chunks.end()) ? op + 1 : op; +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + operationPerformed = true; +# endif +#endif + } else { + lhs = op; + op = rhs; + rhs++; + } + } +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + if (operationPerformed) {std::cout << "*: "; printChunks(chunks); std::cout << std::endl;} +# endif +#endif +} + +template +void Parser::evalAddition(ChunkArray & chunks) +{ +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + bool operationPerformed = false; +# endif +#endif + if (chunks.size() < 3) {return;} + typename ChunkArray::iterator lhs = chunks.begin(), op = lhs + 1, rhs = op + 1; + for (; lhs != chunks.end() && op != chunks.end() && rhs != chunks.end(); ) { + if ((op->type == OPERATOR) && + (op->field == "+" || op->field == "-" || op->field == ".+" || op->field == ".-")) + { + if (lhs->type == VARIABLE) { + if (!isVariable(lhs->field)) { + throw std::runtime_error( + "Attempted operation '" + lhs->field + op->field + rhs->field + + "' on uninitialized variable '" + lhs->field + "'."); + } + lhs->value.setShared(mVariables[lhs->field]); + } + if (rhs->type == VARIABLE) { + if (!isVariable(rhs->field)) { + throw std::runtime_error( + "Attempted operation '" + lhs->field + op->field + rhs->field + + "' on uninitialized variable '" + rhs->field + "'."); + } + rhs->value.setShared(mVariables[rhs->field]); + } + if (rhs->value.matrix().size() == 1) { + if (lhs->value.isLocal()) { + if (op->field == "+" || op->field == ".+") { + lhs->value.local().array() += rhs->value.matrix()(0, 0); + } else { // if(op->field == "-" || op->field == ".-") + lhs->value.local().array() -= rhs->value.matrix()(0, 0); + } + } else { + if (op->field == "+" || op->field == ".+") { + lhs->value.local() = lhs->value.matrix().array() + rhs->value.matrix()(0, 0); + } else { // if(op->field == "-" || op->field == ".-") + lhs->value.local() = lhs->value.matrix().array() - rhs->value.matrix()(0, 0); + } + lhs->value.mapLocal(); + lhs->type = VALUE; + } + } else if (lhs->value.matrix().size() == 1) { + typename Derived::Scalar temp = lhs->value.matrix()(0, 0); + if (op->field == "+" || op->field == ".+") { + lhs->value.local() = rhs->value.matrix().array() + temp; + } else { // if(op->field == "-" || op->field == ".-") + lhs->value.local() = Derived::Constant( + rhs->value.matrix().rows(), + rhs->value.matrix().cols(), temp).array() - rhs->value.matrix().array(); + } + lhs->value.mapLocal(); + lhs->type = VALUE; + } else if ( // NOLINT + lhs->value.matrix().rows() == rhs->value.matrix().rows() && + lhs->value.matrix().cols() == rhs->value.matrix().cols()) + { + if (lhs->value.isLocal()) { + if (op->field == "+" || op->field == ".+") { + lhs->value.local().array() += rhs->value.matrix().array(); + } else { // if(op->field == "-" || op->field == ".-") + lhs->value.local().array() -= rhs->value.matrix().array(); + } + } else { + if (op->field == "+" || op->field == ".+") { + lhs->value.local() = lhs->value.matrix().array() + rhs->value.matrix().array(); + } else { // if(op->field == "-" || op->field == ".-") + lhs->value.local() = lhs->value.matrix().array() - rhs->value.matrix().array(); + } + lhs->value.mapLocal(); + lhs->type = VALUE; + } + } else { + throw std::runtime_error( + "Invalid operand dimensions for operation '" + + lhs->field + op->field + rhs->field + "'."); + } + chunks.erase(op, rhs + 1); + op = lhs + 1; + rhs = (op != chunks.end()) ? op + 1 : op; +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + operationPerformed = true; +# endif +#endif + } else { + lhs = op; + op = rhs; + rhs++; + } + } +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + if (operationPerformed) {std::cout << "+: "; printChunks(chunks); std::cout << std::endl;} +# endif +#endif +} + +template +void Parser::evalAssignment(ChunkArray & chunks) +{ +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + bool operationPerformed = false; +# endif +#endif + if (chunks.size() < 3) {return;} + typename ChunkArray::iterator rhs = chunks.end() - 1, op = rhs - 1, lhs = op - 1; + for (; op != chunks.begin() && rhs != chunks.begin(); ) { + if (op->type == OPERATOR && op->field == "=" && + (lhs->type == VALUE || lhs->type == VARIABLE) && + (rhs->type == VALUE || rhs->type == VARIABLE)) + { + if (rhs->type == VARIABLE) { + if (!isVariable(rhs->field)) { + throw std::runtime_error( + "Attempted operation '" + lhs->field + op->field + + rhs->field + "' on uninitialized variable '" + rhs->field + "'."); + } + rhs->value.setShared(mVariables[rhs->field]); + } + if (lhs->type == VALUE) { + lhs->value.local() = rhs->value.matrix(); + lhs->value.mapLocal(); + } else { // if(lhs->type == VARIABLE) { + if (isVariable(lhs->field)) { + lhs->value.setShared(mVariables[lhs->field]); + if (lhs->row0 == -1) { + if (lhs->value.matrix().rows() == rhs->value.matrix().rows() && + lhs->value.matrix().cols() == rhs->value.matrix().cols()) + { + lhs->value.matrix() = rhs->value.matrix(); + } else { + mVariables[lhs->field].local() = rhs->value.matrix(); + mVariables[lhs->field].mapLocal(); + } + } else { // if(lhs->row0 != -1) { + lhs->value.matrix().block( + lhs->row0, lhs->col0, lhs->rows, + lhs->cols) = rhs->value.matrix(); + } + } else { + mVariables[lhs->field].local() = rhs->value.matrix(); + mVariables[lhs->field].mapLocal(); + } + } + rhs = chunks.erase(op, rhs + 1); + op = (rhs != chunks.begin()) ? rhs - 1 : rhs; + if (op != chunks.begin()) {lhs = op - 1;} +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + operationPerformed = true; +# endif +#endif + } else { + rhs = op; + op = lhs; + lhs--; + } + } +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG + if (operationPerformed) {std::cout << "=: "; printChunks(chunks); std::cout << std::endl;} +# endif +#endif +} + +#ifdef DEBUG +# ifdef EIGENLAB_DEBUG +template +void Parser::printChunks( + ChunkArray & chunks, size_t maxRows, size_t maxCols, + int precision) +{ + std::cout << "__"; + for (typename ChunkArray::iterator it = chunks.begin(); it != chunks.end(); it++) { + switch (it->type) { + case VALUE: + std::cout << textRepresentation(it->value, maxRows, maxCols, precision); + if (it->row0 != -1) { + std::cout << "(" << it->row0 << ":" << it->row0 + it->rows - 1 << "," << it->col0 << + ":" << it->col0 + it->cols - 1 << ")"; + } + break; + case VARIABLE: + std::cout << it->field; + // << "=" << textRepresentation(mVariables[it->field], maxRows, maxCols, precision); + if (it->row0 != -1) { + std::cout << "(" << it->row0 << ":" << it->row0 + it->rows - 1 << "," << it->col0 << + ":" << it->col0 + it->cols - 1 << ")"; + } + break; + case OPERATOR: + std::cout << it->field; + break; + case FUNCTION: + std::cout << "f()=" << it->field; + break; + } + std::cout << "__"; + } +} + +template +void Parser::printVars(size_t maxRows, size_t maxCols, int precision) +{ + for (typename ValueMap::iterator it = mVariables.begin(); it != mVariables.end(); it++) { + std::cout << it->first << " (" << it->second.matrix().rows() << "x" << + it->second.matrix().cols() << ") = " << textRepresentation( + it->second, maxRows, maxCols, + precision) << std::endl; + } +} + +template +std::string Parser::textRepresentation( + Value & val, size_t maxRows, + size_t maxCols, int precision) +{ + if (val.matrix().size() == 1) { + return numberToString(val.matrix()(0, 0), precision); + } else { + std::string str = "["; + for (size_t row = 0; row < val.matrix().rows() && row < maxRows; row++) { + str += (row > 0 ? ";[" : "["); + for (size_t col = 0; col < val.matrix().cols() && col < maxCols; col++) { + if (col > 0) {str += ",";} + str += numberToString(val.matrix()(row, col), precision); + } + str += (val.matrix().cols() > maxCols ? "...]" : "]"); + } + str += (val.matrix().rows() > maxRows ? "...]" : "]"); + return str; + } +} +# endif // #ifdef EIGENLAB_DEBUG +#endif // #ifdef DEBUG + +template +std::string Parser::trim(const std::string & str) +{ + if (str.empty()) {return str;} + std::string::const_iterator first = str.begin(); + std::string::const_iterator last = str.end() - 1; + while (first < last && isspace(*first)) {first++;} + while (first < last && isspace(*last)) {last--;} + return std::string(first, last + 1); +} + +template +std::vector Parser::split( + const std::string & str, + const char delimeter) +{ + std::vector args; + std::string::const_iterator i0 = str.begin(); + for (std::string::const_iterator it = str.begin(); it != str.end(); it++) { + if ((*it) == delimeter) { + args.push_back(trim(std::string(i0, it))); + i0 = it + 1; + } + } + args.push_back(std::string(i0, str.end())); + return args; +} + +template +template +bool Parser::isNumber(const std::string & str, T * num) +{ + std::istringstream iss(str); + if (num) { + iss >> (*num); + } else { + T number; + iss >> number; + } + return !iss.fail() && !iss.bad() && iss.eof(); +} + +template +template +T Parser::stringToNumber(const std::string & str) +{ + std::istringstream iss(str); + T number; + iss >> number; + if (iss.fail() || iss.bad() || !iss.eof()) { + throw std::runtime_error("Failed to convert " + str + " to a number."); + } + return number; +} + +template +template +std::string Parser::numberToString(T num, int precision) +{ + std::ostringstream oss; + if (precision) { + oss << std::setprecision(precision) << num; + } else { + oss << num; + } + return oss.str(); +} + +#ifdef DEBUG +template +void +Parser::test_w_lt( + size_t & numFails, + typename Derived::Scalar & /* s */, + Derived & a34, + Derived & b34, + Derived & /* c43 */, + Derived & /* v */, std::true_type) +{ + // + // tests that only work if Derived::Scalar has operator< + // + Value resultValue; + Derived resultMatrix; + Derived temp; + std::cout << "Test min(a): "; + resultValue = eval("min(a)"); + resultMatrix.setConstant(1, 1, a34.minCoeff()); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test min(a, 0): "; + resultValue = eval("min(a, 0)"); + resultMatrix = a34.colwise().minCoeff(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test min(a, 1): "; + resultValue = eval("min(a, 1)"); + resultMatrix = a34.rowwise().minCoeff(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test max(a): "; + resultValue = eval("max(a)"); + resultMatrix.setConstant(1, 1, a34.maxCoeff()); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test max(a, 0): "; + resultValue = eval("max(a, 0)"); + resultMatrix = a34.colwise().maxCoeff(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test max(a, 1): "; + resultValue = eval("max(a, 1)"); + resultMatrix = a34.rowwise().maxCoeff(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test absmax(a): "; + resultValue = eval("absmax(a)"); + resultMatrix.setConstant( + 1, 1, std::abs(a34.maxCoeff()) >= std::abs( + a34.minCoeff()) ? a34.maxCoeff() : a34.minCoeff()); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test absmax(a, 0): "; + resultValue = eval("absmax(a, 0)"); + resultMatrix = a34.colwise().maxCoeff(); + temp = a34.colwise().minCoeff(); + for (Index i = 0; i < resultMatrix.size(); ++i) { + if (std::abs(resultMatrix(i)) < std::abs(temp(i))) { + resultMatrix(i) = temp(i); + } + } + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test absmax(a, 1): "; + resultValue = eval("absmax(a, 1)"); + resultMatrix = a34.rowwise().maxCoeff(); + temp = a34.rowwise().minCoeff(); + for (Index i = 0; i < resultMatrix.size(); ++i) { + if (std::abs(resultMatrix(i)) < std::abs(temp(i))) { + resultMatrix(i) = temp(i); + } + } + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test cwiseMin(a, b): "; + resultValue = eval("cwiseMin(a, b)"); + resultMatrix = a34.cwiseMin(b34); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test cwiseMax(a, b): "; + resultValue = eval("cwiseMax(a, b)"); + resultMatrix = a34.cwiseMax(b34); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } +} + +template +void +Parser::test_w_lt( + size_t & /* numFails */, + typename Derived::Scalar & /* s */, + Derived & /* a34 */, + Derived & /* b34 */, + Derived & /* c43 */, + Derived & /* v */, std::false_type) +{ + // do nothing +} + +template +size_t Parser::test() +{ + std::cout << std::endl; + std::cout << "BEGIN unit test for EigenLab..." << std::endl; + std::cout << "Make sure this function completes successfuly and prints the message " + "'Successfully completed unit test for EigenLab with no failures.'" << std::endl; + std::cout << std::endl; + + size_t numFails = 0; + Value resultValue; + Derived resultMatrix; + Derived temp; + typename Derived::Scalar s = 2; + + Derived a34 = Derived::Random(3, 4); + Derived b34 = Derived::Random(3, 4); + Derived c43 = Derived::Random(4, 3); + Derived v = Derived::Random(1, 10); + // std::cout << "a34=" << std::endl << a34 << std::endl << std::endl; + // std::cout << "b34=" << std::endl << b34 << std::endl << std::endl; + // std::cout << "c43=" << std::endl << c43 << std::endl << std::endl; + // std::cout << "v=" << std::endl << v << std::endl << std::endl; + + var("a").setShared(a34); + var("b").setShared(b34); + var("c").setShared(c43); + var("v").setShared(v); + var("s").setShared(&s); + + //////////////////////////////////////// + std::cout << "Testing basic operations..." << std::endl << std::endl; + //////////////////////////////////////// + + std::cout << "Test matrix addition a + b: "; + resultValue = eval("a + b"); + resultMatrix = a34 + b34; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix/scalar addition a + s: "; + resultValue = eval("a + s"); + resultMatrix = a34.array() + s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test scalar/matrix addition s + b: "; + resultValue = eval("s + b"); + resultMatrix = b34.array() + s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix addition a .+ b: "; + resultValue = eval("a .+ b"); + resultMatrix = a34 + b34; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix/scalar addition a .+ s: "; + resultValue = eval("a .+ s"); + resultMatrix = a34.array() + s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test scalar/matrix addition s .+ b: "; + resultValue = eval("s .+ b"); + resultMatrix = b34.array() + s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix subtraction a - b: "; + resultValue = eval("a - b"); + resultMatrix = a34 - b34; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix/scalar subtraction a - s: "; + resultValue = eval("a - s"); + resultMatrix = a34.array() - s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test scalar/matrix subtraction s - b: "; + resultValue = eval("s - b"); + resultMatrix = (-b34.array()) + s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix subtraction a .- b: "; + resultValue = eval("a .- b"); + resultMatrix = a34 - b34; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix/scalar subtraction a .- s: "; + resultValue = eval("a .- s"); + resultMatrix = a34.array() - s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test scalar/matrix subtraction s .- b: "; + resultValue = eval("s .- b"); + resultMatrix = (-b34.array()) + s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix negation -a: "; + resultValue = eval("-a"); + resultMatrix = -a34; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test scalar negation -s: "; + resultValue = eval("-s"); + resultMatrix.setConstant(1, 1, -s); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix coefficient-wise multiplication a .* b: "; + resultValue = eval("a .* b"); + resultMatrix = a34.array() * b34.array(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix/scalar coefficient-wise multiplication a * s: "; + resultValue = eval("a * s"); + resultMatrix = a34.array() * s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test scalar/matrix coefficient-wise multiplication s * b: "; + resultValue = eval("s * b"); + resultMatrix = b34.array() * s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix/scalar coefficient-wise multiplication a .* s: "; + resultValue = eval("a .* s"); + resultMatrix = a34.array() * s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test scalar/matrix coefficient-wise multiplication s .* b: "; + resultValue = eval("s .* b"); + resultMatrix = b34.array() * s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix coefficient-wise division a ./ b: "; + resultValue = eval("a ./ b"); + resultMatrix = a34.array() / b34.array(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix/scalar coefficient-wise division a / s: "; + resultValue = eval("a / s"); + resultMatrix = a34.array() / s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test scalar/matrix coefficient-wise division s / b: "; + resultValue = eval("s / b"); + resultMatrix = Derived::Constant(b34.rows(), b34.cols(), s).array() / b34.array(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix/scalar coefficient-wise division a ./ s: "; + resultValue = eval("a ./ s"); + resultMatrix = a34.array() / s; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test scalar/matrix coefficient-wise division s ./ b: "; + resultValue = eval("s ./ b"); + resultMatrix = Derived::Constant(b34.rows(), b34.cols(), s).array() / b34.array(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix coefficient-wise power a .^ b: "; + resultValue = eval("abs(a) .^ b"); + resultMatrix = a34; + for (Index i = 0; i < a34.size(); ++i) { + resultMatrix(i) = pow(std::abs(a34(i)), b34(i)); + } + // std::cout << std::endl; + // std::cout << "a=" << std::endl << a34 << std::endl << std::endl; + // std::cout << "b=" << std::endl << b34 << std::endl << std::endl; + // std::cout << "val=" << std::endl << resultValue.matrix() << std::endl << std::endl; + // std::cout << "mat=" << std::endl << resultMatrix << std::endl << std::endl; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix/scalar coefficient-wise power a ^ s: "; + resultValue = eval("abs(a) ^ s"); + resultMatrix = a34; + for (Index i = 0; i < a34.size(); ++i) { + resultMatrix(i) = pow(std::abs(a34(i)), s); + } + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test scalar/matrix coefficient-wise power s ^ b: "; + resultValue = eval("s ^ b"); + resultMatrix = b34; + for (Index i = 0; i < b34.size(); ++i) { + resultMatrix(i) = pow(s, b34(i)); + } + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix/scalar coefficient-wise power a .^ s: "; + resultValue = eval("abs(a) .^ s"); + resultMatrix = a34; + for (Index i = 0; i < a34.size(); ++i) { + resultMatrix(i) = pow(std::abs(a34(i)), s); + } + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test scalar/matrix coefficient-wise power s .^ b: "; + resultValue = eval("s .^ b"); + resultMatrix = b34; + for (Index i = 0; i < b34.size(); ++i) { + resultMatrix(i) = pow(s, b34(i)); + } + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix multiplication a * b: "; + resultValue = eval("a * c"); + resultMatrix = a34 * c43; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test grouped subexpression (a + b) * c: "; + resultValue = eval("(a + b) * c"); + resultMatrix = (a34 + b34) * c43; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test nested groups ((a + (b * 3 + 1)) * c).^2: "; + resultValue = eval("((a + (b * 3 + 1)) * c).^2"); + resultMatrix = ((a34.array() + (b34.array() * 3 + 1)).matrix() * c43).array().pow(2); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + //////////////////////////////////////// + std::cout << std::endl << "Testing coefficient and submatrix block access..." << std::endl << + std::endl; + //////////////////////////////////////// + + std::cout << "Test matrix coefficient access a(i,j): "; + resultValue = eval("a(1,2)"); + resultMatrix.setConstant(1, 1, a34(1, 2)); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test submatrix block access a(i:p,j:q): "; + resultValue = eval("a(1:2,2:3)"); + resultMatrix = a34.block(1, 2, 2, 2); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test submatrix block access using 'end' and ':' identifiers a(i:end,:): "; + resultValue = eval("a(1:end,:)"); + resultMatrix = a34.block(1, 0, a34.rows() - 1, a34.cols()); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test submatrix block access using subexpressions: "; + resultValue = eval("a(2-1:2-1,0+1:3-1)"); + resultMatrix = a34.block(1, 1, 1, 2); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test submatrix block access using subexpressions with 'end' keyword: "; + resultValue = eval("a(2-1:end-1,0+1:end-1)"); + resultMatrix = a34.block(1, 1, a34.rows() - 2, a34.cols() - 2); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test vector coefficient access v(i): "; + resultValue = eval("v(5)"); + resultMatrix.setConstant(1, 1, v(5)); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test subvector segment access v(i:j): "; + resultValue = eval("v(3:6)"); + resultMatrix = v.block(0, 3, 1, 4); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test subvector segment access using 'end' identifier v(i:end): "; + resultValue = eval("v(5:end)"); + resultMatrix = v.block(0, 5, 1, v.cols() - 5); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test subvector segment access using ':' identifier v(:): "; + resultValue = eval("v(:)"); + resultMatrix = v; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test subvector segment access using subexpressions: "; + resultValue = eval("v(3-1:5+2)"); + resultMatrix = v.block(0, 2, 1, 6); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test subvector segment access using subexpressions with 'end' keyword: "; + resultValue = eval("v((end-8)*2:end-3)"); + resultMatrix = v.block(0, 2, 1, 5); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + //////////////////////////////////////// + std::cout << std::endl << "Testing vector/matrix expressions..." << std::endl << std::endl; + //////////////////////////////////////// + + std::cout << "Test numeric range expression [i:j]: "; + resultValue = eval("[2:5]"); + resultMatrix.resize(1, 4); + resultMatrix << 2, 3, 4, 5; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test numeric range expression [i:s:j]: "; + resultValue = eval("[2:2:10]"); + resultMatrix.resize(1, 5); + resultMatrix << 2, 4, 6, 8, 10; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test numeric range with subexpressions: "; + resultValue = eval("[6-2:5*2-3]"); + std::cout << "val=" << std::endl << resultValue.matrix() << std::endl << std::endl; + resultMatrix.resize(1, 4); + resultMatrix << 4, 5, 6, 7; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix expression [[1, 2]; [3, 4]]: "; + resultValue = eval("[[1, 2]; [3, 4]]"); + resultMatrix.resize(2, 2); + resultMatrix << 1, 2, 3, 4; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test matrix expression [ 1, 2, 3; 4:6 ]: "; + resultValue = eval("[1, 2, 3; 4:6]"); + resultMatrix.resize(2, 3); + resultMatrix << 1, 2, 3, 4, 5, 6; + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + //////////////////////////////////////// + std::cout << std::endl << "Testing coefficient-wise functions..." << std::endl << std::endl; + //////////////////////////////////////// + + std::cout << "Test coefficient-wise abs(a): "; + resultValue = eval("abs(a)"); + resultMatrix.resize(3, 4); + resultMatrix.real() = a34.array().abs(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test coefficient-wise sqrt(a): "; + resultValue = eval("sqrt(abs(a))"); + resultMatrix.real() = a34.array().abs().sqrt(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test coefficient-wise exp(a): "; + resultValue = eval("exp(abs(a) + 0.001)"); + resultMatrix.real() = (a34.array().abs() + 0.001).exp(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test coefficient-wise log(a): "; + resultValue = eval("log(abs(a) + 0.001)"); + resultMatrix.real() = (a34.array().abs() + 0.001).log(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test coefficient-wise log10(a): "; + resultValue = eval("log10(abs(a) + 0.001)"); + resultMatrix.real() = (a34.array().abs() + 0.001).log() * (1.0 / log(10)); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test coefficient-wise sin(a): "; + resultValue = eval("sin(a)"); + resultMatrix = a34.array().sin(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test coefficient-wise cos(a): "; + resultValue = eval("cos(a)"); + resultMatrix = a34.array().cos(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test coefficient-wise tan(a): "; + resultValue = eval("tan(a)"); + resultMatrix = a34.array().tan(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test coefficient-wise asin(a): "; + resultValue = eval("asin(a)"); + resultMatrix = a34.array().asin(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test coefficient-wise acos(a): "; + resultValue = eval("acos(a)"); + resultMatrix = a34.array().acos(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + //////////////////////////////////////// + std::cout << std::endl << "Testing matrix reduction functions..." << std::endl << std::endl; + //////////////////////////////////////// + + std::cout << "Test trace(a): "; + resultValue = eval("trace(a)"); + resultMatrix.setConstant(1, 1, a34.trace()); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test norm(a): "; + resultValue = eval("norm(a)"); + resultMatrix.setConstant(1, 1, a34.norm()); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test size(a, 0): "; + resultValue = eval("size(a, 0)"); + resultMatrix.setConstant(1, 1, a34.rows()); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test size(a, 1): "; + resultValue = eval("size(a, 1)"); + resultMatrix.setConstant(1, 1, a34.cols()); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + test_w_lt(numFails, s, a34, b34, c43, v, has_operator_lt()); + + std::cout << "Test mean(a): "; + resultValue = eval("mean(a)"); + resultMatrix.setConstant(1, 1, a34.mean()); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test mean(a, 0): "; + resultValue = eval("mean(a, 0)"); + resultMatrix = a34.colwise().mean(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test mean(a, 1): "; + resultValue = eval("mean(a, 1)"); + resultMatrix = a34.rowwise().mean(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test sum(a): "; + resultValue = eval("sum(a)"); + resultMatrix.setConstant(1, 1, a34.sum()); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test sum(a, 0): "; + resultValue = eval("sum(a, 0)"); + resultMatrix = a34.colwise().sum(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test sum(a, 1): "; + resultValue = eval("sum(a, 1)"); + resultMatrix = a34.rowwise().sum(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test prod(a): "; + resultValue = eval("prod(a)"); + resultMatrix.setConstant(1, 1, a34.prod()); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test prod(a, 0): "; + resultValue = eval("prod(a, 0)"); + resultMatrix = a34.colwise().prod(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test prod(a, 1): "; + resultValue = eval("prod(a, 1)"); + resultMatrix = a34.rowwise().prod(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + //////////////////////////////////////// + std::cout << std::endl << "Testing matrix functions..." << std::endl << std::endl; + //////////////////////////////////////// + + std::cout << "Test transpose(a): "; + resultValue = eval("transpose(a)"); + resultMatrix = a34.transpose(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test conjugate(a): "; + resultValue = eval("conjugate(a)"); + resultMatrix = a34.conjugate(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test adjoint(a): "; + resultValue = eval("adjoint(a)"); + resultMatrix = a34.adjoint(); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + //////////////////////////////////////// + std::cout << std::endl << "Testing matrix initializers..." << std::endl << std::endl; + //////////////////////////////////////// + + std::cout << "Test zeros(5): "; + resultValue = eval("zeros(5)"); + resultMatrix = Derived::Zero(5, 5); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test ones(5): "; + resultValue = eval("ones(5)"); + resultMatrix = Derived::Ones(5, 5); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test eye(5): "; + resultValue = eval("eye(5)"); + resultMatrix = Derived::Identity(5, 5); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + try { + std::cout << "Test zeros(5.2): "; + resultValue = eval("zeros(5.2)"); // <-- Should NOT succeed!!! + std::cout << "FAIL" << std::endl; ++numFails; + } catch (std::runtime_error & err) { + std::cout << err.what() << std::endl; + std::cout << "Exception caught, so we're OK" << std::endl; + } + + try { + std::cout << "Test eye(-3): "; + resultValue = eval("eye(-3)"); // <-- Should NOT succeed!!! + std::cout << "FAIL" << std::endl; ++numFails; + } catch (std::runtime_error & err) { + std::cout << err.what() << std::endl; + std::cout << "Exception caught, so we're OK" << std::endl; + } + + std::cout << "Test zeros(4,7): "; + resultValue = eval("zeros(4,7)"); + resultMatrix = Derived::Zero(4, 7); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test ones(4,7): "; + resultValue = eval("ones(4,7)"); + resultMatrix = Derived::Ones(4, 7); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test eye(4,7): "; + resultValue = eval("eye(4,7)"); + resultMatrix = Derived::Identity(4, 7); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + //////////////////////////////////////// + std::cout << std::endl << "Testing variable assignment..." << std::endl << std::endl; + //////////////////////////////////////// + + std::cout << "Test assigning to a variable with the same dimensions a = b: "; + eval("a = b"); + if (a34.isApprox(b34)) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test assigning to a variable with different dimensions a = c: "; + eval("a = c"); + if (var("a").matrix().isApprox(c43) && a34.isApprox(b34)) { + std::cout << "OK" << std::endl; + } else { + std::cout << "FAIL" << std::endl; ++numFails; + } + var("a").setShared(a34); + + std::cout << "Test creating a new variable x = [1,2;3,4]: "; + resultValue = eval("x = [1,2;3,4]"); + if (var("x").matrix().isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test assigning to a variable coefficient a(i,j) = s: "; + eval("a(1, 2) = s"); + if (a34(1, 2) == s) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test assigning to a variable submatrix block a(0:1,1:2) = x: "; + resultValue = eval("a(0:1,1:2) = x"); + if (a34.block(0, 1, 2, 2).isApprox(var("x").matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + try { + std::cout << "Test bad function call: "; + resultValue = eval("foobar(-3)"); // <-- Should NOT succeed!!! + std::cout << "FAIL" << std::endl; ++numFails; + } catch (std::runtime_error & err) { + std::cout << err.what() << std::endl; + std::cout << "Exception caught, so we're OK" << std::endl; + } + //////////////////////////////////////// + std::cout << std::endl << "Testing fp parsing..." << std::endl << std::endl; + //////////////////////////////////////// + + std::cout << "Test assigning 1.2e-3: "; + resultValue = eval("s = 1.2e-3"); + resultMatrix.setConstant(1, 1, typename Derived::Scalar(1.2e-3)); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test assigning 1.e3: "; + resultValue = eval("s = 1.e3"); + resultMatrix.setConstant(1, 1, typename Derived::Scalar(1000)); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << "Test assigning 12.34e05: "; + resultValue = eval("s = 12.34e05"); + resultMatrix.setConstant(1, 1, typename Derived::Scalar(123.4e4)); + if (resultMatrix.isApprox(resultValue.matrix())) {std::cout << "OK" << std::endl;} else { + std::cout << "FAIL" << std::endl; ++numFails; + } + + std::cout << std::endl; + if (numFails == 0) { + std::cout << "Successfully completed unit test for EigenLab with no failures." << std::endl; + } else { + std::cout << "Completed unit test for EigenLab with " << numFails << + " failures (see above)." << std::endl; + } + std::cout << std::endl; + return numFails; +} +#endif // DEBUG + +} // namespace EigenLab +#endif // EIGENLAB__EIGENLAB_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/BufferNormalizerFilter.hpp b/grid_map_filters/include/grid_map_filters/BufferNormalizerFilter.hpp index 999191ad6..50add561b 100644 --- a/grid_map_filters/include/grid_map_filters/BufferNormalizerFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/BufferNormalizerFilter.hpp @@ -6,13 +6,15 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__BUFFERNORMALIZERFILTER_HPP_ +#define GRID_MAP_FILTERS__BUFFERNORMALIZERFILTER_HPP_ -#include +#include #include -namespace grid_map { +namespace grid_map +{ /*! * Normalizes the buffer of a map such that it has default (zero) start index. @@ -20,8 +22,7 @@ namespace grid_map { template class BufferNormalizerFilter : public filters::FilterBase { - - public: +public: /*! * Constructor */ @@ -35,14 +36,15 @@ class BufferNormalizerFilter : public filters::FilterBase /*! * Configures the filter from parameters on the parameter server. */ - virtual bool configure(); + bool configure() override; /*! * Normalizes the buffer of a map. * @param mapIn the input map before normalization. * @param mapOut the normalized map. */ - virtual bool update(const T& mapIn, T& mapOut); + bool update(const T & mapIn, T & mapOut) override; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__BUFFERNORMALIZERFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/ColorBlendingFilter.hpp b/grid_map_filters/include/grid_map_filters/ColorBlendingFilter.hpp index c9b51d50c..38ba922a8 100644 --- a/grid_map_filters/include/grid_map_filters/ColorBlendingFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/ColorBlendingFilter.hpp @@ -6,14 +6,16 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__COLORBLENDINGFILTER_HPP_ +#define GRID_MAP_FILTERS__COLORBLENDINGFILTER_HPP_ -#include +#include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Blend two color layers. @@ -21,7 +23,7 @@ namespace grid_map { template class ColorBlendingFilter : public filters::FilterBase { - public: +public: /*! * Constructor */ @@ -35,18 +37,18 @@ class ColorBlendingFilter : public filters::FilterBase /*! * Configures the filter. */ - virtual bool configure(); + bool configure() override; /*! * Compute a new color layer based on blending two color layers. * @param mapIn grid map containing the two color layers. * @param mapOut grid map containing mapIn and the blended color layer. */ - virtual bool update(const T& mapIn, T& mapOut); - - private: + bool update(const T & mapIn, T & mapOut) override; - enum class BlendModes { +private: + enum class BlendModes + { Normal, HardLight, SoftLight @@ -55,14 +57,15 @@ class ColorBlendingFilter : public filters::FilterBase //! Input layers. std::string backgroundLayer_, foregroundLayer_; - //! Blend mode. - BlendModes blendMode_; - //! Opacity of foreground layer. double opacity_; + //! Blend mode. + BlendModes blendMode_; + //! Output layer name. std::string outputLayer_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__COLORBLENDINGFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/ColorFillFilter.hpp b/grid_map_filters/include/grid_map_filters/ColorFillFilter.hpp index 21b6bcb15..c25104085 100644 --- a/grid_map_filters/include/grid_map_filters/ColorFillFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/ColorFillFilter.hpp @@ -6,14 +6,16 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__COLORFILLFILTER_HPP_ +#define GRID_MAP_FILTERS__COLORFILLFILTER_HPP_ -#include +#include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Creates a new color layer. @@ -21,8 +23,7 @@ namespace grid_map { template class ColorFillFilter : public filters::FilterBase { - - public: +public: /*! * Constructor */ @@ -36,16 +37,16 @@ class ColorFillFilter : public filters::FilterBase /*! * Configures the filter. */ - virtual bool configure(); + bool configure() override; /*! * Adds a new color layer. * @param mapIn grid map to add the new layer. * @param mapOut grid map the grid map with the new color layer. */ - virtual bool update(const T& mapIn, T& mapOut); + bool update(const T & mapIn, T & mapOut) override; - private: +private: //! Color. double r_, g_, b_; @@ -56,4 +57,5 @@ class ColorFillFilter : public filters::FilterBase std::string outputLayer_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__COLORFILLFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/ColorMapFilter.hpp b/grid_map_filters/include/grid_map_filters/ColorMapFilter.hpp index 87b1dda38..e9789e21d 100644 --- a/grid_map_filters/include/grid_map_filters/ColorMapFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/ColorMapFilter.hpp @@ -6,15 +6,17 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__COLORMAPFILTER_HPP_ +#define GRID_MAP_FILTERS__COLORMAPFILTER_HPP_ #include -#include +#include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Creates a new color layer with the color mapped between min. and max. value. @@ -22,8 +24,7 @@ namespace grid_map { template class ColorMapFilter : public filters::FilterBase { - - public: +public: /*! * Constructor */ @@ -37,16 +38,16 @@ class ColorMapFilter : public filters::FilterBase /*! * Configures the filter. */ - virtual bool configure(); + bool configure() override; /*! * Adds a new color layer. * @param mapIn grid map to add the new layer. * @param mapOut grid map the grid map with the new color layer. */ - virtual bool update(const T& mapIn, T& mapOut); + bool update(const T & mapIn, T & mapOut) override; - private: +private: //! Min./max. colors. Eigen::Vector3f minColor_, maxColor_; @@ -60,4 +61,5 @@ class ColorMapFilter : public filters::FilterBase std::string outputLayer_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__COLORMAPFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/CurvatureFilter.hpp b/grid_map_filters/include/grid_map_filters/CurvatureFilter.hpp index 18858bd43..c2792948f 100644 --- a/grid_map_filters/include/grid_map_filters/CurvatureFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/CurvatureFilter.hpp @@ -6,14 +6,16 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__CURVATUREFILTER_HPP_ +#define GRID_MAP_FILTERS__CURVATUREFILTER_HPP_ -#include +#include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Compute the curvature (second derivative) of a layer in the map. @@ -21,8 +23,7 @@ namespace grid_map { template class CurvatureFilter : public filters::FilterBase { - - public: +public: /*! * Constructor */ @@ -36,7 +37,7 @@ class CurvatureFilter : public filters::FilterBase /*! * Configures the filter from parameters on the Parameter Server */ - virtual bool configure(); + bool configure() override; /*! * Compute the curvature of a layer in a map and @@ -44,9 +45,9 @@ class CurvatureFilter : public filters::FilterBase * @param mapIn grid map containing the layer for which the curvature is computed for. * @param mapOut grid map containing mapIn and the new layer for the curvature. */ - virtual bool update(const T& mapIn, T& mapOut); + bool update(const T & mapIn, T & mapOut) override; - private: +private: //! Input layer name. std::string inputLayer_; @@ -54,4 +55,5 @@ class CurvatureFilter : public filters::FilterBase std::string outputLayer_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__CURVATUREFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/DeletionFilter.hpp b/grid_map_filters/include/grid_map_filters/DeletionFilter.hpp index 048da3219..6b5651c7e 100644 --- a/grid_map_filters/include/grid_map_filters/DeletionFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/DeletionFilter.hpp @@ -6,14 +6,16 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__DELETIONFILTER_HPP_ +#define GRID_MAP_FILTERS__DELETIONFILTER_HPP_ -#include +#include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Deletion filter class deletes layers of a grid map. @@ -21,8 +23,7 @@ namespace grid_map { template class DeletionFilter : public filters::FilterBase { - - public: +public: /*! * Constructor */ @@ -36,20 +37,19 @@ class DeletionFilter : public filters::FilterBase /*! * Configures the filter from parameters on the parameter server. */ - virtual bool configure(); + bool configure() override; /*! * Deletes the specified layers of a grid map. * @param mapIn gridMap with the different layers. * @param mapOut gridMap without the deleted layers. */ - virtual bool update(const T& mapIn, T& mapOut); - - private: + bool update(const T & mapIn, T & mapOut) override; +private: //! List of layers that should be deleted. std::vector layers_; - }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__DELETIONFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/DuplicationFilter.hpp b/grid_map_filters/include/grid_map_filters/DuplicationFilter.hpp index f93bdac79..e523cd34a 100644 --- a/grid_map_filters/include/grid_map_filters/DuplicationFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/DuplicationFilter.hpp @@ -6,13 +6,15 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__DUPLICATIONFILTER_HPP_ +#define GRID_MAP_FILTERS__DUPLICATIONFILTER_HPP_ -#include +#include #include -namespace grid_map { +namespace grid_map +{ /*! * Duplication filter class duplicates a layer of a grid map. @@ -20,8 +22,7 @@ namespace grid_map { template class DuplicationFilter : public filters::FilterBase { - - public: +public: /*! * Constructor */ @@ -35,16 +36,16 @@ class DuplicationFilter : public filters::FilterBase /*! * Configures the filter from parameters on the parameter server. */ - virtual bool configure(); + bool configure() override; /*! * Duplicates the specified layers of a grid map. * @param mapIn with the layer to duplicate. * @param mapOut with the layer duplicated. */ - virtual bool update(const T& mapIn, T& mapOut); + bool update(const T & mapIn, T & mapOut) override; - private: +private: //! Name of the layer that is duplicated. std::string inputLayer_; @@ -52,4 +53,5 @@ class DuplicationFilter : public filters::FilterBase std::string outputLayer_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__DUPLICATIONFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/LightIntensityFilter.hpp b/grid_map_filters/include/grid_map_filters/LightIntensityFilter.hpp index 581a5dfb6..5b463b4a4 100644 --- a/grid_map_filters/include/grid_map_filters/LightIntensityFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/LightIntensityFilter.hpp @@ -6,14 +6,16 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__LIGHTINTENSITYFILTER_HPP_ +#define GRID_MAP_FILTERS__LIGHTINTENSITYFILTER_HPP_ -#include +#include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Compute the diffuse lighting of a surface as new black and white color layer. @@ -21,8 +23,7 @@ namespace grid_map { template class LightIntensityFilter : public filters::FilterBase { - - public: +public: /*! * Constructor */ @@ -36,16 +37,16 @@ class LightIntensityFilter : public filters::FilterBase /*! * Configures the filter from parameters on the Parameter Server */ - virtual bool configure(); + bool configure() override; /*! * Compute the diffuse lighting layer. * @param mapIn grid map containing the layers of the normal vectors. * @param mapOut grid map containing mapIn and the black and white lighting color layer. */ - virtual bool update(const T& mapIn, T& mapOut); + bool update(const T & mapIn, T & mapOut) override; - private: +private: //! Input layers prefix. std::string inputLayersPrefix_; @@ -56,4 +57,5 @@ class LightIntensityFilter : public filters::FilterBase Eigen::Vector3f lightDirection_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__LIGHTINTENSITYFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/MathExpressionFilter.hpp b/grid_map_filters/include/grid_map_filters/MathExpressionFilter.hpp index c921f40a1..66645b2db 100644 --- a/grid_map_filters/include/grid_map_filters/MathExpressionFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/MathExpressionFilter.hpp @@ -6,15 +6,17 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__MATHEXPRESSIONFILTER_HPP_ +#define GRID_MAP_FILTERS__MATHEXPRESSIONFILTER_HPP_ -#include "EigenLab/EigenLab.h" - -#include +#include #include -namespace grid_map { +#include "EigenLab/EigenLab.hpp" + +namespace grid_map +{ /*! * Parses and evaluates a mathematical matrix expression with layers of a grid map. @@ -22,8 +24,7 @@ namespace grid_map { template class MathExpressionFilter : public filters::FilterBase { - - public: +public: /*! * Constructor */ @@ -37,16 +38,16 @@ class MathExpressionFilter : public filters::FilterBase /*! * Configures the filter from parameters on the parameter server. */ - virtual bool configure(); + bool configure() override; /*! * Takes the minimum out of different layers of a grid map. * @param mapIn gridMap with the different layers to take the min. * @param mapOut gridMap with an additional layer containing the sum. */ - virtual bool update(const T& mapIn, T& mapOut); + bool update(const T & mapIn, T & mapOut) override; - private: +private: //! EigenLab parser. EigenLab::Parser parser_; @@ -57,4 +58,5 @@ class MathExpressionFilter : public filters::FilterBase std::string outputLayer_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__MATHEXPRESSIONFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/MeanInRadiusFilter.hpp b/grid_map_filters/include/grid_map_filters/MeanInRadiusFilter.hpp index 3a652705a..fce6bdf73 100644 --- a/grid_map_filters/include/grid_map_filters/MeanInRadiusFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/MeanInRadiusFilter.hpp @@ -6,22 +6,24 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__MEANINRADIUSFILTER_HPP_ +#define GRID_MAP_FILTERS__MEANINRADIUSFILTER_HPP_ -#include +#include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Filter class to find the mean of the values inside a radius. */ template -class MeanInRadiusFilter : public filters::FilterBase { - - public: +class MeanInRadiusFilter : public filters::FilterBase +{ +public: /*! * Constructor */ @@ -35,7 +37,7 @@ class MeanInRadiusFilter : public filters::FilterBase { /*! * Configures the filter from parameters on the Parameter Server */ - virtual bool configure(); + bool configure() override; /*! * Computes for each value in the input layer the mean of all values in a radius around it @@ -43,9 +45,9 @@ class MeanInRadiusFilter : public filters::FilterBase { * @param mapIn grid map containing the input layer. * @param mapOut grid map containing the layers of the input map and the new layer. */ - virtual bool update(const T& mapIn, T& mapOut); + bool update(const T & mapIn, T & mapOut) override; - private: +private: //! Radius to take the mean from. double radius_; @@ -56,4 +58,5 @@ class MeanInRadiusFilter : public filters::FilterBase { std::string outputLayer_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__MEANINRADIUSFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/MinInRadiusFilter.hpp b/grid_map_filters/include/grid_map_filters/MinInRadiusFilter.hpp index 6e4be578c..81023572e 100644 --- a/grid_map_filters/include/grid_map_filters/MinInRadiusFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/MinInRadiusFilter.hpp @@ -6,21 +6,23 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__MININRADIUSFILTER_HPP_ +#define GRID_MAP_FILTERS__MININRADIUSFILTER_HPP_ -#include +#include #include -namespace grid_map { +namespace grid_map +{ /*! * Filter class to compute the minimal value inside a radius. */ template -class MinInRadiusFilter : public filters::FilterBase { - - public: +class MinInRadiusFilter : public filters::FilterBase +{ +public: /*! * Constructor. */ @@ -34,7 +36,7 @@ class MinInRadiusFilter : public filters::FilterBase { /*! * Configures the filter from parameters on the Parameter Server */ - virtual bool configure(); + bool configure() override; /*! * Computes for each value in the input layer the minimum of all values in a radius around it. @@ -42,9 +44,9 @@ class MinInRadiusFilter : public filters::FilterBase { * @param mapIn grid map containing the input layer. * @param mapOut grid map containing the original layers and the new layer with the minimal values. */ - virtual bool update(const T& mapIn, T& mapOut); + bool update(const T & mapIn, T & mapOut) override; - private: +private: //! Radius to take the minimum in. double radius_; @@ -55,4 +57,5 @@ class MinInRadiusFilter : public filters::FilterBase { std::string outputLayer_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__MININRADIUSFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/NormalColorMapFilter.hpp b/grid_map_filters/include/grid_map_filters/NormalColorMapFilter.hpp index 0a00d02e8..e20f0d0d7 100644 --- a/grid_map_filters/include/grid_map_filters/NormalColorMapFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/NormalColorMapFilter.hpp @@ -6,14 +6,16 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__NORMALCOLORMAPFILTER_HPP_ +#define GRID_MAP_FILTERS__NORMALCOLORMAPFILTER_HPP_ -#include +#include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Compute a new color layer based on normal vectors layers. @@ -21,8 +23,7 @@ namespace grid_map { template class NormalColorMapFilter : public filters::FilterBase { - - public: +public: /*! * Constructor */ @@ -36,16 +37,16 @@ class NormalColorMapFilter : public filters::FilterBase /*! * Configures the filter from parameters on the Parameter Server */ - virtual bool configure(); + bool configure() override; /*! * Compute a new color layer based on normal vectors layers. * @param mapIn grid map containing the layers of the normal vectors. * @param mapOut grid map containing mapIn and the new color layer. */ - virtual bool update(const T& mapIn, T& mapOut); + bool update(const T & mapIn, T & mapOut) override; - private: +private: //! Input layers prefix. std::string inputLayersPrefix_; @@ -53,4 +54,5 @@ class NormalColorMapFilter : public filters::FilterBase std::string outputLayer_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__NORMALCOLORMAPFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/NormalVectorsFilter.hpp b/grid_map_filters/include/grid_map_filters/NormalVectorsFilter.hpp index fc792a79c..e5b2ab8a7 100644 --- a/grid_map_filters/include/grid_map_filters/NormalVectorsFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/NormalVectorsFilter.hpp @@ -6,22 +6,25 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__NORMALVECTORSFILTER_HPP_ +#define GRID_MAP_FILTERS__NORMALVECTORSFILTER_HPP_ -#include +#include #include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Compute the normal vectors of a layer in a map. */ -template -class NormalVectorsFilter : public filters::FilterBase { - public: +template +class NormalVectorsFilter : public filters::FilterBase +{ +public: /*! * Constructor */ @@ -56,9 +59,9 @@ class NormalVectorsFilter : public filters::FilterBase { * @param mapIn grid map containing the layer for which the normal vectors are computed for. * @param mapOut grid map containing mapIn and the new layers for the normal vectors. */ - bool update(const T& mapIn, T& mapOut) override; + bool update(const T & mapIn, T & mapOut) override; - private: +private: /*! * Estimate the normal vector at each point of the input layer by using the areaSingleNormalComputation function. * This function makes use of the area method and is the serial version of such normal vector computation using a @@ -68,7 +71,9 @@ class NormalVectorsFilter : public filters::FilterBase { * @param inputLayer: Layer the normal vector should be computed for. * @param outputLayersPrefix: Output layer name prefix. */ - void computeWithAreaSerial(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix); + void computeWithAreaSerial( + GridMap & map, const std::string & inputLayer, + const std::string & outputLayersPrefix); /*! * Estimate the normal vector at each point of the input layer by using the areaSingleNormalComputation function. @@ -79,7 +84,9 @@ class NormalVectorsFilter : public filters::FilterBase { * @param inputLayer: Layer the normal vector should be computed for. * @param outputLayersPrefix: Output layer name prefix. */ - void computeWithAreaParallel(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix); + void computeWithAreaParallel( + GridMap & map, const std::string & inputLayer, + const std::string & outputLayersPrefix); /*! * Estimate the normal vector at one point of the input layer, specified by the index parameter, by using points within @@ -105,8 +112,9 @@ class NormalVectorsFilter : public filters::FilterBase { * @param outputLayersPrefix: Output layer name prefix. * @param index: Index of point in the grid map for which this function calculates the normal vector. */ - void areaSingleNormalComputation(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix, - const grid_map::Index& index); + void areaSingleNormalComputation( + GridMap & map, const std::string & inputLayer, const std::string & outputLayersPrefix, + const grid_map::Index & index); /*! * Estimate the normal vector at each point of the input layer by using the rasterSingleNormalComputation function. * This function makes use of the raster method and is the serial version of such normal vector computation using a @@ -116,7 +124,9 @@ class NormalVectorsFilter : public filters::FilterBase { * @param inputLayer: Layer the normal vector should be computed for. * @param outputLayersPrefix: Output layer name prefix. */ - void computeWithRasterSerial(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix); + void computeWithRasterSerial( + GridMap & map, const std::string & inputLayer, + const std::string & outputLayersPrefix); /*! * Estimate the normal vector at each point of the input layer by using the rasterSingleNormalComputation function. @@ -127,7 +137,9 @@ class NormalVectorsFilter : public filters::FilterBase { * @param inputLayer: Layer the normal vector should be computed for. * @param outputLayersPrefix: Output layer name prefix. */ - void computeWithRasterParallel(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix); + void computeWithRasterParallel( + GridMap & map, const std::string & inputLayer, + const std::string & outputLayersPrefix); /*! * Estimate the normal vector at one point of the input layer, specified by the index parameter, by using neighboring points @@ -159,8 +171,9 @@ class NormalVectorsFilter : public filters::FilterBase { * @param dataMap: Matrix containing the input layer of the grid map in question. * @param index: Index of point in the grid map for which this function calculates the normal vector. */ - void rasterSingleNormalComputation(GridMap& map, const std::string& outputLayersPrefix, const grid_map::Matrix& dataMap, - const grid_map::Index& index); + void rasterSingleNormalComputation( + GridMap & map, const std::string & outputLayersPrefix, const grid_map::Matrix & dataMap, + const grid_map::Index & index); enum class Method { AreaSerial, AreaParallel, RasterSerial, RasterParallel }; @@ -189,3 +202,4 @@ class NormalVectorsFilter : public filters::FilterBase { }; } // namespace grid_map +#endif // GRID_MAP_FILTERS__NORMALVECTORSFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/SetBasicLayersFilter.hpp b/grid_map_filters/include/grid_map_filters/SetBasicLayersFilter.hpp index 8cf66298f..16bec807e 100644 --- a/grid_map_filters/include/grid_map_filters/SetBasicLayersFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/SetBasicLayersFilter.hpp @@ -6,14 +6,16 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__SETBASICLAYERSFILTER_HPP_ +#define GRID_MAP_FILTERS__SETBASICLAYERSFILTER_HPP_ -#include +#include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Set specified layers of a grid map as basic layers. @@ -21,7 +23,7 @@ namespace grid_map { template class SetBasicLayersFilter : public filters::FilterBase { - public: +public: /*! * Constructor */ @@ -35,19 +37,19 @@ class SetBasicLayersFilter : public filters::FilterBase /*! * Configures the filter from parameters on the parameter server. */ - virtual bool configure(); + bool configure() override; /*! * Set the specified layers as basic layers. * @param mapIn input grid map. * @param mapOut output grid map with basic layers set. */ - virtual bool update(const T& mapIn, T& mapOut); + bool update(const T & mapIn, T & mapOut) override; - private: +private: //! List of layers that should be set as basic layers. std::vector layers_; - }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__SETBASICLAYERSFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/SlidingWindowMathExpressionFilter.hpp b/grid_map_filters/include/grid_map_filters/SlidingWindowMathExpressionFilter.hpp index ee65afb1c..a71179932 100644 --- a/grid_map_filters/include/grid_map_filters/SlidingWindowMathExpressionFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/SlidingWindowMathExpressionFilter.hpp @@ -6,18 +6,21 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__SLIDINGWINDOWMATHEXPRESSIONFILTER_HPP_ +#define GRID_MAP_FILTERS__SLIDINGWINDOWMATHEXPRESSIONFILTER_HPP_ -#include "EigenLab/EigenLab.h" +#include #include -#include +#include -#include #include -namespace grid_map { +#include "EigenLab/EigenLab.hpp" + +namespace grid_map +{ /*! * Parse and evaluate a mathematical matrix expression within a sliding window on a layer of a grid map. @@ -25,8 +28,7 @@ namespace grid_map { template class SlidingWindowMathExpressionFilter : public filters::FilterBase { - - public: +public: /*! * Constructor */ @@ -40,16 +42,16 @@ class SlidingWindowMathExpressionFilter : public filters::FilterBase /*! * Configures the filter from parameters on the parameter server. */ - virtual bool configure(); + bool configure() override; /*! * Takes the minimum out of different layers of a grid map. * @param mapIn gridMap with the different layers to take the min. * @param mapOut gridMap with an additional layer containing the sum. */ - virtual bool update(const T& mapIn, T& mapOut); + bool update(const T & mapIn, T & mapOut) override; - private: +private: //! Input layer name. std::string inputLayer_; @@ -78,4 +80,5 @@ class SlidingWindowMathExpressionFilter : public filters::FilterBase SlidingWindowIterator::EdgeHandling edgeHandling_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__SLIDINGWINDOWMATHEXPRESSIONFILTER_HPP_ diff --git a/grid_map_filters/include/grid_map_filters/ThresholdFilter.hpp b/grid_map_filters/include/grid_map_filters/ThresholdFilter.hpp index 85861f08c..b228c0cbb 100644 --- a/grid_map_filters/include/grid_map_filters/ThresholdFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/ThresholdFilter.hpp @@ -6,14 +6,16 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_FILTERS__THRESHOLDFILTER_HPP_ +#define GRID_MAP_FILTERS__THRESHOLDFILTER_HPP_ -#include +#include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Threshold filter class to set values below/above a threshold to a @@ -22,8 +24,7 @@ namespace grid_map { template class ThresholdFilter : public filters::FilterBase { - - public: +public: /*! * Constructor */ @@ -37,7 +38,7 @@ class ThresholdFilter : public filters::FilterBase /*! * Configures the filter from parameters on the parameter server. */ - virtual bool configure(); + bool configure() override; /*! * Uses either an upper or lower threshold. If the threshold is exceeded @@ -45,10 +46,9 @@ class ThresholdFilter : public filters::FilterBase * @param mapIn GridMap with the different layers to apply a threshold. * @param mapOut GridMap with the threshold applied to the layers. */ - virtual bool update(const T& mapIn, T& mapOut); - - private: + bool update(const T & mapIn, T & mapOut) override; +private: //! Layer the threshold should be applied to. std::string layer_; @@ -65,4 +65,5 @@ class ThresholdFilter : public filters::FilterBase bool useLowerThreshold_, useUpperThreshold_; }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_FILTERS__THRESHOLDFILTER_HPP_ diff --git a/grid_map_filters/package.xml b/grid_map_filters/package.xml index ce5adface..5df0b17ea 100644 --- a/grid_map_filters/package.xml +++ b/grid_map_filters/package.xml @@ -1,23 +1,34 @@ - + + grid_map_filters - 1.6.2 + 2.2.2 Processing grid maps as a sequence of ROS filters. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Péter Fankhauser Martin Wermelinger - catkin + + ament_cmake + grid_map_cmake_helpers + + filters grid_map_core - grid_map_ros grid_map_msgs - filters + grid_map_ros + pluginlib tbb + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + ament_cmake diff --git a/grid_map_filters/src/BufferNormalizerFilter.cpp b/grid_map_filters/src/BufferNormalizerFilter.cpp index 40fed7272..68fa07ed5 100644 --- a/grid_map_filters/src/BufferNormalizerFilter.cpp +++ b/grid_map_filters/src/BufferNormalizerFilter.cpp @@ -9,11 +9,10 @@ #include "grid_map_filters/BufferNormalizerFilter.hpp" #include -#include +#include -using namespace filters; - -namespace grid_map { +namespace grid_map +{ template BufferNormalizerFilter::BufferNormalizerFilter() @@ -32,13 +31,15 @@ bool BufferNormalizerFilter::configure() } template -bool BufferNormalizerFilter::update(const T& mapIn, T& mapOut) +bool BufferNormalizerFilter::update(const T & mapIn, T & mapOut) { mapOut = mapIn; mapOut.convertToDefaultStartIndex(); return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::BufferNormalizerFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::BufferNormalizerFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/ColorBlendingFilter.cpp b/grid_map_filters/src/ColorBlendingFilter.cpp index 480d03f3a..ab38b400c 100644 --- a/grid_map_filters/src/ColorBlendingFilter.cpp +++ b/grid_map_filters/src/ColorBlendingFilter.cpp @@ -9,19 +9,22 @@ #include #include -#include +#include #include #include -using namespace filters; +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template ColorBlendingFilter::ColorBlendingFilter() - : opacity_(1.0), - blendMode_(BlendModes::Normal) +: opacity_(1.0), + blendMode_(BlendModes::Normal) { } @@ -33,57 +36,79 @@ ColorBlendingFilter::~ColorBlendingFilter() template bool ColorBlendingFilter::configure() { - if (!FilterBase < T > ::getParam(std::string("background_layer"), backgroundLayer_)) { - ROS_ERROR("Color blending filter did not find parameter `background_layer`."); + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("background_layer"), backgroundLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color blending filter did not find parameter `background_layer`."); return false; } - ROS_DEBUG("Color blending filter background layer is = %s.", backgroundLayer_.c_str()); - - if (!FilterBase < T > ::getParam(std::string("foreground_layer"), foregroundLayer_)) { - ROS_ERROR("Color blending filter did not find parameter `foreground_layer`."); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Color blending filter background layer is = %s.", + backgroundLayer_.c_str()); + + if (!param_reader.get(std::string("foreground_layer"), foregroundLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color blending filter did not find parameter `foreground_layer`."); return false; } - ROS_DEBUG("Color blending filter foreground layer is = %s.", foregroundLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Color blending filter foreground layer is = %s.", + foregroundLayer_.c_str()); std::string blendMode; - if (!FilterBase < T > ::getParam(std::string("blend_mode"), blendMode)) { + if (!param_reader.get(std::string("blend_mode"), blendMode)) { blendMode = "normal"; } - ROS_DEBUG("Color blending filter blend mode is = %s.", blendMode.c_str()); - if (blendMode == "normal") blendMode_ = BlendModes::Normal; - else if (blendMode == "hard_light") blendMode_ = BlendModes::HardLight; - else if (blendMode == "soft_light") blendMode_ = BlendModes::SoftLight; - else { - ROS_ERROR("Color blending filter blend mode `%s` does not exist.", blendMode.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Color blending filter blend mode is = %s.", + blendMode.c_str()); + if (blendMode == "normal") { + blendMode_ = BlendModes::Normal; + } else if (blendMode == "hard_light") { + blendMode_ = BlendModes::HardLight; + } else if (blendMode == "soft_light") {blendMode_ = BlendModes::SoftLight;} else { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color blending filter blend mode `%s` does not exist.", blendMode.c_str()); return false; } - if (!FilterBase < T > ::getParam(std::string("opacity"), opacity_)) { - ROS_ERROR("Color blending filter did not find parameter `opacity`."); + if (!param_reader.get(std::string("opacity"), opacity_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color blending filter did not find parameter `opacity`."); return false; } - ROS_DEBUG("Color blending filter opacity is = %f.", opacity_); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Color blending filter opacity is = %f.", opacity_); - if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) { - ROS_ERROR("Color blending filter did not find parameter `output_layer`."); + if (!param_reader.get(std::string("output_layer"), outputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color blending filter did not find parameter `output_layer`."); return false; } - ROS_DEBUG("Color blending filter output_layer = %s.", outputLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Color blending filter output_layer = %s.", + outputLayer_.c_str()); return true; } template -bool ColorBlendingFilter::update(const T& mapIn, T& mapOut) +bool ColorBlendingFilter::update(const T & mapIn, T & mapOut) { - const auto& background = mapIn[backgroundLayer_]; - const auto& foreground = mapIn[foregroundLayer_]; + const auto & background = mapIn[backgroundLayer_]; + const auto & foreground = mapIn[foregroundLayer_]; mapOut = mapIn; mapOut.add(outputLayer_); - auto& output = mapOut[outputLayer_]; + auto & output = mapOut[outputLayer_]; // For each cell in map. - for (size_t i = 0; i < output.size(); ++i) { + for (int64_t i = 0; i < output.size(); ++i) { if (std::isnan(background(i))) { output(i) = foreground(i); } else if (std::isnan(foreground(i))) { @@ -95,46 +120,51 @@ bool ColorBlendingFilter::update(const T& mapIn, T& mapOut) backgroundColor = color.array(); colorValueToVector(foreground(i), color); foregroundColor = color.array(); + outputColor = Eigen::Array3f::Zero(); switch (blendMode_) { case BlendModes::Normal: outputColor = (1.0 - opacity_) * backgroundColor + opacity_ * foregroundColor; break; case BlendModes::HardLight: - { - Eigen::Array3f blendedColor; - if (foregroundColor.mean() < 0.5) { - blendedColor = 2.0 * backgroundColor * foregroundColor; - } else { - blendedColor = 1.0 - 2.0 * (1.0 - backgroundColor) * (1.0 - foregroundColor); - } - if (opacity_ == 1.0) { - outputColor = blendedColor; - } else { - outputColor = (1.0 - opacity_) * backgroundColor + opacity_ * blendedColor; + { + Eigen::Array3f blendedColor; + if (foregroundColor.mean() < 0.5) { + blendedColor = 2.0 * backgroundColor * foregroundColor; + } else { + blendedColor = 1.0 - 2.0 * (1.0 - backgroundColor) * (1.0 - foregroundColor); + } + if (opacity_ == 1.0) { + outputColor = blendedColor; + } else { + outputColor = (1.0 - opacity_) * backgroundColor + opacity_ * blendedColor; + } + + break; } - - break; - } case BlendModes::SoftLight: - { - Eigen::Array3f blendedColor; - // Photoshop. + { + Eigen::Array3f blendedColor; + // Photoshop. // if (foregroundColor.mean() < 0.5) { -// blendedColor = 2.0 * backgroundColor * foregroundColor + backgroundColor.square() * (1.0 - 2.0 * foregroundColor); +// blendedColor = 2.0 * backgroundColor * foregroundColor + +// backgroundColor.square() * (1.0 - 2.0 * foregroundColor); // } else { -// blendedColor = 2.0 * backgroundColor * (1.0 - foregroundColor) + backgroundColor.sqrt() * (2.0 * foregroundColor - 1.0); +// blendedColor = 2.0 * backgroundColor * (1.0 - foregroundColor) + +// backgroundColor.sqrt() * (2.0 * foregroundColor - 1.0); // } - // Pegtop. - blendedColor = ((1.0 - 2.0 * foregroundColor) * backgroundColor.square() + 2.0 * backgroundColor * foregroundColor); - if (opacity_ == 1.0) { - outputColor = blendedColor; - } else { - outputColor = (1.0 - opacity_) * backgroundColor + opacity_ * blendedColor; + // Pegtop. + blendedColor = + ((1.0 - 2.0 * foregroundColor) * backgroundColor.square() + 2.0 * backgroundColor * + foregroundColor); + if (opacity_ == 1.0) { + outputColor = blendedColor; + } else { + outputColor = (1.0 - opacity_) * backgroundColor + opacity_ * blendedColor; + } + + break; } - - break; - } } colorVectorToValue(Eigen::Vector3f(outputColor), output(i)); @@ -144,6 +174,8 @@ bool ColorBlendingFilter::update(const T& mapIn, T& mapOut) return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::ColorBlendingFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::ColorBlendingFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/ColorFillFilter.cpp b/grid_map_filters/src/ColorFillFilter.cpp index bc32fb760..510946b8c 100644 --- a/grid_map_filters/src/ColorFillFilter.cpp +++ b/grid_map_filters/src/ColorFillFilter.cpp @@ -9,19 +9,22 @@ #include #include -#include +#include #include -using namespace filters; +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template ColorFillFilter::ColorFillFilter() - : r_(0.0), - g_(0.0), - b_(0.0) +: r_(0.0), + g_(0.0), + b_(0.0) { } @@ -33,37 +36,54 @@ ColorFillFilter::~ColorFillFilter() template bool ColorFillFilter::configure() { - if (!FilterBase < T > ::getParam(std::string("red"), r_)) { - ROS_ERROR("Color fill filter did not find parameter `red`."); + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("red"), r_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), "Color fill filter did not find parameter `red`."); return false; } - ROS_DEBUG("Color fill filter red is = %f.", r_); + RCLCPP_DEBUG(this->logging_interface_->get_logger(), "Color fill filter red is = %f.", r_); - if (!FilterBase < T > ::getParam(std::string("green"), g_)) { - ROS_ERROR("Color fill filter did not find parameter `green`."); + if (!param_reader.get(std::string("green"), g_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color fill filter did not find parameter `green`."); return false; } - ROS_DEBUG("Color fill filter green is = %f.", g_); + RCLCPP_DEBUG(this->logging_interface_->get_logger(), "Color fill filter green is = %f.", g_); - if (!FilterBase < T > ::getParam(std::string("blue"), b_)) { - ROS_ERROR("Color fill filter did not find parameter `blue`."); + if (!param_reader.get(std::string("blue"), b_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), "Color fill filter did not find parameter `blue`."); return false; } - ROS_DEBUG("Color fill filter blue is = %f.", b_); + RCLCPP_DEBUG(this->logging_interface_->get_logger(), "Color fill filter blue is = %f.", b_); - if (!FilterBase < T > ::getParam(std::string("mask_layer"), maskLayer_)); - ROS_DEBUG("Color fill filter mask_layer = %s.", maskLayer_.c_str()); - - if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) { - ROS_ERROR("Color fill filter did not find parameter `output_layer`."); + if (!param_reader.get(std::string("mask_layer"), maskLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color fill filter did not find parameter `mask_layer`."); + return false; + } + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Color fill filter mask_layer = %s.", + maskLayer_.c_str()); + + if (!param_reader.get(std::string("output_layer"), outputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color fill filter did not find parameter `output_layer`."); return false; } - ROS_DEBUG("Color fill filter output_layer = %s.", outputLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Color fill filter output_layer = %s.", + outputLayer_.c_str()); return true; } template -bool ColorFillFilter::update(const T& mapIn, T& mapOut) +bool ColorFillFilter::update(const T & mapIn, T & mapOut) { mapOut = mapIn; const Eigen::Vector3f colorVector(r_, g_, b_); @@ -75,17 +95,19 @@ bool ColorFillFilter::update(const T& mapIn, T& mapOut) } else { mapOut.add(outputLayer_); - auto& output = mapOut[outputLayer_]; - auto& mask = mapOut[maskLayer_]; + auto & output = mapOut[outputLayer_]; + auto & mask = mapOut[maskLayer_]; // For each cell in map. - for (size_t i = 0; i < output.size(); ++i) { + for (int64_t i = 0; i < output.size(); ++i) { output(i) = std::isfinite(mask(i)) ? colorValue : NAN; } } return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::ColorFillFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::ColorFillFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/ColorMapFilter.cpp b/grid_map_filters/src/ColorMapFilter.cpp index 25b7fca4a..e6723e983 100644 --- a/grid_map_filters/src/ColorMapFilter.cpp +++ b/grid_map_filters/src/ColorMapFilter.cpp @@ -9,18 +9,23 @@ #include #include -#include +#include #include -using namespace filters; +#include +#include +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template ColorMapFilter::ColorMapFilter() - : min_(0.0), - max_(1.0) +: min_(0.0), + max_(1.0) { } @@ -32,45 +37,67 @@ ColorMapFilter::~ColorMapFilter() template bool ColorMapFilter::configure() { - if (!FilterBase::getParam(std::string("input_layer"), inputLayer_)) { - ROS_ERROR("Color map filter did not find parameter `input_layer`."); + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("input_layer"), inputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color map filter did not find parameter `input_layer`."); return false; } - ROS_DEBUG("Color map filter input_layer = %s.", inputLayer_.c_str()); - - if (!FilterBase::getParam(std::string("output_layer"), outputLayer_)) { - ROS_ERROR("Color map filter did not find parameter `output_layer`."); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Color map filter input_layer = %s.", + inputLayer_.c_str()); + + if (!param_reader.get(std::string("output_layer"), outputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color map filter did not find parameter `output_layer`."); return false; } - ROS_DEBUG("Color map filter output_layer = %s.", outputLayer_.c_str()); - - if (!FilterBase::getParam(std::string("min/value"), min_)) { - ROS_ERROR("Color map filter did not find parameter `min/value`."); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Color map filter output_layer = %s.", + outputLayer_.c_str()); + + if (!param_reader.get(std::string("min/value"), min_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color map filter did not find parameter `min/value`."); return false; } - if (!FilterBase::getParam(std::string("max/value"), max_)) { - ROS_ERROR("Color map filter did not find parameter `max/value`."); + if (!param_reader.get(std::string("max/value"), max_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color map filter did not find parameter `max/value`."); return false; } std::vector minColor; - if (!FilterBase::getParam(std::string("min/color"), minColor)) { - ROS_ERROR("Color map filter did not find parameter `min/color`."); + if (!param_reader.get(std::string("min/color"), minColor)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color map filter did not find parameter `min/color`."); return false; } if (minColor.size() != 3) { - ROS_ERROR("Color map filter parameter `min/color` needs to be of size 3."); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color map filter parameter `min/color` needs to be of size 3."); return false; } minColor_ << minColor[0], minColor[1], minColor[2]; std::vector maxColor; - if (!FilterBase::getParam(std::string("max/color"), maxColor)) { - ROS_ERROR("Color map filter did not find parameter `max/color`."); + if (!param_reader.get(std::string("max/color"), maxColor)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color map filter did not find parameter `max/color`."); return false; } if (maxColor.size() != 3) { - ROS_ERROR("Color map filter parameter `max/color` needs to be of size 3."); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Color map filter parameter `max/color` needs to be of size 3."); return false; } maxColor_ << maxColor[0], maxColor[1], maxColor[2]; @@ -79,19 +106,19 @@ bool ColorMapFilter::configure() } template -bool ColorMapFilter::update(const T& mapIn, T& mapOut) +bool ColorMapFilter::update(const T & mapIn, T & mapOut) { mapOut = mapIn; - auto& input = mapIn[inputLayer_]; + auto & input = mapIn[inputLayer_]; mapOut.add(outputLayer_); - auto& output = mapOut[outputLayer_]; + auto & output = mapOut[outputLayer_]; const double range = max_ - min_; const Eigen::Vector3f colorRange = maxColor_ - minColor_; // For each cell in map. - for (size_t i = 0; i < output.size(); ++i) { - if (!std::isfinite(input(i))) continue; + for (int64_t i = 0; i < output.size(); ++i) { + if (!std::isfinite(input(i))) {continue;} const double value = std::min(std::max(input(i), min_), max_); const double factor = (value - min_) / range; const Eigen::Vector3f color = minColor_ + factor * colorRange; @@ -101,6 +128,8 @@ bool ColorMapFilter::update(const T& mapIn, T& mapOut) return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::ColorMapFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::ColorMapFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/CurvatureFilter.cpp b/grid_map_filters/src/CurvatureFilter.cpp index ccb9a3d1e..bebcdac1d 100644 --- a/grid_map_filters/src/CurvatureFilter.cpp +++ b/grid_map_filters/src/CurvatureFilter.cpp @@ -9,13 +9,16 @@ #include #include -#include +#include #include -using namespace filters; +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template CurvatureFilter::CurvatureFilter() @@ -30,41 +33,61 @@ CurvatureFilter::~CurvatureFilter() template bool CurvatureFilter::configure() { - if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) { - ROS_ERROR("Curvature filter did not find parameter `input_layer`."); + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("input_layer"), inputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Curvature filter did not find parameter `input_layer`."); return false; } - ROS_DEBUG("Curvature filter input layer is = %s.", inputLayer_.c_str()); - - if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) { - ROS_ERROR("Curvature filter did not find parameter `output_layer`."); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Curvature filter input layer is = %s.", + inputLayer_.c_str()); + + if (!param_reader.get(std::string("output_layer"), outputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Curvature filter did not find parameter `output_layer`."); return false; } - ROS_DEBUG("Curvature filter output_layer = %s.", outputLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Curvature filter output_layer = %s.", + outputLayer_.c_str()); return true; } template -bool CurvatureFilter::update(const T& mapIn, T& mapOut) +bool CurvatureFilter::update(const T & mapIn, T & mapOut) { - if (!mapIn.isDefaultStartIndex()) throw std::runtime_error( - "CurvatureFilter cannot be used with grid maps that don't have a default buffer start index."); + if (!mapIn.isDefaultStartIndex()) { + throw std::runtime_error( + "CurvatureFilter cannot be used with grid maps" + " that don't have a default buffer start index."); + } mapOut = mapIn; mapOut.add(outputLayer_); - auto& input = mapOut[inputLayer_]; - auto& curvature = mapOut[outputLayer_]; + auto & input = mapOut[inputLayer_]; + auto & curvature = mapOut[outputLayer_]; const float L2 = mapOut.getResolution() * mapOut.getResolution(); - for (size_t j = 0; j < input.cols(); ++j) { - for (size_t i = 0; i < input.rows(); ++i) { + for (int64_t j = 0; j < input.cols(); ++j) { + for (int64_t i = 0; i < input.rows(); ++i) { // http://help.arcgis.com/en/arcgisdesktop/10.0/help/index.html#/How_Curvature_works/00q90000000t000000/ - if (!std::isfinite(input(i, j))) continue; - float D = ((input(i, j==0 ? j : j-1) + input(i, j==input.cols()-1 ? j : j + 1)) / 2.0 - input(i, j)) / L2; - float E = ((input(i==0 ? i : i-1, j) + input(i==input.rows()-1 ? i : i + 1, j)) / 2.0 - input(i, j)) / L2; - if (!std::isfinite(D)) D = 0.0; - if (!std::isfinite(E)) E = 0.0; + if (!std::isfinite(input(i, j))) {continue;} + float D = + ((input( + i, + j == + 0 ? j : j - 1) + input(i, j == input.cols() - 1 ? j : j + 1)) / 2.0 - input(i, j)) / L2; + float E = + ((input( + i == 0 ? i : i - 1, + j) + input(i == input.rows() - 1 ? i : i + 1, j)) / 2.0 - input(i, j)) / L2; + if (!std::isfinite(D)) {D = 0.0;} + if (!std::isfinite(E)) {E = 0.0;} curvature(i, j) = -2.0 * (D + E); } } @@ -72,6 +95,8 @@ bool CurvatureFilter::update(const T& mapIn, T& mapOut) return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::CurvatureFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::CurvatureFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/DeletionFilter.cpp b/grid_map_filters/src/DeletionFilter.cpp index d916a9f12..af0d48591 100644 --- a/grid_map_filters/src/DeletionFilter.cpp +++ b/grid_map_filters/src/DeletionFilter.cpp @@ -9,11 +9,14 @@ #include "grid_map_filters/DeletionFilter.hpp" #include -#include +#include -using namespace filters; +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template DeletionFilter::DeletionFilter() @@ -28,9 +31,12 @@ DeletionFilter::~DeletionFilter() template bool DeletionFilter::configure() { + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + // Load Parameters - if (!FilterBase::getParam(std::string("layers"), layers_)) { - ROS_ERROR("DeletionFilter did not find parameter 'layers'."); + if (!param_reader.get(std::string("layers"), layers_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), "DeletionFilter did not find parameter 'layers'."); return false; } @@ -38,26 +44,32 @@ bool DeletionFilter::configure() } template -bool DeletionFilter::update(const T& mapIn, T& mapOut) +bool DeletionFilter::update(const T & mapIn, T & mapOut) { mapOut = mapIn; - for (const auto& layer : layers_) { + for (const auto & layer : layers_) { // Check if layer exists. if (!mapOut.exists(layer)) { - ROS_ERROR("Check your deletion layers! Type %s does not exist.", - layer.c_str()); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Check your deletion layers! Type %s does not exist.", + layer.c_str()); continue; } if (!mapOut.erase(layer)) { - ROS_ERROR("Could not remove type %s.", layer.c_str()); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), "Could not remove type %s.", + layer.c_str()); } } return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::DeletionFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::DeletionFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/DuplicationFilter.cpp b/grid_map_filters/src/DuplicationFilter.cpp index 1244bca67..d19d5ab59 100644 --- a/grid_map_filters/src/DuplicationFilter.cpp +++ b/grid_map_filters/src/DuplicationFilter.cpp @@ -9,11 +9,14 @@ #include "grid_map_filters/DuplicationFilter.hpp" #include -#include +#include -using namespace filters; +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template DuplicationFilter::DuplicationFilter() @@ -28,13 +31,19 @@ DuplicationFilter::~DuplicationFilter() template bool DuplicationFilter::configure() { - if (!FilterBase::getParam(std::string("input_layer"), inputLayer_)) { - ROS_ERROR("DuplicationFilter did not find parameter 'input_layer'."); + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("input_layer"), inputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "DuplicationFilter did not find parameter 'input_layer'."); return false; } - if (!FilterBase::getParam(std::string("output_layer"), outputLayer_)) { - ROS_ERROR("DuplicationFilter did not find parameter 'output_layer'."); + if (!param_reader.get(std::string("output_layer"), outputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "DuplicationFilter did not find parameter 'output_layer'."); return false; } @@ -42,13 +51,15 @@ bool DuplicationFilter::configure() } template -bool DuplicationFilter::update(const T& mapIn, T& mapOut) +bool DuplicationFilter::update(const T & mapIn, T & mapOut) { mapOut = mapIn; mapOut.add(outputLayer_, mapIn[inputLayer_]); return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::DuplicationFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::DuplicationFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/LightIntensityFilter.cpp b/grid_map_filters/src/LightIntensityFilter.cpp index f7afb6568..ef18d8d30 100644 --- a/grid_map_filters/src/LightIntensityFilter.cpp +++ b/grid_map_filters/src/LightIntensityFilter.cpp @@ -9,13 +9,18 @@ #include #include -#include +#include #include -using namespace filters; +#include +#include +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template LightIntensityFilter::LightIntensityFilter() @@ -30,47 +35,64 @@ LightIntensityFilter::~LightIntensityFilter() template bool LightIntensityFilter::configure() { - if (!FilterBase < T > ::getParam(std::string("input_layers_prefix"), inputLayersPrefix_)) { - ROS_ERROR("Light intensity filter did not find parameter `input_layers_prefix`."); + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("input_layers_prefix"), inputLayersPrefix_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Light intensity filter did not find parameter `input_layers_prefix`."); return false; } - ROS_DEBUG("Light intensity filter input layers prefix is = %s.", inputLayersPrefix_.c_str()); - - if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) { - ROS_ERROR("Light intensity filter did not find parameter `output_layer`."); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Light intensity filter input layers prefix is = %s.", + inputLayersPrefix_.c_str()); + + if (!param_reader.get(std::string("output_layer"), outputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Light intensity filter did not find parameter `output_layer`."); return false; } - ROS_DEBUG("Light intensity filter output_layer = %s.", outputLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Light intensity filter output_layer = %s.", + outputLayer_.c_str()); std::vector lightDirection; - if (!FilterBase < T > ::getParam(std::string("light_direction"), lightDirection)) { - ROS_ERROR("Light intensity filter did not find parameter `light_direction`."); + if (!param_reader.get(std::string("light_direction"), lightDirection)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Light intensity filter did not find parameter `light_direction`."); return false; } if (lightDirection.size() != 3) { - ROS_ERROR("Light intensity filter parameter `light_direction` needs to be of size 3."); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Light intensity filter parameter `light_direction` needs to be of size 3."); return false; } lightDirection_ << lightDirection[0], lightDirection[1], lightDirection[2]; lightDirection_.normalize(); - ROS_DEBUG_STREAM("Light intensity filter light_direction: " << lightDirection_.transpose() << "."); + RCLCPP_DEBUG_STREAM( + this->logging_interface_->get_logger(), + "Light intensity filter light_direction: " << lightDirection_.transpose() << + "."); return true; } template -bool LightIntensityFilter::update(const T& mapIn, T& mapOut) +bool LightIntensityFilter::update(const T & mapIn, T & mapOut) { - const auto& normalX = mapIn[inputLayersPrefix_ + "x"]; - const auto& normalY = mapIn[inputLayersPrefix_ + "y"]; - const auto& normalZ = mapIn[inputLayersPrefix_ + "z"]; + const auto & normalX = mapIn[inputLayersPrefix_ + "x"]; + const auto & normalY = mapIn[inputLayersPrefix_ + "y"]; + const auto & normalZ = mapIn[inputLayersPrefix_ + "z"]; mapOut = mapIn; mapOut.add(outputLayer_); - auto& color = mapOut[outputLayer_]; + auto & color = mapOut[outputLayer_]; // For each cell in map. - for (size_t i = 0; i < color.size(); ++i) { + for (int64_t i = 0; i < color.size(); ++i) { if (!std::isfinite(normalZ(i))) { color(i) = NAN; continue; @@ -84,6 +106,8 @@ bool LightIntensityFilter::update(const T& mapIn, T& mapOut) return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::LightIntensityFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::LightIntensityFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/MathExpressionFilter.cpp b/grid_map_filters/src/MathExpressionFilter.cpp index 961af417a..4471e1268 100644 --- a/grid_map_filters/src/MathExpressionFilter.cpp +++ b/grid_map_filters/src/MathExpressionFilter.cpp @@ -9,11 +9,14 @@ #include "grid_map_filters/MathExpressionFilter.hpp" #include -#include +#include -using namespace filters; +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template MathExpressionFilter::MathExpressionFilter() @@ -28,26 +31,32 @@ MathExpressionFilter::~MathExpressionFilter() template bool MathExpressionFilter::configure() { - if (!FilterBase::getParam(std::string("expression"), expression_)) { - ROS_ERROR("MathExpressionFilter did not find parameter 'expression'."); + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("expression"), expression_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "MathExpressionFilter did not find parameter 'expression'."); return false; } - if (!FilterBase::getParam(std::string("output_layer"), outputLayer_)) { - ROS_ERROR("MathExpressionFilter did not find parameter 'output_layer'."); + if (!param_reader.get(std::string("output_layer"), outputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "MathExpressionFilter did not find parameter 'output_layer'."); return false; } - // TODO Can we make caching work with changing shared variable? + // TODO(needs_assignment): Can we make caching work with changing shared variable? // parser_.setCacheExpressions(true); return true; } template -bool MathExpressionFilter::update(const T& mapIn, T& mapOut) +bool MathExpressionFilter::update(const T & mapIn, T & mapOut) { mapOut = mapIn; - for (const auto& layer : mapOut.getLayers()) { + for (const auto & layer : mapOut.getLayers()) { parser_.var(layer).setShared(mapOut[layer]); } EigenLab::Value result(parser_.eval(expression_)); @@ -55,6 +64,8 @@ bool MathExpressionFilter::update(const T& mapIn, T& mapOut) return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::MathExpressionFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::MathExpressionFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/MeanInRadiusFilter.cpp b/grid_map_filters/src/MeanInRadiusFilter.cpp index 956f82cb0..632659e35 100644 --- a/grid_map_filters/src/MeanInRadiusFilter.cpp +++ b/grid_map_filters/src/MeanInRadiusFilter.cpp @@ -9,15 +9,18 @@ #include "grid_map_filters/MeanInRadiusFilter.hpp" #include -#include +#include -using namespace filters; +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template MeanInRadiusFilter::MeanInRadiusFilter() - : radius_(0.0) +: radius_(0.0) { } @@ -29,36 +32,50 @@ MeanInRadiusFilter::~MeanInRadiusFilter() template bool MeanInRadiusFilter::configure() { - if (!FilterBase < T > ::getParam(std::string("radius"), radius_)) { - ROS_ERROR("MeanInRadius filter did not find parameter `radius`."); + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("radius"), radius_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "MeanInRadius filter did not find parameter `radius`."); return false; } if (radius_ < 0.0) { - ROS_ERROR("MeanInRadius filter: Radius must be greater than zero."); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "MeanInRadius filter: Radius must be greater than zero."); return false; } - ROS_DEBUG("Radius = %f.", radius_); + RCLCPP_DEBUG(this->logging_interface_->get_logger(), "Radius = %f.", radius_); - if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) { - ROS_ERROR("MeanInRadius filter did not find parameter `input_layer`."); + if (!param_reader.get(std::string("input_layer"), inputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "MeanInRadius filter did not find parameter `input_layer`."); return false; } - ROS_DEBUG("MeanInRadius input layer is = %s.", inputLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "MeanInRadius input layer is = %s.", + inputLayer_.c_str()); - if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) { - ROS_ERROR("MeanInRadius filter did not find parameter `output_layer`."); + if (!param_reader.get(std::string("output_layer"), outputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "MeanInRadius filter did not find parameter `output_layer`."); return false; } - ROS_DEBUG("MeanInRadius output_layer = %s.", outputLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "MeanInRadius output_layer = %s.", + outputLayer_.c_str()); return true; } template -bool MeanInRadiusFilter::update(const T& mapIn, T& mapOut) +bool MeanInRadiusFilter::update(const T & mapIn, T & mapOut) { // Add new layers to the elevation map. mapOut = mapIn; @@ -68,7 +85,6 @@ bool MeanInRadiusFilter::update(const T& mapIn, T& mapOut) // First iteration through the elevation map. for (grid_map::GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) { - double valueSum = 0.0; int counter = 0; // Requested position (center) of circle in map. @@ -76,22 +92,28 @@ bool MeanInRadiusFilter::update(const T& mapIn, T& mapOut) mapOut.getPosition(*iterator, center); // Find the mean in a circle around the center - for (grid_map::CircleIterator submapIterator(mapOut, center, radius_); !submapIterator.isPastEnd(); - ++submapIterator) { - if (!mapOut.isValid(*submapIterator, inputLayer_)) + for (grid_map::CircleIterator submapIterator(mapOut, center, radius_); + !submapIterator.isPastEnd(); + ++submapIterator) + { + if (!mapOut.isValid(*submapIterator, inputLayer_)) { continue; + } value = mapOut.at(inputLayer_, *submapIterator); valueSum += value; counter++; } - if (counter != 0) + if (counter != 0) { mapOut.at(outputLayer_, *iterator) = valueSum / counter; + } } return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::MeanInRadiusFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::MeanInRadiusFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/MinInRadiusFilter.cpp b/grid_map_filters/src/MinInRadiusFilter.cpp index 5934c1460..e56c7c36d 100644 --- a/grid_map_filters/src/MinInRadiusFilter.cpp +++ b/grid_map_filters/src/MinInRadiusFilter.cpp @@ -9,15 +9,18 @@ #include "grid_map_filters/MinInRadiusFilter.hpp" #include -#include +#include -using namespace filters; +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template MinInRadiusFilter::MinInRadiusFilter() - : radius_(0.0) +: radius_(0.0) { } @@ -29,35 +32,49 @@ MinInRadiusFilter::~MinInRadiusFilter() template bool MinInRadiusFilter::configure() { - if (!FilterBase < T > ::getParam(std::string("radius"), radius_)) { - ROS_ERROR("MinInRadius filter did not find parameter `radius`."); + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("radius"), radius_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "MinInRadius filter did not find parameter `radius`."); return false; } if (radius_ < 0.0) { - ROS_ERROR("MinInRadius filter: Radius must be greater than zero."); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "MinInRadius filter: Radius must be greater than zero."); return false; } - ROS_DEBUG("Radius = %f.", radius_); + RCLCPP_DEBUG(this->logging_interface_->get_logger(), "Radius = %f.", radius_); - if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) { - ROS_ERROR("MinInRadius filter did not find parameter `input_layer`."); + if (!param_reader.get(std::string("input_layer"), inputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "MinInRadius filter did not find parameter `input_layer`."); return false; } - ROS_DEBUG("MinInRadius input layer is = %s.", inputLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "MinInRadius input layer is = %s.", + inputLayer_.c_str()); - if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) { - ROS_ERROR("Step filter did not find parameter `output_layer`."); + if (!param_reader.get(std::string("output_layer"), outputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Step filter did not find parameter `output_layer`."); return false; } - ROS_DEBUG("MinInRadius output_layer = %s.", outputLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "MinInRadius output_layer = %s.", + outputLayer_.c_str()); return true; } template -bool MinInRadiusFilter::update(const T& mapIn, T& mapOut) +bool MinInRadiusFilter::update(const T & mapIn, T & mapOut) { // Add new layer to the elevation map. mapOut = mapIn; @@ -67,8 +84,9 @@ bool MinInRadiusFilter::update(const T& mapIn, T& mapOut) // First iteration through the elevation map. for (grid_map::GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) { - if (!mapOut.isValid(*iterator, inputLayer_)) + if (!mapOut.isValid(*iterator, inputLayer_)) { continue; + } value = mapOut.at(inputLayer_, *iterator); double valueMin = 0.0; @@ -78,10 +96,13 @@ bool MinInRadiusFilter::update(const T& mapIn, T& mapOut) // Get minimal value in the circular window. bool init = false; - for (grid_map::CircleIterator submapIterator(mapOut, center, radius_); !submapIterator.isPastEnd(); - ++submapIterator) { - if (!mapOut.isValid(*submapIterator, inputLayer_)) + for (grid_map::CircleIterator submapIterator(mapOut, center, radius_); + !submapIterator.isPastEnd(); + ++submapIterator) + { + if (!mapOut.isValid(*submapIterator, inputLayer_)) { continue; + } value = mapOut.at(inputLayer_, *submapIterator); if (!init) { @@ -89,17 +110,21 @@ bool MinInRadiusFilter::update(const T& mapIn, T& mapOut) init = true; continue; } - if (value < valueMin) + if (value < valueMin) { valueMin = value; + } } - if (init) + if (init) { mapOut.at(outputLayer_, *iterator) = valueMin; + } } return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::MinInRadiusFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::MinInRadiusFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/NormalColorMapFilter.cpp b/grid_map_filters/src/NormalColorMapFilter.cpp index 3a729735e..72602068e 100644 --- a/grid_map_filters/src/NormalColorMapFilter.cpp +++ b/grid_map_filters/src/NormalColorMapFilter.cpp @@ -9,13 +9,16 @@ #include #include -#include +#include #include -using namespace filters; +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template NormalColorMapFilter::NormalColorMapFilter() @@ -30,46 +33,58 @@ NormalColorMapFilter::~NormalColorMapFilter() template bool NormalColorMapFilter::configure() { - if (!FilterBase < T > ::getParam(std::string("input_layers_prefix"), inputLayersPrefix_)) { - ROS_ERROR("Normal color map filter did not find parameter `input_layers_prefix`."); + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("input_layers_prefix"), inputLayersPrefix_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Normal color map filter did not find parameter `input_layers_prefix`."); return false; } - ROS_DEBUG("Normal color map filter input layers prefix is = %s.", inputLayersPrefix_.c_str()); - - if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) { - ROS_ERROR("Normal color map filter did not find parameter `output_layer`."); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Normal color map filter input layers prefix is = %s.", + inputLayersPrefix_.c_str()); + + if (!param_reader.get(std::string("output_layer"), outputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Normal color map filter did not find parameter `output_layer`."); return false; } - ROS_DEBUG("Normal color map filter output_layer = %s.", outputLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Normal color map filter output_layer = %s.", + outputLayer_.c_str()); return true; } template -bool NormalColorMapFilter::update(const T& mapIn, T& mapOut) +bool NormalColorMapFilter::update(const T & mapIn, T & mapOut) { - const auto& normalX = mapIn[inputLayersPrefix_ + "x"]; - const auto& normalY = mapIn[inputLayersPrefix_ + "y"]; - const auto& normalZ = mapIn[inputLayersPrefix_ + "z"]; + const auto & normalX = mapIn[inputLayersPrefix_ + "x"]; + const auto & normalY = mapIn[inputLayersPrefix_ + "y"]; + const auto & normalZ = mapIn[inputLayersPrefix_ + "z"]; mapOut = mapIn; mapOut.add(outputLayer_); - auto& color = mapOut[outputLayer_]; + auto & color = mapOut[outputLayer_]; // X: -1 to +1 : Red: 0 to 255 // Y: -1 to +1 : Green: 0 to 255 // Z: 0 to 1 : Blue: 128 to 255 // For each cell in map. - for (size_t i = 0; i < color.size(); ++i) { + for (int64_t i = 0; i < color.size(); ++i) { const Eigen::Vector3f colorVector((normalX(i) + 1.0) / 2.0, - (normalY(i) + 1.0) / 2.0, - (normalZ(i) / 2.0) + 0.5); + (normalY(i) + 1.0) / 2.0, + (normalZ(i) / 2.0) + 0.5); colorVectorToValue(colorVector, color(i)); } return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::NormalColorMapFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::NormalColorMapFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/NormalVectorsFilter.cpp b/grid_map_filters/src/NormalVectorsFilter.cpp index f43b19a89..5889244d8 100644 --- a/grid_map_filters/src/NormalVectorsFilter.cpp +++ b/grid_map_filters/src/NormalVectorsFilter.cpp @@ -6,33 +6,44 @@ * Institute: ETH Zurich, ANYbotics */ -#include +#include #include -#include -#include -#include -#include +#include #include #include -namespace grid_map { +#include +#include +#include +#include + +#include "grid_map_cv/utilities.hpp" -template +namespace grid_map +{ + +template NormalVectorsFilter::NormalVectorsFilter() - : method_(Method::RasterSerial), estimationRadius_(0.0), parallelizationEnabled_(false), threadCount_(1), gridMapResolution_(0.02) {} +: method_(Method::RasterSerial), estimationRadius_(0.0), parallelizationEnabled_(false), + threadCount_(1), gridMapResolution_(0.02) {} -template +template NormalVectorsFilter::~NormalVectorsFilter() = default; -template -bool NormalVectorsFilter::configure() { +template +bool NormalVectorsFilter::configure() +{ + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + // Read which algorithm is chosen: area or raster. std::string algorithm; - if (!filters::FilterBase::getParam(std::string("algorithm"), algorithm)) { - ROS_WARN("Could not find the parameter: `algorithm`. Setting to default value: 'area'."); + if (!param_reader.get(std::string("algorithm"), algorithm)) { + RCLCPP_WARN( + this->logging_interface_->get_logger(), + "Could not find the parameter: `algorithm`. Setting to default value: 'area'."); // Default value. algorithm = "area"; } @@ -40,34 +51,38 @@ bool NormalVectorsFilter::configure() { // Read parameters related to area algorithm only if needed, otherwise when using raster method // on purpose it throws unwanted errors. if (algorithm != "raster") { - // Read radius, if found, its value will be used for area method. If radius parameter is not found, raster method will be used. - if (!filters::FilterBase::getParam(std::string("radius"), estimationRadius_)) { - ROS_WARN("Could not find the parameter: `radius`. Switching to raster method."); + // Read radius, if found, its value will be used for area method. + // If radius parameter is not found, raster method will be used. + if (!param_reader.get(std::string("radius"), estimationRadius_)) { + RCLCPP_WARN( + this->logging_interface_->get_logger(), + "Could not find the parameter: `radius`. Switching to raster method."); algorithm = "raster"; } - ROS_DEBUG("Normal vectors estimation radius = %f", estimationRadius_); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Normal vectors estimation radius = %f", + estimationRadius_); // If radius not positive switch to raster method. if (estimationRadius_ <= 0) { - ROS_WARN("Parameter `radius` is not positive. Switching to raster method."); + RCLCPP_WARN( + this->logging_interface_->get_logger(), + "Parameter `radius` is not positive. Switching to raster method."); algorithm = "raster"; } } - // Read parallelization_enabled to decide whether parallelization has to be used, if parameter is not found an error is thrown and - // the false default value will be used. - if (!filters::FilterBase::getParam(std::string("parallelization_enabled"), parallelizationEnabled_)) { - ROS_WARN("Could not find the parameter: `parallelization_enabled`. Setting to default value: 'false'."); + // Read parallelization_enabled to decide whether parallelization has to be used, + // if parameter is not found an error is thrown and the false default value will be used. + if (!param_reader.get(std::string("parallelization_enabled"), parallelizationEnabled_)) { + RCLCPP_WARN( + this->logging_interface_->get_logger(), + "Could not find the parameter:" + " `parallelization_enabled`. Setting to default value: 'false'."); parallelizationEnabled_ = false; } - ROS_DEBUG("Parallelization_enabled = %d", parallelizationEnabled_); - - // Read thread_number to set the number of threads to be used if parallelization is enebled, - // if parameter is not found an error is thrown and the default is to set it to automatic. - if (!filters::FilterBase::getParam(std::string("thread_number"), threadCount_)) { - ROS_WARN("Could not find the parameter: `thread_number`. Setting to default value: 'automatic'."); - threadCount_ = tbb::task_scheduler_init::automatic; - } - ROS_DEBUG("Thread_number = %d", threadCount_); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Parallelization_enabled = %d", + parallelizationEnabled_); // Set wanted method looking at algorithm and parallelization_enabled parameters. // parallelization_enabled is used to select whether to use parallelization or not. @@ -75,27 +90,34 @@ bool NormalVectorsFilter::configure() { // If parallelizationEnabled_=true, use the parallel method, otherwise serial. if (parallelizationEnabled_) { method_ = Method::RasterParallel; - ROS_DEBUG("Method RasterParallel"); + RCLCPP_DEBUG(this->logging_interface_->get_logger(), "Method RasterParallel"); } else { method_ = Method::RasterSerial; - ROS_DEBUG("Method RasterSerial"); + RCLCPP_DEBUG(this->logging_interface_->get_logger(), "Method RasterSerial"); } } else { // If parallelizationEnabled_=true, use the parallel method, otherwise serial. if (parallelizationEnabled_) { method_ = Method::AreaParallel; - ROS_DEBUG("Method AreaParallel"); + RCLCPP_DEBUG(this->logging_interface_->get_logger(), "Method AreaParallel"); } else { method_ = Method::AreaSerial; - ROS_DEBUG("Method AreaSerial"); + RCLCPP_DEBUG(this->logging_interface_->get_logger(), "Method AreaSerial"); } - ROS_DEBUG("estimationRadius_ = %f", estimationRadius_); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "estimationRadius_ = %f", + estimationRadius_); } // Read normal_vector_positive_axis, to define normal vector positive direction. std::string normalVectorPositiveAxis; - if (!filters::FilterBase::getParam(std::string("normal_vector_positive_axis"), normalVectorPositiveAxis)) { - ROS_ERROR("Normal vectors filter did not find parameter `normal_vector_positive_axis`."); + if (!param_reader.get( + std::string("normal_vector_positive_axis"), + normalVectorPositiveAxis)) + { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Normal vectors filter did not find parameter `normal_vector_positive_axis`."); return false; } if (normalVectorPositiveAxis == "z") { @@ -105,37 +127,49 @@ bool NormalVectorsFilter::configure() { } else if (normalVectorPositiveAxis == "x") { normalVectorPositiveAxis_ = Vector3::UnitX(); } else { - ROS_ERROR("The normal vector positive axis '%s' is not valid.", normalVectorPositiveAxis.c_str()); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "The normal vector positive axis '%s' is not valid.", + normalVectorPositiveAxis.c_str()); return false; } // Read input_layer, to define input grid map layer. - if (!filters::FilterBase::getParam(std::string("input_layer"), inputLayer_)) { - ROS_ERROR("Normal vectors filter did not find parameter `input_layer`."); + if (!param_reader.get(std::string("input_layer"), inputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Normal vectors filter did not find parameter `input_layer`."); return false; } - ROS_DEBUG("Normal vectors filter input layer is = %s.", inputLayer_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Normal vectors filter input layer is = %s.", + inputLayer_.c_str()); // Read output_layers_prefix, to define output grid map layers prefix. - if (!filters::FilterBase::getParam(std::string("output_layers_prefix"), outputLayersPrefix_)) { - ROS_ERROR("Normal vectors filter did not find parameter `output_layers_prefix`."); + if (!param_reader.get(std::string("output_layers_prefix"), outputLayersPrefix_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Normal vectors filter did not find parameter `output_layers_prefix`."); return false; } - ROS_DEBUG("Normal vectors filter output_layer = %s.", outputLayersPrefix_.c_str()); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), "Normal vectors filter output_layer = %s.", + outputLayersPrefix_.c_str()); // If everything has been set up correctly return true; } -template -bool NormalVectorsFilter::update(const T& mapIn, T& mapOut) { +template +bool NormalVectorsFilter::update(const T & mapIn, T & mapOut) +{ std::vector normalVectorsLayers; normalVectorsLayers.push_back(outputLayersPrefix_ + "x"); normalVectorsLayers.push_back(outputLayersPrefix_ + "y"); normalVectorsLayers.push_back(outputLayersPrefix_ + "z"); mapOut = mapIn; - for (const auto& layer : normalVectorsLayers) { + for (const auto & layer : normalVectorsLayers) { mapOut.add(layer); } switch (method_) { @@ -157,9 +191,13 @@ bool NormalVectorsFilter::update(const T& mapIn, T& mapOut) { } // SVD Area based methods. -template -void NormalVectorsFilter::computeWithAreaSerial(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix) { - const double start = ros::Time::now().toSec(); +template +void NormalVectorsFilter::computeWithAreaSerial( + GridMap & map, const std::string & inputLayer, + const std::string & outputLayersPrefix) +{ + rclcpp::Clock clock; + const double start = clock.now().seconds(); // For each cell in submap. for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { @@ -170,37 +208,43 @@ void NormalVectorsFilter::computeWithAreaSerial(GridMap& map, const std::stri } } - const double end = ros::Time::now().toSec(); - ROS_DEBUG_THROTTLE(2.0, "NORMAL COMPUTATION TIME = %f", (end - start)); + const double end = clock.now().seconds(); + RCLCPP_DEBUG_THROTTLE( + this->logging_interface_->get_logger(), clock, 2.0, "NORMAL COMPUTATION TIME = %f", + (end - start)); } -template -void NormalVectorsFilter::computeWithAreaParallel(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix) { - const double start = ros::Time::now().toSec(); +template +void NormalVectorsFilter::computeWithAreaParallel( + GridMap & map, const std::string & inputLayer, + const std::string & outputLayersPrefix) +{ + rclcpp::Clock clock; + const double start = clock.now().seconds(); grid_map::Size gridMapSize = map.getSize(); - // Set number of thread to use for parallel programming. - std::unique_ptr TBBInitPtr; - if (threadCount_ != -1) { - TBBInitPtr.reset(new tbb::task_scheduler_init(threadCount_)); - } - // Parallelized iteration through the map. - tbb::parallel_for(0, gridMapSize(0) * gridMapSize(1), [&](int range) { - // Recover Cell index from range iterator. - const Index index(range / gridMapSize(1), range % gridMapSize(1)); - if (map.isValid(index, inputLayer)) { - areaSingleNormalComputation(map, inputLayer, outputLayersPrefix, index); - } - }); + // Number of threads is automatically determined. + tbb::parallel_for( + 0, gridMapSize(0) * gridMapSize(1), [&](int range) { + // Recover Cell index from range iterator. + const Index index(range / gridMapSize(1), range % gridMapSize(1)); + if (map.isValid(index, inputLayer)) { + areaSingleNormalComputation(map, inputLayer, outputLayersPrefix, index); + } + }); - const double end = ros::Time::now().toSec(); - ROS_DEBUG_THROTTLE(2.0, "NORMAL COMPUTATION TIME = %f", (end - start)); + const double end = clock.now().seconds(); + RCLCPP_DEBUG_THROTTLE( + this->logging_interface_->get_logger(), clock, 2.0, "NORMAL COMPUTATION TIME = %f", + (end - start)); } -template -void NormalVectorsFilter::areaSingleNormalComputation(GridMap& map, const std::string& inputLayer, - const std::string& outputLayersPrefix, const grid_map::Index& index) { +template +void NormalVectorsFilter::areaSingleNormalComputation( + GridMap & map, const std::string & inputLayer, + const std::string & outputLayersPrefix, const grid_map::Index & index) +{ // Requested position (center) of circle in map. Position center; map.getPosition(index, center); @@ -208,14 +252,19 @@ void NormalVectorsFilter::areaSingleNormalComputation(GridMap& map, const std // Prepare data computation. Check if area is bigger than cell. const double minAllowedEstimationRadius = 0.5 * map.getResolution(); if (estimationRadius_ <= minAllowedEstimationRadius) { - ROS_WARN("Estimation radius is smaller than allowed by the map resolution (%f < %f)", estimationRadius_, minAllowedEstimationRadius); + RCLCPP_WARN( + this->logging_interface_->get_logger(), + "Estimation radius is smaller than allowed by the map resolution (%f < %f)", + estimationRadius_, minAllowedEstimationRadius); } // Gather surrounding data. size_t nPoints = 0; Position3 sum = Position3::Zero(); Eigen::Matrix3d sumSquared = Eigen::Matrix3d::Zero(); - for (CircleIterator circleIterator(map, center, estimationRadius_); !circleIterator.isPastEnd(); ++circleIterator) { + for (CircleIterator circleIterator(map, center, estimationRadius_); !circleIterator.isPastEnd(); + ++circleIterator) + { Position3 point; if (!map.getPosition3(inputLayer, *circleIterator, point)) { continue; @@ -227,7 +276,10 @@ void NormalVectorsFilter::areaSingleNormalComputation(GridMap& map, const std Vector3 unitaryNormalVector = Vector3::Zero(); if (nPoints < 3) { - ROS_DEBUG("Not enough points to establish normal direction (nPoints = %i)", static_cast(nPoints)); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), + "Not enough points to establish normal direction (nPoints = %i)", + static_cast(nPoints)); unitaryNormalVector = Vector3::UnitZ(); } else { const Position3 mean = sum / nPoints; @@ -241,8 +293,13 @@ void NormalVectorsFilter::areaSingleNormalComputation(GridMap& map, const std if (solver.eigenvalues()(1) > 1e-8) { unitaryNormalVector = solver.eigenvectors().col(0); } else { // If second eigenvalue is zero, the normal is not defined. - ROS_DEBUG("Covariance matrix needed for eigen decomposition is degenerated."); - ROS_DEBUG("Expected cause: data is on a straight line (nPoints = %i)", static_cast(nPoints)); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), + "Covariance matrix needed for eigen decomposition is degenerated."); + RCLCPP_DEBUG( + this->logging_interface_->get_logger(), + "Expected cause: data is on a straight line (nPoints = %i)", + static_cast(nPoints)); unitaryNormalVector = Vector3::UnitZ(); } } @@ -257,62 +314,81 @@ void NormalVectorsFilter::areaSingleNormalComputation(GridMap& map, const std map.at(outputLayersPrefix + "z", index) = unitaryNormalVector.z(); } // Raster based methods. -template -void NormalVectorsFilter::computeWithRasterSerial(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix) { +template +void NormalVectorsFilter::computeWithRasterSerial( + GridMap & map, const std::string & inputLayer, + const std::string & outputLayersPrefix) +{ // Inspiration for algorithm: http://www.flipcode.com/archives/Calculating_Vertex_Normals_for_Height_Maps.shtml - const double start = ros::Time::now().toSec(); + rclcpp::Clock clock; + const double start = clock.now().seconds(); const grid_map::Size gridMapSize = map.getSize(); gridMapResolution_ = map.getResolution(); // Faster access to grid map values. const grid_map::Matrix dataMap = map[inputLayer]; - // Height and width of submap. Submap is Map without the outermost line of cells, no need to check if index is inside. + // Height and width of submap. + // Submap is Map without the outermost line of cells, no need to check if index is inside. const Index submapStartIndex(1, 1); const Index submapBufferSize(gridMapSize(0) - 2, gridMapSize(1) - 2); // For each cell in submap. - for (SubmapIterator iterator(map, submapStartIndex, submapBufferSize); !iterator.isPastEnd(); ++iterator) { + for (SubmapIterator iterator(map, submapStartIndex, submapBufferSize); !iterator.isPastEnd(); + ++iterator) + { const Index index(*iterator); rasterSingleNormalComputation(map, outputLayersPrefix, dataMap, index); } - const double end = ros::Time::now().toSec(); - ROS_DEBUG_THROTTLE(2.0, "NORMAL COMPUTATION TIME = %f", (end - start)); + const double end = clock.now().seconds(); + RCLCPP_DEBUG_THROTTLE( + this->logging_interface_->get_logger(), clock, 2.0, "NORMAL COMPUTATION TIME = %f", + (end - start)); } -template -void NormalVectorsFilter::computeWithRasterParallel(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix) { - const double start = ros::Time::now().toSec(); +template +void NormalVectorsFilter::computeWithRasterParallel( + GridMap & map, + const std::string & inputLayer, + const std::string & outputLayersPrefix) +{ + rclcpp::Clock clock; + const double start = clock.now().seconds(); const grid_map::Size gridMapSize = map.getSize(); gridMapResolution_ = map.getResolution(); // Faster access to grid map values if copy grid map layer into local matrix. const grid_map::Matrix dataMap = map[inputLayer]; - // Height and width of submap. Submap is Map without the outermost line of cells, no need to check if index is inside. + // Height and width of submap. + // Submap is Map without the outermost line of cells, no need to check if index is inside. const Index submapStartIndex(1, 1); const Index submapBufferSize(gridMapSize(0) - 2, gridMapSize(1) - 2); - if (submapBufferSize(1)!=0) { - // Set number of thread to use for parallel programming - std::unique_ptr TBBInitPtr; - if (threadCount_ != -1) { - TBBInitPtr.reset(new tbb::task_scheduler_init(threadCount_)); - } + if (submapBufferSize(1) != 0) { // Parallelized iteration through the map. - tbb::parallel_for(0, submapBufferSize(0) * submapBufferSize(1), [&](int range) { - const Index index(range / submapBufferSize(1) + submapStartIndex(0), range % submapBufferSize(1) + submapStartIndex(1)); - rasterSingleNormalComputation(map, outputLayersPrefix, dataMap, index); - }); + tbb::parallel_for( + 0, submapBufferSize(0) * submapBufferSize(1), [&](int range) { + const Index index( + range / submapBufferSize(1) + submapStartIndex(0), + range % submapBufferSize(1) + submapStartIndex(1)); + rasterSingleNormalComputation(map, outputLayersPrefix, dataMap, index); + }); } else { - ROS_ERROR("Grid map size is too small for normal raster computation"); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Grid map size is too small for normal raster computation"); } - const double end = ros::Time::now().toSec(); - ROS_DEBUG_THROTTLE(2.0, "NORMAL COMPUTATION TIME = %f", (end - start)); + const double end = clock.now().seconds(); + RCLCPP_DEBUG_THROTTLE( + this->logging_interface_->get_logger(), clock, 2.0, "NORMAL COMPUTATION TIME = %f", + (end - start)); } -template -void NormalVectorsFilter::rasterSingleNormalComputation(GridMap& map, const std::string& outputLayersPrefix, - const grid_map::Matrix& dataMap, const grid_map::Index& index) { +template +void NormalVectorsFilter::rasterSingleNormalComputation( + GridMap & map, const std::string & outputLayersPrefix, + const grid_map::Matrix & dataMap, const grid_map::Index & index) +{ // Inspiration for algorithm: // http://www.flipcode.com/archives/Calculating_Vertex_Normals_for_Height_Maps.shtml const double centralCell = dataMap(index(0), index(1)); @@ -322,26 +398,31 @@ void NormalVectorsFilter::rasterSingleNormalComputation(GridMap& map, const s double leftCell = dataMap(index(0), index(1) - 1); // Neighboring cells configuration checked in X and Y direction independently. - // Gridmap frame is defined rotated 90 degrees anticlockwise compared to direction of matrix if we take - // rows as Y coordinate and columns as X coordinate. + // Gridmap frame is defined rotated 90 degrees anticlockwise compared to + // direction of matrix if we take rows as Y coordinate and columns as X coordinate. // In Y direction cell numbered as follows: left 0, center 1, right 2. // In X direction cell numbered as follows: top 0, center 1, bottom 2. - // To find configuration we are in, multiply value of map.isValid of cell in question, True or False, + // To find configuration we are in, + // multiply value of map.isValid of cell in question, True or False, // by 2^(number of the cell). // Each configuration will have a different number associated with it and then use a switch. - const int configurationDirX = 1 * static_cast(std::isfinite(topCell)) + 2 * static_cast(std::isfinite(centralCell)) + - 4 * static_cast(std::isfinite(bottomCell)); - const int configurationDirY = 1 * static_cast(std::isfinite(leftCell)) + 2 * static_cast(std::isfinite(centralCell)) + - 4 * static_cast(std::isfinite(rightCell)); + const int configurationDirX = 1 * static_cast(std::isfinite(topCell)) + 2 * + static_cast(std::isfinite(centralCell)) + + 4 * static_cast(std::isfinite(bottomCell)); + const int configurationDirY = 1 * static_cast(std::isfinite(leftCell)) + 2 * + static_cast(std::isfinite(centralCell)) + + 4 * static_cast(std::isfinite(rightCell)); - // If outer cell height value is missing use the central value, however the formula for the normal calculation - // has to take into account that the distance of the cells used for normal calculation is different. + // If outer cell height value is missing use the central value, however + // the formula for the normal calculation has to take into account that + // the distance of the cells used for normal calculation is different. bool validConfiguration = true; double distanceX; switch (configurationDirX) { - case 7: // All 3 cell height values are valid. - distanceX = 2 * gridMapResolution_; // Top and bottom cell centers are 2 cell resolution distant. + case 7: // All 3 cell height values are valid. + // Top and bottom cell centers are 2 cell resolution distant. + distanceX = 2 * gridMapResolution_; break; case 6: // Top cell height value not valid. topCell = centralCell; @@ -355,14 +436,17 @@ void NormalVectorsFilter::rasterSingleNormalComputation(GridMap& map, const s distanceX = gridMapResolution_; break; - default: // More than 1 cell height values are not valid, normal vector will not be calculated in this location. + default: + // More than 1 cell height values are not valid, + // normal vector will not be calculated in this location. validConfiguration = false; } double distanceY; switch (configurationDirY) { - case 7: // All 3 cell height values are valid. - distanceY = 2 * gridMapResolution_; // Left and right cell centers are 2 call resolution distant. + case 7: // All 3 cell height values are valid. + // Left and right cell centers are 2 call resolution distant. + distanceY = 2 * gridMapResolution_; break; case 6: // Left cell height value not valid. leftCell = centralCell; @@ -376,7 +460,9 @@ void NormalVectorsFilter::rasterSingleNormalComputation(GridMap& map, const s distanceY = gridMapResolution_; break; - default: // More than 1 cell height values are not valid, normal vector will not be calculated in this location. + default: + // More than 1 cell height values are not valid, + // normal vector will not be calculated in this location. validConfiguration = false; } @@ -405,4 +491,6 @@ void NormalVectorsFilter::rasterSingleNormalComputation(GridMap& map, const s } // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::NormalVectorsFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::NormalVectorsFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/SetBasicLayersFilter.cpp b/grid_map_filters/src/SetBasicLayersFilter.cpp index 458bc64f2..c2ea1bce0 100644 --- a/grid_map_filters/src/SetBasicLayersFilter.cpp +++ b/grid_map_filters/src/SetBasicLayersFilter.cpp @@ -9,11 +9,15 @@ #include "../include/grid_map_filters/SetBasicLayersFilter.hpp" #include -#include +#include -using namespace filters; +#include +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template SetBasicLayersFilter::SetBasicLayersFilter() @@ -28,8 +32,12 @@ SetBasicLayersFilter::~SetBasicLayersFilter() template bool SetBasicLayersFilter::configure() { - if (!FilterBase::getParam(std::string("layers"), layers_)) { - ROS_ERROR("SetBasicLayersFilters did not find parameter 'layers'."); + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("layer"), layers_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "SetBasicLayersFilters did not find parameter 'layers'."); return false; } @@ -37,14 +45,17 @@ bool SetBasicLayersFilter::configure() } template -bool SetBasicLayersFilter::update(const T& mapIn, T& mapOut) +bool SetBasicLayersFilter::update(const T & mapIn, T & mapOut) { mapOut = mapIn; std::vector layersChecked; - for (const auto& layer : layers_) { + for (const auto & layer : layers_) { if (!mapOut.exists(layer)) { - ROS_WARN("Layer `%s` does not exist and is not set as basic layer.", layer.c_str()); + RCLCPP_WARN( + this->logging_interface_->get_logger(), + "Layer `%s` does not exist and is not set as basic layer.", + layer.c_str()); continue; } layersChecked.push_back(layer); @@ -54,6 +65,8 @@ bool SetBasicLayersFilter::update(const T& mapIn, T& mapOut) return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::SetBasicLayersFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::SetBasicLayersFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/SlidingWindowMathExpressionFilter.cpp b/grid_map_filters/src/SlidingWindowMathExpressionFilter.cpp index 5f7e4b096..968342028 100644 --- a/grid_map_filters/src/SlidingWindowMathExpressionFilter.cpp +++ b/grid_map_filters/src/SlidingWindowMathExpressionFilter.cpp @@ -8,11 +8,14 @@ #include "grid_map_filters/SlidingWindowMathExpressionFilter.hpp" -#include +#include -using namespace filters; +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template SlidingWindowMathExpressionFilter::SlidingWindowMathExpressionFilter() @@ -32,71 +35,95 @@ SlidingWindowMathExpressionFilter::~SlidingWindowMathExpressionFilter() template bool SlidingWindowMathExpressionFilter::configure() { - if (!FilterBase::getParam(std::string("input_layer"), inputLayer_)) { - ROS_ERROR("SlidingWindowMathExpressionFilter did not find parameter 'input_layer'."); + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (!param_reader.get(std::string("input_layer"), inputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "SlidingWindowMathExpressionFilter did not find parameter 'input_layer'."); return false; } - if (!FilterBase::getParam(std::string("output_layer"), outputLayer_)) { - ROS_ERROR("SlidingWindowMathExpressionFilter did not find parameter 'output_layer'."); + if (!param_reader.get(std::string("output_layer"), outputLayer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "SlidingWindowMathExpressionFilter did not find parameter 'output_layer'."); return false; } - if (!FilterBase::getParam(std::string("expression"), expression_)) { - ROS_ERROR("SlidingWindowMathExpressionFilter did not find parameter 'expression'."); + if (!param_reader.get(std::string("expression"), expression_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "SlidingWindowMathExpressionFilter did not find parameter 'expression'."); return false; } - if (!FilterBase::getParam(std::string("window_size"), windowSize_)) { - if (FilterBase::getParam(std::string("window_length"), windowLength_)) { + if (!param_reader.get(std::string("window_size"), windowSize_)) { + if (param_reader.get(std::string("window_length"), windowLength_)) { useWindowLength_ = true; } } - if (!FilterBase::getParam(std::string("compute_empty_cells"), isComputeEmptyCells_)) { - ROS_ERROR("SlidingWindowMathExpressionFilter did not find parameter 'compute_empty_cells'."); + if (!param_reader.get(std::string("compute_empty_cells"), isComputeEmptyCells_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "SlidingWindowMathExpressionFilter did not find parameter 'compute_empty_cells'."); return false; } std::string edgeHandlingMethod; - if (!FilterBase::getParam(std::string("edge_handling"), edgeHandlingMethod)) { - ROS_ERROR("SlidingWindowMathExpressionFilter did not find parameter 'edge_handling'."); + if (!param_reader.get(std::string("edge_handling"), edgeHandlingMethod)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "SlidingWindowMathExpressionFilter did not find parameter 'edge_handling'."); return false; } - if (edgeHandlingMethod == "inside") edgeHandling_ = SlidingWindowIterator::EdgeHandling::INSIDE; - else if (edgeHandlingMethod == "crop") edgeHandling_ = SlidingWindowIterator::EdgeHandling::CROP; - else if (edgeHandlingMethod == "empty") edgeHandling_ = SlidingWindowIterator::EdgeHandling::EMPTY; - else if (edgeHandlingMethod == "mean") edgeHandling_ = SlidingWindowIterator::EdgeHandling::MEAN; - else { - ROS_ERROR("SlidingWindowMathExpressionFilter did not find method '%s' for edge handling.", edgeHandlingMethod.c_str()); + if (edgeHandlingMethod == "inside") { + edgeHandling_ = SlidingWindowIterator::EdgeHandling::INSIDE; + } else if (edgeHandlingMethod == "crop") { + edgeHandling_ = SlidingWindowIterator::EdgeHandling::CROP; + } else if (edgeHandlingMethod == "empty") { + edgeHandling_ = SlidingWindowIterator::EdgeHandling::EMPTY; + } else if (edgeHandlingMethod == "mean") { + edgeHandling_ = SlidingWindowIterator::EdgeHandling::MEAN; + } else { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "SlidingWindowMathExpressionFilter did not find method '%s' for edge handling.", + edgeHandlingMethod.c_str()); return false; } - // TODO Can we make caching work with changing shared variable? + // TODO(needs_assignment): Can we make caching work with changing shared variable? // parser_.setCacheExpressions(true); return true; } template -bool SlidingWindowMathExpressionFilter::update(const T& mapIn, T& mapOut) +bool SlidingWindowMathExpressionFilter::update(const T & mapIn, T & mapOut) { mapOut = mapIn; mapOut.add(outputLayer_); - Matrix& outputData = mapOut[outputLayer_]; + Matrix & outputData = mapOut[outputLayer_]; grid_map::SlidingWindowIterator iterator(mapIn, inputLayer_, edgeHandling_, windowSize_); - if (useWindowLength_) iterator.setWindowLength(mapIn, windowLength_); + if (useWindowLength_) {iterator.setWindowLength(mapIn, windowLength_);} for (; !iterator.isPastEnd(); ++iterator) { parser_.var(inputLayer_).setLocal(iterator.getData()); EigenLab::Value result(parser_.eval(expression_)); if (result.matrix().cols() == 1 && result.matrix().rows() == 1) { outputData(iterator.getLinearIndex()) = result.matrix()(0); } else { - ROS_ERROR("SlidingWindowMathExpressionFilter could not apply filter because expression has to result in a scalar!"); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "SlidingWindowMathExpressionFilter could not apply filter" + " because expression has to result in a scalar!"); } } return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::SlidingWindowMathExpressionFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::SlidingWindowMathExpressionFilter, + filters::FilterBase) diff --git a/grid_map_filters/src/ThresholdFilter.cpp b/grid_map_filters/src/ThresholdFilter.cpp index 91f01a844..c311596e9 100644 --- a/grid_map_filters/src/ThresholdFilter.cpp +++ b/grid_map_filters/src/ThresholdFilter.cpp @@ -9,19 +9,22 @@ #include "grid_map_filters/ThresholdFilter.hpp" #include -#include +#include -using namespace filters; +#include -namespace grid_map { +#include "grid_map_cv/utilities.hpp" + +namespace grid_map +{ template ThresholdFilter::ThresholdFilter() - : useLowerThreshold_(false), - useUpperThreshold_(false), - lowerThreshold_(0.0), - upperThreshold_(1.0), - setTo_(0.5) +: lowerThreshold_(0.0), + upperThreshold_(1.0), + setTo_(0.5), + useLowerThreshold_(false), + useUpperThreshold_(false) { } @@ -33,38 +36,41 @@ ThresholdFilter::~ThresholdFilter() template bool ThresholdFilter::configure() { - // Load Parameters - if (FilterBase::getParam(std::string("lower_threshold"), - lowerThreshold_)) { + ParameterReader param_reader(this->param_prefix_, this->params_interface_); + + if (param_reader.get(std::string("lower_threshold"), lowerThreshold_)) { useLowerThreshold_ = true; - ROS_DEBUG("lower threshold = %f", lowerThreshold_); + RCLCPP_DEBUG(this->logging_interface_->get_logger(), "lower threshold = %f", lowerThreshold_); } - if (FilterBase::getParam(std::string("upper_threshold"), - upperThreshold_)) { + if (param_reader.get(std::string("upper_threshold"), upperThreshold_)) { useUpperThreshold_ = true; - ROS_DEBUG("upper threshold = %f", upperThreshold_); + RCLCPP_DEBUG(this->logging_interface_->get_logger(), "upper threshold = %f", upperThreshold_); } if (!useLowerThreshold_ && !useUpperThreshold_) { - ROS_ERROR( - "ThresholdFilter did not find parameter 'lower_threshold' or 'upper_threshold',"); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "ThresholdFilter did not find parameter 'lower_threshold' or 'upper_threshold',"); return false; } if (useLowerThreshold_ && useUpperThreshold_) { - ROS_ERROR( - "Set either 'lower_threshold' or 'upper_threshold'! Only one threshold can be used!"); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), + "Set either 'lower_threshold' or 'upper_threshold'! Only one threshold can be used!"); return false; } - if (!FilterBase::getParam(std::string("set_to"), setTo_)) { - ROS_ERROR("ThresholdFilter did not find parameter 'set_to'."); + if (!param_reader.get(std::string("set_to"), setTo_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), "ThresholdFilter did not find parameter 'set_to'."); return false; } - if (!FilterBase::getParam(std::string("layer"), layer_)) { - ROS_ERROR("ThresholdFilter did not find parameter 'layer'."); + if (!param_reader.get(std::string("layer"), layer_)) { + RCLCPP_ERROR( + this->logging_interface_->get_logger(), "ThresholdFilter did not find parameter 'layer'."); return false; } @@ -72,29 +78,33 @@ bool ThresholdFilter::configure() } template -bool ThresholdFilter::update(const T& mapIn, T& mapOut) +bool ThresholdFilter::update(const T & mapIn, T & mapOut) { mapOut = mapIn; // Check if layer exists. if (!mapOut.exists(layer_)) { - ROS_ERROR("Check your threshold types! Type %s does not exist", layer_.c_str()); + RCLCPP_ERROR( + this->logging_interface_->get_logger(), "Check your threshold types! Type %s does not exist", + layer_.c_str()); return false; } // For each cell in map. - auto& data = mapOut[layer_]; + auto & data = mapOut[layer_]; for (grid_map::GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) { - if (!mapOut.isValid(*iterator, layer_)) continue; + if (!mapOut.isValid(*iterator, layer_)) {continue;} const size_t i = iterator.getLinearIndex(); - float& value = data(i); - if (useLowerThreshold_) if (value < lowerThreshold_) value = setTo_; - if (useUpperThreshold_) if (value > upperThreshold_) value = setTo_; + float & value = data(i); + if (useLowerThreshold_) {if (value < lowerThreshold_) {value = setTo_;}} + if (useUpperThreshold_) {if (value > upperThreshold_) {value = setTo_;}} } return true; } -} /* namespace */ +} // namespace grid_map -PLUGINLIB_EXPORT_CLASS(grid_map::ThresholdFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS( + grid_map::ThresholdFilter, + filters::FilterBase) diff --git a/grid_map_loader/CHANGELOG.rst b/grid_map_loader/CHANGELOG.rst index 04b5d8a3e..c6fb2d892 100644 --- a/grid_map_loader/CHANGELOG.rst +++ b/grid_map_loader/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package grid_map_loader ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Contributors: Ryan, Ryan Friedman + +2.1.0 (2022-11-08) +------------------ + +2.0.0 (2022-09-13) +------------------ +* fix: mark Eigen library as SYSTEM +* fix: build error on jammy +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map_loader/CMakeLists.txt b/grid_map_loader/CMakeLists.txt index 4cb0f5f6a..d6407c6e2 100644 --- a/grid_map_loader/CMakeLists.txt +++ b/grid_map_loader/CMakeLists.txt @@ -1,32 +1,17 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(grid_map_loader) -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") +## Find ament macros and libraries +find_package(ament_cmake REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(grid_map_ros REQUIRED) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roscpp - grid_map_ros - grid_map_msgs -) +grid_map_package() -## System dependencies are found with CMake's conventions -#find_package(Eigen3 REQUIRED) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS -# DEPENDS system_lib +set(dependencies + grid_map_msgs + grid_map_ros ) ########### @@ -37,32 +22,75 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} ) -## Declare a cpp executable -add_executable( - ${PROJECT_NAME} +# Declare a cpp executable +add_executable(${PROJECT_NAME} src/grid_map_loader_node.cpp +) + +set(library_name ${PROJECT_NAME}_core) + +add_library(${library_name} SHARED src/GridMapLoader.cpp ) ## Specify libraries to link a library or executable target against -target_link_libraries( - ${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} SYSTEM + ${dependencies} +) + +target_link_libraries(${PROJECT_NAME} ${library_name}) + +ament_target_dependencies(${library_name} SYSTEM + ${dependencies} ) ############# ## Install ## ############# -# Mark executables and/or libraries for installation -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} ) +############# +## Testing ## +############# + +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright -build/include_order + ) + endif() + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_dependencies(${dependencies}) +ament_export_libraries(${PROJECT_NAME}) +ament_package() diff --git a/grid_map_loader/include/grid_map_loader/GridMapLoader.hpp b/grid_map_loader/include/grid_map_loader/GridMapLoader.hpp index 5e96d1986..41cd00dd4 100644 --- a/grid_map_loader/include/grid_map_loader/GridMapLoader.hpp +++ b/grid_map_loader/include/grid_map_loader/GridMapLoader.hpp @@ -7,10 +7,11 @@ * */ -#pragma once +#ifndef GRID_MAP_LOADER__GRIDMAPLOADER_HPP_ +#define GRID_MAP_LOADER__GRIDMAPLOADER_HPP_ // ROS -#include +#include // Grid map #include @@ -18,20 +19,19 @@ // STD #include -namespace grid_map_loader { +namespace grid_map_loader +{ /*! * Loads and publishes a grid map from a bag file. */ -class GridMapLoader +class GridMapLoader : public rclcpp::Node { - public: - +public: /*! * Constructor. - * @param nodeHandle the ROS node handle. */ - GridMapLoader(ros::NodeHandle nodeHandle); + GridMapLoader(); /*! * Destructor. @@ -55,13 +55,9 @@ class GridMapLoader */ void publish(); - private: - - //! ROS nodehandle. - ros::NodeHandle nodeHandle_; - +private: //! Grid map publisher. - ros::Publisher publisher_; + rclcpp::Publisher::SharedPtr publisher_; //! Grid map data. grid_map::GridMap map_; @@ -76,7 +72,11 @@ class GridMapLoader std::string publishTopic_; //! Duration to publish the grid map. - ros::Duration duration_; + double durationInSec; + + //! QOS Transient local state + bool qos_transient_local_; }; -} /* namespace */ +} // namespace grid_map_loader +#endif // GRID_MAP_LOADER__GRIDMAPLOADER_HPP_ diff --git a/grid_map_loader/package.xml b/grid_map_loader/package.xml index 3e084d941..e994c753d 100644 --- a/grid_map_loader/package.xml +++ b/grid_map_loader/package.xml @@ -1,16 +1,27 @@ - + grid_map_loader - 1.6.2 + 2.2.2 Loading and publishing grid maps from bag files. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Péter Fankhauser - catkin - roscpp - grid_map_ros + + ament_cmake + grid_map_cmake_helpers + grid_map_msgs + grid_map_ros + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/grid_map_loader/src/GridMapLoader.cpp b/grid_map_loader/src/GridMapLoader.cpp index c83373c0f..05ab9716c 100644 --- a/grid_map_loader/src/GridMapLoader.cpp +++ b/grid_map_loader/src/GridMapLoader.cpp @@ -7,21 +7,31 @@ * */ -#include -#include +#include "grid_map_loader/GridMapLoader.hpp" +#include -using namespace grid_map; +#include +#include +#include -namespace grid_map_loader { +namespace grid_map_loader +{ -GridMapLoader::GridMapLoader(ros::NodeHandle nodeHandle) - : nodeHandle_(nodeHandle) +GridMapLoader::GridMapLoader() +: Node("grid_map_loader") { readParameters(); - publisher_ = nodeHandle_.advertise(publishTopic_, 1, true); - if (!load()) return; + + rclcpp::QoS custom_qos(10); // initialize to default + + if (qos_transient_local_) { + custom_qos.transient_local(); // Persist messages for “late-joining” subscriptions. + } + + publisher_ = this->create_publisher(publishTopic_, custom_qos); + + if (!load()) {return;} publish(); - ros::requestShutdown(); } GridMapLoader::~GridMapLoader() @@ -30,27 +40,35 @@ GridMapLoader::~GridMapLoader() bool GridMapLoader::readParameters() { - nodeHandle_.param("bag_topic", bagTopic_, std::string("/grid_map")); - nodeHandle_.param("publish_topic", publishTopic_, bagTopic_); - nodeHandle_.param("file_path", filePath_, std::string()); - double durationInSec; - nodeHandle_.param("duration", durationInSec, 5.0); - duration_.fromSec(durationInSec); + this->declare_parameter("bag_topic", std::string("/grid_map")); + this->declare_parameter("publish_topic", std::string("/grid_map")); + this->declare_parameter("file_path", std::string()); + this->declare_parameter("duration", rclcpp::ParameterValue(5.0)); + this->declare_parameter("qos_transient_local", rclcpp::ParameterValue(true)); + + this->get_parameter("bag_topic", bagTopic_); + this->get_parameter("publish_topic", publishTopic_); + this->get_parameter("file_path", filePath_); + this->get_parameter("duration", durationInSec); + this->get_parameter("qos_transient_local", qos_transient_local_); + return true; } bool GridMapLoader::load() { - ROS_INFO_STREAM("Loading grid map from path " << filePath_ << "."); - return GridMapRosConverter::loadFromBag(filePath_, bagTopic_, map_); + RCLCPP_INFO_STREAM(this->get_logger(), "Loading grid map from path " << filePath_ << "."); + return grid_map::GridMapRosConverter::loadFromBag(filePath_, bagTopic_, map_); } void GridMapLoader::publish() { - grid_map_msgs::GridMap message; - grid_map::GridMapRosConverter::toMessage(map_, message); - publisher_.publish(message); - duration_.sleep(); -} + auto message = grid_map::GridMapRosConverter::toMessage(map_); + publisher_->publish(std::move(message)); -} /* namespace */ + if (durationInSec > 0) { + auto sleep_duration = rclcpp::Duration::from_seconds(durationInSec); + rclcpp::sleep_for(std::chrono::nanoseconds(sleep_duration.nanoseconds())); + } +} +} // namespace grid_map_loader diff --git a/grid_map_loader/src/grid_map_loader_node.cpp b/grid_map_loader/src/grid_map_loader_node.cpp index 2ae806dc0..12503fa42 100644 --- a/grid_map_loader/src/grid_map_loader_node.cpp +++ b/grid_map_loader/src/grid_map_loader_node.cpp @@ -7,13 +7,14 @@ */ #include -#include +#include -int main(int argc, char** argv) +#include + +int main(int argc, char ** argv) { - ros::init(argc, argv, "grid_map_loader"); - ros::NodeHandle nodeHandle("~"); - grid_map_loader::GridMapLoader gridMapLoader(nodeHandle); - ros::waitForShutdown(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } diff --git a/grid_map_msgs/CHANGELOG.rst b/grid_map_msgs/CHANGELOG.rst index e517e8795..a909d6239 100644 --- a/grid_map_msgs/CHANGELOG.rst +++ b/grid_map_msgs/CHANGELOG.rst @@ -2,6 +2,31 @@ Changelog for package grid_map_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ +* Merge pull request `#527 `_ from ANYbotics/feature/ros1_bridge_mapping_support + Added explicit ros1_bridge mapping support +* Added ros1_bridge mapping support +* Contributors: Zichong Li + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Contributors: Ryan, Ryan Friedman + +2.1.0 (2022-11-08) +------------------ + +2.0.0 (2022-09-13) +------------------ +* Initial ROS2 port +* Contributors: Steve Macenski + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map_msgs/CMakeLists.txt b/grid_map_msgs/CMakeLists.txt index 9b553d4cf..1c6eb2dad 100644 --- a/grid_map_msgs/CMakeLists.txt +++ b/grid_map_msgs/CMakeLists.txt @@ -1,57 +1,32 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(grid_map_msgs) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - std_msgs - geometry_msgs - message_generation -) +find_package(ament_cmake REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +grid_map_package() ####################################### ## Declare ROS messages and services ## ####################################### -## Generate messages in the 'msg' folder -add_message_files( - FILES - GridMapInfo.msg - GridMap.msg -) - -## Generate services in the 'srv' folder -add_service_files( - FILES - SetGridMap.srv - GetGridMap.srv - GetGridMapInfo.srv - ProcessFile.srv +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/GridMapInfo.msg" + "msg/GridMap.msg" + "srv/SetGridMap.srv" + "srv/GetGridMap.srv" + "srv/GetGridMapInfo.srv" + "srv/ProcessFile.srv" + DEPENDENCIES std_msgs geometry_msgs ) -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need - -catkin_package( - #INCLUDE_DIRS include - #LIBRARIES - CATKIN_DEPENDS - geometry_msgs - message_runtime - std_msgs - #DEPENDS -) +install( + FILES ros1_bridge_mapping.yaml + DESTINATION share/${PROJECT_NAME}) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/grid_map_msgs/msg/GridMap.msg b/grid_map_msgs/msg/GridMap.msg index 9af03d740..644ff8522 100644 --- a/grid_map_msgs/msg/GridMap.msg +++ b/grid_map_msgs/msg/GridMap.msg @@ -1,3 +1,6 @@ +# Header (time and frame) +std_msgs/Header header + # Grid map header GridMapInfo info diff --git a/grid_map_msgs/msg/GridMapInfo.msg b/grid_map_msgs/msg/GridMapInfo.msg index 74511f5c1..676a53318 100644 --- a/grid_map_msgs/msg/GridMapInfo.msg +++ b/grid_map_msgs/msg/GridMapInfo.msg @@ -1,6 +1,3 @@ -# Header (time and frame) -Header header - # Resolution of the grid [m/cell]. float64 resolution diff --git a/grid_map_msgs/package.xml b/grid_map_msgs/package.xml index 7081a3118..7dac12e22 100644 --- a/grid_map_msgs/package.xml +++ b/grid_map_msgs/package.xml @@ -1,18 +1,29 @@ - + + grid_map_msgs - 1.6.2 + 2.2.2 Definition of the multi-layered grid map message type. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Péter Fankhauser - catkin - roscpp - message_generation + + ament_cmake + grid_map_cmake_helpers + + rclcpp std_msgs geometry_msgs - message_runtime + rosidl_default_generators + + rosidl_interface_packages + + + ament_cmake + + diff --git a/grid_map_msgs/ros1_bridge_mapping.yaml b/grid_map_msgs/ros1_bridge_mapping.yaml new file mode 100644 index 000000000..5d18ddc6e --- /dev/null +++ b/grid_map_msgs/ros1_bridge_mapping.yaml @@ -0,0 +1,13 @@ +- + ros1_package_name: 'grid_map_msgs' + ros1_message_name: 'GridMap' + ros2_package_name: 'grid_map_msgs' + ros2_message_name: 'GridMap' + fields_2_to_1: + header: "info.header" + info: "info" + layers: "layers" + basic_layers: "basic_layers" + data: "data" + outer_start_index: "outer_start_index" + inner_start_index: "inner_start_index" \ No newline at end of file diff --git a/grid_map_octomap/CHANGELOG.rst b/grid_map_octomap/CHANGELOG.rst index e49c1fde3..a195ed3bb 100644 --- a/grid_map_octomap/CHANGELOG.rst +++ b/grid_map_octomap/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package grid_map_octomap ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Contributors: Ryan, Ryan Friedman + +2.1.0 (2022-11-08) +------------------ + +2.0.0 (2022-09-13) +------------------ +* fix: mark Eigen library as SYSTEM +* fix: build error on jammy +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map_octomap/CMakeLists.txt b/grid_map_octomap/CMakeLists.txt index e9a469147..bd060259e 100644 --- a/grid_map_octomap/CMakeLists.txt +++ b/grid_map_octomap/CMakeLists.txt @@ -1,68 +1,31 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(grid_map_octomap) -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") +## Find ament_cmake macros and libraries +find_package(ament_cmake REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(OCTOMAP REQUIRED) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - grid_map_core -) -find_package(octomap REQUIRED) message(STATUS "Found Octomap (version ${octomap_VERSION}): ${OCTOMAP_INCLUDE_DIRS}") -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS - include - ${OCTOMAP_INCLUDE_DIRS} - LIBRARIES - ${PROJECT_NAME} - ${OCTOMAP_LIBRARIES} - CATKIN_DEPENDS - grid_map_core - DEPENDS -) - ########### ## Build ## ########### +include_directories(include) -## Specify additional locations of header files -if(${octomap_VERSION} VERSION_LESS 1.8) # ROS Indigo and Jade. - add_definitions(-DOCTOMAP_VERSION_BEFORE_ROS_KINETIC) - include_directories( - include - ${catkin_INCLUDE_DIRS} - ${OCTOMAP_INCLUDE_DIR} - ) -else() # ROS Kinetic and above. - include_directories( - include - ${catkin_INCLUDE_DIRS} - ${OCTOMAP_INCLUDE_DIRS} - ) -endif() +set( dependencies + grid_map_core + OCTOMAP +) ## Declare a cpp library add_library(${PROJECT_NAME} src/GridMapOctomapConverter.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${OCTOMAP_LIBRARIES} -) - -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(${PROJECT_NAME} SYSTEM + ${dependencies} ) ############# @@ -72,15 +35,15 @@ add_dependencies(${PROJECT_NAME} # Mark executables and/or libraries for installation install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) # Mark cpp header files for installation install( DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} FILES_MATCHING PATTERN "*.hpp" ) @@ -88,10 +51,35 @@ install( ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") - # Add gtest based cpp test target and link libraries - catkin_add_gtest(${PROJECT_NAME}-test +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright -build/include_order + ) + endif() + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + ## Add gtest based cpp test target and link libraries + ament_add_gtest(${PROJECT_NAME}-test test/test_grid_map_octomap.cpp test/OctomapConverterTest.cpp ) @@ -103,3 +91,11 @@ if(TARGET ${PROJECT_NAME}-test) ${OCTOMAP_LIBRARIES} ) endif() + +ament_export_dependencies(${dependencies}) +ament_export_include_directories(include ${OCTOMAP_INCLUDE_DIRS}) +ament_export_libraries( + ${PROJECT_NAME} + ${OCTOMAP_LIBRARIES} +) +ament_package() diff --git a/grid_map_octomap/include/grid_map_octomap/GridMapOctomapConverter.hpp b/grid_map_octomap/include/grid_map_octomap/GridMapOctomapConverter.hpp index 661a129e9..0c8219090 100644 --- a/grid_map_octomap/include/grid_map_octomap/GridMapOctomapConverter.hpp +++ b/grid_map_octomap/include/grid_map_octomap/GridMapOctomapConverter.hpp @@ -6,7 +6,8 @@ * Institute: University of Zürich, Robotics and Perception Group */ -#pragma once +#ifndef GRID_MAP_OCTOMAP__GRIDMAPOCTOMAPCONVERTER_HPP_ +#define GRID_MAP_OCTOMAP__GRIDMAPOCTOMAPCONVERTER_HPP_ #include @@ -16,17 +17,19 @@ // STD #include #include +#include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Conversions between grid maps and Octomap types. */ class GridMapOctomapConverter { - public: +public: /*! * Default constructor. */ @@ -51,12 +54,13 @@ class GridMapOctomapConverter * @param[in] maxPoint (optional) maximum coordinate for bounding box. * @return true if successful, false otherwise. */ - static bool fromOctomap(const octomap::OcTree& octomap, - const std::string& layer, - grid_map::GridMap& gridMap, - const grid_map::Position3* minPoint = nullptr, - const grid_map::Position3* maxPoint = nullptr); - + static bool fromOctomap( + const octomap::OcTree & octomap, + const std::string & layer, + grid_map::GridMap & gridMap, + const grid_map::Position3 * minPoint = nullptr, + const grid_map::Position3 * maxPoint = nullptr); }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_OCTOMAP__GRIDMAPOCTOMAPCONVERTER_HPP_ diff --git a/grid_map_octomap/include/grid_map_octomap/grid_map_octomap.hpp b/grid_map_octomap/include/grid_map_octomap/grid_map_octomap.hpp index bf0e40412..554fe9aae 100644 --- a/grid_map_octomap/include/grid_map_octomap/grid_map_octomap.hpp +++ b/grid_map_octomap/include/grid_map_octomap/grid_map_octomap.hpp @@ -6,7 +6,10 @@ * Institute: University of Zürich, Robotics and Perception Group */ -#pragma once +#ifndef GRID_MAP_OCTOMAP__GRID_MAP_OCTOMAP_HPP_ +#define GRID_MAP_OCTOMAP__GRID_MAP_OCTOMAP_HPP_ #include #include + +#endif // GRID_MAP_OCTOMAP__GRID_MAP_OCTOMAP_HPP_ diff --git a/grid_map_octomap/package.xml b/grid_map_octomap/package.xml index c580958d7..a8ba81857 100644 --- a/grid_map_octomap/package.xml +++ b/grid_map_octomap/package.xml @@ -1,16 +1,28 @@ grid_map_octomap - 1.6.2 + 2.2.2 Conversions between grid maps and OctoMap types. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Jeff Delmerico Péter Fankhauser - catkin + + ament_cmake + grid_map_cmake_helpers + grid_map_core octomap + + ament_cmake_gtest + ament_lint_common + ament_lint_auto + + + ament_cmake + diff --git a/grid_map_octomap/src/GridMapOctomapConverter.cpp b/grid_map_octomap/src/GridMapOctomapConverter.cpp index f5f021fab..ed8fe2a37 100644 --- a/grid_map_octomap/src/GridMapOctomapConverter.cpp +++ b/grid_map_octomap/src/GridMapOctomapConverter.cpp @@ -8,7 +8,11 @@ #include "grid_map_octomap/GridMapOctomapConverter.hpp" -namespace grid_map { +#include +#include + +namespace grid_map +{ GridMapOctomapConverter::GridMapOctomapConverter() { @@ -18,11 +22,12 @@ GridMapOctomapConverter::~GridMapOctomapConverter() { } -bool GridMapOctomapConverter::fromOctomap(const octomap::OcTree& octomap, - const std::string& layer, - grid_map::GridMap& gridMap, - const grid_map::Position3* minPoint, - const grid_map::Position3* maxPoint) +bool GridMapOctomapConverter::fromOctomap( + const octomap::OcTree & octomap, + const std::string & layer, + grid_map::GridMap & gridMap, + const grid_map::Position3 * minPoint, + const grid_map::Position3 * maxPoint) { if (octomap.getTreeType() != "OcTree") { std::cerr << "Octomap conversion only implemented for standard OcTree type." << std::endl; @@ -36,7 +41,7 @@ bool GridMapOctomapConverter::fromOctomap(const octomap::OcTree& octomap, // On the first pass, expand all occupied cells that are not at maximum depth. unsigned int max_depth = octomapCopy.getTreeDepth(); // Adapted from octomap octree2pointcloud.cpp. - std::vector collapsed_occ_nodes; + std::vector collapsed_occ_nodes; do { collapsed_occ_nodes.clear(); for (octomap::OcTree::iterator it = octomapCopy.begin(); it != octomapCopy.end(); ++it) { @@ -44,19 +49,20 @@ bool GridMapOctomapConverter::fromOctomap(const octomap::OcTree& octomap, collapsed_occ_nodes.push_back(&(*it)); } } - for (std::vector::iterator it = collapsed_occ_nodes.begin(); - it != collapsed_occ_nodes.end(); ++it) { + for (std::vector::iterator it = collapsed_occ_nodes.begin(); + it != collapsed_occ_nodes.end(); ++it) + { #if OCTOMAP_VERSION_BEFORE_ROS_KINETIC - (*it)->expandNode(); + (*it)->expandNode(); #else - octomapCopy.expandNode(*it); + octomapCopy.expandNode(*it); #endif } // std::cout << "Expanded " << collapsed_occ_nodes.size() << " nodes" << std::endl; } while (collapsed_occ_nodes.size() > 0); // Set up grid map geometry. - // TODO Figure out whether to center map. + // TODO(needs_assignment): Figure out whether to center map. double resolution = octomapCopy.getResolution(); grid_map::Position3 minBound; grid_map::Position3 maxBound; @@ -76,8 +82,9 @@ bool GridMapOctomapConverter::fromOctomap(const octomap::OcTree& octomap, } grid_map::Length length = grid_map::Length(maxBound(0) - minBound(0), maxBound(1) - minBound(1)); - grid_map::Position position = grid_map::Position((maxBound(0) + minBound(0)) / 2.0, - (maxBound(1) + minBound(1)) / 2.0); + grid_map::Position position = grid_map::Position( + (maxBound(0) + minBound(0)) / 2.0, + (maxBound(1) + minBound(1)) / 2.0); gridMap.setGeometry(length, resolution, position); // std::cout << "grid map geometry: " << std::endl; // std::cout << "Length: [" << length(0) << ", " << length(1) << "]" << std::endl; @@ -91,9 +98,10 @@ bool GridMapOctomapConverter::fromOctomap(const octomap::OcTree& octomap, // For each voxel, if its elevation is higher than the existing value for the // corresponding grid map cell, overwrite it. // std::cout << "Iterating from " << min_bbx << " to " << max_bbx << std::endl; - grid_map::Matrix& gridMapData = gridMap[layer]; - for(octomap::OcTree::leaf_bbx_iterator it = octomapCopy.begin_leafs_bbx(minBbx, maxBbx), - end = octomapCopy.end_leafs_bbx(); it != end; ++it) { + grid_map::Matrix & gridMapData = gridMap[layer]; + for (octomap::OcTree::leaf_bbx_iterator it = octomapCopy.begin_leafs_bbx(minBbx, maxBbx), + end = octomapCopy.end_leafs_bbx(); it != end; ++it) + { if (octomapCopy.isNodeOccupied(*it)) { octomap::point3d octoPos = it.getCoordinate(); grid_map::Position position(octoPos.x(), octoPos.y()); @@ -102,9 +110,7 @@ bool GridMapOctomapConverter::fromOctomap(const octomap::OcTree& octomap, // If no elevation has been set, use current elevation. if (!gridMap.isValid(index)) { gridMapData(index(0), index(1)) = octoPos.z(); - } - // Check existing elevation, keep higher. - else { + } else { // Check existing elevation, keep higher. if (gridMapData(index(0), index(1)) < octoPos.z()) { gridMapData(index(0), index(1)) = octoPos.z(); } @@ -115,4 +121,4 @@ bool GridMapOctomapConverter::fromOctomap(const octomap::OcTree& octomap, return true; } -} /* namespace */ +} // namespace grid_map diff --git a/grid_map_octomap/test/OctomapConverterTest.cpp b/grid_map_octomap/test/OctomapConverterTest.cpp index 921cf21e0..7157b05fb 100644 --- a/grid_map_octomap/test/OctomapConverterTest.cpp +++ b/grid_map_octomap/test/OctomapConverterTest.cpp @@ -20,14 +20,12 @@ // Eigen #include -using namespace grid_map; - TEST(OctomapConversion, convertOctomapToGridMap) { // Generate Octomap (a 1m sphere centered at (1,1,1)) // Adapted from octomap unit_tests.cpp octomap::OcTree octomap(0.05); - octomap::Pointcloud* measurement = new octomap::Pointcloud(); + octomap::Pointcloud * measurement = new octomap::Pointcloud(); octomap::point3d origin(1.0f, 1.0f, 1.0f); octomap::point3d point_on_surface(1.0f, 0.0f, 0.0f); @@ -43,18 +41,20 @@ TEST(OctomapConversion, convertOctomapToGridMap) octomap.insertPointCloud(*measurement, origin); // Convert to grid map. - GridMap gridMap; - bool res = GridMapOctomapConverter::fromOctomap(octomap, "elevation", gridMap); + grid_map::GridMap gridMap; + bool res = grid_map::GridMapOctomapConverter::fromOctomap(octomap, "elevation", gridMap); ASSERT_TRUE(res); // Check map info. - Length length = gridMap.getLength(); - Vector3 octo_length; + grid_map::Length length = gridMap.getLength(); + grid_map::Vector3 octo_length; octomap.getMetricSize(octo_length.x(), octo_length.y(), octo_length.z()); EXPECT_FLOAT_EQ(octo_length.x(), length.x()); EXPECT_FLOAT_EQ(octo_length.y(), length.y()); EXPECT_FLOAT_EQ(octomap.getResolution(), gridMap.getResolution()); - EXPECT_FLOAT_EQ(2.0 + 0.5 * octomap.getResolution(), gridMap.atPosition("elevation", Position(1.0, 1.0))); + EXPECT_FLOAT_EQ( + 2.0 + 0.5 * octomap.getResolution(), + gridMap.atPosition("elevation", grid_map::Position(1.0, 1.0))); } TEST(OctomapConversion, convertOctomapToGridMapWithBoundingBox) @@ -64,7 +64,7 @@ TEST(OctomapConversion, convertOctomapToGridMapWithBoundingBox) // Generate Octomap (a 1m sphere centered at (1,1,1)) // Adapted from Octomap unit_tests.cpp octomap::OcTree octomap(0.05); - octomap::Pointcloud* measurement = new octomap::Pointcloud(); + octomap::Pointcloud * measurement = new octomap::Pointcloud(); octomap::point3d origin(1.0f, 1.0f, 1.0f); octomap::point3d point_on_surface(1.0f, 0.0f, 0.0f); @@ -73,21 +73,25 @@ TEST(OctomapConversion, convertOctomapToGridMapWithBoundingBox) for (int j = 0; j < 360; j++) { octomap::point3d p = origin + point_on_surface; measurement->push_back(p); - point_on_surface.rotate_IP (0,0,DEG2RAD(1.)); + point_on_surface.rotate_IP(0, 0, DEG2RAD(1.)); } - point_on_surface.rotate_IP (0,DEG2RAD(1.),0); + point_on_surface.rotate_IP(0, DEG2RAD(1.), 0); } octomap.insertPointCloud(*measurement, origin); // Convert to grid map. - GridMap gridmap; + grid_map::GridMap gridmap; grid_map::Position3 minpt(0.5, 0.5, 0.0); grid_map::Position3 maxpt(2.0, 2.0, 1.0); - bool res = GridMapOctomapConverter::fromOctomap(octomap, "elevation", gridmap, &minpt, &maxpt); + bool res = grid_map::GridMapOctomapConverter::fromOctomap( + octomap, "elevation", gridmap, &minpt, + &maxpt); ASSERT_TRUE(res); - Length length = gridmap.getLength(); + grid_map::Length length = gridmap.getLength(); EXPECT_FLOAT_EQ(1.5, length.x()); EXPECT_FLOAT_EQ(1.5, length.y()); EXPECT_FLOAT_EQ(octomap.getResolution(), gridmap.getResolution()); - EXPECT_FLOAT_EQ(0.5 * octomap.getResolution(), gridmap.atPosition("elevation", Position(1.0, 1.0))); + EXPECT_FLOAT_EQ( + 0.5 * octomap.getResolution(), + gridmap.atPosition("elevation", grid_map::Position(1.0, 1.0))); } diff --git a/grid_map_octomap/test/test_grid_map_octomap.cpp b/grid_map_octomap/test/test_grid_map_octomap.cpp index 91eb387b9..e50e62fb3 100644 --- a/grid_map_octomap/test/test_grid_map_octomap.cpp +++ b/grid_map_octomap/test/test_grid_map_octomap.cpp @@ -10,9 +10,9 @@ #include // Run all the tests that were declared with TEST() -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - srand((int)time(0)); + srand(static_cast(time(0))); return RUN_ALL_TESTS(); } diff --git a/grid_map_pcl/CHANGELOG.rst b/grid_map_pcl/CHANGELOG.rst index 44656beed..c0923d989 100644 --- a/grid_map_pcl/CHANGELOG.rst +++ b/grid_map_pcl/CHANGELOG.rst @@ -2,6 +2,39 @@ Changelog for package grid_map_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +unreleased (2021-06-24) +------------------ +* Replaced pcl_ros dependency with PCL +* Contributors: Matthew Young (Trimble Inc) + +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Merge pull request `#423 `_ from Ryanf55/export-pcl-deps-correctly + Split PCL deps for dev and runtime +* Split PCL deps to dev and runtime +* Contributors: Ryan, Ryan Friedman + +2.1.0 (2022-11-08) +------------------ + +2.0.0 (2022-09-13) +------------------ +* ci: fix tests in grid map pcl +* fix: mark Eigen library as SYSTEM +* fix: build error on jammy +* Add height of cluster with the most points method +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map_pcl/CMakeLists.txt b/grid_map_pcl/CMakeLists.txt index 81dd8f22b..489f2adb9 100644 --- a/grid_map_pcl/CMakeLists.txt +++ b/grid_map_pcl/CMakeLists.txt @@ -1,56 +1,43 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(grid_map_pcl) -set(CMAKE_CXX_STANDARD 14) -add_compile_options(-Wall -Wextra -Wpedantic) -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +## Find ament macros and libraries +find_package(ament_cmake REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(PCL REQUIRED) ## See ROS REP2000 for expected PCL version +find_package(rclcpp REQUIRED) +find_package(rcutils REQUIRED) + +find_package(OpenMP QUIET) +if(OpenMP_FOUND) + add_compile_options("${OpenMP_CXX_FLAGS}") + add_definitions(-DGRID_MAP_PCL_OPENMP_FOUND=${OpenMP_FOUND}) +endif() + +# Add links to PCL definitons and directories +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +grid_map_package() set(SRC_FILES - src/GridMapPclConverter.cpp src/GridMapPclLoader.cpp - src/helpers.cpp src/PclLoaderParameters.cpp src/PointcloudProcessor.cpp + src/helpers.cpp + src/GridMapPclConverter.cpp ) -set(CATKIN_PACKAGE_DEPENDENCIES +set(dependencies grid_map_core grid_map_msgs grid_map_ros - pcl_ros - roscpp -) - -find_package(OpenMP QUIET) -if (OpenMP_FOUND) - add_compile_options("${OpenMP_CXX_FLAGS}") - add_definitions(-DGRID_MAP_PCL_OPENMP_FOUND=${OpenMP_FOUND}) -endif() - -## Find catkin macros and libraries -find_package(catkin REQUIRED - COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - yaml-cpp - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS + rclcpp + rcutils ) ########### @@ -61,24 +48,23 @@ catkin_package( include_directories( include SYSTEM - ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${OpenMP_CXX_INCLUDE_DIRS} ) ## Declare a cpp library -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED ${SRC_FILES} ) target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} ${OpenMP_CXX_LIBRARIES} + ${PCL_LIBRARIES} yaml-cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(${PROJECT_NAME} SYSTEM + ${dependencies} ) add_executable(grid_map_pcl_loader_node @@ -87,10 +73,13 @@ add_executable(grid_map_pcl_loader_node target_link_libraries(grid_map_pcl_loader_node ${PROJECT_NAME} - ${catkin_LIBRARIES} ${OpenMP_CXX_LIBRARIES} ) +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} +) + ############# ## Install ## ############# @@ -100,38 +89,80 @@ install( TARGETS ${PROJECT_NAME} grid_map_pcl_loader_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) # Mark cpp header files for installation install( DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} FILES_MATCHING PATTERN "*.hpp" ) +install( + DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME} +) + ############# ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") -## Add gtest based cpp test target and link libraries -catkin_add_gtest(${PROJECT_NAME}-test - test/test_grid_map_pcl.cpp - test/GridMapPclLoaderTest.cpp - test/HelpersTest.cpp - test/PointcloudProcessorTest.cpp - test/test_helpers.cpp - test/PointcloudCreator.cpp +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright -build/include_order + ) + endif() + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + # Adding test data to the package share directory + install( + DIRECTORY test/test_data/ + DESTINATION share/${PROJECT_NAME}/test_data/ + ) + + ## Add gtest based cpp test target and link libraries + ament_add_gtest(${PROJECT_NAME}-test + test/GridMapPclLoaderTest.cpp + test/HelpersTest.cpp + test/PointcloudCreator.cpp + test/PointcloudProcessorTest.cpp + test/test_helpers.cpp + test/test_grid_map_pcl.cpp + TIMEOUT 300 ) endif() if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test + target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} - ${catkin_LIBRARIES}) + ) endif() +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_package() diff --git a/grid_map_pcl/README.md b/grid_map_pcl/README.md index 9ff5d5f70..b280d7d34 100644 --- a/grid_map_pcl/README.md +++ b/grid_map_pcl/README.md @@ -51,6 +51,10 @@ The resulting grid map will be saved in the folder given by *folder_path* variab Resulting grid map parameters. * **pcl_grid_map_extraction/grid_map/min_num_points_per_cell** Minimum number of points in the point cloud that have to fall within any of the grid map cells. Otherwise the cell elevation will be set to NaN. * **pcl_grid_map_extraction/grid_map/resolution** Resolution of the grid map. Width and lengts are computed automatically. +* **pcl_grid_map_extraction/grid_map/height_type** How to calculate elevation value. +0: Smallest value among the average values ​​of each cluster +1: Mean value of the cluster with the most points +* **pcl_grid_map_extraction/grid_map/height_thresh** Height range from the smallest cluster.(Only for height_type 1) ### Point Cloud Pre-processing Parameters diff --git a/grid_map_pcl/config/parameters.yaml b/grid_map_pcl/config/parameters.yaml index 47c31c99f..391ca5063 100644 --- a/grid_map_pcl/config/parameters.yaml +++ b/grid_map_pcl/config/parameters.yaml @@ -26,6 +26,11 @@ pcl_grid_map_extraction: grid_map: min_num_points_per_cell: 4 resolution: 0.1 + # 0: Smallest value among the average values ​​of each cluster + # 1: Mean value of the cluster with the most points + height_type: 0 + # For height_type 1 + height_thresh: 1.0 diff --git a/grid_map_pcl/include/grid_map_pcl/GridMapPclConverter.hpp b/grid_map_pcl/include/grid_map_pcl/GridMapPclConverter.hpp index e7a000cd0..e7aa03b03 100644 --- a/grid_map_pcl/include/grid_map_pcl/GridMapPclConverter.hpp +++ b/grid_map_pcl/include/grid_map_pcl/GridMapPclConverter.hpp @@ -6,7 +6,8 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_PCL__GRIDMAPPCLCONVERTER_HPP_ +#define GRID_MAP_PCL__GRIDMAPPCLCONVERTER_HPP_ #include @@ -19,18 +20,20 @@ // STD #include +#include #include #include #include -namespace grid_map { +namespace grid_map +{ /*! * Conversions between grid maps and PCL types. */ class GridMapPclConverter { - public: +public: /*! * Default constructor. */ @@ -49,8 +52,9 @@ class GridMapPclConverter * @param[out] gridMap the grid map to be initialized. * @return true if successful, false otherwise. */ - static bool initializeFromPolygonMesh(const pcl::PolygonMesh& mesh, const double resolution, - grid_map::GridMap& gridMap); + static bool initializeFromPolygonMesh( + const pcl::PolygonMesh & mesh, const double resolution, + grid_map::GridMap & gridMap); /*! * Adds a layer with data from a polygon mesh. The mesh is ray traced from @@ -60,15 +64,17 @@ class GridMapPclConverter * @param[out] gridMap the grid map to be populated. * @return true if successful, false otherwise. */ - static bool addLayerFromPolygonMesh(const pcl::PolygonMesh& mesh, const std::string& layer, - grid_map::GridMap& gridMap); - - private: - static bool rayTriangleIntersect(const Eigen::Vector3f& point, - const Eigen::Vector3f& ray, - const Eigen::Matrix3f& triangleVertices, - Eigen::Vector3f& intersectionPoint); + static bool addLayerFromPolygonMesh( + const pcl::PolygonMesh & mesh, const std::string & layer, + grid_map::GridMap & gridMap); +private: + static bool rayTriangleIntersect( + const Eigen::Vector3f & point, + const Eigen::Vector3f & ray, + const Eigen::Matrix3f & triangleVertices, + Eigen::Vector3f & intersectionPoint); }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_PCL__GRIDMAPPCLCONVERTER_HPP_ diff --git a/grid_map_pcl/include/grid_map_pcl/GridMapPclLoader.hpp b/grid_map_pcl/include/grid_map_pcl/GridMapPclLoader.hpp index aba15b4f5..cbc8d1271 100644 --- a/grid_map_pcl/include/grid_map_pcl/GridMapPclLoader.hpp +++ b/grid_map_pcl/include/grid_map_pcl/GridMapPclLoader.hpp @@ -6,20 +6,25 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#pragma once -#include -#include +#ifndef GRID_MAP_PCL__GRIDMAPPCLLOADER_HPP_ +#define GRID_MAP_PCL__GRIDMAPPCLLOADER_HPP_ + #include #include +#include +#include +#include #include #include "grid_map_core/GridMap.hpp" #include "grid_map_core/iterators/GridMapIterator.hpp" #include "grid_map_pcl/PclLoaderParameters.hpp" #include "grid_map_pcl/PointcloudProcessor.hpp" -namespace grid_map { +namespace grid_map +{ -namespace grid_map_pcl_test { +namespace grid_map_pcl_test +{ class GridMapPclLoaderTest_CalculateElevation_Test; } /* @@ -34,7 +39,6 @@ class GridMapPclLoaderTest_CalculateElevation_Test; class GridMapPclLoader { - friend class grid_map_pcl_test::GridMapPclLoaderTest_CalculateElevation_Test; struct ClusterParameters @@ -42,18 +46,23 @@ class GridMapPclLoader Eigen::Vector3d mean_; }; - public: +public: using Point = ::pcl::PointXYZ; using Pointcloud = ::pcl::PointCloud; - GridMapPclLoader(); + /*! + * Constructor + * @param[in] node logging interface. + */ + explicit GridMapPclLoader( + const rclcpp::Logger & node_logger); ~GridMapPclLoader(); /*! * Loads the point cloud into memory * @param[in] fullpath to the point cloud. */ - void loadCloudFromPcdFile(const std::string& filename); + void loadCloudFromPcdFile(const std::string & filename); /*! * Allows the user to set the input cloud @@ -79,28 +88,27 @@ class GridMapPclLoader * Adds a layer in the grid map. The algorithm is described above. * @param[in] Layer name that will be added */ - void addLayerFromInputCloud(const std::string& layer); + void addLayerFromInputCloud(const std::string & layer); /*! * Get a const reference to a grid map * @param[out] grid map */ - const grid_map::GridMap& getGridMap() const; + const grid_map::GridMap & getGridMap() const; /*! - * Saves a point cloud to a pcd file. + * Saves a point cloud to a pcd file. * @param[in] full path to the output cloud */ - void savePointCloudAsPcdFile(const std::string& filename) const; + void savePointCloudAsPcdFile(const std::string & filename) const; /*! * Load algorithm's parameters. * @param[in] full path to the config file with parameters */ - void loadParameters(const std::string& filename); - - private: + void loadParameters(const std::string & filename); +private: /*! * Copies the input cloud into the memory. This cloud is expected to be * changed if you run the pcl filters. For example if you run the @@ -119,14 +127,14 @@ class GridMapPclLoader */ void setRawInputCloud(Pointcloud::ConstPtr rawInputCloud); - //processing the grid map + // processing the grid map /*! * @param[in] index of a cell in the grid map * @return Point cloud made from points in the working point cloud that fall within * the requested cell in the grid map. */ - Pointcloud::Ptr getPointcloudInsideGridMapCellBorder(const grid_map::Index& index) const; + Pointcloud::Ptr getPointcloudInsideGridMapCellBorder(const grid_map::Index & index) const; /*! * Calculates the elevation at the linear index. The @@ -137,8 +145,9 @@ class GridMapPclLoader * @param[in] linear index of a grid map cell currently being processed * @param[out] matrix of elevation values which need to be computed */ - void processGridMapCell(const unsigned int linearGridMapIndex, - grid_map::Matrix* gridMapData) const; + void processGridMapCell( + const unsigned int linearGridMapIndex, + grid_map::Matrix * gridMapData) const; /*! * Given a point cloud it computes the elevation from it. The algorithm is suited for 2.5 D @@ -175,7 +184,7 @@ class GridMapPclLoader void dispatchWorkingCloudToGridMapCells(); // Matrix of point clouds. Each point cloud has only points that fall within a grid map cell. - std::vector > pointcloudWithinGridMapCell_; + std::vector> pointcloudWithinGridMapCell_; // Point cloud that pcl filters have been applied to (it can change). Pointcloud::Ptr workingCloud_; @@ -189,9 +198,12 @@ class GridMapPclLoader // Parameters for the algorithm. Also includes parameters for the pcl filters. std::unique_ptr params_; + // Logging interface + rclcpp::Logger node_logger_; + // Class that handles point cloud processing grid_map_pcl::PointcloudProcessor pointcloudProcessor_; - }; } // namespace grid_map +#endif // GRID_MAP_PCL__GRIDMAPPCLLOADER_HPP_ diff --git a/grid_map_pcl/include/grid_map_pcl/PclLoaderParameters.hpp b/grid_map_pcl/include/grid_map_pcl/PclLoaderParameters.hpp index cdb52bcde..90882ef67 100644 --- a/grid_map_pcl/include/grid_map_pcl/PclLoaderParameters.hpp +++ b/grid_map_pcl/include/grid_map_pcl/PclLoaderParameters.hpp @@ -6,92 +6,115 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#pragma once +#ifndef GRID_MAP_PCL__PCLLOADERPARAMETERS_HPP_ +#define GRID_MAP_PCL__PCLLOADERPARAMETERS_HPP_ + +#include #include + +#include + #include #include -#include -namespace grid_map { +namespace grid_map +{ class GridMapPclLoader; -namespace grid_map_pcl { +namespace grid_map_pcl +{ class PointcloudProcessor; -class PclLoaderParameters { - - struct DownsamplingParameters { - Eigen::Vector3d voxelSize_ { 0.05, 0.05, 0.05 }; - bool isDownsampleCloud_ = false; - }; - - struct ClusterExtractionParameters { - double clusterTolerance_ = 0.3; - unsigned int minNumPoints_ = 2; - unsigned int maxNumPoints_ = 1000000; - }; - struct OutlierRemovalParameters { - bool isRemoveOutliers_ = false; - double meanK_ = 10.0; - double stddevThreshold_ = 1.0; - }; - struct RigidBodyTransformation { - Eigen::Vector3d translation_ { 0.0, 0.0, 0.0 }; - Eigen::Vector3d rpyIntrinsic_ { 0.0, 0.0, 0.0 }; // intrinsic rotation (opposite from the ROS convention), order X-Y-Z - }; - - struct GridMapParameters { - double resolution_ = 0.1; - unsigned int minCloudPointsPerCell_ = 2; - }; - - struct Parameters { - unsigned int numThreads_ = 4; - RigidBodyTransformation cloudTransformation_; - OutlierRemovalParameters outlierRemoval_; - ClusterExtractionParameters clusterExtraction_; - DownsamplingParameters downsampling_; - GridMapParameters gridMap_; - }; - - friend class grid_map::GridMapPclLoader; - friend class grid_map::grid_map_pcl::PointcloudProcessor; +class PclLoaderParameters +{ + struct DownsamplingParameters + { + Eigen::Vector3d voxelSize_ {0.05, 0.05, 0.05}; + bool isDownsampleCloud_ = false; + }; + + struct ClusterExtractionParameters + { + double clusterTolerance_ = 0.3; + unsigned int minNumPoints_ = 2; + unsigned int maxNumPoints_ = 1000000; + }; + + struct OutlierRemovalParameters + { + bool isRemoveOutliers_ = false; + double meanK_ = 10.0; + double stddevThreshold_ = 1.0; + }; + + struct RigidBodyTransformation + { + Eigen::Vector3d translation_ {0.0, 0.0, 0.0}; + + // intrinsic rotation (opposite from the ROS convention), order X-Y-Z + Eigen::Vector3d rpyIntrinsic_ {0.0, 0.0, 0.0}; + }; + + struct GridMapParameters + { + double resolution_ = 0.1; + unsigned int minCloudPointsPerCell_ = 2; + // 0: Smallest value among the average values ​​of each cluster + // 1: Mean value of the cluster with the most points + unsigned int height_type_ = 0; + // For height_type 1 + double height_thresh_ = 1.0; + }; + + struct Parameters + { + unsigned int numThreads_ = 4; + RigidBodyTransformation cloudTransformation_; + OutlierRemovalParameters outlierRemoval_; + ClusterExtractionParameters clusterExtraction_; + DownsamplingParameters downsampling_; + GridMapParameters gridMap_; + }; + + friend class grid_map::GridMapPclLoader; + friend class grid_map::grid_map_pcl::PointcloudProcessor; public: - - PclLoaderParameters() = default; - ~PclLoaderParameters() = default; + explicit PclLoaderParameters(const rclcpp::Logger & node_logger); + ~PclLoaderParameters() = default; private: - - /*! - * Load parameters for the GridMapPclLoader class. - * @param[in] full path to the config file with parameters - * @return true if operation was okay - */ - bool loadParameters(const std::string &filename); - - /*! - * Invoke operator[] on the yaml node. Finds - * the parameters in the yaml tree. - */ - void handleYamlNode(const YAML::Node &yamlNode); - - /*! - * Saves typing parameters_ in the owner class. The owner of this - * class can type parameters_.get() instead of parameters_.parameters_ - * which would be weird - */ - const Parameters& get() const; - - // Parameters for the GridMapPclLoader class. - Parameters parameters_; - + /*! + * Load parameters for the GridMapPclLoader class. + * @param[in] full path to the config file with parameters + * @return true if operation was okay + */ + bool loadParameters(const std::string & filename); + + /*! + * Invoke operator[] on the yaml node. Finds + * the parameters in the yaml tree. + */ + void handleYamlNode(const YAML::Node & yamlNode); + + /*! + * Saves typing parameters_ in the owner class. The owner of this + * class can type parameters_.get() instead of parameters_.parameters_ + * which would be weird + */ + const Parameters & get() const; + + // Parameters for the GridMapPclLoader class. + Parameters parameters_; + + // Node logger + rclcpp::Logger node_logger_; }; -} /* namespace grid_map_pcl*/ +} // namespace grid_map_pcl -} /* namespace grid_map */ +} // namespace grid_map +#endif // GRID_MAP_PCL__PCLLOADERPARAMETERS_HPP_ diff --git a/grid_map_pcl/include/grid_map_pcl/PointcloudProcessor.hpp b/grid_map_pcl/include/grid_map_pcl/PointcloudProcessor.hpp index a82161a3d..169935757 100644 --- a/grid_map_pcl/include/grid_map_pcl/PointcloudProcessor.hpp +++ b/grid_map_pcl/include/grid_map_pcl/PointcloudProcessor.hpp @@ -6,28 +6,32 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#pragma once +#ifndef GRID_MAP_PCL__POINTCLOUDPROCESSOR_HPP_ +#define GRID_MAP_PCL__POINTCLOUDPROCESSOR_HPP_ + +#include +#include +#include #include "grid_map_pcl/helpers.hpp" #include "grid_map_pcl/PclLoaderParameters.hpp" -#include -namespace grid_map { -namespace grid_map_pcl { +namespace grid_map +{ +namespace grid_map_pcl +{ class PointcloudProcessor { - - public: - - PointcloudProcessor(); +public: + explicit PointcloudProcessor(const rclcpp::Logger & node_logger); virtual ~PointcloudProcessor() = default; /*! * Load parameters for the point cloud processing. * @param[in] full path to the config file with parameters */ - void loadParameters(const std::string& filename); + void loadParameters(const std::string & filename); /*! * Remove outliers from the point cloud. Function is based on @@ -57,7 +61,7 @@ class PointcloudProcessor * @return vector of sets of indices. Vector will be empty if no clusters are found. */ std::vector extractClusterIndicesFromPointcloud( - Pointcloud::ConstPtr inputCloud) const; + Pointcloud::ConstPtr inputCloud) const; /*! * Finds clusters in the input cloud and returns vector point clouds. @@ -67,7 +71,7 @@ class PointcloudProcessor * @return vector of point clouds. Vector will be empty if no clusters are found. */ std::vector extractClusterCloudsFromPointcloud( - Pointcloud::ConstPtr inputCloud) const; + Pointcloud::ConstPtr inputCloud) const; /*! * Given a vector of indices and an input point cloud, the function @@ -77,14 +81,15 @@ class PointcloudProcessor * @param[in] pointer to the pcl point cloud * @return Pointer to the point cloud with points indexed by indices. */ - Pointcloud::Ptr makeCloudFromIndices(const std::vector& indices, - Pointcloud::ConstPtr inputCloud) const; + Pointcloud::Ptr makeCloudFromIndices( + const std::vector & indices, + Pointcloud::ConstPtr inputCloud) const; /*! * Saves a point cloud to a pcd file. * @param[in] full path to the output cloud */ - void savePointCloudAsPcdFile(const std::string& filename, const Pointcloud &cloud) const; + void savePointCloudAsPcdFile(const std::string & filename, const Pointcloud & cloud) const; /*! @@ -95,13 +100,16 @@ class PointcloudProcessor */ Pointcloud::Ptr applyRigidBodyTransformation(Pointcloud::ConstPtr inputCloud) const; - protected: - +protected: // Parameters for the pcl filters. std::unique_ptr params_; +private: + // Logging interface + rclcpp::Logger node_logger_; }; -} /* namespace grid_map_pcl */ +} // namespace grid_map_pcl -} /* namespace grid_map*/ +} // namespace grid_map +#endif // GRID_MAP_PCL__POINTCLOUDPROCESSOR_HPP_ diff --git a/grid_map_pcl/include/grid_map_pcl/grid_map_pcl.hpp b/grid_map_pcl/include/grid_map_pcl/grid_map_pcl.hpp index 942f881be..36977408a 100644 --- a/grid_map_pcl/include/grid_map_pcl/grid_map_pcl.hpp +++ b/grid_map_pcl/include/grid_map_pcl/grid_map_pcl.hpp @@ -6,8 +6,11 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_PCL__GRID_MAP_PCL_HPP_ +#define GRID_MAP_PCL__GRID_MAP_PCL_HPP_ #include #include #include + +#endif // GRID_MAP_PCL__GRID_MAP_PCL_HPP_ diff --git a/grid_map_pcl/include/grid_map_pcl/helpers.hpp b/grid_map_pcl/include/grid_map_pcl/helpers.hpp index 1bf838b06..f3d9f38af 100644 --- a/grid_map_pcl/include/grid_map_pcl/helpers.hpp +++ b/grid_map_pcl/include/grid_map_pcl/helpers.hpp @@ -6,52 +6,77 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#pragma once +#ifndef GRID_MAP_PCL__HELPERS_HPP_ +#define GRID_MAP_PCL__HELPERS_HPP_ -#include #include -#include +#include #include +#include -namespace grid_map { +namespace grid_map +{ class GridMapPclLoader; class GridMap; -namespace grid_map_pcl { +namespace grid_map_pcl +{ -void setVerbosityLevelToDebugIfFlagSet(const ros::NodeHandle& nh) ; +void setVerbosityLevelToDebugIfFlagSet(rclcpp::Node::SharedPtr & node); -std::string getParameterPath() ; +std::string getParameterPath(); -std::string getOutputBagPath(const ros::NodeHandle& nh) ; +std::string getOutputBagPath(rclcpp::Node::SharedPtr & node); -std::string getPcdFilePath(const ros::NodeHandle& nh); +std::string getPcdFilePath(rclcpp::Node::SharedPtr & node); -std::string getMapFrame(const ros::NodeHandle& nh); +std::string getMapFrame(rclcpp::Node::SharedPtr & node); -std::string getMapRosbagTopic(const ros::NodeHandle& nh); +std::string getMapRosbagTopic(rclcpp::Node::SharedPtr & node); -std::string getMapLayerName(const ros::NodeHandle& nh) ; +std::string getMapLayerName(rclcpp::Node::SharedPtr & node); -void saveGridMap(const grid_map::GridMap& gridMap, const ros::NodeHandle& nh, const std::string& mapTopic); +void saveGridMap( + const grid_map::GridMap & gridMap, rclcpp::Node::SharedPtr & node, + const std::string & mapTopic); -inline void printTimeElapsedToRosInfoStream(const std::chrono::system_clock::time_point& start, const std::string& prefix); +inline void printTimeElapsedToRosInfoStream( + const std::chrono::system_clock::time_point & start, + const std::string & prefix, + const rclcpp::Logger & node_logger) +{ + const auto stop = std::chrono::high_resolution_clock::now(); + const auto duration = + std::chrono::duration_cast(stop - start).count() / 1000.0; + RCLCPP_INFO_STREAM(node_logger, prefix << duration << " sec"); +} -void processPointcloud(grid_map::GridMapPclLoader* gridMapPclLoader, const ros::NodeHandle& nh); +void processPointcloud( + grid_map::GridMapPclLoader * gridMapPclLoader, + rclcpp::Node::SharedPtr & node); using Point = ::pcl::PointXYZ; using Pointcloud = ::pcl::PointCloud; -enum class XYZ: int {X,Y,Z}; +enum class XYZ : int {X, Y, Z}; + +Eigen::Affine3f getRigidBodyTransform( + const Eigen::Vector3d & translation, + const Eigen::Vector3d & intrinsicRpy, + const rclcpp::Logger & node_logger); -Eigen::Affine3f getRigidBodyTransform(const Eigen::Vector3d &translation, const Eigen::Vector3d &intrinsicRpy); -Eigen::Matrix3f getRotationMatrix(double angle, XYZ axis); +Eigen::Matrix3f getRotationMatrix( + double angle, XYZ axis, + const rclcpp::Logger & node_logger); -//processing point clouds +// processing point clouds Eigen::Vector3d calculateMeanOfPointPositions(Pointcloud::ConstPtr inputCloud); -Pointcloud::Ptr transformCloud(Pointcloud::ConstPtr inputCloud, const Eigen::Affine3f& transformMatrix); -Pointcloud::Ptr loadPointcloudFromPcd(const std::string& filename); +Pointcloud::Ptr transformCloud( + Pointcloud::ConstPtr inputCloud, + const Eigen::Affine3f & transformMatrix); +Pointcloud::Ptr loadPointcloudFromPcd(const std::string & filename); -} /* namespace grid_map_pcl*/ +} // namespace grid_map_pcl -} /* namespace grid_map */ +} // namespace grid_map +#endif // GRID_MAP_PCL__HELPERS_HPP_ diff --git a/grid_map_pcl/launch/grid_map_pcl_loader_launch.py b/grid_map_pcl/launch/grid_map_pcl_loader_launch.py new file mode 100644 index 000000000..1e5b692cf --- /dev/null +++ b/grid_map_pcl/launch/grid_map_pcl_loader_launch.py @@ -0,0 +1,30 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + + node_params = [ + {'folder_path': 'path/to/pcd/file'}, + {'pcd_filename': 'plane_noisy'}, + {'map_rosbag_topic': 'grid_map'}, + {'output_grid_map': 'elevation_map.bag'}, + {'map_frame': 'map'}, + {'map_layer_name': 'elevation'}, + {'prefix': ''}, + {'set_verbosity_to_debug': False} + ] + + pcl_loader_node = Node( + package='grid_map_pcl', + executable='grid_map_pcl_loader_node', + name='grid_map_pcl_loader_node', + output='screen', + parameters=node_params + ) + + ld = LaunchDescription() + + ld.add_action(pcl_loader_node) + + return ld diff --git a/grid_map_pcl/launch/grid_map_pcl_loader_node.launch b/grid_map_pcl/launch/grid_map_pcl_loader_node.launch deleted file mode 100644 index e8f2a8256..000000000 --- a/grid_map_pcl/launch/grid_map_pcl_loader_node.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/grid_map_pcl/package.xml b/grid_map_pcl/package.xml index b10773be3..637f1af5b 100644 --- a/grid_map_pcl/package.xml +++ b/grid_map_pcl/package.xml @@ -1,21 +1,36 @@ - + + grid_map_pcl - 1.6.2 + 2.2.2 Conversions between grid maps and Point Cloud Library (PCL) types. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Dominic Jud Edo Jelavic - catkin + ament_cmake + grid_map_cmake_helpers + grid_map_core grid_map_msgs grid_map_ros - pcl_ros - roscpp + rclcpp + rcutils yaml-cpp + libpcl-all-dev + libpcl-all-dev + libpcl-all + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/grid_map_pcl/src/GridMapPclConverter.cpp b/grid_map_pcl/src/GridMapPclConverter.cpp index a0df7da55..e4ca0b565 100644 --- a/grid_map_pcl/src/GridMapPclConverter.cpp +++ b/grid_map_pcl/src/GridMapPclConverter.cpp @@ -6,9 +6,13 @@ * Institute: ETH Zurich, ANYbotics */ +#include +#include + #include "grid_map_pcl/GridMapPclConverter.hpp" -namespace grid_map { +namespace grid_map +{ GridMapPclConverter::GridMapPclConverter() { @@ -18,32 +22,35 @@ GridMapPclConverter::~GridMapPclConverter() { } -bool GridMapPclConverter::initializeFromPolygonMesh(const pcl::PolygonMesh& mesh, - const double resolution, - grid_map::GridMap& gridMap) +bool GridMapPclConverter::initializeFromPolygonMesh( + const pcl::PolygonMesh & mesh, + const double resolution, + grid_map::GridMap & gridMap) { - pcl::PointCloud < pcl::PointXYZ > cloud; + pcl::PointCloud cloud; pcl::fromPCLPointCloud2(mesh.cloud, cloud); pcl::PointXYZ minBound; pcl::PointXYZ maxBound; pcl::getMinMax3D(cloud, minBound, maxBound); grid_map::Length length = grid_map::Length(maxBound.x - minBound.x, maxBound.y - minBound.y); - grid_map::Position position = grid_map::Position((maxBound.x + minBound.x) / 2.0, - (maxBound.y + minBound.y) / 2.0); + grid_map::Position position = grid_map::Position( + (maxBound.x + minBound.x) / 2.0, + (maxBound.y + minBound.y) / 2.0); gridMap.setGeometry(length, resolution, position); return true; } -bool GridMapPclConverter::addLayerFromPolygonMesh(const pcl::PolygonMesh& mesh, - const std::string& layer, - grid_map::GridMap& gridMap) +bool GridMapPclConverter::addLayerFromPolygonMesh( + const pcl::PolygonMesh & mesh, + const std::string & layer, + grid_map::GridMap & gridMap) { // Adding a layer to the grid map to put data into gridMap.add(layer); // Converting out of binary cloud data - pcl::PointCloud cloud; + pcl::PointCloud cloud; pcl::fromPCLPointCloud2(mesh.cloud, cloud); // Direction and max height for projection ray const Eigen::Vector3f ray = -Eigen::Vector3f::UnitZ(); @@ -52,7 +59,7 @@ bool GridMapPclConverter::addLayerFromPolygonMesh(const pcl::PolygonMesh& mesh, pcl::getMinMax3D(cloud, minBound, maxBound); // Iterating over the triangles in the mesh - for (const pcl::Vertices& polygon : mesh.polygons) { + for (const pcl::Vertices & polygon : mesh.polygons) { // Testing this is a triangle assert(polygon.vertices.size() == 3); // Getting the vertices of the triangle (as a single matrix) @@ -72,14 +79,15 @@ bool GridMapPclConverter::addLayerFromPolygonMesh(const pcl::PolygonMesh& mesh, SubmapGeometry submap(gridMap, position, length, isSuccess); if (isSuccess) { for (grid_map::SubmapIterator iterator(submap); !iterator.isPastEnd(); - ++iterator) { + ++iterator) + { // Cell position const Index index(*iterator); grid_map::Position vertexPositionXY; gridMap.getPosition(index, vertexPositionXY); // Ray origin Eigen::Vector3f point(vertexPositionXY.x(), vertexPositionXY.y(), - maxBound.z + 1.0); + maxBound.z + 1.0); // Vertical ray/triangle intersection Eigen::Vector3f intersectionPoint; if (rayTriangleIntersect(point, ray, triangleVertexMatrix, intersectionPoint)) { @@ -98,9 +106,10 @@ bool GridMapPclConverter::addLayerFromPolygonMesh(const pcl::PolygonMesh& mesh, } bool GridMapPclConverter::rayTriangleIntersect( - const Eigen::Vector3f& point, const Eigen::Vector3f& ray, - const Eigen::Matrix3f& triangleVertexMatrix, - Eigen::Vector3f& intersectionPoint) { + const Eigen::Vector3f & point, const Eigen::Vector3f & ray, + const Eigen::Matrix3f & triangleVertexMatrix, + Eigen::Vector3f & intersectionPoint) +{ // Algorithm here is adapted from: // http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle() // @@ -117,11 +126,11 @@ bool GridMapPclConverter::rayTriangleIntersect( const Eigen::Vector3f n = u.cross(v); const float n_dot_ray = n.dot(ray); - if (std::fabs(n_dot_ray) < 1e-9) return false; + if (std::fabs(n_dot_ray) < 1e-9) {return false;} const float r = n.dot(a - point) / n_dot_ray; - if (r < 0) return false; + if (r < 0) {return false;} // Note(alexmillane): The addition of this comparison delta (not in the // original algorithm) means that rays intersecting the edge of triangles are @@ -132,15 +141,15 @@ bool GridMapPclConverter::rayTriangleIntersect( const float denominator = u.dot(v) * u.dot(v) - u.dot(u) * v.dot(v); const float s_numerator = u.dot(v) * w.dot(v) - v.dot(v) * w.dot(u); const float s = s_numerator / denominator; - if (s < (0 - delta) || s > (1 + delta)) return false; + if (s < (0 - delta) || s > (1 + delta)) {return false;} const float t_numerator = u.dot(v) * w.dot(u) - u.dot(u) * w.dot(v); const float t = t_numerator / denominator; - if (t < (0 - delta) || s + t > (1 + delta)) return false; + if (t < (0 - delta) || s + t > (1 + delta)) {return false;} intersectionPoint = a + s * u + t * v; return true; } -} /* namespace */ +} // namespace grid_map diff --git a/grid_map_pcl/src/GridMapPclLoader.cpp b/grid_map_pcl/src/GridMapPclLoader.cpp index 364c5fc96..7158db28c 100644 --- a/grid_map_pcl/src/GridMapPclLoader.cpp +++ b/grid_map_pcl/src/GridMapPclLoader.cpp @@ -8,34 +8,41 @@ #include +#include +#include +#include +#include + #ifdef GRID_MAP_PCL_OPENMP_FOUND #include #endif -#include -#include +#include + +#include #include "grid_map_core/GridMapMath.hpp" #include "grid_map_pcl/GridMapPclLoader.hpp" #include "grid_map_pcl/helpers.hpp" -namespace grid_map { - -constexpr double kRadToDeg = 180.0 / M_PI; +namespace grid_map +{ -GridMapPclLoader::GridMapPclLoader() +GridMapPclLoader::GridMapPclLoader(const rclcpp::Logger & node_logger) +: node_logger_(node_logger), + pointcloudProcessor_(node_logger_) { - params_ = std::make_unique(); + params_ = std::make_unique(node_logger_); } GridMapPclLoader::~GridMapPclLoader() = default; -const grid_map::GridMap& GridMapPclLoader::getGridMap() const +const grid_map::GridMap & GridMapPclLoader::getGridMap() const { return workingGridMap_; } -void GridMapPclLoader::loadCloudFromPcdFile(const std::string& filename) +void GridMapPclLoader::loadCloudFromPcdFile(const std::string & filename) { Pointcloud::Ptr inputCloud(new pcl::PointCloud); inputCloud = grid_map_pcl::loadPointcloudFromPcd(filename); @@ -67,7 +74,7 @@ void GridMapPclLoader::setWorkingCloud(Pointcloud::ConstPtr workingCloud) void GridMapPclLoader::preProcessInputCloud() { // Preprocess: Remove outliers, downsample cloud, transform cloud - ROS_INFO_STREAM("Preprocessing of the pointcloud started"); + RCLCPP_INFO_STREAM(node_logger_, "Preprocessing of the pointcloud started"); if (params_->get().outlierRemoval_.isRemoveOutliers_) { auto filteredCloud = pointcloudProcessor_.removeOutliersFromInputCloud(workingCloud_); @@ -81,12 +88,11 @@ void GridMapPclLoader::preProcessInputCloud() auto transformedCloud = pointcloudProcessor_.applyRigidBodyTransformation(workingCloud_); setWorkingCloud(transformedCloud); - ROS_INFO_STREAM("Preprocessing and filtering finished"); + RCLCPP_INFO_STREAM(node_logger_, "Preprocessing and filtering finished"); } void GridMapPclLoader::initializeGridMapGeometryFromInputCloud() { - workingGridMap_.clearAll(); const double resolution = params_->get().gridMap_.resolution_; if (resolution < 1e-4) { @@ -99,33 +105,41 @@ void GridMapPclLoader::initializeGridMapGeometryFromInputCloud() pcl::PointXYZ maxBound; pcl::getMinMax3D(*workingCloud_, minBound, maxBound); - //from min and max points we can compute the length + // from min and max points we can compute the length grid_map::Length length = grid_map::Length(maxBound.x - minBound.x, maxBound.y - minBound.y); - //we put the center of the grid map to be in the middle of the point cloud - grid_map::Position position = grid_map::Position((maxBound.x + minBound.x) / 2.0, - (maxBound.y + minBound.y) / 2.0); + // we put the center of the grid map to be in the middle of the point cloud + grid_map::Position position = grid_map::Position( + (maxBound.x + minBound.x) / 2.0, + (maxBound.y + minBound.y) / 2.0); workingGridMap_.setGeometry(length, resolution, position); - ROS_INFO_STREAM( - "Grid map dimensions: " << workingGridMap_.getLength()(0) << " x " << workingGridMap_.getLength()(1)); - ROS_INFO_STREAM("Grid map resolution: " << workingGridMap_.getResolution()); - ROS_INFO_STREAM( - "Grid map num cells: " << workingGridMap_.getSize()(0) << " x " << workingGridMap_.getSize()(1)); - ROS_INFO_STREAM("Initialized map geometry"); + RCLCPP_INFO_STREAM( + node_logger_, + "Grid map dimensions: " << workingGridMap_.getLength()(0) << + " x " << workingGridMap_.getLength()(1)); + RCLCPP_INFO_STREAM( + node_logger_, "Grid map resolution: " << workingGridMap_.getResolution()); + RCLCPP_INFO_STREAM( + node_logger_, + "Grid map num cells: " << workingGridMap_.getSize()(0) << " x " << + workingGridMap_.getSize()(1)); + RCLCPP_INFO_STREAM(node_logger_, "Initialized map geometry"); } -void GridMapPclLoader::addLayerFromInputCloud(const std::string& layer) +void GridMapPclLoader::addLayerFromInputCloud(const std::string & layer) { - ROS_INFO_STREAM("Started adding layer: " << layer); + RCLCPP_INFO_STREAM(node_logger_, "Started adding layer: " << layer); // Preprocess: allocate memory in the internal data structure preprocessGridMapCells(); workingGridMap_.add(layer); - grid_map::Matrix& gridMapData = workingGridMap_.get(layer); + grid_map::Matrix & gridMapData = workingGridMap_.get(layer); unsigned int linearGridMapSize = workingGridMap_.getSize().prod(); #ifndef GRID_MAP_PCL_OPENMP_FOUND - ROS_WARN_STREAM("OpemMP not found, defaulting to single threaded implementation"); + RCLCPP_WARN_STREAM( + node_logger_, + "OpemMP not found, defaulting to single threaded implementation"); #else omp_set_num_threads(params_->get().numThreads_); #pragma omp parallel for schedule(dynamic, 10) @@ -134,64 +148,97 @@ void GridMapPclLoader::addLayerFromInputCloud(const std::string& layer) for (unsigned int linearIndex = 0; linearIndex < linearGridMapSize; ++linearIndex) { processGridMapCell(linearIndex, &gridMapData); } - ROS_INFO_STREAM("Finished adding layer: " << layer); + RCLCPP_INFO_STREAM(node_logger_, "Finished adding layer: " << layer); } -void GridMapPclLoader::processGridMapCell(const unsigned int linearGridMapIndex, - grid_map::Matrix* gridMapData) const +void GridMapPclLoader::processGridMapCell( + const unsigned int linearGridMapIndex, + grid_map::Matrix * gridMapData) const { // Get grid map index from linear index and check if enough points lie within the cell const grid_map::Index index( - grid_map::getIndexFromLinearIndex(linearGridMapIndex, workingGridMap_.getSize())); + grid_map::getIndexFromLinearIndex(linearGridMapIndex, workingGridMap_.getSize())); Pointcloud::Ptr pointsInsideCellBorder(new Pointcloud()); pointsInsideCellBorder = getPointcloudInsideGridMapCellBorder(index); - const bool isTooFewPointsInCell = pointsInsideCellBorder->size() - < params_->get().gridMap_.minCloudPointsPerCell_; + const bool isTooFewPointsInCell = pointsInsideCellBorder->size() < + params_->get().gridMap_.minCloudPointsPerCell_; if (isTooFewPointsInCell) { - ROS_WARN_STREAM_THROTTLE( - 10.0, "Less than " << params_->get().gridMap_.minCloudPointsPerCell_ << " points in a cell"); + rclcpp::Clock clock; + RCLCPP_WARN_STREAM_THROTTLE( + node_logger_, clock, + 10.0, "Less than " << params_->get().gridMap_.minCloudPointsPerCell_ << " points in a cell"); return; } (*gridMapData)(index(0), index(1)) = calculateElevationFromPointsInsideGridMapCell( - pointsInsideCellBorder); + pointsInsideCellBorder); } double GridMapPclLoader::calculateElevationFromPointsInsideGridMapCell( - Pointcloud::ConstPtr cloud) const + Pointcloud::ConstPtr cloud) const { // Extract point cloud cluster from point cloud and return if none is found. - std::vector clusterClouds = pointcloudProcessor_.extractClusterCloudsFromPointcloud(cloud); + std::vector clusterClouds = + pointcloudProcessor_.extractClusterCloudsFromPointcloud(cloud); const bool isNoClustersFound = clusterClouds.empty(); if (isNoClustersFound) { - ROS_WARN_STREAM_THROTTLE(10.0, "No clusters found in the grid map cell"); + rclcpp::Clock clock; + RCLCPP_WARN_STREAM_THROTTLE( + node_logger_, clock, 10.0, "No clusters found in the grid map cell"); return std::nan("1"); // this will leave the grid map cell uninitialized } // Extract mean z value of cluster vector and return smallest height value std::vector clusterHeights(clusterClouds.size()); - std::transform(clusterClouds.begin(), clusterClouds.end(), clusterHeights.begin(), - [this](Pointcloud::ConstPtr cloud) -> double {return grid_map_pcl::calculateMeanOfPointPositions(cloud).z();}); - - double minClusterHeight = *(std::min_element(clusterHeights.begin(), clusterHeights.end())); + std::transform( + clusterClouds.begin(), clusterClouds.end(), clusterHeights.begin(), + [](Pointcloud::ConstPtr cloud) -> double { + return grid_map_pcl::calculateMeanOfPointPositions(cloud).z(); + }); + + double height; + const int height_type = params_->get().gridMap_.height_type_; + // 0: Smallest value among the average values ​​of each cluster + if (height_type == 0) { + height = *(std::min_element(clusterHeights.begin(), clusterHeights.end())); + // 1: Mean value of the cluster with the most points + } else if (height_type == 1) { + const float min_height = *(std::min_element(clusterHeights.begin(), clusterHeights.end())); + std::vector clusterSizes(clusterClouds.size()); + for (size_t i = 0; i < clusterClouds.size(); i++) { + clusterSizes[i] = clusterHeights[i] - min_height < params_->get().gridMap_.height_thresh_ ? + clusterClouds[i]->size() : + -1; + } + const std::vector::iterator maxIt = + std::max_element(clusterSizes.begin(), clusterSizes.end()); + const size_t maxIndex = std::distance(clusterSizes.begin(), maxIt); + height = clusterHeights[maxIndex]; + } else { + throw std::invalid_argument( + "Invalid height type: " + std::to_string(height_type) + + "\nValid types are below" + + "\n0: Smallest value among the average values ​​of each cluster" + + "\n1: Mean value of the cluster with the most points"); + } - return minClusterHeight; + return height; } GridMapPclLoader::Pointcloud::Ptr GridMapPclLoader::getPointcloudInsideGridMapCellBorder( - const grid_map::Index& index) const + const grid_map::Index & index) const { return pointcloudWithinGridMapCell_[index.x()][index.y()]; } -void GridMapPclLoader::loadParameters(const std::string& filename) +void GridMapPclLoader::loadParameters(const std::string & filename) { params_->loadParameters(filename); pointcloudProcessor_.loadParameters(filename); } -void GridMapPclLoader::savePointCloudAsPcdFile(const std::string& filename) const +void GridMapPclLoader::savePointCloudAsPcdFile(const std::string & filename) const { pointcloudProcessor_.savePointCloudAsPcdFile(filename, *workingCloud_); } @@ -226,7 +273,7 @@ void GridMapPclLoader::dispatchWorkingCloudToGridMapCells() // right cell in the matrix of point clouds data structure. // This allows for faster access in the clustering stage. for (unsigned int i = 0; i < workingCloud_->points.size(); ++i) { - const Point& point = workingCloud_->points[i]; + const Point & point = workingCloud_->points[i]; const double x = point.x; const double y = point.y; grid_map::Index index; diff --git a/grid_map_pcl/src/PclLoaderParameters.cpp b/grid_map_pcl/src/PclLoaderParameters.cpp index 6d9c70eb5..337e67f62 100644 --- a/grid_map_pcl/src/PclLoaderParameters.cpp +++ b/grid_map_pcl/src/PclLoaderParameters.cpp @@ -8,91 +8,101 @@ #include "grid_map_pcl/PclLoaderParameters.hpp" -#include - - -namespace grid_map { - -namespace grid_map_pcl { - -void PclLoaderParameters::handleYamlNode(const YAML::Node &yamlNode) { - - const std::string prefix = "pcl_grid_map_extraction"; - - parameters_.numThreads_ = - yamlNode[prefix]["num_processing_threads"].as(); - - parameters_.cloudTransformation_.translation_.x() = - yamlNode[prefix]["cloud_transform"]["translation"]["x"].as(); - parameters_.cloudTransformation_.translation_.y() = - yamlNode[prefix]["cloud_transform"]["translation"]["y"].as(); - parameters_.cloudTransformation_.translation_.z() = - yamlNode[prefix]["cloud_transform"]["translation"]["z"].as(); - - parameters_.cloudTransformation_.rpyIntrinsic_.x() = - yamlNode[prefix]["cloud_transform"]["rotation"]["r"].as(); - parameters_.cloudTransformation_.rpyIntrinsic_.y() = - yamlNode[prefix]["cloud_transform"]["rotation"]["p"].as(); - parameters_.cloudTransformation_.rpyIntrinsic_.z() = - yamlNode[prefix]["cloud_transform"]["rotation"]["y"].as(); - - parameters_.clusterExtraction_.clusterTolerance_ = - yamlNode[prefix]["cluster_extraction"]["cluster_tolerance"].as< - double>(); - parameters_.clusterExtraction_.minNumPoints_ = - yamlNode[prefix]["cluster_extraction"]["min_num_points"].as(); - parameters_.clusterExtraction_.maxNumPoints_ = - yamlNode[prefix]["cluster_extraction"]["max_num_points"].as(); - - parameters_.outlierRemoval_.isRemoveOutliers_ = - yamlNode[prefix]["outlier_removal"]["is_remove_outliers"].as(); - parameters_.outlierRemoval_.meanK_ = - yamlNode[prefix]["outlier_removal"]["mean_K"].as(); - parameters_.outlierRemoval_.stddevThreshold_ = - yamlNode[prefix]["outlier_removal"]["stddev_threshold"].as(); - - parameters_.gridMap_.resolution_ = - yamlNode[prefix]["grid_map"]["resolution"].as(); - parameters_.gridMap_.minCloudPointsPerCell_ = - yamlNode[prefix]["grid_map"]["min_num_points_per_cell"].as(); - - parameters_.downsampling_.isDownsampleCloud_ = - yamlNode[prefix]["downsampling"]["is_downsample_cloud"].as(); - parameters_.downsampling_.voxelSize_.x() = - yamlNode[prefix]["downsampling"]["voxel_size"]["x"].as(); - parameters_.downsampling_.voxelSize_.y() = - yamlNode[prefix]["downsampling"]["voxel_size"]["y"].as(); - parameters_.downsampling_.voxelSize_.z() = - yamlNode[prefix]["downsampling"]["voxel_size"]["z"].as(); - +#include + +#include + +namespace grid_map +{ + +namespace grid_map_pcl +{ + +PclLoaderParameters::PclLoaderParameters(const rclcpp::Logger & node_logger) +: node_logger_(node_logger) {} + +void PclLoaderParameters::handleYamlNode(const YAML::Node & yamlNode) +{ + const std::string prefix = "pcl_grid_map_extraction"; + + parameters_.numThreads_ = + yamlNode[prefix]["num_processing_threads"].as(); + + parameters_.cloudTransformation_.translation_.x() = + yamlNode[prefix]["cloud_transform"]["translation"]["x"].as(); + parameters_.cloudTransformation_.translation_.y() = + yamlNode[prefix]["cloud_transform"]["translation"]["y"].as(); + parameters_.cloudTransformation_.translation_.z() = + yamlNode[prefix]["cloud_transform"]["translation"]["z"].as(); + + parameters_.cloudTransformation_.rpyIntrinsic_.x() = + yamlNode[prefix]["cloud_transform"]["rotation"]["r"].as(); + parameters_.cloudTransformation_.rpyIntrinsic_.y() = + yamlNode[prefix]["cloud_transform"]["rotation"]["p"].as(); + parameters_.cloudTransformation_.rpyIntrinsic_.z() = + yamlNode[prefix]["cloud_transform"]["rotation"]["y"].as(); + + parameters_.clusterExtraction_.clusterTolerance_ = + yamlNode[prefix]["cluster_extraction"]["cluster_tolerance"].as< + double>(); + parameters_.clusterExtraction_.minNumPoints_ = + yamlNode[prefix]["cluster_extraction"]["min_num_points"].as(); + parameters_.clusterExtraction_.maxNumPoints_ = + yamlNode[prefix]["cluster_extraction"]["max_num_points"].as(); + + parameters_.outlierRemoval_.isRemoveOutliers_ = + yamlNode[prefix]["outlier_removal"]["is_remove_outliers"].as(); + parameters_.outlierRemoval_.meanK_ = + yamlNode[prefix]["outlier_removal"]["mean_K"].as(); + parameters_.outlierRemoval_.stddevThreshold_ = + yamlNode[prefix]["outlier_removal"]["stddev_threshold"].as(); + + parameters_.gridMap_.resolution_ = + yamlNode[prefix]["grid_map"]["resolution"].as(); + parameters_.gridMap_.minCloudPointsPerCell_ = + yamlNode[prefix]["grid_map"]["min_num_points_per_cell"].as(); + parameters_.gridMap_.height_type_ = + yamlNode[prefix]["grid_map"]["height_type"].as(); + parameters_.gridMap_.height_thresh_ = + yamlNode[prefix]["grid_map"]["height_thresh"].as(); + + parameters_.downsampling_.isDownsampleCloud_ = + yamlNode[prefix]["downsampling"]["is_downsample_cloud"].as(); + parameters_.downsampling_.voxelSize_.x() = + yamlNode[prefix]["downsampling"]["voxel_size"]["x"].as(); + parameters_.downsampling_.voxelSize_.y() = + yamlNode[prefix]["downsampling"]["voxel_size"]["y"].as(); + parameters_.downsampling_.voxelSize_.z() = + yamlNode[prefix]["downsampling"]["voxel_size"]["z"].as(); } -bool PclLoaderParameters::loadParameters(const std::string &filename) { - +bool PclLoaderParameters::loadParameters(const std::string & filename) +{ YAML::Node yamlNode = YAML::LoadFile(filename); - const bool loadingFailed = yamlNode.IsNull(); - if (loadingFailed) { - ROS_ERROR_STREAM("PclLoaderParameters: Reading from file failed"); - return false; - } - - try { - handleYamlNode(yamlNode); - }catch(const std::runtime_error &exception){ - ROS_ERROR_STREAM("PclLoaderParameters: Loading parameters failed: " << exception.what()); - return false; - } - - return true; - + const bool loadingFailed = yamlNode.IsNull(); + if (loadingFailed) { + RCLCPP_ERROR_STREAM(node_logger_, "PclLoaderParameters: Reading from file failed"); + return false; + } + + try { + handleYamlNode(yamlNode); + } catch (const std::runtime_error & exception) { + RCLCPP_ERROR_STREAM( + node_logger_, + "PclLoaderParameters: Loading parameters failed: " << exception.what()); + return false; + } + + return true; } -const PclLoaderParameters::Parameters &PclLoaderParameters::get() const{ - return parameters_; +const PclLoaderParameters::Parameters & PclLoaderParameters::get() const +{ + return parameters_; } -} // namespace grid_map_pcl - -} // namespace grid_map +} // namespace grid_map_pcl +} // namespace grid_map diff --git a/grid_map_pcl/src/PointcloudProcessor.cpp b/grid_map_pcl/src/PointcloudProcessor.cpp index 6301cb452..98577c745 100644 --- a/grid_map_pcl/src/PointcloudProcessor.cpp +++ b/grid_map_pcl/src/PointcloudProcessor.cpp @@ -20,23 +20,30 @@ #include #include -#include +#include -namespace grid_map { -namespace grid_map_pcl { +#include +#include +#include -PointcloudProcessor::PointcloudProcessor() +namespace grid_map { - params_ = std::make_unique(); +namespace grid_map_pcl +{ + +PointcloudProcessor::PointcloudProcessor(const rclcpp::Logger & node_logger) +: node_logger_(node_logger) +{ + params_ = std::make_unique(node_logger_); } -void PointcloudProcessor::loadParameters(const std::string& filename) +void PointcloudProcessor::loadParameters(const std::string & filename) { params_->loadParameters(filename); } Pointcloud::Ptr PointcloudProcessor::removeOutliersFromInputCloud( - Pointcloud::ConstPtr inputCloud) const + Pointcloud::ConstPtr inputCloud) const { pcl::StatisticalOutlierRemoval sor; sor.setInputCloud(inputCloud); @@ -48,12 +55,12 @@ Pointcloud::Ptr PointcloudProcessor::removeOutliersFromInputCloud( } std::vector PointcloudProcessor::extractClusterCloudsFromPointcloud( - Pointcloud::ConstPtr inputCloud) const + Pointcloud::ConstPtr inputCloud) const { std::vector clusterIndices = extractClusterIndicesFromPointcloud(inputCloud); std::vector clusterClouds; clusterClouds.reserve(clusterIndices.size()); - for (const auto& indicesSet : clusterIndices) { + for (const auto & indicesSet : clusterIndices) { Pointcloud::Ptr clusterCloud = makeCloudFromIndices(indicesSet.indices, inputCloud); clusterClouds.push_back(clusterCloud); } @@ -63,7 +70,7 @@ std::vector PointcloudProcessor::extractClusterCloudsFromPointc // todo (jelavice) maybe use the libpointmatcher for this?? faster? std::vector PointcloudProcessor::extractClusterIndicesFromPointcloud( - Pointcloud::ConstPtr inputCloud) const + Pointcloud::ConstPtr inputCloud) const { // Create a kd tree to cluster the input point cloud pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); @@ -71,7 +78,7 @@ std::vector PointcloudProcessor::extractClusterIndicesFromPoi std::vector clusterIndices; pcl::EuclideanClusterExtraction euclideanClusterExtraction; euclideanClusterExtraction.setClusterTolerance( - params_->get().clusterExtraction_.clusterTolerance_); + params_->get().clusterExtraction_.clusterTolerance_); euclideanClusterExtraction.setMinClusterSize(params_->get().clusterExtraction_.minNumPoints_); euclideanClusterExtraction.setMaxClusterSize(params_->get().clusterExtraction_.maxNumPoints_); euclideanClusterExtraction.setSearchMethod(tree); @@ -81,8 +88,9 @@ std::vector PointcloudProcessor::extractClusterIndicesFromPoi return clusterIndices; } -Pointcloud::Ptr PointcloudProcessor::makeCloudFromIndices(const std::vector& indices, - Pointcloud::ConstPtr inputCloud) const +Pointcloud::Ptr PointcloudProcessor::makeCloudFromIndices( + const std::vector & indices, + Pointcloud::ConstPtr inputCloud) const { Pointcloud::Ptr cloud(new Pointcloud()); @@ -100,35 +108,37 @@ Pointcloud::Ptr PointcloudProcessor::downsampleInputCloud(Pointcloud::ConstPtr i { pcl::VoxelGrid voxelGrid; voxelGrid.setInputCloud(inputCloud); - const auto& voxelSize = params_->get().downsampling_.voxelSize_; + const auto & voxelSize = params_->get().downsampling_.voxelSize_; voxelGrid.setLeafSize(voxelSize.x(), voxelSize.y(), voxelSize.z()); Pointcloud::Ptr downsampledCloud(new Pointcloud()); voxelGrid.filter(*downsampledCloud); return downsampledCloud; } -void PointcloudProcessor::savePointCloudAsPcdFile(const std::string& filename, - const Pointcloud &cloud) const +void PointcloudProcessor::savePointCloudAsPcdFile( + const std::string & filename, + const Pointcloud & cloud) const { pcl::PCDWriter writer; pcl::PCLPointCloud2 pointCloud2; pcl::toPCLPointCloud2(cloud, pointCloud2); - writer.write(filename, pointCloud2, Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), - false); + writer.write( + filename, pointCloud2, Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), + false); } Pointcloud::Ptr PointcloudProcessor::applyRigidBodyTransformation( - Pointcloud::ConstPtr inputCloud) const + Pointcloud::ConstPtr inputCloud) const { auto transformedCloud = grid_map_pcl::transformCloud( - inputCloud, - grid_map_pcl::getRigidBodyTransform(params_->get().cloudTransformation_.translation_, - params_->get().cloudTransformation_.rpyIntrinsic_)); + inputCloud, + grid_map_pcl::getRigidBodyTransform( + params_->get().cloudTransformation_.translation_, + params_->get().cloudTransformation_.rpyIntrinsic_, + node_logger_)); return transformedCloud; - } -} /* namespace grid_map_pcl */ - -} /* namespace grid_map*/ +} // namespace grid_map_pcl +} // namespace grid_map diff --git a/grid_map_pcl/src/grid_map_pcl_loader_node.cpp b/grid_map_pcl/src/grid_map_pcl_loader_node.cpp index c9ce24a24..bd0182840 100644 --- a/grid_map_pcl/src/grid_map_pcl_loader_node.cpp +++ b/grid_map_pcl/src/grid_map_pcl_loader_node.cpp @@ -6,7 +6,12 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#include +#include +#include + +#include +#include +#include #include "grid_map_core/GridMap.hpp" #include "grid_map_ros/GridMapRosConverter.hpp" @@ -15,34 +20,33 @@ namespace gm = ::grid_map::grid_map_pcl; -int main(int argc, char** argv) { - ros::init(argc, argv, "grid_map_pcl_loader_node"); - ros::NodeHandle nh("~"); - gm::setVerbosityLevelToDebugIfFlagSet(nh); +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("grid_map_pcl_loader_node"); + gm::setVerbosityLevelToDebugIfFlagSet(node); - ros::Publisher gridMapPub; - gridMapPub = nh.advertise("grid_map_from_raw_pointcloud", 1, true); + rclcpp::QoS custom_qos = rclcpp::QoS(1).transient_local(); + auto gridMapPub = node->create_publisher( + "grid_map_from_raw_pointcloud", custom_qos); - grid_map::GridMapPclLoader gridMapPclLoader; - const std::string pathToCloud = gm::getPcdFilePath(nh); + grid_map::GridMapPclLoader gridMapPclLoader(node->get_logger()); + const std::string pathToCloud = gm::getPcdFilePath(node); gridMapPclLoader.loadParameters(gm::getParameterPath()); gridMapPclLoader.loadCloudFromPcdFile(pathToCloud); - gm::processPointcloud(&gridMapPclLoader, nh); + gm::processPointcloud(&gridMapPclLoader, node); grid_map::GridMap gridMap = gridMapPclLoader.getGridMap(); - gridMap.setFrameId(gm::getMapFrame(nh)); - - gm::saveGridMap(gridMap, nh, gm::getMapRosbagTopic(nh)); - - //publish grid map + gridMap.setFrameId(gm::getMapFrame(node)); - grid_map_msgs::GridMap msg; - grid_map::GridMapRosConverter::toMessage(gridMap, msg); - gridMapPub.publish(msg); + gm::saveGridMap(gridMap, node, gm::getMapRosbagTopic(node)); + // publish grid map + auto msg = grid_map::GridMapRosConverter::toMessage(gridMap); + gridMapPub->publish(std::move(msg)); // run - ros::spin(); + rclcpp::spin(node->get_node_base_interface()); return EXIT_SUCCESS; } diff --git a/grid_map_pcl/src/helpers.cpp b/grid_map_pcl/src/helpers.cpp index 1a78a0214..ea840b59d 100644 --- a/grid_map_pcl/src/helpers.cpp +++ b/grid_map_pcl/src/helpers.cpp @@ -6,13 +6,7 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#include "grid_map_pcl/helpers.hpp" -#include "grid_map_core/GridMap.hpp" -#include "grid_map_ros/GridMapRosConverter.hpp" -#include "grid_map_pcl/GridMapPclLoader.hpp" - -#include -#include +#include #include #include @@ -20,120 +14,166 @@ #include #include -#include -#include +#include +#include + +#include +#include +#include +#include "grid_map_pcl/helpers.hpp" +#include "grid_map_core/GridMap.hpp" +#include "grid_map_ros/GridMapRosConverter.hpp" +#include "grid_map_pcl/GridMapPclLoader.hpp" -namespace grid_map{ -namespace grid_map_pcl { +namespace grid_map +{ +namespace grid_map_pcl +{ -void setVerbosityLevelToDebugIfFlagSet(const ros::NodeHandle& nh) { - bool isSetVerbosityLevelToDebug; - nh.param("set_verbosity_to_debug", isSetVerbosityLevelToDebug, false); +void setVerbosityLevelToDebugIfFlagSet(rclcpp::Node::SharedPtr & node) +{ + bool isSetVerbosityLevelToDebug = node->declare_parameter("set_verbosity_to_debug", false); - if (!isSetVerbosityLevelToDebug){ + if (!isSetVerbosityLevelToDebug) { return; } - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { - ros::console::notifyLoggerLevelsChanged(); + auto ret = rcutils_logging_set_logger_level( + node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR( + node->get_logger(), "Failed to change logging severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); } } -std::string getParameterPath() { - std::string filePath = ros::package::getPath("grid_map_pcl") + "/config/parameters.yaml"; +std::string getParameterPath() +{ + std::string filePath = ament_index_cpp::get_package_share_directory("grid_map_pcl") + + "/config/parameters.yaml"; return filePath; } -std::string getOutputBagPath(const ros::NodeHandle& nh) { +std::string getOutputBagPath(rclcpp::Node::SharedPtr & node) +{ + if (!node->has_parameter("folder_path")) { + node->declare_parameter("folder_path", std::string("")); + } + node->declare_parameter("output_grid_map", "output_grid_map.bag"); + std::string outputRosbagName, folderPath; - nh.param("folder_path", folderPath, ""); - nh.param("output_grid_map", outputRosbagName, "output_grid_map.bag"); + node->get_parameter("folder_path", folderPath); + node->get_parameter("output_grid_map", outputRosbagName); std::string pathToOutputBag = folderPath + "/" + outputRosbagName; return pathToOutputBag; } -std::string getPcdFilePath(const ros::NodeHandle& nh) { +std::string getPcdFilePath(rclcpp::Node::SharedPtr & node) +{ + if (!node->has_parameter("folder_path")) { + node->declare_parameter("folder_path", std::string("")); + } + node->declare_parameter("pcd_filename", "input_cloud"); + std::string inputCloudName, folderPath; - nh.param("folder_path", folderPath, ""); - nh.param("pcd_filename", inputCloudName, "input_cloud"); + node->get_parameter("folder_path", folderPath); + node->get_parameter("pcd_filename", inputCloudName); std::string pathToCloud = folderPath + "/" + inputCloudName; return pathToCloud; } -std::string getMapFrame(const ros::NodeHandle& nh) { +std::string getMapFrame(rclcpp::Node::SharedPtr & node) +{ + node->declare_parameter("map_frame", std::string("map")); + std::string mapFrame; - nh.param("map_frame", mapFrame, "map"); + node->get_parameter("map_frame", mapFrame); return mapFrame; } -std::string getMapRosbagTopic(const ros::NodeHandle& nh) { +std::string getMapRosbagTopic(rclcpp::Node::SharedPtr & node) +{ + node->declare_parameter("map_rosbag_topic", std::string("grid_map")); + std::string mapRosbagTopic; - nh.param("map_rosbag_topic", mapRosbagTopic, "grid_map"); + node->get_parameter("map_rosbag_topic", mapRosbagTopic); return mapRosbagTopic; } -std::string getMapLayerName(const ros::NodeHandle& nh) { +std::string getMapLayerName(rclcpp::Node::SharedPtr & node) +{ + node->declare_parameter("map_layer_name", std::string("elevation")); + std::string mapLayerName; - nh.param("map_layer_name", mapLayerName, "elevation"); + node->get_parameter("map_layer_name", mapLayerName); return mapLayerName; } -void saveGridMap(const grid_map::GridMap& gridMap, const ros::NodeHandle& nh, const std::string& mapTopic) { - std::string pathToOutputBag = getOutputBagPath(nh); - const bool savingSuccessful = grid_map::GridMapRosConverter::saveToBag(gridMap, pathToOutputBag, mapTopic); - ROS_INFO_STREAM("Saving grid map successful: " << std::boolalpha << savingSuccessful); -} - -inline void printTimeElapsedToRosInfoStream(const std::chrono::system_clock::time_point& start, const std::string& prefix) { - const auto stop = std::chrono::high_resolution_clock::now(); - const auto duration = std::chrono::duration_cast(stop - start).count() / 1000.0; - ROS_INFO_STREAM(prefix << duration << " sec"); +void saveGridMap( + const grid_map::GridMap & gridMap, rclcpp::Node::SharedPtr & node, + const std::string & mapTopic) +{ + std::string pathToOutputBag = getOutputBagPath(node); + const bool savingSuccessful = grid_map::GridMapRosConverter::saveToBag( + gridMap, pathToOutputBag, + mapTopic); + RCLCPP_INFO_STREAM( + node->get_logger(), "Saving grid map successful: " << std::boolalpha << savingSuccessful); } -void processPointcloud(grid_map::GridMapPclLoader* gridMapPclLoader, const ros::NodeHandle& nh) { +void processPointcloud( + grid_map::GridMapPclLoader * gridMapPclLoader, + rclcpp::Node::SharedPtr & node) +{ const auto start = std::chrono::high_resolution_clock::now(); gridMapPclLoader->preProcessInputCloud(); gridMapPclLoader->initializeGridMapGeometryFromInputCloud(); - printTimeElapsedToRosInfoStream(start, "Initialization took: "); - gridMapPclLoader->addLayerFromInputCloud(getMapLayerName(nh)); - printTimeElapsedToRosInfoStream(start, "Total time: "); + printTimeElapsedToRosInfoStream( + start, "Initialization took: ", node->get_logger()); + gridMapPclLoader->addLayerFromInputCloud(getMapLayerName(node)); + printTimeElapsedToRosInfoStream(start, "Total time: ", node->get_logger()); } -Eigen::Affine3f getRigidBodyTransform(const Eigen::Vector3d &translation, - const Eigen::Vector3d &intrinsicRpy) +Eigen::Affine3f getRigidBodyTransform( + const Eigen::Vector3d & translation, + const Eigen::Vector3d & intrinsicRpy, + const rclcpp::Logger & node_logger) { Eigen::Affine3f rigidBodyTransform; rigidBodyTransform.setIdentity(); rigidBodyTransform.translation() << translation.x(), translation.y(), translation.z(); Eigen::Matrix3f rotation(Eigen::Matrix3f::Identity()); - rotation *= getRotationMatrix(intrinsicRpy.x(), XYZ::X); - rotation *= getRotationMatrix(intrinsicRpy.y(), XYZ::Y); - rotation *= getRotationMatrix(intrinsicRpy.z(), XYZ::Z); + rotation *= getRotationMatrix(intrinsicRpy.x(), XYZ::X, node_logger); + rotation *= getRotationMatrix(intrinsicRpy.y(), XYZ::Y, node_logger); + rotation *= getRotationMatrix(intrinsicRpy.z(), XYZ::Z, node_logger); rigidBodyTransform.rotate(rotation); return rigidBodyTransform; } -Eigen::Matrix3f getRotationMatrix(double angle, XYZ axis) +Eigen::Matrix3f getRotationMatrix( + double angle, XYZ axis, + const rclcpp::Logger & node_logger) { Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity(); switch (axis) { case XYZ::X: { - rotationMatrix = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitX()); - break; - } + rotationMatrix = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitX()); + break; + } case XYZ::Y: { - rotationMatrix = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitY()); - break; - } + rotationMatrix = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitY()); + break; + } case XYZ::Z: { - rotationMatrix = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ()); - break; - } + rotationMatrix = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ()); + break; + } default: - ROS_ERROR("Unknown axis while trying to rotate the pointcloud"); + RCLCPP_ERROR(node_logger, "Unknown axis while trying to rotate the pointcloud"); } return rotationMatrix; } @@ -141,7 +181,7 @@ Eigen::Matrix3f getRotationMatrix(double angle, XYZ axis) Eigen::Vector3d calculateMeanOfPointPositions(Pointcloud::ConstPtr inputCloud) { Eigen::Vector3d mean = Eigen::Vector3d::Zero(); - for (const auto& point : inputCloud->points) { + for (const auto & point : inputCloud->points) { mean += Eigen::Vector3d(point.x, point.y, point.z); } mean /= inputCloud->points.size(); @@ -149,7 +189,7 @@ Eigen::Vector3d calculateMeanOfPointPositions(Pointcloud::ConstPtr inputCloud) return mean; } -Pointcloud::Ptr loadPointcloudFromPcd(const std::string& filename) +Pointcloud::Ptr loadPointcloudFromPcd(const std::string & filename) { Pointcloud::Ptr cloud(new pcl::PointCloud); pcl::PCLPointCloud2 cloudBlob; @@ -158,8 +198,9 @@ Pointcloud::Ptr loadPointcloudFromPcd(const std::string& filename) return cloud; } -Pointcloud::Ptr transformCloud(Pointcloud::ConstPtr inputCloud, - const Eigen::Affine3f& transformMatrix) +Pointcloud::Ptr transformCloud( + Pointcloud::ConstPtr inputCloud, + const Eigen::Affine3f & transformMatrix) { Pointcloud::Ptr transformedCloud(new Pointcloud()); pcl::transformPointCloud(*inputCloud, *transformedCloud, transformMatrix); @@ -167,6 +208,5 @@ Pointcloud::Ptr transformCloud(Pointcloud::ConstPtr inputCloud, } -} /* namespace grid_map_pcl*/ -} /* namespace grid_map */ - +} // namespace grid_map_pcl +} // namespace grid_map diff --git a/grid_map_pcl/test/GridMapPclLoaderTest.cpp b/grid_map_pcl/test/GridMapPclLoaderTest.cpp index b74b3d523..f779326e1 100644 --- a/grid_map_pcl/test/GridMapPclLoaderTest.cpp +++ b/grid_map_pcl/test/GridMapPclLoaderTest.cpp @@ -6,61 +6,82 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#include #include +#include +#include -#include "grid_map_pcl/GridMapPclLoader.hpp" +#include +#include +#include +#include "grid_map_pcl/GridMapPclLoader.hpp" #include "PointcloudCreator.hpp" #include "test_helpers.hpp" -namespace grid_map { -namespace grid_map_pcl_test { - -TEST(GridMapPclLoaderTest, FlatGroundRealDataset) +namespace grid_map +{ +namespace grid_map_pcl_test { - grid_map_pcl_test::setVerbosityLevel(ros::console::levels::Warn); +class GridMapPclLoaderTest : public ::testing::Test +{ +protected: + void SetUp() override + { + // Changing the logging verbosity to WARN + auto ret = rcutils_logging_set_logger_level( + test_logger_.get_name(), RCUTILS_LOG_SEVERITY_WARN); + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR( + test_logger_, "Failed to change logging severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + } - grid_map::GridMapPclLoader gridMapPclLoader; + rclcpp::Logger test_logger_ = rclcpp::get_logger("GridMapPclLoaderTest"); +}; + +// cppcheck-suppress syntaxError +TEST_F(GridMapPclLoaderTest, FlatGroundRealDataset) +{ + grid_map::GridMapPclLoader gridMapPclLoader(test_logger_); gridMapPclLoader.loadParameters(grid_map_pcl_test::getConfigFilePath()); gridMapPclLoader.loadCloudFromPcdFile(grid_map_pcl_test::getTestPcdFilePath()); gridMapPclLoader.preProcessInputCloud(); gridMapPclLoader.initializeGridMapGeometryFromInputCloud(); gridMapPclLoader.addLayerFromInputCloud(grid_map_pcl_test::layerName); - const auto &gridMap = gridMapPclLoader.getGridMap(); + const auto & gridMap = gridMapPclLoader.getGridMap(); const auto elevationValues = grid_map_pcl_test::getNonNanElevationValues(gridMap); - EXPECT_TRUE(elevationValues.size() > 0); + EXPECT_GT(elevationValues.size(), 0ul); - //test that all the elevation values are equal + // test that all the elevation values are equal // allow for some difference (2cm) since the input cloud is noisy (real dataset) double referenceElevation = elevationValues.front(); - for (const auto &elevation : elevationValues) { + for (const auto & elevation : elevationValues) { EXPECT_NEAR(elevation, referenceElevation, 2e-2); } - } -TEST(GridMapPclLoaderTest, PerfectPlane) +TEST_F(GridMapPclLoaderTest, PerfectPlane) { - grid_map_pcl_test::setVerbosityLevel(ros::console::levels::Warn); - const auto seed = rand(); + const auto seed = std::rand(); grid_map_pcl_test::rndGenerator.seed(seed); const int numTests = 10; for (int i = 0; i < numTests; ++i) { double height; auto cloud = grid_map_pcl_test::PointcloudCreator::createPerfectPlane(&height); - grid_map::GridMapPclLoader gridMapPclLoader; + grid_map::GridMapPclLoader gridMapPclLoader(test_logger_); grid_map_pcl_test::runGridMapPclLoaderOnInputCloud(cloud, &gridMapPclLoader); const auto elevationValues = grid_map_pcl_test::getNonNanElevationValues( - gridMapPclLoader.getGridMap()); + gridMapPclLoader.getGridMap()); - EXPECT_TRUE(elevationValues.size() > 0); + EXPECT_GT(elevationValues.size(), 0ul); const double tolerance = 1e-5; - for (const auto &elevation : elevationValues) { + for (const auto & elevation : elevationValues) { EXPECT_NEAR(elevation, height, tolerance); } } @@ -68,27 +89,25 @@ TEST(GridMapPclLoaderTest, PerfectPlane) if (::testing::Test::HasFailure()) { std::cout << "\n Test PerfectPlane failed with seed: " << seed << std::endl; } - } -TEST(GridMapPclLoaderTest, NoisyPlane) +TEST_F(GridMapPclLoaderTest, NoisyPlane) { - grid_map_pcl_test::setVerbosityLevel(ros::console::levels::Warn); - const auto seed = rand(); + const auto seed = std::rand(); grid_map_pcl_test::rndGenerator.seed(seed); const int numTests = 10; for (int i = 0; i < numTests; ++i) { double height, stdDevZ; auto cloud = grid_map_pcl_test::PointcloudCreator::createNoisyPlane(&height, &stdDevZ); - grid_map::GridMapPclLoader gridMapPclLoader; + grid_map::GridMapPclLoader gridMapPclLoader(test_logger_); grid_map_pcl_test::runGridMapPclLoaderOnInputCloud(cloud, &gridMapPclLoader); const auto elevationValues = grid_map_pcl_test::getNonNanElevationValues( - gridMapPclLoader.getGridMap()); + gridMapPclLoader.getGridMap()); - EXPECT_TRUE(elevationValues.size() > 0); - for (const auto &elevation : elevationValues) { - EXPECT_NEAR(elevation, height, 3*stdDevZ); + EXPECT_GT(elevationValues.size(), 0ul); + for (const auto & elevation : elevationValues) { + EXPECT_NEAR(elevation, height, 3 * stdDevZ); } } @@ -97,22 +116,21 @@ TEST(GridMapPclLoaderTest, NoisyPlane) } } -TEST(GridMapPclLoaderTest, NoisyDoublePlane) +TEST_F(GridMapPclLoaderTest, NoisyDoublePlane) { - grid_map_pcl_test::setVerbosityLevel(ros::console::levels::Warn); - const auto seed = rand(); + const auto seed = std::rand(); grid_map_pcl_test::rndGenerator.seed(seed); double minZ, stdDevZ; auto cloud = grid_map_pcl_test::PointcloudCreator::createNoisyDoublePlane(&minZ, &stdDevZ); - grid_map::GridMapPclLoader gridMapPclLoader; + grid_map::GridMapPclLoader gridMapPclLoader(test_logger_); grid_map_pcl_test::runGridMapPclLoaderOnInputCloud(cloud, &gridMapPclLoader); const auto elevationValues = grid_map_pcl_test::getNonNanElevationValues( - gridMapPclLoader.getGridMap()); + gridMapPclLoader.getGridMap()); - EXPECT_TRUE(elevationValues.size() > 0); - for (const auto &elevation : elevationValues) { - EXPECT_NEAR(elevation, minZ, 3*stdDevZ); + EXPECT_GT(elevationValues.size(), 0ul); + for (const auto & elevation : elevationValues) { + EXPECT_NEAR(elevation, minZ, 3 * stdDevZ); } if (::testing::Test::HasFailure()) { @@ -120,19 +138,18 @@ TEST(GridMapPclLoaderTest, NoisyDoublePlane) } } -TEST(GridMapPclLoaderTest, InitializeGeometry) +TEST_F(GridMapPclLoaderTest, InitializeGeometry) { - grid_map_pcl_test::setVerbosityLevel(ros::console::levels::Warn); - const auto seed = rand(); + const auto seed = std::rand(); grid_map_pcl_test::rndGenerator.seed(seed); const unsigned int numTests = 1000; for (unsigned int i = 0; i < numTests; ++i) { - double xLocation, yLocation; - auto cloud = grid_map_pcl_test::PointcloudCreator::createVerticesOfASquare(&xLocation, - &yLocation); - grid_map::GridMapPclLoader gridMapPclLoader; + auto cloud = grid_map_pcl_test::PointcloudCreator::createVerticesOfASquare( + &xLocation, + &yLocation); + grid_map::GridMapPclLoader gridMapPclLoader(test_logger_); gridMapPclLoader.loadParameters(grid_map_pcl_test::getConfigFilePath()); gridMapPclLoader.setInputCloud(cloud); gridMapPclLoader.initializeGridMapGeometryFromInputCloud(); @@ -152,32 +169,30 @@ TEST(GridMapPclLoaderTest, InitializeGeometry) if (::testing::Test::HasFailure()) { std::cout << "\n Test InitializeGeometry failed with seed: " << seed << std::endl; } - } -TEST(GridMapPclLoaderTest, NoisyStepTerrain) +TEST_F(GridMapPclLoaderTest, NoisyStepTerrain) { - grid_map_pcl_test::setVerbosityLevel(ros::console::levels::Warn); - const auto seed = rand(); + const auto seed = std::rand(); grid_map_pcl_test::rndGenerator.seed(seed); const int numTests = 10; for (int i = 0; i < numTests; ++i) { double stepLocationX, zHigh, zLow, stdDevZ; const auto cloud = grid_map_pcl_test::PointcloudCreator::createNoisyPointcloudOfStepTerrain( - &stepLocationX, &zHigh, &zLow, &stdDevZ); - grid_map::GridMapPclLoader gridMapPclLoader; + &stepLocationX, &zHigh, &zLow, &stdDevZ); + grid_map::GridMapPclLoader gridMapPclLoader(test_logger_); grid_map_pcl_test::runGridMapPclLoaderOnInputCloud(cloud, &gridMapPclLoader); const auto coordinates = grid_map_pcl_test::getNonNanElevationValuesWithCoordinates( - gridMapPclLoader.getGridMap()); + gridMapPclLoader.getGridMap()); - EXPECT_TRUE(coordinates.size() > 0); - for (const auto &coordinate : coordinates) { + EXPECT_GT(coordinates.size(), 0ul); + for (const auto & coordinate : coordinates) { if (coordinate.x() > stepLocationX) { - EXPECT_NEAR(coordinate.z(), zHigh, 3*stdDevZ); + EXPECT_NEAR(coordinate.z(), zHigh, 3 * stdDevZ); } if (coordinate.x() < stepLocationX) { - EXPECT_NEAR(coordinate.z(), zLow, 3*stdDevZ); + EXPECT_NEAR(coordinate.z(), zLow, 3 * stdDevZ); } } } @@ -185,52 +200,48 @@ TEST(GridMapPclLoaderTest, NoisyStepTerrain) if (::testing::Test::HasFailure()) { std::cout << "\n Test NoisyStepTerrain failed with seed: " << seed << std::endl; } - } // end StepTerrainPointcloud test -TEST(GridMapPclLoaderTest, CalculateElevation) +TEST_F(GridMapPclLoaderTest, CalculateElevation) { - grid_map_pcl_test::setVerbosityLevel(ros::console::levels::Warn); - const auto seed = rand(); + const auto seed = std::rand(); grid_map_pcl_test::rndGenerator.seed(seed); const unsigned int numTests = 10; for (unsigned int i = 0; i < numTests; ++i) { - double minZ, stdDevZ; int nBlobs; - auto cloud = grid_map_pcl_test::PointcloudCreator::createNBlobsAboveEachOther(&minZ, &stdDevZ, - &nBlobs); - grid_map::GridMapPclLoader gridMapPclLoader; + auto cloud = grid_map_pcl_test::PointcloudCreator::createNBlobsAboveEachOther( + &minZ, &stdDevZ, + &nBlobs); + grid_map::GridMapPclLoader gridMapPclLoader(test_logger_); gridMapPclLoader.loadParameters(grid_map_pcl_test::getConfigFilePath()); gridMapPclLoader.setInputCloud(cloud); double elevation = gridMapPclLoader.calculateElevationFromPointsInsideGridMapCell(cloud); - EXPECT_NEAR(elevation, minZ, 3*stdDevZ); + EXPECT_NEAR(elevation, minZ, 3 * stdDevZ); } if (::testing::Test::HasFailure()) { std::cout << "\n Test CalculateElevation failed with seed: " << seed << std::endl; } - } -TEST(GridMapPclLoaderTest, SavePointclouds) +TEST_F(GridMapPclLoaderTest, SavePointclouds) { - if (!grid_map_pcl_test::savePointclouds){ + if (!grid_map_pcl_test::savePointclouds) { return; } - grid_map_pcl_test::setVerbosityLevel(ros::console::levels::Warn); - const auto seed = rand(); + const auto seed = std::rand(); grid_map_pcl_test::rndGenerator.seed(seed); double dummyDouble1, dummyDouble2, dummyDouble3, dummyDouble4; int dummyInt; - grid_map::GridMapPclLoader gridMapPclLoader; + grid_map::GridMapPclLoader gridMapPclLoader(test_logger_); gridMapPclLoader.loadParameters(grid_map_pcl_test::getConfigFilePath()); - //perfect plane + // perfect plane std::string filename = grid_map_pcl_test::getTestDataFolderPath() + "/perfectPlane.pcd"; auto cloud = grid_map_pcl_test::PointcloudCreator::createPerfectPlane(&dummyDouble1); gridMapPclLoader.setInputCloud(cloud); @@ -238,16 +249,18 @@ TEST(GridMapPclLoaderTest, SavePointclouds) // n blobs filename = grid_map_pcl_test::getTestDataFolderPath() + "/Nblobs.pcd"; - cloud = grid_map_pcl_test::PointcloudCreator::createNBlobsAboveEachOther(&dummyDouble1, - &dummyDouble2, - &dummyInt); + cloud = grid_map_pcl_test::PointcloudCreator::createNBlobsAboveEachOther( + &dummyDouble1, + &dummyDouble2, + &dummyInt); gridMapPclLoader.setInputCloud(cloud); gridMapPclLoader.savePointCloudAsPcdFile(filename); // 4 points filename = grid_map_pcl_test::getTestDataFolderPath() + "/4pointSquare.pcd"; - cloud = grid_map_pcl_test::PointcloudCreator::createVerticesOfASquare(&dummyDouble1, - &dummyDouble2); + cloud = grid_map_pcl_test::PointcloudCreator::createVerticesOfASquare( + &dummyDouble1, + &dummyDouble2); gridMapPclLoader.setInputCloud(cloud); gridMapPclLoader.savePointCloudAsPcdFile(filename); @@ -259,7 +272,11 @@ TEST(GridMapPclLoaderTest, SavePointclouds) // step terrain filename = grid_map_pcl_test::getTestDataFolderPath() + "/stepTerrain.pcd"; - cloud = grid_map_pcl_test::PointcloudCreator::createNoisyPointcloudOfStepTerrain(&dummyDouble1, &dummyDouble2, &dummyDouble3, &dummyDouble4); + cloud = grid_map_pcl_test::PointcloudCreator::createNoisyPointcloudOfStepTerrain( + &dummyDouble1, + &dummyDouble2, + &dummyDouble3, + &dummyDouble4); gridMapPclLoader.setInputCloud(cloud); gridMapPclLoader.savePointCloudAsPcdFile(filename); @@ -271,13 +288,13 @@ TEST(GridMapPclLoaderTest, SavePointclouds) // noisy double plane filename = grid_map_pcl_test::getTestDataFolderPath() + "/doublePlane.pcd"; - cloud = grid_map_pcl_test::PointcloudCreator::createNoisyDoublePlane(&dummyDouble1, - &dummyDouble2); + cloud = grid_map_pcl_test::PointcloudCreator::createNoisyDoublePlane( + &dummyDouble1, + &dummyDouble2); gridMapPclLoader.setInputCloud(cloud); gridMapPclLoader.savePointCloudAsPcdFile(filename); - } -} /*namespace grid_map_pcl_test */ +} // namespace grid_map_pcl_test -} /*namesapce grid_map */ +} // namespace grid_map diff --git a/grid_map_pcl/test/HelpersTest.cpp b/grid_map_pcl/test/HelpersTest.cpp index 9eaca5a6d..0742c7062 100644 --- a/grid_map_pcl/test/HelpersTest.cpp +++ b/grid_map_pcl/test/HelpersTest.cpp @@ -6,9 +6,10 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#include - #include +#include + +#include #include "grid_map_pcl/helpers.hpp" @@ -16,18 +17,18 @@ #include "test_helpers.hpp" -namespace grid_map { -namespace grid_map_pcl_test { +namespace grid_map +{ +namespace grid_map_pcl_test +{ TEST(HelpersTest, MeanPositionTest) { - grid_map_pcl_test::setVerbosityLevel(ros::console::levels::Warn); - const auto seed = rand(); + const auto seed = std::rand(); grid_map_pcl_test::rndGenerator.seed(seed); const unsigned int numTests = 10; for (unsigned int i = 0; i < numTests; ++i) { - double mean, stdDev; auto cloud = grid_map_pcl_test::PointcloudCreator::createBlobOfPoints(&mean, &stdDev); auto meanOfAllPoints = grid_map_pcl::calculateMeanOfPointPositions(cloud); @@ -40,9 +41,7 @@ TEST(HelpersTest, MeanPositionTest) if (::testing::Test::HasFailure()) { std::cout << "\n Test MeanPositionTest failed with seed: " << seed << std::endl; } - } -} /*namespace grid_map_pcl_test */ -} /*namesapce grid_map */ - +} // namespace grid_map_pcl_test +} // namespace grid_map diff --git a/grid_map_pcl/test/PointcloudCreator.cpp b/grid_map_pcl/test/PointcloudCreator.cpp index 9b262a63b..50d7e5200 100644 --- a/grid_map_pcl/test/PointcloudCreator.cpp +++ b/grid_map_pcl/test/PointcloudCreator.cpp @@ -8,16 +8,16 @@ #include "PointcloudCreator.hpp" -namespace grid_map { -namespace grid_map_pcl_test { - -Pointcloud::Ptr PointcloudCreator::createNoisyPointcloudOfStepTerrain(double *stepLocationX, - double *zHigh, double *zLow, - double *stdDev) +namespace grid_map +{ +namespace grid_map_pcl_test { - const double minHeight = 0.0; - const double maxHeight = 10.0; +Pointcloud::Ptr PointcloudCreator::createNoisyPointcloudOfStepTerrain( + double * stepLocationX, + double * zHigh, double * zLow, + double * stdDev) +{ std::uniform_real_distribution heightDist(-2.0, 2.0); const double maxXY = 3.0; const double minXY = -3.0; @@ -25,16 +25,15 @@ Pointcloud::Ptr PointcloudCreator::createNoisyPointcloudOfStepTerrain(double *st const unsigned int nPointsInCloud = 1000000; *zHigh = heightDist(rndGenerator) + 2.1; *zLow = heightDist(rndGenerator) - 2.1; - auto cloud = grid_map_pcl_test::createStepTerrain(nPointsInCloud, minXY, maxXY, *zHigh, *zLow, - *stdDev, &rndGenerator, stepLocationX); + auto cloud = grid_map_pcl_test::createStepTerrain( + nPointsInCloud, minXY, maxXY, *zHigh, *zLow, + *stdDev, &rndGenerator, stepLocationX); return cloud; - } -Pointcloud::Ptr PointcloudCreator::createBlobOfPoints(double *mean, double *stdDev) +Pointcloud::Ptr PointcloudCreator::createBlobOfPoints(double * mean, double * stdDev) { - const unsigned int nPointsInCloud = 10000; std::uniform_real_distribution meanDist(-10.0, 10.0); std::uniform_real_distribution sigmaDist(0.001, 0.1); @@ -42,16 +41,15 @@ Pointcloud::Ptr PointcloudCreator::createBlobOfPoints(double *mean, double *stdD *mean = meanDist(rndGenerator); *stdDev = sigmaDist(rndGenerator); - auto cloud = grid_map_pcl_test::createNormallyDistributedBlobOfPoints(nPointsInCloud, *mean, - *stdDev, &rndGenerator); + auto cloud = grid_map_pcl_test::createNormallyDistributedBlobOfPoints( + nPointsInCloud, *mean, + *stdDev, &rndGenerator); return cloud; - } -Pointcloud::Ptr PointcloudCreator::createVerticesOfASquare(double *x, double *y) +Pointcloud::Ptr PointcloudCreator::createVerticesOfASquare(double * x, double * y) { - grid_map_pcl_test::Pointcloud::Ptr cloud(new grid_map_pcl_test::Pointcloud()); std::uniform_real_distribution zDist(-10.0, 10.0); std::uniform_int_distribution xDist(10, 20); @@ -66,12 +64,10 @@ Pointcloud::Ptr PointcloudCreator::createVerticesOfASquare(double *x, double *y) cloud->is_dense = true; return cloud; - } -Pointcloud::Ptr PointcloudCreator::createNoisyDoublePlane(double *minZ, double *stdDevZ) +Pointcloud::Ptr PointcloudCreator::createNoisyDoublePlane(double * minZ, double * stdDevZ) { - std::uniform_real_distribution upperPlaneZDist(0.0, 10.0); std::uniform_real_distribution lowerPlaneZDist(-10.0, -5.0); std::uniform_real_distribution stdDevDist(0.001, 0.1); @@ -80,24 +76,25 @@ Pointcloud::Ptr PointcloudCreator::createNoisyDoublePlane(double *minZ, double * const double minXY = -1.0; *stdDevZ = stdDevDist(rndGenerator); - //make it very dense such that we ensure that there is enough points + // make it very dense such that we ensure that there is enough points // that will make it into the cell // todo should sample in a better way const unsigned int nPointsInCloud = 1000000; *minZ = lowerPlaneZDist(rndGenerator); - auto cloudLower = grid_map_pcl_test::createNoisyPlanePointcloud(nPointsInCloud, minXY, maxXY, - *minZ, *stdDevZ, &rndGenerator); - auto cloudUpper = grid_map_pcl_test::createNoisyPlanePointcloud(nPointsInCloud, minXY, maxXY, - upperPlaneZDist(rndGenerator), - *stdDevZ, &rndGenerator); + auto cloudLower = grid_map_pcl_test::createNoisyPlanePointcloud( + nPointsInCloud, minXY, maxXY, + *minZ, *stdDevZ, &rndGenerator); + auto cloudUpper = grid_map_pcl_test::createNoisyPlanePointcloud( + nPointsInCloud, minXY, maxXY, + upperPlaneZDist(rndGenerator), + *stdDevZ, &rndGenerator); auto cloud = grid_map_pcl_test::concatenate(cloudUpper, cloudLower); return cloud; } -Pointcloud::Ptr PointcloudCreator::createNoisyPlane(double *height, double *stdDevZ) +Pointcloud::Ptr PointcloudCreator::createNoisyPlane(double * height, double * stdDevZ) { - std::uniform_real_distribution heightDist(-10.0, 10.0); std::uniform_real_distribution stdDevDist(0.001, 0.1); @@ -105,34 +102,36 @@ Pointcloud::Ptr PointcloudCreator::createNoisyPlane(double *height, double *stdD const double minXY = -1.0; *stdDevZ = stdDevDist(rndGenerator); - //make it very dense such that we ensure that there is enough points + // make it very dense such that we ensure that there is enough points // that will make it into the cell // todo should sample in a better way const unsigned int nPointsInCloud = 1000000; *height = heightDist(rndGenerator); - auto cloud = grid_map_pcl_test::createNoisyPlanePointcloud(nPointsInCloud, minXY, maxXY, *height, - *stdDevZ, &rndGenerator); + auto cloud = grid_map_pcl_test::createNoisyPlanePointcloud( + nPointsInCloud, minXY, maxXY, *height, + *stdDevZ, &rndGenerator); return cloud; } -Pointcloud::Ptr PointcloudCreator::createPerfectPlane(double *height) +Pointcloud::Ptr PointcloudCreator::createPerfectPlane(double * height) { - std::uniform_real_distribution heightDist(-10.0, 10.0); const double maxXY = 3.0; const double minXY = -3.0; const unsigned int nPointsInCloud = 100000; *height = heightDist(rndGenerator); - auto cloud = grid_map_pcl_test::createPerfectPlane(nPointsInCloud, minXY, maxXY, *height, - &rndGenerator); + auto cloud = grid_map_pcl_test::createPerfectPlane( + nPointsInCloud, minXY, maxXY, *height, + &rndGenerator); return cloud; } -Pointcloud::Ptr PointcloudCreator::createNBlobsAboveEachOther(double *minZ, double *stdDevZ, - int *nBlobs) +Pointcloud::Ptr PointcloudCreator::createNBlobsAboveEachOther( + double * minZ, double * stdDevZ, + int * nBlobs) { const unsigned int nPointsInCloud = 1000; std::uniform_real_distribution sigmaDist(0.001, 0.015); @@ -147,17 +146,16 @@ Pointcloud::Ptr PointcloudCreator::createNBlobsAboveEachOther(double *minZ, doub Pointcloud::Ptr cloud(new Pointcloud()); for (int i = 0; i < *nBlobs; ++i) { - auto blob = grid_map_pcl_test::createNormallyDistributedBlobOfPoints(nPointsInCloud, - *minZ + i * zStep, - *stdDevZ, &rndGenerator); + auto blob = grid_map_pcl_test::createNormallyDistributedBlobOfPoints( + nPointsInCloud, + *minZ + i * zStep, + *stdDevZ, &rndGenerator); auto temp = concatenate(cloud, blob); cloud = temp; } return cloud; - } -} /* namespace grid_map_pcl_test*/ -} /* namespace grid_map*/ - +} // namespace grid_map_pcl_test +} // namespace grid_map diff --git a/grid_map_pcl/test/PointcloudCreator.hpp b/grid_map_pcl/test/PointcloudCreator.hpp index d0fe76c9d..03f75c7f9 100644 --- a/grid_map_pcl/test/PointcloudCreator.hpp +++ b/grid_map_pcl/test/PointcloudCreator.hpp @@ -6,12 +6,15 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#pragma once +#ifndef POINTCLOUDCREATOR_HPP_ +#define POINTCLOUDCREATOR_HPP_ #include "test_helpers.hpp" -namespace grid_map { -namespace grid_map_pcl_test { +namespace grid_map +{ +namespace grid_map_pcl_test +{ /* * Creates point clouds with some noise in the z direction (which is @@ -20,9 +23,7 @@ namespace grid_map_pcl_test { */ class PointcloudCreator { - - public: - +public: /*! * Creates a noisy point cloud of a step (i.e. two planes with some * displacement in z direction in between). Visualize stepTerrain.pcd inside @@ -34,8 +35,9 @@ class PointcloudCreator * @param[out] std deviation in z direction of the points that form a plane * @return point cloud of the step terrain */ - static Pointcloud::Ptr createNoisyPointcloudOfStepTerrain(double *stepLocation, double *zHigh, - double *zLow, double *stdDev); + static Pointcloud::Ptr createNoisyPointcloudOfStepTerrain( + double * stepLocation, double * zHigh, + double * zLow, double * stdDev); /*! * Creates a blob of points (x,y,z). All coordinates (x,y,z) @@ -45,7 +47,7 @@ class PointcloudCreator * @param[out] standard deviation for points * @return point cloud that represents that blob of points */ - static Pointcloud::Ptr createBlobOfPoints(double *mean, double *stdDev); + static Pointcloud::Ptr createBlobOfPoints(double * mean, double * stdDev); /*! * Creates a set of 4 points that form a square, with @@ -57,7 +59,7 @@ class PointcloudCreator * @param[out] varialbe y * @return point cloud with 4 points that form a square */ - static Pointcloud::Ptr createVerticesOfASquare(double *x, double *y); + static Pointcloud::Ptr createVerticesOfASquare(double * x, double * y); /*! * Creates a plane where z coordinate of the points is normally distributed @@ -69,7 +71,7 @@ class PointcloudCreator * @param[out] standard deviation in z coordinate * @return point cloud that represents a noisy plane */ - static Pointcloud::Ptr createNoisyPlane(double *height, double *stdDevZ); + static Pointcloud::Ptr createNoisyPlane(double * height, double * stdDevZ); /*! * Creates two planes (top and bottom) where z coordinate of the @@ -81,7 +83,7 @@ class PointcloudCreator * @param[out] standard deviation in z coordinate * @return point cloud that represents a two planes above each other */ - static Pointcloud::Ptr createNoisyDoublePlane(double *minZ, double *stdDevZ); + static Pointcloud::Ptr createNoisyDoublePlane(double * minZ, double * stdDevZ); /*! * Creates a plane with no noisy in z direction at a height that is @@ -91,7 +93,7 @@ class PointcloudCreator * @param[out] height (z coordinate) of the plane * @return point cloud of a perfect plane */ - static Pointcloud::Ptr createPerfectPlane(double *height); + static Pointcloud::Ptr createPerfectPlane(double * height); /*! * Creates N blobs of points where N is uniformly distributed. @@ -104,9 +106,9 @@ class PointcloudCreator * @param[out] number of blobs * @return point cloud with n blobs */ - static Pointcloud::Ptr createNBlobsAboveEachOther(double *minZ, double *stdDevZ, int *nBlobs); - + static Pointcloud::Ptr createNBlobsAboveEachOther(double * minZ, double * stdDevZ, int * nBlobs); }; -} /* namespace grid_map_pcl_test*/ -} /* namespace grid_map*/ +} // namespace grid_map_pcl_test +} // namespace grid_map +#endif // POINTCLOUDCREATOR_HPP_ diff --git a/grid_map_pcl/test/PointcloudProcessorTest.cpp b/grid_map_pcl/test/PointcloudProcessorTest.cpp index e70b7d770..f1f8b00a3 100644 --- a/grid_map_pcl/test/PointcloudProcessorTest.cpp +++ b/grid_map_pcl/test/PointcloudProcessorTest.cpp @@ -6,9 +6,11 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#include - #include +#include +#include + +#include #include "grid_map_pcl/PointcloudProcessor.hpp" @@ -16,32 +18,44 @@ #include "test_helpers.hpp" -namespace grid_map { -namespace grid_map_pcl_test { +namespace grid_map +{ +namespace grid_map_pcl_test +{ TEST(PointcloudProcessorTest, ExtractClusters) { - grid_map_pcl_test::setVerbosityLevel(ros::console::levels::Warn); - const auto seed = rand(); + rclcpp::Logger logger = rclcpp::get_logger("PointcloudProcessorTest"); + + auto ret = rcutils_logging_set_logger_level( + logger.get_name(), RCUTILS_LOG_SEVERITY_WARN); + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR( + logger, + "Failed to change logging severity: %s", rcutils_get_error_string().str); + rcutils_reset_error(); + } + + const auto seed = std::rand(); grid_map_pcl_test::rndGenerator.seed(seed); const unsigned int numTests = 10; for (unsigned int i = 0; i < numTests; ++i) { double minZ, stdDevZ; int nBlobs; - auto cloud = grid_map_pcl_test::PointcloudCreator::createNBlobsAboveEachOther(&minZ, &stdDevZ, - &nBlobs); - grid_map::grid_map_pcl::PointcloudProcessor pointcloudProcessor; + auto cloud = grid_map_pcl_test::PointcloudCreator::createNBlobsAboveEachOther( + &minZ, &stdDevZ, + &nBlobs); + grid_map::grid_map_pcl::PointcloudProcessor pointcloudProcessor(logger); pointcloudProcessor.loadParameters(grid_map_pcl_test::getConfigFilePath()); auto clusters = pointcloudProcessor.extractClusterCloudsFromPointcloud(cloud); - EXPECT_EQ(clusters.size(), nBlobs); + EXPECT_EQ(clusters.size(), static_cast(nBlobs)); } if (::testing::Test::HasFailure()) { std::cout << "\n Test ExtractClusters failed with seed: " << seed << std::endl; } - } -} /*namespace grid_map_pcl_test */ -} /*namesapce grid_map */ +} // namespace grid_map_pcl_test +} // namespace grid_map diff --git a/grid_map_pcl/test/test_data/parameters.yaml b/grid_map_pcl/test/test_data/parameters.yaml index 47c31c99f..95dc43df3 100644 --- a/grid_map_pcl/test/test_data/parameters.yaml +++ b/grid_map_pcl/test/test_data/parameters.yaml @@ -26,6 +26,8 @@ pcl_grid_map_extraction: grid_map: min_num_points_per_cell: 4 resolution: 0.1 + height_type: 0 + height_thresh: 1.0 diff --git a/grid_map_pcl/test/test_grid_map_pcl.cpp b/grid_map_pcl/test/test_grid_map_pcl.cpp index 23531bc7d..08278bd6e 100644 --- a/grid_map_pcl/test/test_grid_map_pcl.cpp +++ b/grid_map_pcl/test/test_grid_map_pcl.cpp @@ -8,18 +8,17 @@ // gtest #include -#include +#include int argc; -char **argv; - +char ** argv; // Run all the tests that were declared with TEST() -int main(int _argc, char **_argv) { - ros::Time::init(); - argc = _argc; - argv = _argv; - testing::InitGoogleTest(&_argc, _argv); - srand((int) time(0)); - return RUN_ALL_TESTS(); +int main(int _argc, char ** _argv) +{ + argc = _argc; + argv = _argv; + testing::InitGoogleTest(&_argc, _argv); + srand(static_cast(time(0))); + return RUN_ALL_TESTS(); } diff --git a/grid_map_pcl/test/test_helpers.cpp b/grid_map_pcl/test/test_helpers.cpp index 9d428cb7d..9ca5e9288 100644 --- a/grid_map_pcl/test/test_helpers.cpp +++ b/grid_map_pcl/test/test_helpers.cpp @@ -6,12 +6,18 @@ * Institute: ETH Zurich, Robotic Systems Lab */ +#include + +#include +#include + #include "grid_map_pcl/GridMapPclLoader.hpp" -#include #include "test_helpers.hpp" -namespace grid_map { -namespace grid_map_pcl_test { +namespace grid_map +{ +namespace grid_map_pcl_test +{ std::mt19937 rndGenerator; @@ -23,14 +29,14 @@ std::string getConfigFilePath() std::string getTestDataFolderPath() { - std::string filename = ros::package::getPath("grid_map_pcl") + "/test/test_data"; - return filename; + std::string dir = ament_index_cpp::get_package_share_directory("grid_map_pcl") + + "/test_data"; + return dir; } std::vector getNonNanElevationValuesWithCoordinates( - const grid_map::GridMap &gridMap) + const grid_map::GridMap & gridMap) { - std::vector nonNanCoordinates; for (grid_map::GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { double value = gridMap.at(layerName, *iterator); @@ -44,9 +50,8 @@ std::vector getNonNanElevationValuesWithCoordinates( return nonNanCoordinates; } -std::vector getNonNanElevationValues(const grid_map::GridMap &gridMap) +std::vector getNonNanElevationValues(const grid_map::GridMap & gridMap) { - std::vector nonNanElevations; for (grid_map::GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { double value = gridMap.at(layerName, *iterator); @@ -58,10 +63,10 @@ std::vector getNonNanElevationValues(const grid_map::GridMap &gridMap) return nonNanElevations; } -Pointcloud::Ptr createNormallyDistributedBlobOfPoints(unsigned int nPoints, double mean, - double stdDev, std::mt19937 *generator) +Pointcloud::Ptr createNormallyDistributedBlobOfPoints( + unsigned int nPoints, double mean, + double stdDev, std::mt19937 * generator) { - std::normal_distribution normalDist(mean, stdDev); // N Pointcloud::Ptr cloud(new Pointcloud()); @@ -75,20 +80,18 @@ Pointcloud::Ptr createNormallyDistributedBlobOfPoints(unsigned int nPoints, doub } return cloud; - } -Pointcloud::Ptr createNoisyPlanePointcloud(unsigned int nPoints, double minXY, double maxXY, - double meanZ, double stdDevZ, std::mt19937 *generator) +Pointcloud::Ptr createNoisyPlanePointcloud( + unsigned int nPoints, double minXY, double maxXY, + double meanZ, double stdDevZ, std::mt19937 * generator) { - const double upperBound = maxXY; const double lowerBound = minXY; std::uniform_real_distribution uniformDist(lowerBound, upperBound); const double mean = meanZ; const double stdDev = stdDevZ; std::normal_distribution normalDist(mean, stdDev); // N - //std::cout << distr(generator) << '\n'; Pointcloud::Ptr cloud(new Pointcloud()); cloud->points.reserve(nPoints); @@ -101,13 +104,12 @@ Pointcloud::Ptr createNoisyPlanePointcloud(unsigned int nPoints, double minXY, d } return cloud; - } -Pointcloud::Ptr createPerfectPlane(unsigned int nPoints, double minXY, double maxXY, - double desiredHeight, std::mt19937 *generator) +Pointcloud::Ptr createPerfectPlane( + unsigned int nPoints, double minXY, double maxXY, + double desiredHeight, std::mt19937 * generator) { - const double upperBound = maxXY; const double lowerBound = minXY; std::uniform_real_distribution uniformDist(lowerBound, upperBound); @@ -123,30 +125,28 @@ Pointcloud::Ptr createPerfectPlane(unsigned int nPoints, double minXY, double ma } return cloud; - } Pointcloud::Ptr concatenate(Pointcloud::Ptr cloud1, Pointcloud::Ptr cloud2) { - - //ghetto concatenate + // ghetto concatenate Pointcloud::Ptr concatenatedCloud(new grid_map_pcl_test::Pointcloud()); concatenatedCloud->points.reserve(cloud1->points.size() + cloud2->points.size()); - for (const auto &point : cloud2->points) { + for (const auto & point : cloud2->points) { concatenatedCloud->push_back(point); } - for (const auto &point : cloud1->points) { + for (const auto & point : cloud1->points) { concatenatedCloud->push_back(point); } return concatenatedCloud; - } -void runGridMapPclLoaderOnInputCloud(Pointcloud::ConstPtr inputCloud, - grid_map::GridMapPclLoader *gridMapPclLoader) +void runGridMapPclLoaderOnInputCloud( + Pointcloud::ConstPtr inputCloud, + grid_map::GridMapPclLoader * gridMapPclLoader) { gridMapPclLoader->loadParameters(getConfigFilePath()); gridMapPclLoader->setInputCloud(inputCloud); @@ -155,19 +155,20 @@ void runGridMapPclLoaderOnInputCloud(Pointcloud::ConstPtr inputCloud, gridMapPclLoader->addLayerFromInputCloud(layerName); } -void runGridMapPclLoaderOnInputCloudAndSavePointCloud(Pointcloud::ConstPtr inputCloud, - grid_map::GridMapPclLoader *gridMapPclLoader, - const std::string &filename) +void runGridMapPclLoaderOnInputCloudAndSavePointCloud( + Pointcloud::ConstPtr inputCloud, + grid_map::GridMapPclLoader * gridMapPclLoader, + const std::string & filename) { runGridMapPclLoaderOnInputCloud(inputCloud, gridMapPclLoader); gridMapPclLoader->savePointCloudAsPcdFile(filename); } -Pointcloud::Ptr createStepTerrain(unsigned int nPoints, double minXY, double maxXY, double zHigh, - double zLow, double stdDevZ, std::mt19937 *generator, - double *center) +Pointcloud::Ptr createStepTerrain( + unsigned int nPoints, double minXY, double maxXY, double zHigh, + double zLow, double stdDevZ, std::mt19937 * generator, + double * center) { - *center = (maxXY + minXY) / 2.0; std::uniform_real_distribution uniformDist(minXY, maxXY); std::normal_distribution zLowDist(zLow, stdDevZ); @@ -190,20 +191,14 @@ Pointcloud::Ptr createStepTerrain(unsigned int nPoints, double minXY, double max } return cloud; - -} -void setVerbosityLevel(ros::console::levels::Level level) -{ - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, level)) { - ros::console::notifyLoggerLevelsChanged(); - } } std::string getTestPcdFilePath() { - std::string filename = ros::package::getPath("grid_map_pcl") + "/test/test_data/plane_noisy.pcd"; + std::string filename = ament_index_cpp::get_package_share_directory("grid_map_pcl") + + "/test_data/plane_noisy.pcd"; return filename; } -} /* namespace grid_map_pcl_test */ -} /* namespace grid_map*/ +} // namespace grid_map_pcl_test +} // namespace grid_map diff --git a/grid_map_pcl/test/test_helpers.hpp b/grid_map_pcl/test/test_helpers.hpp index 680887ae7..73bbc0195 100644 --- a/grid_map_pcl/test/test_helpers.hpp +++ b/grid_map_pcl/test/test_helpers.hpp @@ -6,26 +6,32 @@ * Institute: ETH Zurich, Robotic Systems Lab */ -#pragma once +#ifndef TEST_HELPERS_HPP_ +#define TEST_HELPERS_HPP_ #include -#include "grid_map_core/GridMap.hpp" -#include "grid_map_core/iterators/GridMapIterator.hpp" #include -#include -#include +#include +#include +#include #include -namespace grid_map { + +#include "grid_map_core/GridMap.hpp" +#include "grid_map_core/iterators/GridMapIterator.hpp" + +namespace grid_map +{ class GridMapPclLoader; -namespace grid_map_pcl_test { +namespace grid_map_pcl_test +{ using Point = pcl::PointXYZ; using Pointcloud = pcl::PointCloud; -static const std::string layerName = "elevation"; +static const std::string layerName = "elevation"; // NOLINT static const double savePointclouds = true; extern std::mt19937 rndGenerator; @@ -33,29 +39,35 @@ std::string getConfigFilePath(); std::string getTestDataFolderPath(); Pointcloud::Ptr concatenate(Pointcloud::Ptr cloud1, Pointcloud::Ptr cloud2); -std::vector getNonNanElevationValues(const grid_map::GridMap &gridMap); +std::vector getNonNanElevationValues(const grid_map::GridMap & gridMap); std::vector getNonNanElevationValuesWithCoordinates( - const grid_map::GridMap &gridMap); - -Pointcloud::Ptr createNormallyDistributedBlobOfPoints(unsigned int nPoints, double mean, - double stdDev, std::mt19937 *generator); -Pointcloud::Ptr createNoisyPlanePointcloud(unsigned int nPoints, double minXY, double maxXY, - double meanZ, double stdDevZ, std::mt19937 *generator); -Pointcloud::Ptr createPerfectPlane(unsigned int nPoints, double minXY, double maxXY, - double desiredHeight, std::mt19937 *generator); - -void runGridMapPclLoaderOnInputCloud(Pointcloud::ConstPtr inputCloud, - grid_map::GridMapPclLoader *gridMapPclLoader); -void runGridMapPclLoaderOnInputCloudAndSavePointCloud(Pointcloud::ConstPtr inputCloud, - grid_map::GridMapPclLoader *gridMapPclLoader, - const std::string &filename); - -Pointcloud::Ptr createStepTerrain(unsigned int nPoints, double minXY, double maxXY, double zHigh, - double zLow, double stdDevZ, std::mt19937 *generator, - double *center); - -void setVerbosityLevel(ros::console::levels::Level level); + const grid_map::GridMap & gridMap); + +Pointcloud::Ptr createNormallyDistributedBlobOfPoints( + unsigned int nPoints, double mean, + double stdDev, std::mt19937 * generator); +Pointcloud::Ptr createNoisyPlanePointcloud( + unsigned int nPoints, double minXY, double maxXY, + double meanZ, double stdDevZ, std::mt19937 * generator); +Pointcloud::Ptr createPerfectPlane( + unsigned int nPoints, double minXY, double maxXY, + double desiredHeight, std::mt19937 * generator); + +void runGridMapPclLoaderOnInputCloud( + Pointcloud::ConstPtr inputCloud, + grid_map::GridMapPclLoader * gridMapPclLoader); +void runGridMapPclLoaderOnInputCloudAndSavePointCloud( + Pointcloud::ConstPtr inputCloud, + grid_map::GridMapPclLoader * gridMapPclLoader, + const std::string & filename); + +Pointcloud::Ptr createStepTerrain( + unsigned int nPoints, double minXY, double maxXY, double zHigh, + double zLow, double stdDevZ, std::mt19937 * generator, + double * center); + std::string getTestPcdFilePath(); -} /* namespace grid_map_pcl_test */ -} /* namespace grid_map*/ +} // namespace grid_map_pcl_test +} // namespace grid_map +#endif // TEST_HELPERS_HPP_ diff --git a/grid_map_ros/CHANGELOG.rst b/grid_map_ros/CHANGELOG.rst index 7d51ea2c3..02910e6e0 100644 --- a/grid_map_ros/CHANGELOG.rst +++ b/grid_map_ros/CHANGELOG.rst @@ -2,6 +2,49 @@ Changelog for package grid_map_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ +* Add test dependency to on rosbad default plugins (`#491 `_) (`#495 `_) + (cherry picked from commit 73d9427add8429b299da8f266b562c5c3f0e13cb) + Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com> +* Fix LoadFromBag assumptions causing C++ exceptions during serialization (`#438 `_) (`#468 `_) + Fix LoadFromBag assumptions + * Not all bags have only GridMap messages + * Not all bags have GridMap on the right topic + * Add test for trying to load a grid map from a bag that doesn't + contain it on the expected topic + * Add nullptr check on reader messages + * Remove unused include + * Don't skip reporting when tests fail + (cherry picked from commit 25a1ea53297ac739d0b2fd6eff6a5c0145cee5c6) + Co-authored-by: Ryan +* Contributors: mergify[bot] + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#458 `_ from ANYbotics/ci-temp-skip-octomap-server + build: treat several build issues on rolling +* Rosbag timestamp changes. +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Contributors: Roderick Taylor, Ryan, Ryan Friedman + +2.1.0 (2022-11-08) +------------------ +* fix: change cv_bridge.h -> .hpp (`#376 `_) +* Contributors: Iván López Broceño + +2.0.0 (2022-09-13) +------------------ +* fix: mark Eigen library as SYSTEM +* fix: build error on jammy +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map_ros/CMakeLists.txt b/grid_map_ros/CMakeLists.txt index f360baf0d..2a75ce805 100644 --- a/grid_map_ros/CMakeLists.txt +++ b/grid_map_ros/CMakeLists.txt @@ -1,55 +1,41 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(grid_map_ros) -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") - ## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roscpp +find_package(ament_cmake REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(grid_map_cv REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcutils REQUIRED) +find_package(rosbag2_cpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(visualization_msgs REQUIRED) + +grid_map_package() + +set(dependencies + cv_bridge + geometry_msgs grid_map_core - grid_map_msgs grid_map_cv - sensor_msgs + grid_map_msgs + nav2_msgs nav_msgs + rcutils + rosbag2_cpp + sensor_msgs std_msgs - geometry_msgs - cv_bridge - rosbag - tf + tf2 visualization_msgs -) - -## System dependencies are found with CMake's conventions -#find_package(Eigen3 REQUIRED) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - roscpp - grid_map_core - grid_map_msgs - grid_map_cv - sensor_msgs - nav_msgs - std_msgs - geometry_msgs - cv_bridge - rosbag - tf - visualization_msgs - DEPENDS + rclcpp ) ########### @@ -59,23 +45,19 @@ catkin_package( ## Specify additional locations of header files include_directories( include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} + SYSTEM + ${EIGEN3_INCLUDE_DIR} ) ## Declare a cpp library -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/GridMapRosConverter.cpp src/GridMapMsgHelpers.cpp src/PolygonRosConverter.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) - -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +ament_target_dependencies(${PROJECT_NAME} SYSTEM + ${dependencies} ) ############# @@ -85,15 +67,15 @@ add_dependencies(${PROJECT_NAME} # Mark executables and/or libraries for installation install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) # Mark cpp header files for installation install( DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} FILES_MATCHING PATTERN "*.hpp" ) @@ -101,15 +83,48 @@ install( ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") -## Add gtest based cpp test target and link libraries -catkin_add_gtest(${PROJECT_NAME}-test - test/test_grid_map_ros.cpp - test/GridMapRosTest.cpp -) +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright -build/include_order + ) + endif() + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + ## Add gtest based cpp test target and link libraries + ament_add_gtest(${PROJECT_NAME}-test + test/test_grid_map_ros.cpp + test/GridMapRosTest.cpp + ) + + file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/resource/double_chatter DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) + file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/resource/test_multitopic DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) endif() if(TARGET ${PROJECT_NAME}-test) target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) endif() + +ament_export_include_directories(include) +ament_export_dependencies(${dependencies}) +ament_export_libraries(${PROJECT_NAME}) +ament_package() diff --git a/grid_map_ros/include/grid_map_ros/GridMapMsgHelpers.hpp b/grid_map_ros/include/grid_map_ros/GridMapMsgHelpers.hpp index fef985d2c..3fd8e8ff9 100644 --- a/grid_map_ros/include/grid_map_ros/GridMapMsgHelpers.hpp +++ b/grid_map_ros/include/grid_map_ros/GridMapMsgHelpers.hpp @@ -6,18 +6,23 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_ROS__GRIDMAPMSGHELPERS_HPP_ +#define GRID_MAP_ROS__GRIDMAPMSGHELPERS_HPP_ // ROS -#include -#include -#include -#include +#include +#include +#include +#include // Eigen #include -namespace grid_map { +#include +#include + +namespace grid_map +{ /*! * Returns the number of dimensions of the grid map. @@ -25,12 +30,13 @@ namespace grid_map { */ int nDimensions(); -enum class StorageIndices { - Column, - Row +enum class StorageIndices +{ + Column, + Row }; -//! Holds the names of the storage indeces. +//! Holds the names of the storage indices. extern std::map storageIndexNames; /*! @@ -40,11 +46,17 @@ extern std::map storageIndexNames; * @return true if is in row-major format, false if is in column-major format. */ template -bool isRowMajor(const MultiArrayMessageType_& message) +bool isRowMajor(const MultiArrayMessageType_ & message) { - if (message.layout.dim[0].label == grid_map::storageIndexNames[grid_map::StorageIndices::Column]) return false; - else if (message.layout.dim[0].label == grid_map::storageIndexNames[grid_map::StorageIndices::Row]) return true; - ROS_ERROR("isRowMajor() failed because layout label is not set correctly."); + if (message.layout.dim[0].label == + grid_map::storageIndexNames[grid_map::StorageIndices::Column]) + { + return false; + } else if (message.layout.dim[0].label == + grid_map::storageIndexNames[grid_map::StorageIndices::Row]) {return true;} + RCLCPP_ERROR( + rclcpp::get_logger( + "isRowMajor"), "isRowMajor() failed because layout label is not set correctly."); return false; } @@ -55,9 +67,9 @@ bool isRowMajor(const MultiArrayMessageType_& message) * @return the number of columns. */ template -unsigned int getCols(const MultiArrayMessageType_& message) +unsigned int getCols(const MultiArrayMessageType_ & message) { - if (isRowMajor(message)) return message.layout.dim.at(1).size; + if (isRowMajor(message)) {return message.layout.dim.at(1).size;} return message.layout.dim.at(0).size; } @@ -68,9 +80,9 @@ unsigned int getCols(const MultiArrayMessageType_& message) * @return the number of rows. */ template -unsigned int getRows(const MultiArrayMessageType_& message) +unsigned int getRows(const MultiArrayMessageType_ & message) { - if (isRowMajor(message)) return message.layout.dim.at(0).size; + if (isRowMajor(message)) {return message.layout.dim.at(0).size;} return message.layout.dim.at(1).size; } @@ -85,7 +97,7 @@ unsigned int getRows(const MultiArrayMessageType_& message) * @return true if successful. */ template -bool matrixEigenCopyToMultiArrayMessage(const EigenType_& e, MultiArrayMessageType_& m) +bool matrixEigenCopyToMultiArrayMessage(const EigenType_ & e, MultiArrayMessageType_ & m) { m.layout.dim.resize(nDimensions()); m.layout.dim[0].stride = e.size(); @@ -114,10 +126,12 @@ bool matrixEigenCopyToMultiArrayMessage(const EigenType_& e, MultiArrayMessageTy * @return true if successful. */ template -bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_& m, EigenType_& e) +bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_ & m, EigenType_ & e) { if (e.IsRowMajor != isRowMajor(m)) { - ROS_ERROR("multiArrayMessageToMatrixEigen() failed because the storage order is not compatible."); + RCLCPP_ERROR( + rclcpp::get_logger("multiArrayMessageCopyToMatrixEigen"), "multiArrayMessageToMatrixEigen() " + "failed because the storage order is not compatible."); return false; } @@ -135,10 +149,12 @@ bool multiArrayMessageCopyToMatrixEigen(const MultiArrayMessageType_& m, EigenTy * @return true if successful. */ template -bool multiArrayMessageMapToMatrixEigen(MultiArrayMessageType_& m, EigenType_& e) +bool multiArrayMessageMapToMatrixEigen(MultiArrayMessageType_ & m, EigenType_ & e) { if (e.IsRowMajor != isRowMajor(m)) { - ROS_ERROR("multiArrayMessageToMatrixEigen() failed because the storage order is not compatible."); + RCLCPP_ERROR( + rclcpp::get_logger("multiArrayMessageMapToMatrixEigen"), "multiArrayMessageToMatrixEigen() " + "failed because the storage order is not compatible."); return false; } @@ -147,4 +163,5 @@ bool multiArrayMessageMapToMatrixEigen(MultiArrayMessageType_& m, EigenType_& e) return true; } -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_ROS__GRIDMAPMSGHELPERS_HPP_ diff --git a/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp b/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp index d5ddad1ff..a97de0160 100644 --- a/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp +++ b/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp @@ -6,35 +6,40 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_ROS__GRIDMAPROSCONVERTER_HPP_ +#define GRID_MAP_ROS__GRIDMAPROSCONVERTER_HPP_ #include -#include - -// STL -#include -#include +#include // Eigen #include // ROS -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include + +// STL +#include +#include +#include +#include -namespace grid_map { +namespace grid_map +{ /*! * ROS interface for the Grid Map library. */ class GridMapRosConverter { - public: +public: /*! * Default constructor. */ @@ -54,7 +59,10 @@ class GridMapRosConverter * @param[out] gridMap the grid map object to be initialized. * @return true if successful, false otherwise. */ - static bool fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap, const std::vector& layers, bool copyBasicLayers = true, bool copyAllNonBasicLayers = true); + static bool fromMessage( + const grid_map_msgs::msg::GridMap & message, grid_map::GridMap & gridMap, + const std::vector & layers, bool copyBasicLayers = true, + bool copyAllNonBasicLayers = true); /*! * Converts a ROS grid map message to a grid map object. @@ -62,14 +70,15 @@ class GridMapRosConverter * @param[out] gridMap the grid map object to be initialized. * @return true if successful, false otherwise. */ - static bool fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap); + static bool fromMessage(const grid_map_msgs::msg::GridMap & message, grid_map::GridMap & gridMap); /*! * Converts all layers of a grid map object to a ROS grid map message. * @param[in] gridMap the grid map object. * @param[out] message the grid map message to be populated. */ - static void toMessage(const grid_map::GridMap& gridMap, grid_map_msgs::GridMap& message); + static std::unique_ptr toMessage( + const grid_map::GridMap & gridMap); /*! * Converts requested layers of a grid map object to a ROS grid map message. @@ -77,8 +86,8 @@ class GridMapRosConverter * @param[in] layers the layers to be added to the message. * @param[out] message the grid map message to be populated. */ - static void toMessage(const grid_map::GridMap& gridMap, const std::vector& layers, - grid_map_msgs::GridMap& message); + static std::unique_ptr toMessage( + const grid_map::GridMap & gridMap, const std::vector & layers); /*! * Converts a grid map object to a ROS PointCloud2 message. Set the layer to be transformed @@ -88,9 +97,10 @@ class GridMapRosConverter * @param[in] pointLayer the type that is transformed to points. * @param[out] pointCloud the message to be populated. */ - static void toPointCloud(const grid_map::GridMap& gridMap, - const std::string& pointLayer, - sensor_msgs::PointCloud2& pointCloud); + static void toPointCloud( + const grid_map::GridMap & gridMap, + const std::string & pointLayer, + sensor_msgs::msg::PointCloud2 & pointCloud); /*! * Converts a grid map object to a ROS PointCloud2 message. Set the layer to be transformed @@ -101,10 +111,11 @@ class GridMapRosConverter * @param[in] pointLayer the layer that is transformed to points. * @param[out] pointCloud the message to be populated. */ - static void toPointCloud(const grid_map::GridMap& gridMap, - const std::vector& layers, - const std::string& pointLayer, - sensor_msgs::PointCloud2& pointCloud); + static void toPointCloud( + const grid_map::GridMap & gridMap, + const std::vector & layers, + const std::string & pointLayer, + sensor_msgs::msg::PointCloud2 & pointCloud); /*! * Converts an occupancy grid message to a layer of a grid map. @@ -113,8 +124,9 @@ class GridMapRosConverter * @param[out] gridMap the grid map to be populated. * @return true if successful. */ - static bool fromOccupancyGrid(const nav_msgs::OccupancyGrid& occupancyGrid, - const std::string& layer, grid_map::GridMap& gridMap); + static bool fromOccupancyGrid( + const nav_msgs::msg::OccupancyGrid & occupancyGrid, + const std::string & layer, grid_map::GridMap & gridMap); /*! * Converts a grid map object to a ROS OccupancyGrid message. Set the layer to be transformed @@ -125,8 +137,33 @@ class GridMapRosConverter * @param[in] dataMax the maximum value of the grid map data (used to normalize the cell data in [min, max]). * @param[out] occupancyGrid the message to be populated. */ - static void toOccupancyGrid(const grid_map::GridMap& gridMap, const std::string& layer, - float dataMin, float dataMax, nav_msgs::OccupancyGrid& occupancyGrid); + static void toOccupancyGrid( + const grid_map::GridMap & gridMap, const std::string & layer, + float dataMin, float dataMax, nav_msgs::msg::OccupancyGrid & occupancyGrid); + + /*! + * Converts an Costmap message to a layer of a grid map. + * @param[in] Costmap the occupancy grid to be converted. + * @param[in] layer the layer to which the costmap will be converted. + * @param[out] gridMap the grid map to be populated. + * @return true if successful. + */ + static bool fromCostmap( + const nav2_msgs::msg::Costmap & costmap, + const std::string & layer, grid_map::GridMap & gridMap); + + /*! + * Converts a grid map object to a ROS Costmap message. Set the layer to be transformed + * as the cell data of the occupancy grid with `layer`, all other layers will be neglected. + * @param[in] gridMap the grid map object. + * @param[in] layer the layer that is transformed to the occupancy cell data. + * @param[in] dataMin the minimum value of the grid map data (used to normalize the cell data in [min, max]). + * @param[in] dataMax the maximum value of the grid map data (used to normalize the cell data in [min, max]). + * @param[out] Costmap the message to be populated. + */ + static void toCostmap( + const grid_map::GridMap & gridMap, const std::string & layer, + float dataMin, float dataMax, nav2_msgs::msg::Costmap & costmap); /*! * Converts a grid map object to a ROS GridCells message. Set the layer to be transformed @@ -138,9 +175,10 @@ class GridMapRosConverter * @param[in] upperThreshold the upper threshold. * @param[out] gridCells the message to be populated. */ - static void toGridCells(const grid_map::GridMap& gridMap, const std::string& layer, - float lowerThreshold, float upperThreshold, - nav_msgs::GridCells& gridCells); + static void toGridCells( + const grid_map::GridMap & gridMap, const std::string & layer, + float lowerThreshold, float upperThreshold, + nav_msgs::msg::GridCells & gridCells); /*! * Initializes the geometry of a grid map from an image messages. This changes @@ -151,9 +189,10 @@ class GridMapRosConverter * @param[in](optional) position the position of the grid map. * @return true if successful, false otherwise. */ - static bool initializeFromImage(const sensor_msgs::Image& image, const double resolution, - grid_map::GridMap& gridMap, - const grid_map::Position& position = grid_map::Position::Zero()); + static bool initializeFromImage( + const sensor_msgs::msg::Image & image, const double resolution, + grid_map::GridMap & gridMap, + const grid_map::Position & position = grid_map::Position::Zero()); /*! * Adds a layer with data from image. @@ -167,9 +206,10 @@ class GridMapRosConverter * cells in the grid map are marked as empty. * @return true if successful, false otherwise. */ - static bool addLayerFromImage(const sensor_msgs::Image& image, const std::string& layer, - grid_map::GridMap& gridMap, const float lowerValue = 0.0, - const float upperValue = 1.0, const double alphaThreshold = 0.5); + static bool addLayerFromImage( + const sensor_msgs::msg::Image & image, const std::string & layer, + grid_map::GridMap & gridMap, const float lowerValue = 0.0, + const float upperValue = 1.0, const double alphaThreshold = 0.5); /*! * Adds a color layer with data from an image. @@ -178,8 +218,9 @@ class GridMapRosConverter * @param[out] gridMap the grid map to be populated. * @return true if successful, false otherwise. */ - static bool addColorLayerFromImage(const sensor_msgs::Image& image, const std::string& layer, - grid_map::GridMap& gridMap); + static bool addColorLayerFromImage( + const sensor_msgs::msg::Image & image, const std::string & layer, + grid_map::GridMap & gridMap); /*! * Creates an image message from a grid map layer. @@ -191,8 +232,9 @@ class GridMapRosConverter * @param[out] image the message to be populated. * @return true if successful, false otherwise. */ - static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer, - const std::string encoding, sensor_msgs::Image& image); + static bool toImage( + const grid_map::GridMap & gridMap, const std::string & layer, + const std::string encoding, sensor_msgs::msg::Image & image); /*! * Creates an image message from a grid map layer. @@ -204,9 +246,10 @@ class GridMapRosConverter * @param[out] image the message to be populated. * @return true if successful, false otherwise. */ - static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer, - const std::string encoding, const float lowerValue, const float upperValue, - sensor_msgs::Image& image); + static bool toImage( + const grid_map::GridMap & gridMap, const std::string & layer, + const std::string encoding, const float lowerValue, const float upperValue, + sensor_msgs::msg::Image & image); /*! * Creates a cv image from a grid map layer. @@ -218,8 +261,9 @@ class GridMapRosConverter * @param[out] cvImage the to be populated. * @return true if successful, false otherwise. */ - static bool toCvImage(const grid_map::GridMap& gridMap, const std::string& layer, - const std::string encoding, cv_bridge::CvImage& cvImage); + static bool toCvImage( + const grid_map::GridMap & gridMap, const std::string & layer, + const std::string encoding, cv_bridge::CvImage & cvImage); /*! * Creates a cv image from a grid map layer. @@ -231,9 +275,10 @@ class GridMapRosConverter * @param[out] cvImage to be populated. * @return true if successful, false otherwise. */ - static bool toCvImage(const grid_map::GridMap& gridMap, const std::string& layer, - const std::string encoding, const float lowerValue, - const float upperValue, cv_bridge::CvImage& cvImage); + static bool toCvImage( + const grid_map::GridMap & gridMap, const std::string & layer, + const std::string encoding, const float lowerValue, + const float upperValue, cv_bridge::CvImage & cvImage); /*! * Saves a grid map into a ROS bag. The timestamp of the grid map @@ -245,8 +290,9 @@ class GridMapRosConverter * @param[in] topic the name of the topic in the ROS bag. * @return true if successful, false otherwise. */ - static bool saveToBag(const grid_map::GridMap& gridMap, const std::string& pathToBag, - const std::string& topic); + static bool saveToBag( + const grid_map::GridMap & gridMap, const std::string & pathToBag, + const std::string & topic); /*! * Loads a GridMap from a ROS bag. @@ -255,9 +301,10 @@ class GridMapRosConverter * @param[out] gridMap the grid map object to be initialized. * @return true if successful, false otherwise. */ - static bool loadFromBag(const std::string& pathToBag, const std::string& topic, - grid_map::GridMap& gridMap); - + static bool loadFromBag( + const std::string & pathToBag, const std::string & topic, + grid_map::GridMap & gridMap); }; -} /* namespace */ +} // namespace grid_map +#endif // GRID_MAP_ROS__GRIDMAPROSCONVERTER_HPP_ diff --git a/grid_map_ros/include/grid_map_ros/PolygonRosConverter.hpp b/grid_map_ros/include/grid_map_ros/PolygonRosConverter.hpp index 256964626..6364c678d 100644 --- a/grid_map_ros/include/grid_map_ros/PolygonRosConverter.hpp +++ b/grid_map_ros/include/grid_map_ros/PolygonRosConverter.hpp @@ -6,25 +6,26 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_ROS__POLYGONROSCONVERTER_HPP_ +#define GRID_MAP_ROS__POLYGONROSCONVERTER_HPP_ #include +// ROS +#include +#include +#include +#include + // STL #include -// ROS -#include -#include -#include -#include - -namespace grid_map { +namespace grid_map +{ class PolygonRosConverter { - public: - +public: /*! * Default constructor. */ @@ -41,7 +42,9 @@ class PolygonRosConverter * @param[in] polygon the polygon object. * @param[out] message the ROS PolygonStamped message to be populated. */ - static void toMessage(const grid_map::Polygon& polygon, geometry_msgs::PolygonStamped& message); + static void toMessage( + const grid_map::Polygon & polygon, + geometry_msgs::msg::PolygonStamped & message); /*! * Converts a polygon object to a ROS line strip marker message. @@ -51,8 +54,10 @@ class PolygonRosConverter * @param[in] zCoordinate z-coordinate of the planar polygon. * @param[out] marker the ROS marker message to be populated. */ - static void toLineMarker(const grid_map::Polygon& polygon, const std_msgs::ColorRGBA& color, const double lineWidth, - const double zCoordinate, visualization_msgs::Marker& marker); + static void toLineMarker( + const grid_map::Polygon & polygon, const std_msgs::msg::ColorRGBA & color, + const double lineWidth, + const double zCoordinate, visualization_msgs::msg::Marker & marker); /*! * Converts a polygon object to a ROS triangle list marker message. @@ -61,8 +66,10 @@ class PolygonRosConverter * @param[in] zCoordinate z-coordinate of the planar polygon. * @param[out] marker the ROS marker message to be populated. */ - static void toTriangleListMarker(const grid_map::Polygon& polygon, const std_msgs::ColorRGBA& color, - const double zCoordinate, visualization_msgs::Marker& marker); + static void toTriangleListMarker( + const grid_map::Polygon & polygon, const std_msgs::msg::ColorRGBA & color, + const double zCoordinate, visualization_msgs::msg::Marker & marker); }; -} /* namespace grid_map */ +} // namespace grid_map +#endif // GRID_MAP_ROS__POLYGONROSCONVERTER_HPP_ diff --git a/grid_map_ros/include/grid_map_ros/grid_map_ros.hpp b/grid_map_ros/include/grid_map_ros/grid_map_ros.hpp index 053e60faf..c50d0ea9e 100644 --- a/grid_map_ros/include/grid_map_ros/grid_map_ros.hpp +++ b/grid_map_ros/include/grid_map_ros/grid_map_ros.hpp @@ -6,10 +6,13 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_ROS__GRID_MAP_ROS_HPP_ +#define GRID_MAP_ROS__GRID_MAP_ROS_HPP_ #include #include #include #include #include + +#endif // GRID_MAP_ROS__GRID_MAP_ROS_HPP_ diff --git a/grid_map_ros/include/grid_map_ros/message_traits.hpp b/grid_map_ros/include/grid_map_ros/message_traits.hpp index cbc531a8e..f785a1db6 100644 --- a/grid_map_ros/include/grid_map_ros/message_traits.hpp +++ b/grid_map_ros/include/grid_map_ros/message_traits.hpp @@ -1,65 +1,117 @@ -#pragma once +#ifndef GRID_MAP_ROS__MESSAGE_TRAITS_HPP_ +#define GRID_MAP_ROS__MESSAGE_TRAITS_HPP_ -#include -#include +#include +#include -namespace ros { -namespace message_traits { +#include +#include +#include -template<> -struct HasHeader : public TrueType {}; +namespace ros +{ +namespace message_traits +{ + +template +struct HasHeader : public std::false_type {}; -template <> -struct Header>::type> +template +struct HasHeader: std::true_type {}; + +template +struct Header { - static std_msgs::Header* pointer(grid_map_msgs::GridMap& m) + static std_msgs::msg::Header * pointer(M & m) { - return &m.info.header; + (void)m; + return nullptr; } - - static std_msgs::Header const* pointer(const grid_map_msgs::GridMap& m) + static std_msgs::msg::Header const * pointer(const M & m) { - return &m.info.header; + (void)m; + return nullptr; } }; -template<> -struct FrameId>::type> +template +struct Header::value>::type> { - static std::string* pointer(grid_map_msgs::GridMap& m) + static std_msgs::msg::Header * pointer(M & m) { - return &m.info.header.frame_id; + return &m.header; } - static std::string const* pointer(const grid_map_msgs::GridMap& m) + static std_msgs::msg::Header const * pointer(const M & m) { - return &m.info.header.frame_id; + return &m.header; } +}; - static std::string value(const grid_map_msgs::GridMap& m) +template +struct FrameId +{ + static std::string * pointer(M & m) + { + (void)m; + return nullptr; + } + static std::string const * pointer(const M & m) { - return m.info.header.frame_id; + (void)m; + return nullptr; } }; -template<> -struct TimeStamp>::type> +template +struct FrameId::value>::type> { - static ros::Time* pointer(grid_map_msgs::GridMap& m) + static std::string * pointer(M & m) { - return &m.info.header.stamp; + return &m.header.frame_id; + } + static std::string const * pointer(const M & m) + { + return &m.header.frame_id; + } + static std::string value(const M & m) + { + return m.header.frame_id; + } +}; + +template +struct TimeStamp +{ + static std::unique_ptr pointer(const M & m) + { + (void)m; + return nullptr; } - static ros::Time const* pointer(const grid_map_msgs::GridMap& m) + static rclcpp::Time value(const M & m) + { + (void)m; + return rclcpp::Time(); + } +}; + +template +struct TimeStamp::value>::type> +{ + static std::unique_ptr pointer(const M & m) { - return &m.info.header.stamp; + auto stamp = m.header.stamp; + return std::make_unique(rclcpp::Time(stamp.sec, stamp.nanosec)); } - static ros::Time value(const grid_map_msgs::GridMap& m) + static rclcpp::Time value(const M & m) { - return m.info.header.stamp; + auto stamp = m.header.stamp; + return rclcpp::Time(stamp.sec, stamp.nanosec); } }; -} -} +} // namespace message_traits +} // namespace ros +#endif // GRID_MAP_ROS__MESSAGE_TRAITS_HPP_ diff --git a/grid_map_ros/package.xml b/grid_map_ros/package.xml index 0d747c168..0bcb42af6 100644 --- a/grid_map_ros/package.xml +++ b/grid_map_ros/package.xml @@ -1,25 +1,40 @@ - + grid_map_ros - 1.6.2 + 2.2.2 ROS interface for the grid map library to manage two-dimensional grid maps with multiple data layers. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Péter Fankhauser - catkin - roscpp + + ament_cmake + grid_map_cmake_helpers + + cv_bridge + geometry_msgs grid_map_core - grid_map_msgs grid_map_cv - sensor_msgs + grid_map_msgs + nav2_msgs nav_msgs + rclcpp + rcutils + rosbag2_cpp + sensor_msgs std_msgs - geometry_msgs - cv_bridge - rosbag - tf + tf2 visualization_msgs + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + rosbag2_storage_default_plugins + + + ament_cmake + diff --git a/grid_map_ros/resource/double_chatter/double_chatter.db3 b/grid_map_ros/resource/double_chatter/double_chatter.db3 new file mode 100644 index 000000000..10eb9a495 Binary files /dev/null and b/grid_map_ros/resource/double_chatter/double_chatter.db3 differ diff --git a/grid_map_ros/resource/double_chatter/metadata.yaml b/grid_map_ros/resource/double_chatter/metadata.yaml new file mode 100644 index 000000000..0216d7d10 --- /dev/null +++ b/grid_map_ros/resource/double_chatter/metadata.yaml @@ -0,0 +1,57 @@ +rosbag2_bagfile_information: + version: 8 + storage_identifier: sqlite3 + duration: + nanoseconds: 2844280786 + starting_time: + nanoseconds_since_epoch: 1708069847130953754 + message_count: 25 + topics_with_message_count: + - topic_metadata: + name: /chatter1 + type: std_msgs/msg/String + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18 + message_count: 3 + - topic_metadata: + name: /chatter2 + type: std_msgs/msg/String + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18 + message_count: 3 + - topic_metadata: + name: /events/write_split + type: rosbag2_interfaces/msg/WriteSplitEvent + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + type_description_hash: RIHS01_5ef58f7106a5cff8f5a794028c18117ee21015850ddcc567df449f41932960f8 + message_count: 0 + - topic_metadata: + name: /parameter_events + type: rcl_interfaces/msg/ParameterEvent + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + type_description_hash: RIHS01_043e627780fcad87a22d225bc2a037361dba713fca6a6b9f4b869a5aa0393204 + message_count: 0 + - topic_metadata: + name: /rosout + type: rcl_interfaces/msg/Log + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + type_description_hash: RIHS01_e28ce254ca8abc06abf92773b74602cdbf116ed34fbaf294fb9f81da9f318eac + message_count: 19 + compression_format: "" + compression_mode: "" + relative_file_paths: + - double_chatter.db3 + files: + - path: double_chatter.db3 + starting_time: + nanoseconds_since_epoch: 1708069847130953754 + duration: + nanoseconds: 2844280786 + message_count: 25 + custom_data: ~ + ros_distro: rolling diff --git a/grid_map_ros/resource/test_multitopic/metadata.yaml b/grid_map_ros/resource/test_multitopic/metadata.yaml new file mode 100644 index 000000000..5e74e8863 --- /dev/null +++ b/grid_map_ros/resource/test_multitopic/metadata.yaml @@ -0,0 +1,35 @@ +rosbag2_bagfile_information: + version: 7 + storage_identifier: sqlite3 + duration: + nanoseconds: 2640963805 + starting_time: + nanoseconds_since_epoch: 1708420680345493502 + message_count: 2 + topics_with_message_count: + - topic_metadata: + name: /chatter + type: std_msgs/msg/String + serialization_format: cdr + offered_qos_profiles: "- history: 1\n depth: 7\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + type_description_hash: RIHS01_df668c740482bbd48fb39d76a70dfd4bd59db1288021743503259e948f6b1a18 + message_count: 1 + - topic_metadata: + name: /grid_map + type: grid_map_msgs/msg/GridMap + serialization_format: cdr + offered_qos_profiles: "- history: 1\n depth: 10\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + type_description_hash: RIHS01_343b0e72887541beda6b035cc053f2d6fffaad9d6dcb2773c15a808dfca31fde + message_count: 1 + compression_format: "" + compression_mode: "" + relative_file_paths: + - test_multitopic_0.db3 + files: + - path: test_multitopic_0.db3 + starting_time: + nanoseconds_since_epoch: 1708420680345493502 + duration: + nanoseconds: 2640963805 + message_count: 2 + custom_data: ~ \ No newline at end of file diff --git a/grid_map_ros/resource/test_multitopic/test_multitopic_0.db3 b/grid_map_ros/resource/test_multitopic/test_multitopic_0.db3 new file mode 100644 index 000000000..2820fc4cd Binary files /dev/null and b/grid_map_ros/resource/test_multitopic/test_multitopic_0.db3 differ diff --git a/grid_map_ros/src/GridMapMsgHelpers.cpp b/grid_map_ros/src/GridMapMsgHelpers.cpp index 09c71e51d..34ba62dd2 100644 --- a/grid_map_ros/src/GridMapMsgHelpers.cpp +++ b/grid_map_ros/src/GridMapMsgHelpers.cpp @@ -6,20 +6,23 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_ros/GridMapMsgHelpers.hpp" - // Boost #include -namespace grid_map { +#include +#include + +#include "grid_map_ros/GridMapMsgHelpers.hpp" + +namespace grid_map +{ int nDimensions() { return 2; } -std::map storageIndexNames = boost::assign::map_list_of - (StorageIndices::Column, "column_index") - (StorageIndices::Row, "row_index"); +std::map storageIndexNames = boost::assign::map_list_of( + StorageIndices::Column, "column_index")(StorageIndices::Row, "row_index"); -} /* namespace */ +} // namespace grid_map diff --git a/grid_map_ros/src/GridMapRosConverter.cpp b/grid_map_ros/src/GridMapRosConverter.cpp index addbf1d9d..8ec6efada 100644 --- a/grid_map_ros/src/GridMapRosConverter.cpp +++ b/grid_map_ros/src/GridMapRosConverter.cpp @@ -6,26 +6,38 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_ros/GridMapRosConverter.hpp" -#include "grid_map_ros/GridMapMsgHelpers.hpp" -#include - // ROS -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include // STL #include #include #include +#include +#include +#include +#include -using namespace std; -using namespace Eigen; +#include "grid_map_ros/GridMapMsgHelpers.hpp" +#include "grid_map_ros/GridMapRosConverter.hpp" -namespace grid_map { +using namespace std; // NOLINT +using namespace Eigen; // NOLINT + +namespace grid_map +{ GridMapRosConverter::GridMapRosConverter() { @@ -35,33 +47,42 @@ GridMapRosConverter::~GridMapRosConverter() { } -bool GridMapRosConverter::fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap, const std::vector& layers, bool copyBasicLayers, bool copyAllNonBasicLayers) +bool GridMapRosConverter::fromMessage( + const grid_map_msgs::msg::GridMap & message, + grid_map::GridMap & gridMap, + const std::vector & layers, bool copyBasicLayers, + bool copyAllNonBasicLayers) { - gridMap.setTimestamp(message.info.header.stamp.toNSec()); - gridMap.setFrameId(message.info.header.frame_id); - gridMap.setGeometry(Length(message.info.length_x, message.info.length_y), message.info.resolution, - Position(message.info.pose.position.x, message.info.pose.position.y)); + gridMap.setTimestamp(rclcpp::Time(message.header.stamp).nanoseconds()); + gridMap.setFrameId(message.header.frame_id); + gridMap.setGeometry( + Length(message.info.length_x, message.info.length_y), message.info.resolution, + Position(message.info.pose.position.x, message.info.pose.position.y)); if (message.layers.size() != message.data.size()) { - ROS_ERROR("Different number of layers and data in grid map message."); + RCLCPP_ERROR( + rclcpp::get_logger( + "fromMessage"), "Different number of layers and data in grid map message."); return false; } // Copy non-basic layers. for (unsigned int i = 0u; i < message.layers.size(); ++i) { - // check if layer should be copied. - if (!copyAllNonBasicLayers && std::find(layers.begin(), layers.end(), message.layers[i]) == layers.end()) { + if (!copyAllNonBasicLayers && + std::find(layers.begin(), layers.end(), message.layers[i]) == layers.end()) + { continue; } - // TODO Could we use the data mapping (instead of copying) method here? + // TODO(needs_assignment) Could we use the data mapping (instead of copying) method here? Matrix data; - if(!multiArrayMessageCopyToMatrixEigen(message.data[i], data)) { + if (!multiArrayMessageCopyToMatrixEigen(message.data[i], data)) { return false; } - // TODO Check if size is good. size_ << getRows(message.data[0]), getCols(message.data[0]); + // TODO(needs_assignment) Check if size is good. + // size_ << getRows(message.data[0]), getCols(message.data[0]); gridMap.add(message.layers[i], data); } @@ -74,67 +95,76 @@ bool GridMapRosConverter::fromMessage(const grid_map_msgs::GridMap& message, gri return true; } -bool GridMapRosConverter::fromMessage(const grid_map_msgs::GridMap& message, grid_map::GridMap& gridMap) +bool GridMapRosConverter::fromMessage( + const grid_map_msgs::msg::GridMap & message, + grid_map::GridMap & gridMap) { return fromMessage(message, gridMap, std::vector(), true, true); } -void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, grid_map_msgs::GridMap& message) +std::unique_ptr GridMapRosConverter::toMessage( + const grid_map::GridMap & gridMap) { - toMessage(gridMap, gridMap.getLayers(), message); + return toMessage(gridMap, gridMap.getLayers()); } -void GridMapRosConverter::toMessage(const grid_map::GridMap& gridMap, const std::vector& layers, - grid_map_msgs::GridMap& message) +std::unique_ptr GridMapRosConverter::toMessage( + const grid_map::GridMap & gridMap, const std::vector & layers) { - message.info.header.stamp.fromNSec(gridMap.getTimestamp()); - message.info.header.frame_id = gridMap.getFrameId(); - message.info.resolution = gridMap.getResolution(); - message.info.length_x = gridMap.getLength().x(); - message.info.length_y = gridMap.getLength().y(); - message.info.pose.position.x = gridMap.getPosition().x(); - message.info.pose.position.y = gridMap.getPosition().y(); - message.info.pose.position.z = 0.0; - message.info.pose.orientation.x = 0.0; - message.info.pose.orientation.y = 0.0; - message.info.pose.orientation.z = 0.0; - message.info.pose.orientation.w = 1.0; - - message.layers = layers; - message.basic_layers = gridMap.getBasicLayers(); - - message.data.clear(); - for (const auto& layer : layers) { - std_msgs::Float32MultiArray dataArray; + auto message = std::make_unique(); + + message->header.stamp = rclcpp::Time(gridMap.getTimestamp()); + message->header.frame_id = gridMap.getFrameId(); + message->info.resolution = gridMap.getResolution(); + message->info.length_x = gridMap.getLength().x(); + message->info.length_y = gridMap.getLength().y(); + message->info.pose.position.x = gridMap.getPosition().x(); + message->info.pose.position.y = gridMap.getPosition().y(); + message->info.pose.position.z = 0.0; + message->info.pose.orientation.x = 0.0; + message->info.pose.orientation.y = 0.0; + message->info.pose.orientation.z = 0.0; + message->info.pose.orientation.w = 1.0; + + message->layers = layers; + message->basic_layers = gridMap.getBasicLayers(); + + message->data.clear(); + for (const auto & layer : layers) { + std_msgs::msg::Float32MultiArray dataArray; matrixEigenCopyToMultiArrayMessage(gridMap.get(layer), dataArray); - message.data.push_back(dataArray); + message->data.push_back(dataArray); } - message.outer_start_index = gridMap.getStartIndex()(0); - message.inner_start_index = gridMap.getStartIndex()(1); + message->outer_start_index = gridMap.getStartIndex()(0); + message->inner_start_index = gridMap.getStartIndex()(1); + + return message; } -void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap, - const std::string& pointLayer, - sensor_msgs::PointCloud2& pointCloud) +void GridMapRosConverter::toPointCloud( + const grid_map::GridMap & gridMap, + const std::string & pointLayer, + sensor_msgs::msg::PointCloud2 & pointCloud) { toPointCloud(gridMap, gridMap.getLayers(), pointLayer, pointCloud); } -void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap, - const std::vector& layers, - const std::string& pointLayer, - sensor_msgs::PointCloud2& pointCloud) +void GridMapRosConverter::toPointCloud( + const grid_map::GridMap & gridMap, + const std::vector & layers, + const std::string & pointLayer, + sensor_msgs::msg::PointCloud2 & pointCloud) { // Header. pointCloud.header.frame_id = gridMap.getFrameId(); - pointCloud.header.stamp.fromNSec(gridMap.getTimestamp()); + pointCloud.header.stamp = rclcpp::Time(gridMap.getTimestamp()); pointCloud.is_dense = false; // Fields. - std::vector fieldNames; + std::vector fieldNames; - for (const auto& layer : layers) { + for (const auto & layer : layers) { if (layer == pointLayer) { fieldNames.push_back("x"); fieldNames.push_back("y"); @@ -150,14 +180,14 @@ void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap, pointCloud.fields.reserve(fieldNames.size()); int offset = 0; - for (auto& name : fieldNames) { - sensor_msgs::PointField pointField; + for (auto & name : fieldNames) { + sensor_msgs::msg::PointField pointField; pointField.name = name; pointField.count = 1; - pointField.datatype = sensor_msgs::PointField::FLOAT32; + pointField.datatype = sensor_msgs::msg::PointField::FLOAT32; pointField.offset = offset; pointCloud.fields.push_back(pointField); - offset = offset + pointField.count * 4; // 4 for sensor_msgs::PointField::FLOAT32 + offset = offset + pointField.count * 4; // 4 for sensor_msgs::msg::PointField::FLOAT32 } // Resize. @@ -170,10 +200,10 @@ void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap, // Points. std::unordered_map> fieldIterators; - for (auto& name : fieldNames) { + for (auto & name : fieldNames) { fieldIterators.insert( - std::pair>( - name, sensor_msgs::PointCloud2Iterator(pointCloud, name))); + std::pair>( + name, sensor_msgs::PointCloud2Iterator(pointCloud, name))); } GridMapIterator mapIterator(gridMap); @@ -194,13 +224,13 @@ void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap, continue; } - for (auto& iterator : fieldIterators) { + for (auto & iterator : fieldIterators) { if (iterator.first == "x") { - *iterator.second = (float) position.x(); + *iterator.second = static_cast(position.x()); } else if (iterator.first == "y") { - *iterator.second = (float) position.y(); + *iterator.second = static_cast(position.y()); } else if (iterator.first == "z") { - *iterator.second = (float) position.z(); + *iterator.second = static_cast(position.z()); } else if (iterator.first == "rgb") { *iterator.second = gridMap.at("color", *mapIterator); } else { @@ -209,7 +239,7 @@ void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap, } ++mapIterator; - for (auto& iterator : fieldIterators) { + for (auto & iterator : fieldIterators) { ++iterator.second; } ++realNumberOfPoints; @@ -222,35 +252,46 @@ void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap, } } -bool GridMapRosConverter::fromOccupancyGrid(const nav_msgs::OccupancyGrid& occupancyGrid, - const std::string& layer, grid_map::GridMap& gridMap) +bool GridMapRosConverter::fromOccupancyGrid( + const nav_msgs::msg::OccupancyGrid & occupancyGrid, + const std::string & layer, grid_map::GridMap & gridMap) { const Size size(occupancyGrid.info.width, occupancyGrid.info.height); const double resolution = occupancyGrid.info.resolution; const Length length = resolution * size.cast(); - const string& frameId = occupancyGrid.header.frame_id; + const string & frameId = occupancyGrid.header.frame_id; Position position(occupancyGrid.info.origin.position.x, occupancyGrid.info.origin.position.y); // Different conventions of center of map. position += 0.5 * length.matrix(); - const auto& orientation = occupancyGrid.info.origin.orientation; - if (orientation.w != 1.0 && !(orientation.x == 0 && orientation.y == 0 - && orientation.z == 0 && orientation.w == 0)) { - ROS_WARN_STREAM("Conversion of occupancy grid: Grid maps do not support orientation."); - ROS_INFO_STREAM("Orientation of occupancy grid: " << endl << occupancyGrid.info.origin.orientation); + const auto & orientation = occupancyGrid.info.origin.orientation; + if (orientation.w != 1.0 && !(orientation.x == 0 && orientation.y == 0 && + orientation.z == 0 && orientation.w == 0)) + { + RCLCPP_WARN( + rclcpp::get_logger( + "fromOccupancyGrid"), + "Conversion of occupancy grid: Grid maps do not support orientation."); + RCLCPP_INFO( + rclcpp::get_logger("fromOccupancyGrid"), + "Orientation of occupancy grid: \n%s", + geometry_msgs::msg::to_yaml(occupancyGrid.info.origin.orientation).c_str()); return false; } - if (size.prod() != occupancyGrid.data.size()) { - ROS_WARN_STREAM("Conversion of occupancy grid: Size of data does not correspond to width * height."); + if (static_cast(size.prod()) != occupancyGrid.data.size()) { + RCLCPP_WARN( + rclcpp::get_logger("fromOccupancyGrid"), + "Conversion of occupancy grid: Size of data does not correspond to width * height."); return false; } - // TODO: Split to `initializeFrom` and `from` as for Costmap2d. - if ((gridMap.getSize() != size).any() || gridMap.getResolution() != resolution - || (gridMap.getLength() != length).any() || gridMap.getPosition() != position - || gridMap.getFrameId() != frameId || !gridMap.getStartIndex().isZero()) { - gridMap.setTimestamp(occupancyGrid.header.stamp.toNSec()); + // TODO(needs_assignment): Split to `initializeFrom` and `from` as for Costmap2d. + if ((gridMap.getSize() != size).any() || gridMap.getResolution() != resolution || + (gridMap.getLength() != length).any() || gridMap.getPosition() != position || + gridMap.getFrameId() != frameId || !gridMap.getStartIndex().isZero()) + { + gridMap.setTimestamp(rclcpp::Time(occupancyGrid.header.stamp).nanoseconds()); gridMap.setFrameId(frameId); gridMap.setGeometry(length, resolution, position); } @@ -259,7 +300,8 @@ bool GridMapRosConverter::fromOccupancyGrid(const nav_msgs::OccupancyGrid& occup // between occupancy grid and grid map. grid_map::Matrix data(size(0), size(1)); for (std::vector::const_reverse_iterator iterator = occupancyGrid.data.rbegin(); - iterator != occupancyGrid.data.rend(); ++iterator) { + iterator != occupancyGrid.data.rend(); ++iterator) + { size_t i = std::distance(occupancyGrid.data.rbegin(), iterator); data(i) = *iterator != -1 ? *iterator : NAN; } @@ -268,13 +310,15 @@ bool GridMapRosConverter::fromOccupancyGrid(const nav_msgs::OccupancyGrid& occup return true; } -void GridMapRosConverter::toOccupancyGrid(const grid_map::GridMap& gridMap, - const std::string& layer, float dataMin, float dataMax, - nav_msgs::OccupancyGrid& occupancyGrid) +void GridMapRosConverter::toOccupancyGrid( + const grid_map::GridMap & gridMap, + const std::string & layer, float dataMin, float dataMax, + nav_msgs::msg::OccupancyGrid & occupancyGrid) { occupancyGrid.header.frame_id = gridMap.getFrameId(); - occupancyGrid.header.stamp.fromNSec(gridMap.getTimestamp()); - occupancyGrid.info.map_load_time = occupancyGrid.header.stamp; // Same as header stamp as we do not load the map. + occupancyGrid.header.stamp = rclcpp::Time(gridMap.getTimestamp()); + // Same as header stamp as we do not load the map. + occupancyGrid.info.map_load_time = occupancyGrid.header.stamp; occupancyGrid.info.resolution = gridMap.getResolution(); occupancyGrid.info.width = gridMap.getSize()(0); occupancyGrid.info.height = gridMap.getSize()(1); @@ -296,32 +340,134 @@ void GridMapRosConverter::toOccupancyGrid(const grid_map::GridMap& gridMap, for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { float value = (gridMap.at(layer, *iterator) - dataMin) / (dataMax - dataMin); - if (isnan(value)) + if (isnan(value)) { value = -1; - else + } else { value = cellMin + min(max(0.0f, value), 1.0f) * cellRange; + } size_t index = getLinearIndexFromIndex(iterator.getUnwrappedIndex(), gridMap.getSize(), false); // Reverse cell order because of different conventions between occupancy grid and grid map. occupancyGrid.data[nCells - index - 1] = value; } } -void GridMapRosConverter::toGridCells(const grid_map::GridMap& gridMap, const std::string& layer, - float lowerThreshold, float upperThreshold, - nav_msgs::GridCells& gridCells) +bool GridMapRosConverter::fromCostmap( + const nav2_msgs::msg::Costmap & costmap, + const std::string & layer, grid_map::GridMap & gridMap) +{ + const Size size(costmap.metadata.size_x, costmap.metadata.size_y); + const double resolution = costmap.metadata.resolution; + const Length length = resolution * size.cast(); + const string & frameId = costmap.header.frame_id; + Position position(costmap.metadata.origin.position.x, costmap.metadata.origin.position.y); + // Different conventions of center of map. + position += 0.5 * length.matrix(); + + const auto & orientation = costmap.metadata.origin.orientation; + if (orientation.w != 1.0 && !(orientation.x == 0 && orientation.y == 0 && + orientation.z == 0 && orientation.w == 0)) + { + RCLCPP_WARN( + rclcpp::get_logger( + "fromcostmap"), + "Conversion of costmap: Grid maps do not support orientation."); + RCLCPP_INFO( + rclcpp::get_logger("fromcostmap"), + "Orientation of costmap: \n%s", + geometry_msgs::msg::to_yaml(costmap.metadata.origin.orientation).c_str()); + return false; + } + + if (static_cast(size.prod()) != costmap.data.size()) { + RCLCPP_WARN( + rclcpp::get_logger("fromcostmap"), + "Conversion of costmap: Size of data does not correspond to width * height."); + return false; + } + + // TODO(needs_assignment): Split to `initializeFrom` and `from` as for Costmap2d. + if ((gridMap.getSize() != size).any() || gridMap.getResolution() != resolution || + (gridMap.getLength() != length).any() || gridMap.getPosition() != position || + gridMap.getFrameId() != frameId || !gridMap.getStartIndex().isZero()) + { + gridMap.setTimestamp(rclcpp::Time(costmap.header.stamp).nanoseconds()); + gridMap.setFrameId(frameId); + gridMap.setGeometry(length, resolution, position); + } + + // Reverse iteration is required because of different conventions + // between costmap and grid map. + grid_map::Matrix data(size(0), size(1)); + for (std::vector::const_reverse_iterator iterator = costmap.data.rbegin(); + iterator != costmap.data.rend(); ++iterator) + { + size_t i = std::distance(costmap.data.rbegin(), iterator); + data(i) = *iterator != 255 ? *iterator : NAN; + } + + gridMap.add(layer, data); + return true; +} + +void GridMapRosConverter::toCostmap( + const grid_map::GridMap & gridMap, + const std::string & layer, float dataMin, float dataMax, + nav2_msgs::msg::Costmap & costmap) +{ + costmap.header.frame_id = gridMap.getFrameId(); + costmap.header.stamp = rclcpp::Time(gridMap.getTimestamp()); + // Same as header stamp as we do not load the map. + costmap.metadata.map_load_time = costmap.header.stamp; + costmap.metadata.resolution = gridMap.getResolution(); + costmap.metadata.size_x = gridMap.getSize()(0); + costmap.metadata.size_y = gridMap.getSize()(1); + Position position = gridMap.getPosition() - 0.5 * gridMap.getLength().matrix(); + costmap.metadata.origin.position.x = position.x(); + costmap.metadata.origin.position.y = position.y(); + costmap.metadata.origin.position.z = 0.0; + costmap.metadata.origin.orientation.x = 0.0; + costmap.metadata.origin.orientation.y = 0.0; + costmap.metadata.origin.orientation.z = 0.0; + costmap.metadata.origin.orientation.w = 1.0; + size_t nCells = gridMap.getSize().prod(); + costmap.data.resize(nCells); + + // Costmap cell values are in the range [0,254], 255 is unknown space. + const float cellMin = 0; + const float cellMax = 254; + const float cellRange = cellMax - cellMin; + + for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { + float value = (gridMap.at(layer, *iterator) - dataMin) / (dataMax - dataMin); + if (isnan(value)) { + value = 255; + } else { + value = cellMin + min(max(0.0f, value), 1.0f) * cellRange; + } + size_t index = getLinearIndexFromIndex(iterator.getUnwrappedIndex(), gridMap.getSize(), false); + // Reverse cell order because of different conventions between Costmap and grid map. + costmap.data[nCells - index - 1] = value; + } +} + +void GridMapRosConverter::toGridCells( + const grid_map::GridMap & gridMap, const std::string & layer, + float lowerThreshold, float upperThreshold, + nav_msgs::msg::GridCells & gridCells) { gridCells.header.frame_id = gridMap.getFrameId(); - gridCells.header.stamp.fromNSec(gridMap.getTimestamp()); + gridCells.header.stamp = rclcpp::Time(gridMap.getTimestamp()); gridCells.cell_width = gridMap.getResolution(); gridCells.cell_height = gridMap.getResolution(); for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { - if (!gridMap.isValid(*iterator, layer)) continue; - if (gridMap.at(layer, *iterator) >= lowerThreshold - && gridMap.at(layer, *iterator) <= upperThreshold) { + if (!gridMap.isValid(*iterator, layer)) {continue;} + if (gridMap.at(layer, *iterator) >= lowerThreshold && + gridMap.at(layer, *iterator) <= upperThreshold) + { Position position; gridMap.getPosition(*iterator, position); - geometry_msgs::Point point; + geometry_msgs::msg::Point point; point.x = position.x(); point.y = position.y(); gridCells.cells.push_back(point); @@ -329,178 +475,296 @@ void GridMapRosConverter::toGridCells(const grid_map::GridMap& gridMap, const st } } -bool GridMapRosConverter::initializeFromImage(const sensor_msgs::Image& image, - const double resolution, grid_map::GridMap& gridMap, - const grid_map::Position& position) +bool GridMapRosConverter::initializeFromImage( + const sensor_msgs::msg::Image & image, + const double resolution, grid_map::GridMap & gridMap, + const grid_map::Position & position) { const double lengthX = resolution * image.height; const double lengthY = resolution * image.width; Length length(lengthX, lengthY); gridMap.setGeometry(length, resolution, position); gridMap.setFrameId(image.header.frame_id); - gridMap.setTimestamp(image.header.stamp.toNSec()); + gridMap.setTimestamp(rclcpp::Time(image.header.stamp).nanoseconds()); return true; } -bool GridMapRosConverter::addLayerFromImage(const sensor_msgs::Image& image, - const std::string& layer, grid_map::GridMap& gridMap, - const float lowerValue, const float upperValue, - const double alphaThreshold) +bool GridMapRosConverter::addLayerFromImage( + const sensor_msgs::msg::Image & image, + const std::string & layer, grid_map::GridMap & gridMap, + const float lowerValue, const float upperValue, + const double alphaThreshold) { cv_bridge::CvImageConstPtr cvImage; try { - // TODO Use `toCvShared()`? + // TODO(needs_assignment) Use `toCvShared()`? cvImage = cv_bridge::toCvCopy(image, image.encoding); - } catch (cv_bridge::Exception& e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("addLayerFromImage"), "cv_bridge exception: %s", e.what()); return false; } const int cvEncoding = cv_bridge::getCvType(image.encoding); switch (cvEncoding) { case CV_8UC1: - return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold); + return GridMapCvConverter::addLayerFromImage( + cvImage->image, layer, gridMap, + lowerValue, upperValue, + alphaThreshold); case CV_8UC3: - return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold); + return GridMapCvConverter::addLayerFromImage( + cvImage->image, layer, gridMap, + lowerValue, upperValue, + alphaThreshold); case CV_8UC4: - return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold); + return GridMapCvConverter::addLayerFromImage( + cvImage->image, layer, gridMap, + lowerValue, upperValue, + alphaThreshold); case CV_16UC1: - return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold); + return GridMapCvConverter::addLayerFromImage( + cvImage->image, layer, + gridMap, lowerValue, + upperValue, alphaThreshold); case CV_16UC3: - return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold); + return GridMapCvConverter::addLayerFromImage( + cvImage->image, layer, + gridMap, lowerValue, + upperValue, alphaThreshold); case CV_16UC4: - return GridMapCvConverter::addLayerFromImage(cvImage->image, layer, gridMap, lowerValue, upperValue, alphaThreshold); + return GridMapCvConverter::addLayerFromImage( + cvImage->image, layer, + gridMap, lowerValue, + upperValue, alphaThreshold); default: - ROS_ERROR("Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding."); + RCLCPP_ERROR( + rclcpp::get_logger( + "addLayerFromImage"), + "Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding."); return false; } } -bool GridMapRosConverter::addColorLayerFromImage(const sensor_msgs::Image& image, - const std::string& layer, - grid_map::GridMap& gridMap) +bool GridMapRosConverter::addColorLayerFromImage( + const sensor_msgs::msg::Image & image, + const std::string & layer, + grid_map::GridMap & gridMap) { cv_bridge::CvImageConstPtr cvImage; try { cvImage = cv_bridge::toCvCopy(image, image.encoding); - } catch (cv_bridge::Exception& e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("loadFromBag"), "cv_bridge exception: %s", e.what()); return false; } const int cvEncoding = cv_bridge::getCvType(image.encoding); switch (cvEncoding) { case CV_8UC3: - return GridMapCvConverter::addColorLayerFromImage(cvImage->image, layer, gridMap); + return GridMapCvConverter::addColorLayerFromImage( + cvImage->image, layer, + gridMap); case CV_8UC4: - return GridMapCvConverter::addColorLayerFromImage(cvImage->image, layer, gridMap); + return GridMapCvConverter::addColorLayerFromImage( + cvImage->image, layer, + gridMap); case CV_16UC3: - return GridMapCvConverter::addColorLayerFromImage(cvImage->image, layer, gridMap); + return GridMapCvConverter::addColorLayerFromImage( + cvImage->image, layer, + gridMap); case CV_16UC4: - return GridMapCvConverter::addColorLayerFromImage(cvImage->image, layer, gridMap); + return GridMapCvConverter::addColorLayerFromImage( + cvImage->image, layer, + gridMap); default: - ROS_ERROR("Expected RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding."); + RCLCPP_ERROR( + rclcpp::get_logger( + "addColorLayerFromImage"), + "Expected RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding."); return false; } } -bool GridMapRosConverter::toImage(const grid_map::GridMap& gridMap, const std::string& layer, - const std::string encoding, sensor_msgs::Image& image) +bool GridMapRosConverter::toImage( + const grid_map::GridMap & gridMap, const std::string & layer, + const std::string encoding, sensor_msgs::msg::Image & image) { cv_bridge::CvImage cvImage; - if (!toCvImage(gridMap, layer, encoding, cvImage)) return false; + if (!toCvImage(gridMap, layer, encoding, cvImage)) {return false;} cvImage.toImageMsg(image); return true; } -bool GridMapRosConverter::toImage(const grid_map::GridMap& gridMap, const std::string& layer, - const std::string encoding, const float lowerValue, - const float upperValue, sensor_msgs::Image& image) +bool GridMapRosConverter::toImage( + const grid_map::GridMap & gridMap, const std::string & layer, + const std::string encoding, const float lowerValue, + const float upperValue, sensor_msgs::msg::Image & image) { cv_bridge::CvImage cvImage; - if (!toCvImage(gridMap, layer, encoding, lowerValue, upperValue, cvImage)) return false; + if (!toCvImage(gridMap, layer, encoding, lowerValue, upperValue, cvImage)) {return false;} cvImage.toImageMsg(image); return true; } -bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std::string& layer, - const std::string encoding, cv_bridge::CvImage& cvImage) +bool GridMapRosConverter::toCvImage( + const grid_map::GridMap & gridMap, const std::string & layer, + const std::string encoding, cv_bridge::CvImage & cvImage) { const float minValue = gridMap.get(layer).minCoeffOfFinites(); const float maxValue = gridMap.get(layer).maxCoeffOfFinites(); return toCvImage(gridMap, layer, encoding, minValue, maxValue, cvImage); } -bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std::string& layer, - const std::string encoding, const float lowerValue, - const float upperValue, cv_bridge::CvImage& cvImage) +bool GridMapRosConverter::toCvImage( + const grid_map::GridMap & gridMap, const std::string & layer, + const std::string encoding, const float lowerValue, + const float upperValue, cv_bridge::CvImage & cvImage) { - cvImage.header.stamp.fromNSec(gridMap.getTimestamp()); + cvImage.header.stamp = rclcpp::Time(gridMap.getTimestamp()); cvImage.header.frame_id = gridMap.getFrameId(); cvImage.encoding = encoding; const int cvEncoding = cv_bridge::getCvType(encoding); switch (cvEncoding) { case CV_8UC1: - return GridMapCvConverter::toImage(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); + return GridMapCvConverter::toImage( + gridMap, layer, cvEncoding, lowerValue, + upperValue, cvImage.image); case CV_8UC3: - return GridMapCvConverter::toImage(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); + return GridMapCvConverter::toImage( + gridMap, layer, cvEncoding, lowerValue, + upperValue, cvImage.image); case CV_8UC4: - return GridMapCvConverter::toImage(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); + return GridMapCvConverter::toImage( + gridMap, layer, cvEncoding, lowerValue, + upperValue, cvImage.image); case CV_16UC1: - return GridMapCvConverter::toImage(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); + return GridMapCvConverter::toImage( + gridMap, layer, cvEncoding, lowerValue, + upperValue, cvImage.image); case CV_16UC3: - return GridMapCvConverter::toImage(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); + return GridMapCvConverter::toImage( + gridMap, layer, cvEncoding, lowerValue, + upperValue, cvImage.image); case CV_16UC4: - return GridMapCvConverter::toImage(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image); + return GridMapCvConverter::toImage( + gridMap, layer, cvEncoding, lowerValue, + upperValue, cvImage.image); default: - ROS_ERROR("Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding."); + RCLCPP_ERROR( + rclcpp::get_logger( + "toCvImage"), + "Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding."); return false; } } -bool GridMapRosConverter::saveToBag(const grid_map::GridMap& gridMap, const std::string& pathToBag, - const std::string& topic) +bool GridMapRosConverter::saveToBag( + const grid_map::GridMap & gridMap, const std::string & pathToBag, + const std::string & topic) { - grid_map_msgs::GridMap message; - toMessage(gridMap, message); - ros::Time time; - time.fromNSec(gridMap.getTimestamp()); - - if (!time.isValid() || time.isZero()) { - if (!ros::Time::isValid()) ros::Time::init(); - time = ros::Time::now(); + auto message = toMessage(gridMap); + + rclcpp::SerializedMessage serialized_msg; + rclcpp::Serialization serialization; + serialization.serialize_message(message.get(), &serialized_msg); + + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = pathToBag; + storage_options.storage_id = "sqlite3"; + + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + + rosbag2_cpp::Writer writer(std::make_unique()); + writer.open(storage_options, converter_options); + + rosbag2_storage::TopicMetadata tm; + tm.name = topic; + tm.type = "grid_map_msgs/msg/GridMap"; + tm.serialization_format = "cdr"; + writer.create_topic(tm); + + auto bag_message = std::make_shared(); + + auto ret = rcutils_system_time_now(&bag_message->send_timestamp); + if (ret != RCL_RET_OK) { + RCLCPP_ERROR(rclcpp::get_logger("saveToBag"), "couldn't assign time rosbag message"); } - rosbag::Bag bag; - bag.open(pathToBag, rosbag::bagmode::Write); - bag.write(topic, time, message); - bag.close(); + bag_message->topic_name = tm.name; + bag_message->serialized_data = std::shared_ptr( + &serialized_msg.get_rcl_serialized_message(), [](rcutils_uint8_array_t * /* data */) {}); + + writer.write(bag_message); return true; } -bool GridMapRosConverter::loadFromBag(const std::string& pathToBag, const std::string& topic, - grid_map::GridMap& gridMap) +bool GridMapRosConverter::loadFromBag( + const std::string & pathToBag, const std::string & topic, + grid_map::GridMap & gridMap) { - rosbag::Bag bag; - bag.open(pathToBag, rosbag::bagmode::Read); - rosbag::View view(bag, rosbag::TopicQuery(topic)); + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = pathToBag; + storage_options.storage_id = "sqlite3"; + + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + + rosbag2_cpp::Reader reader(std::make_unique()); + reader.open(storage_options, converter_options); bool isDataFound = false; - for (const auto& messageInstance : view) { - grid_map_msgs::GridMap::ConstPtr message = messageInstance.instantiate(); - if (message != NULL) { - fromMessage(*message, gridMap); + + grid_map_msgs::msg::GridMap extracted_gridmap_msg; + rclcpp::Serialization serialization; + + // Validate the received bag topic exists and + // is of the correct type to prevent later serialization exception. + const auto topic_metadata = reader.get_all_topics_and_types(); + bool topic_is_correct_type = false; + for (const auto & m : topic_metadata) { + if (m.name == topic && m.type == "grid_map_msgs/msg/GridMap") { + topic_is_correct_type = true; + } + } + if (!topic_is_correct_type) { + RCLCPP_ERROR( + rclcpp::get_logger( + "loadFromBag"), "Bagfile does not contain a GridMap message on the expected topic '%s'", + topic.c_str()); + return false; + } + + while (reader.has_next()) { + auto bag_message = reader.read_next(); + if (bag_message == nullptr) { + continue; + } + + // Only read messages on the correct topic. + // https://github.com/ANYbotics/grid_map/issues/401 + if (bag_message->topic_name != topic) { + continue; + } + + if (bag_message->serialized_data != NULL) { + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_gridmap_msg); + fromMessage(extracted_gridmap_msg, gridMap); isDataFound = true; } else { - bag.close(); - ROS_WARN("Unable to load data from ROS bag."); + RCLCPP_WARN(rclcpp::get_logger("loadFromBag"), "Unable to load data from ROS bag."); return false; } } - if (!isDataFound) ROS_WARN_STREAM("No data under the topic '" << topic << "' was found."); - bag.close(); + if (!isDataFound) { + RCLCPP_WARN( + rclcpp::get_logger( + "loadFromBag"), "No data under the topic %s was found.", topic.c_str()); + } return true; } - -} /* namespace */ +} // namespace grid_map diff --git a/grid_map_ros/src/PolygonRosConverter.cpp b/grid_map_ros/src/PolygonRosConverter.cpp index 3a7648443..aa3c6f5d2 100644 --- a/grid_map_ros/src/PolygonRosConverter.cpp +++ b/grid_map_ros/src/PolygonRosConverter.cpp @@ -5,24 +5,33 @@ * Author: Péter Fankhauser * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_ros/PolygonRosConverter.hpp" // ROS -#include +#include + +#include + +#include "grid_map_ros/PolygonRosConverter.hpp" + +using namespace std::chrono_literals; -namespace grid_map { +namespace grid_map +{ PolygonRosConverter::PolygonRosConverter() {} PolygonRosConverter::~PolygonRosConverter() {} -void PolygonRosConverter::toMessage(const grid_map::Polygon& polygon, geometry_msgs::PolygonStamped& message) +void PolygonRosConverter::toMessage( + const grid_map::Polygon & polygon, + geometry_msgs::msg::PolygonStamped & message) { - message.header.stamp.fromNSec(polygon.getTimestamp()); + rclcpp::Time time_stamp(polygon.getTimestamp()); + message.header.stamp = time_stamp; message.header.frame_id = polygon.getFrameId(); - for (const auto& vertex : polygon.getVertices()) { - geometry_msgs::Point32 point; + for (const auto & vertex : polygon.getVertices()) { + geometry_msgs::msg::Point32 point; point.x = vertex.x(); point.y = vertex.y(); point.z = 0.0; @@ -30,49 +39,53 @@ void PolygonRosConverter::toMessage(const grid_map::Polygon& polygon, geometry_m } } -void PolygonRosConverter::toLineMarker(const grid_map::Polygon& polygon, const std_msgs::ColorRGBA& color, const double lineWidth, - const double zCoordinate, visualization_msgs::Marker& marker) +void PolygonRosConverter::toLineMarker( + const grid_map::Polygon & polygon, const std_msgs::msg::ColorRGBA & color, const double lineWidth, + const double zCoordinate, visualization_msgs::msg::Marker & marker) { - marker.header.stamp.fromNSec(polygon.getTimestamp()); - marker.header.frame_id = polygon.getFrameId(); - marker.lifetime = ros::Duration(0.0); - marker.action = visualization_msgs::Marker::ADD; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.color = color; - marker.scale.x = lineWidth; - - unsigned int startIndex = marker.points.size(); - unsigned int nVertices = polygon.nVertices() + 1; - unsigned int nTotalVertices = marker.points.size() + nVertices; - marker.points.resize(nTotalVertices); - marker.colors.resize(nTotalVertices, color); - - unsigned int i = startIndex; - for( ; i < nTotalVertices - 1; i++) { - marker.points[i].x = polygon[i].x(); - marker.points[i].y = polygon[i].y(); - marker.points[i].z = zCoordinate; - } - marker.points[i].x = marker.points[startIndex].x; - marker.points[i].y = marker.points[startIndex].y; - marker.points[i].z = marker.points[startIndex].z; + rclcpp::Time time_stamp(polygon.getTimestamp()); + marker.header.stamp = time_stamp; + marker.header.frame_id = polygon.getFrameId(); + marker.lifetime = rclcpp::Duration(0.0ns); + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.color = color; + marker.scale.x = lineWidth; + + unsigned int startIndex = marker.points.size(); + unsigned int nVertices = polygon.nVertices() + 1; + unsigned int nTotalVertices = marker.points.size() + nVertices; + marker.points.resize(nTotalVertices); + marker.colors.resize(nTotalVertices, color); + + unsigned int i = startIndex; + for ( ; i < nTotalVertices - 1; i++) { + marker.points[i].x = polygon[i].x(); + marker.points[i].y = polygon[i].y(); + marker.points[i].z = zCoordinate; + } + marker.points[i].x = marker.points[startIndex].x; + marker.points[i].y = marker.points[startIndex].y; + marker.points[i].z = marker.points[startIndex].z; } -void PolygonRosConverter::toTriangleListMarker(const grid_map::Polygon& polygon, const std_msgs::ColorRGBA& color, - const double zCoordinate, visualization_msgs::Marker& marker) +void PolygonRosConverter::toTriangleListMarker( + const grid_map::Polygon & polygon, const std_msgs::msg::ColorRGBA & color, + const double zCoordinate, visualization_msgs::msg::Marker & marker) { - marker.header.stamp.fromNSec(polygon.getTimestamp()); + rclcpp::Time time_stamp(polygon.getTimestamp()); + marker.header.stamp = time_stamp; marker.header.frame_id = polygon.getFrameId(); - marker.lifetime = ros::Duration(0.0); - marker.action = visualization_msgs::Marker::ADD; - marker.type = visualization_msgs::Marker::TRIANGLE_LIST; + marker.lifetime = rclcpp::Duration(0.0ns); + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; marker.scale.x = 1.0; marker.scale.y = 1.0; marker.scale.z = 1.0; marker.color = color; std::vector polygons = polygon.triangulate(); - if (polygons.size() < 1) return; + if (polygons.size() < 1) {return;} size_t nPoints = 3 * polygons.size(); marker.points.resize(nPoints); @@ -88,4 +101,4 @@ void PolygonRosConverter::toTriangleListMarker(const grid_map::Polygon& polygon, } } -} /* namespace grid_map */ +} // namespace grid_map diff --git a/grid_map_ros/test/GridMapRosTest.cpp b/grid_map_ros/test/GridMapRosTest.cpp index 8727610a1..e48fc350d 100644 --- a/grid_map_ros/test/GridMapRosTest.cpp +++ b/grid_map_ros/test/GridMapRosTest.cpp @@ -6,31 +6,35 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/GridMap.hpp" -#include "grid_map_core/iterators/GridMapIterator.hpp" -#include "grid_map_core/gtest_eigen.hpp" -#include "grid_map_ros/GridMapRosConverter.hpp" -#include "grid_map_msgs/GridMap.h" - // gtest #include +#include // Eigen #include +// ROS +#include +#include +#include +#include +#include + // STD #include #include -#include #include +#include +#include -// ROS -#include -#include -#include +#include "grid_map_core/GridMap.hpp" +#include "grid_map_core/iterators/GridMapIterator.hpp" +#include "grid_map_core/gtest_eigen.hpp" +#include "grid_map_ros/GridMapRosConverter.hpp" +#include "grid_map_msgs/msg/grid_map.hpp" -using namespace std; -using namespace grid_map; +using namespace std; // NOLINT +using namespace grid_map; // NOLINT TEST(RosMessageConversion, roundTrip) { @@ -38,10 +42,9 @@ TEST(RosMessageConversion, roundTrip) mapIn.setGeometry(Length(2.0, 3.0), 0.5, Position(1.0, 1.5)); mapIn["layer"].setRandom(); - grid_map_msgs::GridMap message; - GridMapRosConverter::toMessage(mapIn, message); + auto message = GridMapRosConverter::toMessage(mapIn); GridMap mapOut; - GridMapRosConverter::fromMessage(message, mapOut); + GridMapRosConverter::fromMessage(*message, mapOut); for (size_t i = 0; i < mapIn.getLayers().size(); ++i) { EXPECT_EQ(mapIn.getLayers().at(i), mapOut.getLayers().at(i)); @@ -55,6 +58,7 @@ TEST(RosMessageConversion, roundTrip) EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); } + TEST(RosbagHandling, saveLoad) { string layer = "layer"; @@ -72,14 +76,21 @@ TEST(RosbagHandling, saveLoad) EXPECT_FALSE(gridMapOut.exists(layer)); + // Cleaning in case the previous bag was not removed + rcpputils::fs::path dir(pathToBag); + EXPECT_TRUE(!dir.exists() || rcpputils::fs::remove_all(dir)); + EXPECT_TRUE(!dir.exists() || rcpputils::fs::create_directories(dir)); + EXPECT_TRUE(GridMapRosConverter::saveToBag(gridMapIn, pathToBag, topic)); EXPECT_TRUE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut)); - EXPECT_TRUE(gridMapOut.exists(layer)); for (GridMapIterator iterator(gridMapIn); !iterator.isPastEnd(); ++iterator) { EXPECT_DOUBLE_EQ(gridMapIn.at(layer, *iterator), gridMapOut.at(layer, *iterator)); } + + // Removing the created bag + rcpputils::fs::remove_all(dir); } TEST(RosbagHandling, saveLoadWithTime) @@ -99,9 +110,14 @@ TEST(RosbagHandling, saveLoadWithTime) EXPECT_FALSE(gridMapOut.exists(layer)); - if (!ros::Time::isValid()) ros::Time::init(); - // TODO Do other time than now. - gridMapIn.setTimestamp(ros::Time::now().toNSec()); + // TODO(needs_assignment): Do other time than now. + rclcpp::Clock clock; + gridMapIn.setTimestamp(clock.now().nanoseconds()); + + // Cleaning in case the previous bag was not removed + rcpputils::fs::path dir(pathToBag); + EXPECT_TRUE(!dir.exists() || rcpputils::fs::remove_all(dir)); + EXPECT_TRUE(!dir.exists() || rcpputils::fs::create_directories(dir)); EXPECT_TRUE(GridMapRosConverter::saveToBag(gridMapIn, pathToBag, topic)); EXPECT_TRUE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut)); @@ -111,6 +127,39 @@ TEST(RosbagHandling, saveLoadWithTime) for (GridMapIterator iterator(gridMapIn); !iterator.isPastEnd(); ++iterator) { EXPECT_DOUBLE_EQ(gridMapIn.at(layer, *iterator), gridMapOut.at(layer, *iterator)); } + + // Removing the created bag + rcpputils::fs::remove_all(dir); +} + +TEST(RosbagHandling, wrongTopicType) +{ + // This test validates LoadFromBag will reject a topic of the wrong type. + // See https://github.com/ANYbotics/grid_map/issues/401. + + std::string pathToBag = "double_chatter"; + string topic = "/chatter1"; + GridMap gridMapOut; + rcpputils::fs::path dir(pathToBag); + + EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut)); +} + +TEST(RosbagHandling, checkTopicTypes) +{ + // This test validates loadFromBag will reject a topic of the wrong type or + // non-existing topic and accept a correct topic. + + std::string pathToBag = "test_multitopic"; + string topic_wrong = "/chatter"; + string topic_correct = "/grid_map"; + string topic_non_existing = "/grid_map_non_existing"; + GridMap gridMapOut; + rcpputils::fs::path dir(pathToBag); + + EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic_wrong, gridMapOut)); + EXPECT_TRUE(GridMapRosConverter::loadFromBag(pathToBag, topic_correct, gridMapOut)); + EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic_non_existing, gridMapOut)); } TEST(OccupancyGridConversion, withMove) @@ -120,7 +169,7 @@ TEST(OccupancyGridConversion, withMove) map.add("layer", 1.0); // Convert to OccupancyGrid msg. - nav_msgs::OccupancyGrid occupancyGrid; + nav_msgs::msg::OccupancyGrid occupancyGrid; GridMapRosConverter::toOccupancyGrid(map, "layer", 0.0, 1.0, occupancyGrid); // Expect the (0, 0) cell to have value 100. @@ -130,7 +179,7 @@ TEST(OccupancyGridConversion, withMove) map.move(grid_map::Position(-1.0, -1.0)); // Convert again to OccupancyGrid msg. - nav_msgs::OccupancyGrid occupancyGridNew; + nav_msgs::msg::OccupancyGrid occupancyGridNew; GridMapRosConverter::toOccupancyGrid(map, "layer", 0.0, 1.0, occupancyGridNew); // Now the (0, 0) cell should be unobserved (-1). @@ -140,8 +189,8 @@ TEST(OccupancyGridConversion, withMove) TEST(OccupancyGridConversion, roundTrip) { // Create occupancy grid. - nav_msgs::OccupancyGrid occupancyGrid; - occupancyGrid.header.stamp = ros::Time(5.0); + nav_msgs::msg::OccupancyGrid occupancyGrid; + occupancyGrid.header.stamp = rclcpp::Time(5.0); occupancyGrid.header.frame_id = "map"; occupancyGrid.info.resolution = 0.1; occupancyGrid.info.width = 50; @@ -151,8 +200,9 @@ TEST(OccupancyGridConversion, roundTrip) occupancyGrid.info.origin.orientation.w = 1.0; occupancyGrid.data.resize(occupancyGrid.info.width * occupancyGrid.info.height); - for (auto& cell : occupancyGrid.data) { - cell = rand() % 102 - 1; // [-1, 100] + unsigned int seed = time(0); + for (auto & cell : occupancyGrid.data) { + cell = rand_r(&seed) % 102 - 1; // [-1, 100] } // Convert to grid map. @@ -160,7 +210,7 @@ TEST(OccupancyGridConversion, roundTrip) GridMapRosConverter::fromOccupancyGrid(occupancyGrid, "layer", gridMap); // Convert back to occupancy grid. - nav_msgs::OccupancyGrid occupancyGridResult; + nav_msgs::msg::OccupancyGrid occupancyGridResult; GridMapRosConverter::toOccupancyGrid(gridMap, "layer", -1.0, 100.0, occupancyGridResult); // Check map info. @@ -168,18 +218,115 @@ TEST(OccupancyGridConversion, roundTrip) EXPECT_EQ(occupancyGrid.header.frame_id, occupancyGridResult.header.frame_id); EXPECT_EQ(occupancyGrid.info.width, occupancyGridResult.info.width); EXPECT_EQ(occupancyGrid.info.height, occupancyGridResult.info.height); - EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.position.x, occupancyGridResult.info.origin.position.x); - EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.position.x, occupancyGridResult.info.origin.position.x); - EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.orientation.x, occupancyGridResult.info.origin.orientation.x); - EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.orientation.y, occupancyGridResult.info.origin.orientation.y); - EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.orientation.z, occupancyGridResult.info.origin.orientation.z); - EXPECT_DOUBLE_EQ(occupancyGrid.info.origin.orientation.w, occupancyGridResult.info.origin.orientation.w); + EXPECT_DOUBLE_EQ( + occupancyGrid.info.origin.position.x, + occupancyGridResult.info.origin.position.x); + EXPECT_DOUBLE_EQ( + occupancyGrid.info.origin.position.x, + occupancyGridResult.info.origin.position.x); + EXPECT_DOUBLE_EQ( + occupancyGrid.info.origin.orientation.x, + occupancyGridResult.info.origin.orientation.x); + EXPECT_DOUBLE_EQ( + occupancyGrid.info.origin.orientation.y, + occupancyGridResult.info.origin.orientation.y); + EXPECT_DOUBLE_EQ( + occupancyGrid.info.origin.orientation.z, + occupancyGridResult.info.origin.orientation.z); + EXPECT_DOUBLE_EQ( + occupancyGrid.info.origin.orientation.w, + occupancyGridResult.info.origin.orientation.w); // Check map data. for (std::vector::iterator iterator = occupancyGrid.data.begin(); - iterator != occupancyGrid.data.end(); ++iterator) { + iterator != occupancyGrid.data.end(); ++iterator) + { size_t i = std::distance(occupancyGrid.data.begin(), iterator); - EXPECT_EQ((int)*iterator, (int)occupancyGridResult.data[i]); + EXPECT_EQ(static_cast(*iterator), static_cast(occupancyGridResult.data[i])); + } +} + +TEST(CostmapConversion, withMove) +{ + grid_map::GridMap map; + map.setGeometry(grid_map::Length(8.0, 5.0), 0.5, grid_map::Position(0.0, 0.0)); + map.add("layer", 1.0); + + // Convert to Costmap msg. + nav2_msgs::msg::Costmap costmap; + GridMapRosConverter::toCostmap(map, "layer", 0.0, 1.0, costmap); + + // Expect the (0, 0) cell to have value 254. + EXPECT_EQ(254, costmap.data[0]); + + // Move the map, so the cell (0, 0) will move to unobserved space. + map.move(grid_map::Position(-1.0, -1.0)); + + // Convert again to Costmap msg. + nav2_msgs::msg::Costmap costmapNew; + GridMapRosConverter::toCostmap(map, "layer", 0.0, 1.0, costmapNew); + + // Now the (0, 0) cell should be unobserved (255). + EXPECT_EQ(255, costmapNew.data[0]); +} + +TEST(CostmapConversion, roundTrip) +{ + // Create Costmap grid. + nav2_msgs::msg::Costmap costmap; + costmap.header.stamp = rclcpp::Time(5.0); + costmap.header.frame_id = "map"; + costmap.metadata.resolution = 0.1; + costmap.metadata.size_x = 50; + costmap.metadata.size_y = 100; + costmap.metadata.origin.position.x = 3.0; + costmap.metadata.origin.position.y = 6.0; + costmap.metadata.origin.orientation.w = 1.0; + costmap.data.resize(costmap.metadata.size_x * costmap.metadata.size_y); + + unsigned int seed = time(0); + for (auto & cell : costmap.data) { + cell = rand_r(&seed) % 256; // [0, 255] + } + + // Convert to grid map. + GridMap gridMap; + GridMapRosConverter::fromCostmap(costmap, "layer", gridMap); + + // Convert back to costmap. + nav2_msgs::msg::Costmap costmapResult; + GridMapRosConverter::toCostmap(gridMap, "layer", 0, 254.0, costmapResult); + + // Check map info. + EXPECT_EQ(costmap.header.stamp, costmapResult.header.stamp); + EXPECT_EQ(costmap.header.frame_id, costmapResult.header.frame_id); + EXPECT_EQ(costmap.metadata.size_x, costmapResult.metadata.size_x); + EXPECT_EQ(costmap.metadata.size_y, costmapResult.metadata.size_y); + EXPECT_DOUBLE_EQ( + costmap.metadata.origin.position.x, + costmapResult.metadata.origin.position.x); + EXPECT_DOUBLE_EQ( + costmap.metadata.origin.position.x, + costmapResult.metadata.origin.position.x); + EXPECT_DOUBLE_EQ( + costmap.metadata.origin.orientation.x, + costmapResult.metadata.origin.orientation.x); + EXPECT_DOUBLE_EQ( + costmap.metadata.origin.orientation.y, + costmapResult.metadata.origin.orientation.y); + EXPECT_DOUBLE_EQ( + costmap.metadata.origin.orientation.z, + costmapResult.metadata.origin.orientation.z); + EXPECT_DOUBLE_EQ( + costmap.metadata.origin.orientation.w, + costmapResult.metadata.origin.orientation.w); + + // Check map data. + for (std::vector::iterator iterator = costmap.data.begin(); + iterator != costmap.data.end(); ++iterator) + { + size_t i = std::distance(costmap.data.begin(), iterator); + EXPECT_EQ(*iterator, costmapResult.data[i]); } } @@ -193,9 +340,10 @@ TEST(ImageConversion, roundTripBGRA8) const float maxValue = 1.0; // Convert to image message. - sensor_msgs::Image image; - GridMapRosConverter::toImage(mapIn, "layer", sensor_msgs::image_encodings::BGRA8, minValue, - maxValue, image); + sensor_msgs::msg::Image image; + GridMapRosConverter::toImage( + mapIn, "layer", sensor_msgs::image_encodings::BGRA8, minValue, + maxValue, image); // Convert back to grid map. GridMap mapOut; @@ -203,7 +351,8 @@ TEST(ImageConversion, roundTripBGRA8) GridMapRosConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue); // Check data. - const float resolution = (maxValue - minValue) / (float) std::numeric_limits::max(); + const float resolution = (maxValue - minValue) / + static_cast(std::numeric_limits::max()); expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); EXPECT_TRUE((mapIn.getSize() == mapOut.getSize()).all()); @@ -219,9 +368,10 @@ TEST(ImageConversion, roundTripMONO16) const float maxValue = 1.0; // Convert to image message. - sensor_msgs::Image image; - GridMapRosConverter::toImage(mapIn, "layer", sensor_msgs::image_encodings::MONO16, - minValue, maxValue, image); + sensor_msgs::msg::Image image; + GridMapRosConverter::toImage( + mapIn, "layer", sensor_msgs::image_encodings::MONO16, + minValue, maxValue, image); // Convert back to grid map. GridMap mapOut; @@ -229,8 +379,9 @@ TEST(ImageConversion, roundTripMONO16) GridMapRosConverter::addLayerFromImage(image, "layer", mapOut, minValue, maxValue); // Check data. - // TODO Why is factor 300 necessary? - const float resolution = 300.0 * (maxValue - minValue) / (float) std::numeric_limits::max(); + // TODO(needs_assignment) Why is factor 300 necessary? + const float resolution = 300.0 * (maxValue - minValue) / + static_cast(std::numeric_limits::max()); expectNear(mapIn["layer"], mapOut["layer"], resolution, ""); EXPECT_EQ(mapIn.getTimestamp(), mapOut.getTimestamp()); EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all()); diff --git a/grid_map_ros/test/test_grid_map_ros.cpp b/grid_map_ros/test/test_grid_map_ros.cpp index 862017d3b..5e08f2233 100644 --- a/grid_map_ros/test/test_grid_map_ros.cpp +++ b/grid_map_ros/test/test_grid_map_ros.cpp @@ -10,9 +10,9 @@ #include // Run all the tests that were declared with TEST() -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - srand((int)time(0)); + srand(static_cast(time(0))); return RUN_ALL_TESTS(); } diff --git a/grid_map_rviz_plugin/CHANGELOG.rst b/grid_map_rviz_plugin/CHANGELOG.rst index 1c69b3af8..7b404fa67 100644 --- a/grid_map_rviz_plugin/CHANGELOG.rst +++ b/grid_map_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package grid_map_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Merge pull request `#422 `_ from jtaveau/feature/deprecation_ogre_vector3_h + update deprecated ogre header file +* update header file +* Contributors: JTaveau, Ryan, Ryan Friedman + +2.1.0 (2022-11-08) +------------------ + +2.0.0 (2022-09-13) +------------------ +* fix: mark Eigen library as SYSTEM +* fix: build error on jammy +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map_rviz_plugin/CMakeLists.txt b/grid_map_rviz_plugin/CMakeLists.txt index c1bb22010..92bb25b57 100644 --- a/grid_map_rviz_plugin/CMakeLists.txt +++ b/grid_map_rviz_plugin/CMakeLists.txt @@ -1,62 +1,40 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(grid_map_rviz_plugin) -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") - -find_package(catkin REQUIRED - COMPONENTS - rviz - grid_map_ros - grid_map_msgs -) - -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - rviz - grid_map_ros - grid_map_msgs +find_package(ament_cmake REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(Qt5 REQUIRED COMPONENTS Core Widgets) +find_package(rclcpp REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) +find_package(rviz_rendering REQUIRED COMPONENTS Core Widgets) + +grid_map_package() + +## Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html +set(CMAKE_INCLUDE_CURRENT_DIR ON) +set(CMAKE_AUTOMOC ON) # This setting causes Qt's "MOC" generation to happen automatically. + +set(dependencies + grid_map_msgs + grid_map_ros + rclcpp + rviz_common + rviz_ogre_vendor + rviz_rendering ) include_directories( include - ${catkin_INCLUDE_DIRS} ) -link_directories(${catkin_LIBRARY_DIRS}) - -## This setting causes Qt's "MOC" generation to happen automatically. -set(CMAKE_AUTOMOC ON) - set(INCLUDE_FILES_QT include/grid_map_rviz_plugin/GridMapDisplay.hpp ) -set(INCLUDE_FILES - include/grid_map_rviz_plugin/GridMapVisual.hpp -) -## This plugin includes Qt widgets, so we must include Qt. -## We'll use the version that rviz used so they are compatible. -if(rviz_QT_VERSION VERSION_LESS "5") - message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) - ## pull in all required include dirs, define QT_LIBRARIES, etc. - include(${QT_USE_FILE}) - qt4_wrap_cpp(MOC_FILES - ${INCLUDE_FILES_QT} - ) -else() - message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) - ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies - set(QT_LIBRARIES Qt5::Widgets) - QT5_WRAP_CPP(MOC_FILES - ${INCLUDE_FILES_QT} - ) -endif() +qt5_wrap_cpp(MOC_FILES ${INCLUDE_FILES_QT}) ## Avoid Qt signals and slots defining "emit", "slots", etc. add_definitions(-DQT_NO_KEYWORDS) @@ -71,44 +49,96 @@ set(SOURCE_FILES ## An rviz plugin is just a shared library, so here we declare the ## library to be called ${PROJECT_NAME} and specify the list of ## source files we collected above in ${SOURCE_FILES}. -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${MOC_FILES} ) -## Link the executable with whatever Qt libraries have been defined by -## the find_package(Qt4 ...) line above, or by the -## set(QT_LIBRARIES Qt5::Widgets), and with whatever libraries -## catkin has included. -target_link_libraries(${PROJECT_NAME} - ${QT_LIBRARIES} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} SYSTEM + ${dependencies} ) -## Install rules +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") + +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(rviz_common plugin_description.xml) + +## Avoid Qt signals and slots defining "emit", "slots", etc. +add_definitions(-DQT_NO_KEYWORDS) + +# Install rules install( - DIRECTORY include/${PROJECT_NAME} - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DIRECTORY include/ + DESTINATION include ) install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) install( FILES plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) install( DIRECTORY icons - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons + DESTINATION share/${PROJECT_NAME}/icons ) install( DIRECTORY doc - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) + +############# +## Testing ## +############# + +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright -build/include_order + ) + endif() +endif() + + +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + Qt5 + rviz_common + geometry_msgs + map_msgs + nav_msgs + rclcpp +) + +ament_package() diff --git a/grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapDisplay.hpp b/grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapDisplay.hpp index 986ae27bf..8fbc27fbf 100644 --- a/grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapDisplay.hpp +++ b/grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapDisplay.hpp @@ -6,44 +6,58 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_RVIZ_PLUGIN__GRIDMAPDISPLAY_HPP_ +#define GRID_MAP_RVIZ_PLUGIN__GRIDMAPDISPLAY_HPP_ -#ifndef Q_MOC_RUN #include -#include +#include + +#include +#include +#include +#include +#include +#include +#include + #include -#include "grid_map_rviz_plugin/modified/message_filter_display.h" -#endif -namespace Ogre { +namespace Ogre +{ class SceneNode; } -namespace rviz { +namespace rviz_common +{ +namespace properties +{ class BoolProperty; class ColorProperty; class FloatProperty; class IntProperty; class EnumProperty; class EditableEnumProperty; -} +} // namespace properties +} // namespace rviz_common -namespace grid_map_rviz_plugin { +namespace grid_map_rviz_plugin +{ class GridMapVisual; -class GridMapDisplay : public MessageFilterDisplayMod +class GridMapDisplay : public rviz_common::MessageFilterDisplay { -Q_OBJECT - public: + Q_OBJECT + +public: GridMapDisplay(); virtual ~GridMapDisplay(); - protected: - virtual void onInitialize(); +protected: + void onInitialize() override; - virtual void reset(); + void reset() override; - private Q_SLOTS: +private Q_SLOTS: void updateHistoryLength(); void updateHeightMode(); void updateColorMode(); @@ -51,29 +65,30 @@ Q_OBJECT void updateAutocomputeIntensityBounds(); void updateVisualization(); - private: +private: // Callback for incoming ROS messages - void processMessage(const grid_map_msgs::GridMap::ConstPtr& msg); + void processMessage(grid_map_msgs::msg::GridMap::ConstSharedPtr msg) override; // Circular buffer for visuals - boost::circular_buffer > visuals_; + boost::circular_buffer> visuals_; // Property variables - rviz::FloatProperty* alphaProperty_; - rviz::IntProperty* historyLengthProperty_; - rviz::BoolProperty* showGridLinesProperty_; - rviz::EnumProperty* heightModeProperty_; - rviz::EditableEnumProperty* heightTransformerProperty_; - rviz::EnumProperty* colorModeProperty_; - rviz::EditableEnumProperty* colorTransformerProperty_; - rviz::ColorProperty* colorProperty_; - rviz::BoolProperty* useRainbowProperty_; - rviz::BoolProperty* invertRainbowProperty_; - rviz::ColorProperty* minColorProperty_; - rviz::ColorProperty* maxColorProperty_; - rviz::BoolProperty* autocomputeIntensityBoundsProperty_; - rviz::FloatProperty* minIntensityProperty_; - rviz::FloatProperty* maxIntensityProperty_; + rviz_common::properties::FloatProperty * alphaProperty_; + rviz_common::properties::IntProperty * historyLengthProperty_; + rviz_common::properties::BoolProperty * showGridLinesProperty_; + rviz_common::properties::EnumProperty * heightModeProperty_; + rviz_common::properties::EditableEnumProperty * heightTransformerProperty_; + rviz_common::properties::EnumProperty * colorModeProperty_; + rviz_common::properties::EditableEnumProperty * colorTransformerProperty_; + rviz_common::properties::ColorProperty * colorProperty_; + rviz_common::properties::BoolProperty * useRainbowProperty_; + rviz_common::properties::BoolProperty * invertRainbowProperty_; + rviz_common::properties::ColorProperty * minColorProperty_; + rviz_common::properties::ColorProperty * maxColorProperty_; + rviz_common::properties::BoolProperty * autocomputeIntensityBoundsProperty_; + rviz_common::properties::FloatProperty * minIntensityProperty_; + rviz_common::properties::FloatProperty * maxIntensityProperty_; }; -} // end namespace grid_map_rviz_plugin +} // namespace grid_map_rviz_plugin +#endif // GRID_MAP_RVIZ_PLUGIN__GRIDMAPDISPLAY_HPP_ diff --git a/grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapVisual.hpp b/grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapVisual.hpp index 0c0294efb..680f91086 100644 --- a/grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapVisual.hpp +++ b/grid_map_rviz_plugin/include/grid_map_rviz_plugin/GridMapVisual.hpp @@ -6,70 +6,78 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_RVIZ_PLUGIN__GRIDMAPVISUAL_HPP_ +#define GRID_MAP_RVIZ_PLUGIN__GRIDMAPVISUAL_HPP_ -#include -#include +#ifndef Q_MOC_RUN + +#include +#include + +#endif // Q_MOC_RUN -#include #include +#include +#include -namespace Ogre { -class Vector3; -class Quaternion; -class ManualObject; -class ColourValue; -} +#include +#include +#include -namespace rviz { +namespace rviz_rendering +{ class BillboardLine; } -namespace grid_map_rviz_plugin { +namespace grid_map_rviz_plugin +{ -// Visualizes a single grid_map_msgs::GridMap message. +// Visualizes a single grid_map_msgs::msg::GridMap message. class GridMapVisual { - public: - GridMapVisual(Ogre::SceneManager* sceneManager, Ogre::SceneNode* parentNode); +public: + GridMapVisual(Ogre::SceneManager * sceneManager, Ogre::SceneNode * parentNode); virtual ~GridMapVisual(); // Copy the grid map data to map_. - void setMessage(const grid_map_msgs::GridMap::ConstPtr& msg); + void setMessage(grid_map_msgs::msg::GridMap::ConstSharedPtr msg); // Compute the visualization of map_. - void computeVisualization(float alpha, bool showGridLines, bool flatTerrain, std::string heightLayer, bool flatColor, - bool noColor, Ogre::ColourValue meshColor, bool mapLayerColor, std::string colorLayer, - bool useRainbow, bool invertRainbow, Ogre::ColourValue minColor, Ogre::ColourValue maxColor, - bool autocomputeIntensity, float minIntensity, float maxIntensity); + void computeVisualization( + float alpha, bool showGridLines, bool flatTerrain, std::string heightLayer, bool flatColor, + bool noColor, Ogre::ColourValue meshColor, bool mapLayerColor, std::string colorLayer, + bool useRainbow, bool invertRainbow, Ogre::ColourValue minColor, Ogre::ColourValue maxColor, + bool autocomputeIntensity, float minIntensity, float maxIntensity); // Set the coordinate frame pose. - void setFramePosition(const Ogre::Vector3& position); - void setFrameOrientation(const Ogre::Quaternion& orientation); + void setFramePosition(const Ogre::Vector3 & position); + void setFrameOrientation(const Ogre::Quaternion & orientation); // Get grid map layer names. std::vector getLayerNames(); - private: - Ogre::SceneNode* frameNode_; - Ogre::SceneManager* sceneManager_; +private: + Ogre::SceneNode * frameNode_; + Ogre::SceneManager * sceneManager_; // ManualObject for mesh display. - Ogre::ManualObject* manualObject_; + Ogre::ManualObject * manualObject_; Ogre::MaterialPtr material_; std::string materialName_; // Lines for mesh. - boost::shared_ptr meshLines_; + boost::shared_ptr meshLines_; // Grid map. grid_map::GridMap map_; bool haveMap_; // Helper methods. - void normalizeIntensity(float& intensity, float minIntensity, float maxIntensity); + void normalizeIntensity(float & intensity, float minIntensity, float maxIntensity); Ogre::ColourValue getRainbowColor(float intensity); - Ogre::ColourValue getInterpolatedColor(float intensity, Ogre::ColourValue minColor, - Ogre::ColourValue maxColor); + Ogre::ColourValue getInterpolatedColor( + float intensity, Ogre::ColourValue minColor, + Ogre::ColourValue maxColor); }; -} // namespace +} // namespace grid_map_rviz_plugin +#endif // GRID_MAP_RVIZ_PLUGIN__GRIDMAPVISUAL_HPP_ diff --git a/grid_map_rviz_plugin/include/grid_map_rviz_plugin/modified/frame_manager.h b/grid_map_rviz_plugin/include/grid_map_rviz_plugin/modified/frame_manager.h deleted file mode 100644 index 0e338f02a..000000000 --- a/grid_map_rviz_plugin/include/grid_map_rviz_plugin/modified/frame_manager.h +++ /dev/null @@ -1,333 +0,0 @@ -/* - * Copyright (c) 2009, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef RVIZ_FRAME_MANAGER_H -#define RVIZ_FRAME_MANAGER_H - -#include - -#include - -#include - -#include -#include - -#include - -#include - -#include -#include - -#include - -#ifndef Q_MOC_RUN -#endif - -#if ROS_VERSION_MINIMUM(1,14,0) -namespace tf2_ros -{ -class TransformListener; -} -#else -namespace tf -{ -class TransformListener; -} -#endif - -namespace rviz -{ -class Display; - -/** @brief Helper class for transforming data into Ogre's world frame (the fixed frame). - * - * During one frame update (nominally 33ms), the tf tree stays consistent and queries are cached for speedup. - */ -class FrameManager: public QObject -{ -Q_OBJECT -public: - - using TransformListener = std::conditional::type; - using FilterFailureReason = std::conditional::type; -#if ROS_VERSION_MINIMUM(1,14,0) - template - using MessageFilter = tf2_ros::MessageFilter; -#else - template - using MessageFilter = tf::MessageFilter; -#endif - - enum SyncMode { - SyncOff = 0, - SyncExact, - SyncApprox - }; - - /** @brief Constructor - * @param tf a pointer to tf::TransformListener (should not be used anywhere else because of thread safety) - */ - FrameManager(boost::shared_ptr tf = boost::shared_ptr()); - - /** @brief Destructor. - * - * FrameManager should not need to be destroyed by hand, just - * destroy the boost::shared_ptr returned by instance(), and it will - * be deleted when the last reference is removed. */ - ~FrameManager(); - - /** @brief Set the frame to consider "fixed", into which incoming data is transformed. - * - * The fixed frame serves as the reference for all getTransform() - * and transform() functions in FrameManager. */ - void setFixedFrame(const std::string& frame); - - /** @brief Enable/disable pause mode */ - void setPause( bool pause ); - - bool getPause() { return pause_; } - - /** @brief Set synchronization mode (off/exact/approximate) */ - void setSyncMode( SyncMode mode ); - - SyncMode getSyncMode() { return sync_mode_; } - - /** @brief Synchronize with given time. */ - void syncTime( ros::Time time ); - - /** @brief Get current time, depending on the sync mode. */ - ros::Time getTime() { return sync_time_; } - - /** @brief Return the pose for a header, relative to the fixed frame, in Ogre classes. - * @param[in] header The source of the frame name and time. - * @param[out] position The position of the header frame relative to the fixed frame. - * @param[out] orientation The orientation of the header frame relative to the fixed frame. - * @return true on success, false on failure. */ - template - bool getTransform(const Header& header, Ogre::Vector3& position, Ogre::Quaternion& orientation) - { - return getTransform(header.frame_id, header.stamp, position, orientation); - } - - /** @brief Return the pose for a frame relative to the fixed frame, in Ogre classes, at a given time. - * @param[in] frame The frame to find the pose of. - * @param[in] time The time at which to get the pose. - * @param[out] position The position of the frame relative to the fixed frame. - * @param[out] orientation The orientation of the frame relative to the fixed frame. - * @return true on success, false on failure. */ - bool getTransform(const std::string& frame, ros::Time time, Ogre::Vector3& position, Ogre::Quaternion& orientation); - - /** @brief Transform a pose from a frame into the fixed frame. - * @param[in] header The source of the input frame and time. - * @param[in] pose The input pose, relative to the header frame. - * @param[out] position Position part of pose relative to the fixed frame. - * @param[out] orientation: Orientation part of pose relative to the fixed frame. - * @return true on success, false on failure. */ - template - bool transform(const Header& header, const geometry_msgs::Pose& pose, Ogre::Vector3& position, Ogre::Quaternion& orientation) - { - return transform(header.frame_id, header.stamp, pose, position, orientation); - } - - /** @brief Transform a pose from a frame into the fixed frame. - * @param[in] frame The input frame. - * @param[in] time The time at which to get the pose. - * @param[in] pose The input pose, relative to the input frame. - * @param[out] position Position part of pose relative to the fixed frame. - * @param[out] orientation: Orientation part of pose relative to the fixed frame. - * @return true on success, false on failure. */ - bool transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose, Ogre::Vector3& position, Ogre::Quaternion& orientation); - - /** @brief Clear the internal cache. */ - void update(); - - /** @brief Check to see if a frame exists in the TransformListener. - * @param[in] frame The name of the frame to check. - * @param[in] time Dummy parameter, not actually used. - * @param[out] error If the frame does not exist, an error message is stored here. - * @return true if the frame does not exist, false if it does exist. */ - bool frameHasProblems(const std::string& frame, ros::Time time, std::string& error); - - /** @brief Check to see if a transform is known between a given frame and the fixed frame. - * @param[in] frame The name of the frame to check. - * @param[in] time The time at which the transform is desired. - * @param[out] error If the transform is not known, an error message is stored here. - * @return true if the transform is not known, false if it is. */ - bool transformHasProblems(const std::string& frame, ros::Time time, std::string& error); - - /** @brief Connect a MessageFilter's callbacks to success and failure handler functions in this FrameManager. - * @param filter The MessageFilter to connect to. - * @param display The Display using the filter. - * - * FrameManager has internal functions for handling success and - * failure of MessageFilters which call Display::setStatus() - * based on success or failure of the filter, including appropriate - * error messages. */ - template - void registerFilterForTransformStatusCheck(MessageFilter* filter, Display* display) - { - filter->registerCallback(boost::bind(&FrameManager::messageCallback, this, _1, display)); - filter->registerFailureCallback(boost::bind(&FrameManager::failureCallback, this, _1, _2, display)); - } - - /** @brief Return the current fixed frame name. */ - const std::string& getFixedFrame() { return fixed_frame_; } - - /** @brief Return the TransformListener used to receive transform data. */ - TransformListener* getTFClient() { return tf_.get(); } - - /** @brief Return a boost shared pointer to the TransformListener used to receive transform data. */ - const boost::shared_ptr& getTFClientPtr() { return tf_; } - - /** @brief Create a description of a transform problem. - * @param frame_id The name of the frame with issues. - * @param stamp The time for which the problem was detected. - * @param caller_id Dummy parameter, not used. - * @param reason The reason given by the MessageFilter in its failure callback. - * @return An error message describing the problem. - * - * Once a problem has been detected with a given frame or transform, - * call this to get an error message describing the problem. */ - std::string discoverFailureReason(const std::string& frame_id, - const ros::Time& stamp, - const std::string& caller_id, - FilterFailureReason reason); - -Q_SIGNALS: - /** @brief Emitted whenever the fixed frame changes. */ - void fixedFrameChanged(); - -private: - - bool adjustTime( const std::string &frame, ros::Time &time ); - - template - void messageCallback(const ros::MessageEvent& msg_evt, Display* display) - { - boost::shared_ptr const &msg = msg_evt.getConstMessage(); - std::string authority = msg_evt.getPublisherName(); - - messageArrived(msg->info.header.frame_id, msg->info.header.stamp, authority, display); - } - - template - void failureCallback(const ros::MessageEvent& msg_evt, FilterFailureReason reason, Display* display) - { - boost::shared_ptr const &msg = msg_evt.getConstMessage(); - std::string authority = msg_evt.getPublisherName(); - - messageFailed(msg->info.header.frame_id, msg->info.header.stamp, authority, reason, display); - } - - void messageArrived(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, Display* display); - - void messageFailedImpl( - const std::string& caller_id, - const std::string& status_text, - Display* display); - - template - void messageFailed( - const std::string& frame_id, - const ros::Time& stamp, - const std::string& caller_id, - TfFilterFailureReasonType reason, - Display* display) - { - // TODO(wjwwood): remove this when only Tf2 is supported -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#endif - - std::string status_text = discoverFailureReason( frame_id, stamp, caller_id, reason ); - -#ifndef _WIN32 -# pragma GCC diagnostic pop -#endif - - messageFailedImpl(caller_id, status_text, display); - } - - struct CacheKey - { - CacheKey(const std::string& f, ros::Time t) - : frame(f) - , time(t) - {} - - bool operator<(const CacheKey& rhs) const - { - if (frame != rhs.frame) - { - return frame < rhs.frame; - } - - return time < rhs.time; - } - - std::string frame; - ros::Time time; - }; - - struct CacheEntry - { - CacheEntry(const Ogre::Vector3& p, const Ogre::Quaternion& o) - : position(p) - , orientation(o) - {} - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - }; - typedef std::map M_Cache; - - boost::mutex cache_mutex_; - M_Cache cache_; - - boost::shared_ptr tf_; - std::string fixed_frame_; - - bool pause_; - - SyncMode sync_mode_; - - // the current synchronized time, used to overwrite ros:Time(0) - ros::Time sync_time_; - - // used for approx. syncing - double sync_delta_; - double current_delta_; -}; - -} - -#endif // RVIZ_FRAME_MANAGER_H diff --git a/grid_map_rviz_plugin/include/grid_map_rviz_plugin/modified/message_filter_display.h b/grid_map_rviz_plugin/include/grid_map_rviz_plugin/modified/message_filter_display.h deleted file mode 100644 index 882cb230c..000000000 --- a/grid_map_rviz_plugin/include/grid_map_rviz_plugin/modified/message_filter_display.h +++ /dev/null @@ -1,45 +0,0 @@ -/* - * message_filter_display.h - * - * Created on: Jan 24, 2016 - * Author: Péter Fankhauser - * Institute: ETH Zurich - */ - -#pragma once - -#ifndef Q_MOC_RUN -#include "grid_map_rviz_plugin/modified/frame_manager.h" -#include -#endif - -namespace grid_map_rviz_plugin { - -template -class MessageFilterDisplayMod : public rviz::MessageFilterDisplay -{ - public: - typedef MessageFilterDisplayMod MFDClass; - - void onInitialize() - { - #if ROS_VERSION_MINIMUM(1,14,0) - MFDClass::tf_filter_ = new tf2_ros::MessageFilter(*MFDClass::context_->getTF2BufferPtr(), - MFDClass::fixed_frame_.toStdString(), - 10, MFDClass::update_nh_); - #else - MFDClass::tf_filter_ = new tf::MessageFilter(*MFDClass::context_->getTFClient(), - MFDClass::fixed_frame_.toStdString(), - 10, MFDClass::update_nh_); - #endif - - MFDClass::tf_filter_->connectInput(MFDClass::sub_); - MFDClass::tf_filter_->registerCallback( - boost::bind(&MFDClass::incomingMessage, this, _1)); - MFDClass::context_->getFrameManager()->registerFilterForTransformStatusCheck( - MFDClass::tf_filter_, this); - } - -}; - -} // end namespace diff --git a/grid_map_rviz_plugin/package.xml b/grid_map_rviz_plugin/package.xml index da50f1ba6..e4bc7d358 100644 --- a/grid_map_rviz_plugin/package.xml +++ b/grid_map_rviz_plugin/package.xml @@ -1,26 +1,37 @@ - + + grid_map_rviz_plugin - 1.6.2 + 2.2.2 RViz plugin for displaying grid map messages. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD Philipp Krüsi Péter Fankhauser - catkin + ament_cmake + + grid_map_cmake_helpers qtbase5-dev - rviz - grid_map_ros - grid_map_msgs - libqt5-core - libqt5-gui - libqt5-widgets - rviz - grid_map_ros - grid_map_msgs + + grid_map_msgs + grid_map_ros + rclcpp + rviz_common + rviz_ogre_vendor + rviz_rendering + + libqt5-core + libqt5-gui + libqt5-widgets + + ament_lint_auto + ament_lint_common + - + ament_cmake + diff --git a/grid_map_rviz_plugin/plugin_description.xml b/grid_map_rviz_plugin/plugin_description.xml index efc4e587a..7fe6c5033 100644 --- a/grid_map_rviz_plugin/plugin_description.xml +++ b/grid_map_rviz_plugin/plugin_description.xml @@ -1,8 +1,8 @@ - + + base_class_type="rviz_common::Display"> Displays grid_map_msgs/GridMap messages. - grid_map_msgs/GridMap + diff --git a/grid_map_rviz_plugin/src/GridMapDisplay.cpp b/grid_map_rviz_plugin/src/GridMapDisplay.cpp index d637c222d..4205d19d1 100644 --- a/grid_map_rviz_plugin/src/GridMapDisplay.cpp +++ b/grid_map_rviz_plugin/src/GridMapDisplay.cpp @@ -6,103 +6,113 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_rviz_plugin/GridMapVisual.hpp" -#include "grid_map_rviz_plugin/GridMapDisplay.hpp" -#include "grid_map_rviz_plugin/modified/frame_manager.h" +#include +#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include -#include -#include -#include -#include -#include -#include +#include +#include -namespace grid_map_rviz_plugin { +#include "grid_map_rviz_plugin/GridMapVisual.hpp" +#include "grid_map_rviz_plugin/GridMapDisplay.hpp" -GridMapDisplay::GridMapDisplay() +namespace grid_map_rviz_plugin { - alphaProperty_ = new rviz::FloatProperty("Alpha", 1.0, - "0 is fully transparent, 1.0 is fully opaque.", this, - SLOT(updateVisualization())); - - historyLengthProperty_ = new rviz::IntProperty("History Length", 1, - "Number of prior grid maps to display.", this, - SLOT(updateHistoryLength())); - showGridLinesProperty_ = new rviz::BoolProperty( - "Show Grid Lines", true, "Whether to show the lines connecting the grid cells.", this, - SLOT(updateVisualization())); - - heightModeProperty_ = new rviz::EnumProperty("Height Transformer", "GridMapLayer", - "Select the transformer to use to set the height.", - this, SLOT(updateHeightMode())); +GridMapDisplay::GridMapDisplay() +{ + alphaProperty_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, + "0 is fully transparent, 1.0 is fully opaque.", this, + SLOT(updateVisualization())); + + historyLengthProperty_ = new rviz_common::properties::IntProperty( + "History Length", 1, + "Number of prior grid maps to display.", this, + SLOT(updateHistoryLength())); + + showGridLinesProperty_ = new rviz_common::properties::BoolProperty( + "Show Grid Lines", true, "Whether to show the lines connecting the grid cells.", this, + SLOT(updateVisualization())); + + heightModeProperty_ = new rviz_common::properties::EnumProperty( + "Height Transformer", "GridMapLayer", + "Select the transformer to use to set the height.", + this, SLOT(updateHeightMode())); heightModeProperty_->addOption("Layer", 0); heightModeProperty_->addOption("Flat", 1); - heightTransformerProperty_ = new rviz::EditableEnumProperty( - "Height Layer", "elevation", "Select the grid map layer to compute the height.", this, - SLOT(updateVisualization())); + heightTransformerProperty_ = new rviz_common::properties::EditableEnumProperty( + "Height Layer", "elevation", "Select the grid map layer to compute the height.", this, + SLOT(updateVisualization())); - colorModeProperty_ = new rviz::EnumProperty("Color Transformer", "GridMapLayer", - "Select the transformer to use to set the color.", - this, SLOT(updateColorMode())); + colorModeProperty_ = new rviz_common::properties::EnumProperty( + "Color Transformer", "GridMapLayer", + "Select the transformer to use to set the color.", + this, SLOT(updateColorMode())); colorModeProperty_->addOption("IntensityLayer", 0); colorModeProperty_->addOption("ColorLayer", 1); colorModeProperty_->addOption("FlatColor", 2); colorModeProperty_->addOption("None", 3); - colorTransformerProperty_ = new rviz::EditableEnumProperty( - "Color Layer", "elevation", "Select the grid map layer to compute the color.", this, - SLOT(updateVisualization())); + colorTransformerProperty_ = new rviz_common::properties::EditableEnumProperty( + "Color Layer", "elevation", "Select the grid map layer to compute the color.", this, + SLOT(updateVisualization())); - colorProperty_ = new rviz::ColorProperty("Color", QColor(200, 200, 200), - "Color to draw the mesh.", this, - SLOT(updateVisualization())); + colorProperty_ = new rviz_common::properties::ColorProperty( + "Color", QColor(200, 200, 200), + "Color to draw the mesh.", this, + SLOT(updateVisualization())); colorProperty_->hide(); - useRainbowProperty_ = new rviz::BoolProperty( - "Use Rainbow", true, - "Whether to use a rainbow of colors or to interpolate between two colors.", this, - SLOT(updateUseRainbow())); - - invertRainbowProperty_ = new rviz::BoolProperty( - "Invert Rainbow", false, - "Whether to invert the rainbow colors.", this, - SLOT(updateVisualization())); - - minColorProperty_ = new rviz::ColorProperty( - "Min Color", QColor(0, 0, 0), "Color to assign to cells with the minimum intensity. " - "Actual color is interpolated between this and Max Color.", - this, SLOT(updateVisualization())); + useRainbowProperty_ = new rviz_common::properties::BoolProperty( + "Use Rainbow", true, + "Whether to use a rainbow of colors or to interpolate between two colors.", this, + SLOT(updateUseRainbow())); + + invertRainbowProperty_ = new rviz_common::properties::BoolProperty( + "Invert Rainbow", false, + "Whether to invert the rainbow colors.", this, + SLOT(updateVisualization())); + + minColorProperty_ = new rviz_common::properties::ColorProperty( + "Min Color", QColor(0, 0, 0), "Color to assign to cells with the minimum intensity. " + "Actual color is interpolated between this and Max Color.", + this, SLOT(updateVisualization())); minColorProperty_->hide(); - maxColorProperty_ = new rviz::ColorProperty( - "Max Color", QColor(255, 255, 255), "Color to assign to cells with the maximum intensity. " - "Actual color is interpolated between Min Color and this.", - this, SLOT(updateVisualization())); + maxColorProperty_ = new rviz_common::properties::ColorProperty( + "Max Color", QColor(255, 255, 255), "Color to assign to cells with the maximum intensity. " + "Actual color is interpolated between Min Color and this.", + this, SLOT(updateVisualization())); maxColorProperty_->hide(); - autocomputeIntensityBoundsProperty_ = new BoolProperty( - "Autocompute Intensity Bounds", true, - "Whether to automatically compute the intensity min/max values.", this, - SLOT(updateAutocomputeIntensityBounds())); + autocomputeIntensityBoundsProperty_ = new rviz_common::properties::BoolProperty( + "Autocompute Intensity Bounds", true, + "Whether to automatically compute the intensity min/max values.", this, + SLOT(updateAutocomputeIntensityBounds())); - minIntensityProperty_ = new rviz::FloatProperty( - "Min Intensity", 0.0, - "Minimum possible intensity value, used to interpolate from Min Color to Max Color.", this, - SLOT(updateVisualization())); + minIntensityProperty_ = new rviz_common::properties::FloatProperty( + "Min Intensity", 0.0, + "Minimum possible intensity value, used to interpolate from Min Color to Max Color.", this, + SLOT(updateVisualization())); minIntensityProperty_->hide(); - maxIntensityProperty_ = new rviz::FloatProperty( - "Max Intensity", 10.0, - "Maximum possible intensity value, used to interpolate from Min Color to Max Color.", this, - SLOT(updateVisualization())); + maxIntensityProperty_ = new rviz_common::properties::FloatProperty( + "Max Intensity", 10.0, + "Maximum possible intensity value, used to interpolate from Min Color to Max Color.", this, + SLOT(updateVisualization())); maxIntensityProperty_->hide(); historyLengthProperty_->setMin(1); @@ -115,7 +125,8 @@ GridMapDisplay::~GridMapDisplay() void GridMapDisplay::onInitialize() { - MFDClass::onInitialize(); // "MFDClass" = typedef of "MessageFilterDisplay" + MFDClass::onInitialize(); // "MFDClass" = typedef of "MessageFilterDisplay" + MFDClass::subscribe(); updateHistoryLength(); } @@ -139,7 +150,7 @@ void GridMapDisplay::updateHeightMode() void GridMapDisplay::updateColorMode() { updateVisualization(); - + bool intensityColor = colorModeProperty_->getOptionInt() == 0; bool flatColor = colorModeProperty_->getOptionInt() == 2; bool none = colorModeProperty_->getOptionInt() == 3; @@ -192,21 +203,26 @@ void GridMapDisplay::updateVisualization() float maxIntensity = maxIntensityProperty_->getFloat(); for (size_t i = 0; i < visuals_.size(); i++) { - visuals_[i]->computeVisualization(alpha, showGridLines, flatTerrain, heightLayer, flatColor, noColor, meshColor, - mapLayerColor, colorLayer, useRainbow, invertRainbow, minColor, maxColor, - autocomputeIntensity, minIntensity, maxIntensity); + visuals_[i]->computeVisualization( + alpha, showGridLines, flatTerrain, heightLayer, flatColor, noColor, meshColor, + mapLayerColor, colorLayer, useRainbow, invertRainbow, minColor, maxColor, + autocomputeIntensity, minIntensity, maxIntensity); } } -void GridMapDisplay::processMessage(const grid_map_msgs::GridMap::ConstPtr& msg) +void GridMapDisplay::processMessage(grid_map_msgs::msg::GridMap::ConstSharedPtr msg) { // Check if transform between the message's frame and the fixed frame exists. Ogre::Quaternion orientation; Ogre::Vector3 position; - if (!context_->getFrameManager()->getTransform(msg->info.header.frame_id, msg->info.header.stamp, - position, orientation)) { - ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", msg->info.header.frame_id.c_str(), - qPrintable(fixed_frame_)); + if (!context_->getFrameManager()->getTransform( + msg->header.frame_id, msg->header.stamp, + position, orientation)) + { + RCLCPP_DEBUG( + rclcpp::get_logger("GridMapDisplay::processMessage"), + "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), + qPrintable(fixed_frame_)); return; } @@ -221,14 +237,15 @@ void GridMapDisplay::processMessage(const grid_map_msgs::GridMap::ConstPtr& msg) visual->setFramePosition(position); visual->setFrameOrientation(orientation); - visual->computeVisualization(alphaProperty_->getFloat(), showGridLinesProperty_->getBool(), - heightModeProperty_->getOptionInt() == 1, heightTransformerProperty_->getStdString(), - colorModeProperty_->getOptionInt() == 2, colorModeProperty_->getOptionInt() == 3, - colorProperty_->getOgreColor(), colorModeProperty_->getOptionInt() == 1, - colorTransformerProperty_->getStdString(), useRainbowProperty_->getBool(), - invertRainbowProperty_->getBool(), minColorProperty_->getOgreColor(), - maxColorProperty_->getOgreColor(), autocomputeIntensityBoundsProperty_->getBool(), - minIntensityProperty_->getFloat(), maxIntensityProperty_->getFloat()); + visual->computeVisualization( + alphaProperty_->getFloat(), showGridLinesProperty_->getBool(), + heightModeProperty_->getOptionInt() == 1, heightTransformerProperty_->getStdString(), + colorModeProperty_->getOptionInt() == 2, colorModeProperty_->getOptionInt() == 3, + colorProperty_->getOgreColor(), colorModeProperty_->getOptionInt() == 1, + colorTransformerProperty_->getStdString(), useRainbowProperty_->getBool(), + invertRainbowProperty_->getBool(), minColorProperty_->getOgreColor(), + maxColorProperty_->getOgreColor(), autocomputeIntensityBoundsProperty_->getBool(), + minIntensityProperty_->getFloat(), maxIntensityProperty_->getFloat()); std::vector layer_names = visual->getLayerNames(); heightTransformerProperty_->clearOptions(); @@ -243,5 +260,4 @@ void GridMapDisplay::processMessage(const grid_map_msgs::GridMap::ConstPtr& msg) } // end namespace grid_map_rviz_plugin -#include -PLUGINLIB_EXPORT_CLASS(grid_map_rviz_plugin::GridMapDisplay, rviz::Display) +PLUGINLIB_EXPORT_CLASS(grid_map_rviz_plugin::GridMapDisplay, rviz_common::Display) diff --git a/grid_map_rviz_plugin/src/GridMapVisual.cpp b/grid_map_rviz_plugin/src/GridMapVisual.cpp index 729e5d782..0255b2c03 100644 --- a/grid_map_rviz_plugin/src/GridMapVisual.cpp +++ b/grid_map_rviz_plugin/src/GridMapVisual.cpp @@ -6,33 +6,38 @@ * Institute: ETH Zurich, ANYbotics */ -#include -#include -#include -#include +#include +#include +#include -#include -#include -#include -#include +#include +#include +#include +#include -#include - -#include +#include #include +#include +#include + +#include +#include +#include + #include "grid_map_rviz_plugin/GridMapVisual.hpp" -namespace grid_map_rviz_plugin { +namespace grid_map_rviz_plugin +{ -GridMapVisual::GridMapVisual(Ogre::SceneManager* sceneManager, Ogre::SceneNode* parentNode) - : manualObject_(0), - haveMap_(false) +GridMapVisual::GridMapVisual(Ogre::SceneManager * sceneManager, Ogre::SceneNode * parentNode) +: manualObject_(0), + haveMap_(false) { sceneManager_ = sceneManager; frameNode_ = parentNode->createChildSceneNode(); // Create BillboardLine object. - meshLines_.reset(new rviz::BillboardLine(sceneManager_, frameNode_)); + meshLines_.reset(new rviz_rendering::BillboardLine(sceneManager_, frameNode_)); } GridMapVisual::~GridMapVisual() @@ -40,38 +45,47 @@ GridMapVisual::~GridMapVisual() // Destroy the ManualObject. sceneManager_->destroyManualObject(manualObject_); material_->unload(); - Ogre::MaterialManager::getSingleton().remove(material_->getName()); + Ogre::MaterialManager::getSingleton().remove(material_->getName(), "rviz_rendering"); // Destroy the frame node. sceneManager_->destroySceneNode(frameNode_); } -void GridMapVisual::setMessage(const grid_map_msgs::GridMap::ConstPtr& msg) +void GridMapVisual::setMessage(grid_map_msgs::msg::GridMap::ConstSharedPtr msg) { // Convert grid map message. grid_map::GridMapRosConverter::fromMessage(*msg, map_); haveMap_ = true; } -void GridMapVisual::computeVisualization(float alpha, bool showGridLines, bool flatTerrain, std::string heightLayer, - bool flatColor, bool noColor, Ogre::ColourValue meshColor, bool mapLayerColor, - std::string colorLayer, bool useRainbow, bool invertRainbow, - Ogre::ColourValue minColor, Ogre::ColourValue maxColor, - bool autocomputeIntensity, float minIntensity, float maxIntensity) +void GridMapVisual::computeVisualization( + float alpha, bool showGridLines, bool flatTerrain, std::string heightLayer, + bool flatColor, bool noColor, Ogre::ColourValue meshColor, bool mapLayerColor, + std::string colorLayer, bool useRainbow, bool invertRainbow, + Ogre::ColourValue minColor, Ogre::ColourValue maxColor, + bool autocomputeIntensity, float minIntensity, float maxIntensity) { if (!haveMap_) { - ROS_DEBUG("Unable to visualize grid map, no map data. Use setMessage() first!"); + RCLCPP_DEBUG( + rclcpp::get_logger( + "grid_map_display"), "Unable to visualize grid map, no map data. Use setMessage() first!"); return; } // Get list of layers and check if the requested ones are present. std::vector layerNames = map_.getLayers(); if (layerNames.size() < 1) { - ROS_DEBUG("Unable to visualize grid map, map must contain at least one layer."); + RCLCPP_DEBUG( + rclcpp::get_logger( + "grid_map_display"), "Unable to visualize grid map, map must contain at least one layer."); return; } - if ((!flatTerrain && !map_.exists(heightLayer)) || (!noColor && !flatColor && !map_.exists(colorLayer))) { - ROS_DEBUG("Unable to visualize grid map, requested layer(s) not available."); + if ((!flatTerrain && !map_.exists(heightLayer)) || + (!noColor && !flatColor && !map_.exists(colorLayer))) + { + RCLCPP_DEBUG( + rclcpp::get_logger( + "grid_map_display"), "Unable to visualize grid map, requested layer(s) not available."); return; } @@ -82,24 +96,24 @@ void GridMapVisual::computeVisualization(float alpha, bool showGridLines, bool f const size_t rows = map_.getSize()(0); const size_t cols = map_.getSize()(1); if (rows < 2 || cols < 2) { - ROS_DEBUG("GridMap has not enough cells."); + RCLCPP_DEBUG(rclcpp::get_logger("grid_map_display"), "GridMap has not enough cells."); return; } const double resolution = map_.getResolution(); - const grid_map::Matrix& heightData = map_[flatTerrain ? layerNames[0] : heightLayer]; - const grid_map::Matrix& colorData = map_[flatColor ? layerNames[0] : colorLayer]; + const grid_map::Matrix & heightData = map_[flatTerrain ? layerNames[0] : heightLayer]; + const grid_map::Matrix & colorData = map_[flatColor ? layerNames[0] : colorLayer]; // initialize ManualObject if (!manualObject_) { static uint32_t count = 0; - rviz::UniformStringStream ss; + rviz_common::UniformStringStream ss; ss << "Mesh" << count++; manualObject_ = sceneManager_->createManualObject(ss.str()); frameNode_->attachObject(manualObject_); ss << "Material"; materialName_ = ss.str(); - material_ = Ogre::MaterialManager::getSingleton().create(materialName_, "rviz"); + material_ = Ogre::MaterialManager::getSingleton().create(materialName_, "rviz_rendering"); material_->setReceiveShadows(false); material_->getTechnique(0)->setLightingEnabled(true); material_->setCullingMode(Ogre::CULL_NONE); @@ -108,7 +122,7 @@ void GridMapVisual::computeVisualization(float alpha, bool showGridLines, bool f manualObject_->clear(); size_t nVertices = 4 + 6 * (cols * rows - cols - rows); manualObject_->estimateVertexCount(nVertices); - manualObject_->begin(materialName_, Ogre::RenderOperation::OT_TRIANGLE_LIST); + manualObject_->begin(materialName_, Ogre::RenderOperation::OT_TRIANGLE_LIST, "rviz_rendering"); meshLines_->clear(); if (showGridLines) { @@ -126,7 +140,7 @@ void GridMapVisual::computeVisualization(float alpha, bool showGridLines, bool f maxIntensity = colorData.maxCoeffOfFinites(); } - if (!map_.hasBasicLayers()) map_.setBasicLayers({heightLayer}); + if (!map_.hasBasicLayers()) {map_.setBasicLayers({heightLayer});} // Plot mesh. for (size_t i = 0; i < rows - 1; ++i) { @@ -137,7 +151,7 @@ void GridMapVisual::computeVisualization(float alpha, bool showGridLines, bool f for (size_t l = 0; l < 2; l++) { grid_map::Position position; grid_map::Index index(i + k, j + l); - if (!map_.isValid(index)) continue; + if (!map_.isValid(index)) {continue;} map_.getPosition(index, position); float height = heightData(index(0), index(1)); @@ -148,12 +162,13 @@ void GridMapVisual::computeVisualization(float alpha, bool showGridLines, bool f if (mapLayerColor) { Eigen::Vector3f colorVectorRGB; grid_map::colorValueToVector(color, colorVectorRGB); - colorValue = Ogre::ColourValue(colorVectorRGB(0), colorVectorRGB(1), colorVectorRGB(2)); + colorValue = + Ogre::ColourValue(colorVectorRGB(0), colorVectorRGB(1), colorVectorRGB(2)); } else { normalizeIntensity(color, minIntensity, maxIntensity); - colorValue = useRainbow ? (invertRainbow ? getRainbowColor(1.0f - color) - : getRainbowColor(color)) - : getInterpolatedColor(color, minColor, maxColor); + colorValue = useRainbow ? (invertRainbow ? getRainbowColor(1.0f - color) : + getRainbowColor(color)) : + getInterpolatedColor(color, minColor, maxColor); } colors.push_back(colorValue); } @@ -162,16 +177,16 @@ void GridMapVisual::computeVisualization(float alpha, bool showGridLines, bool f // Plot triangles if we have enough vertices. if (vertices.size() > 2) { - Ogre::Vector3 normal = vertices.size() == 4 - ? (vertices[3] - vertices[0]).crossProduct(vertices[2] -vertices[1]) - : (vertices[2] - vertices[1]).crossProduct(vertices[1] - vertices[0]); + Ogre::Vector3 normal = vertices.size() == 4 ? + (vertices[3] - vertices[0]).crossProduct(vertices[2] - vertices[1]) : + (vertices[2] - vertices[1]).crossProduct(vertices[1] - vertices[0]); normal.normalise(); // Create one or two triangles from the vertices depending on how many vertices we have. if (!noColor) { for (size_t m = 1; m < vertices.size() - 1; m++) { - manualObject_->position(vertices[m-1]); + manualObject_->position(vertices[m - 1]); manualObject_->normal(normal); - Ogre::ColourValue color = flatColor ? meshColor : colors[m-1]; + Ogre::ColourValue color = flatColor ? meshColor : colors[m - 1]; manualObject_->colour(color.r, color.g, color.b, alpha); manualObject_->position(vertices[m]); @@ -179,9 +194,9 @@ void GridMapVisual::computeVisualization(float alpha, bool showGridLines, bool f color = flatColor ? meshColor : colors[m]; manualObject_->colour(color.r, color.g, color.b, alpha); - manualObject_->position(vertices[m+1]); + manualObject_->position(vertices[m + 1]); manualObject_->normal(normal); - color = flatColor ? meshColor : colors[m+1]; + color = flatColor ? meshColor : colors[m + 1]; manualObject_->colour(color.r, color.g, color.b, alpha); } } @@ -190,25 +205,25 @@ void GridMapVisual::computeVisualization(float alpha, bool showGridLines, bool f if (showGridLines) { meshLines_->addPoint(vertices[0]); meshLines_->addPoint(vertices[1]); - meshLines_->newLine(); + meshLines_->finishLine(); if (vertices.size() == 3) { meshLines_->addPoint(vertices[1]); meshLines_->addPoint(vertices[2]); - meshLines_->newLine(); + meshLines_->finishLine(); } else { meshLines_->addPoint(vertices[1]); meshLines_->addPoint(vertices[3]); - meshLines_->newLine(); + meshLines_->finishLine(); meshLines_->addPoint(vertices[3]); meshLines_->addPoint(vertices[2]); - meshLines_->newLine(); + meshLines_->finishLine(); } meshLines_->addPoint(vertices[2]); meshLines_->addPoint(vertices[0]); - meshLines_->newLine(); + meshLines_->finishLine(); } } } @@ -226,12 +241,12 @@ void GridMapVisual::computeVisualization(float alpha, bool showGridLines, bool f } } -void GridMapVisual::setFramePosition(const Ogre::Vector3& position) +void GridMapVisual::setFramePosition(const Ogre::Vector3 & position) { frameNode_->setPosition(position); } -void GridMapVisual::setFrameOrientation(const Ogre::Quaternion& orientation) +void GridMapVisual::setFrameOrientation(const Ogre::Quaternion & orientation) { frameNode_->setOrientation(orientation); } @@ -242,7 +257,7 @@ std::vector GridMapVisual::getLayerNames() } // Compute intensity value in the interval [0,1]. -void GridMapVisual::normalizeIntensity(float& intensity, float min_intensity, float max_intensity) +void GridMapVisual::normalizeIntensity(float & intensity, float min_intensity, float max_intensity) { intensity = std::min(intensity, max_intensity); intensity = std::max(intensity, min_intensity); @@ -258,22 +273,25 @@ Ogre::ColourValue GridMapVisual::getRainbowColor(float intensity) float h = intensity * 5.0f + 1.0f; int i = floor(h); float f = h - i; - if (!(i & 1)) f = 1 - f; // if i is even + if (!(i & 1)) { + f = 1 - f; // if i is even + } float n = 1 - f; Ogre::ColourValue color; - if (i <= 1) color[0] = n, color[1] = 0, color[2] = 1; - else if (i == 2) color[0] = 0, color[1] = n, color[2] = 1; - else if (i == 3) color[0] = 0, color[1] = 1, color[2] = n; - else if (i == 4) color[0] = n, color[1] = 1, color[2] = 0; - else if (i >= 5) color[0] = 1, color[1] = n, color[2] = 0; + if (i <= 1) {color[0] = n, color[1] = 0, color[2] = 1;} else if (i == 2) { + color[0] = 0, color[1] = n, color[2] = 1; + } else if (i == 3) {color[0] = 0, color[1] = 1, color[2] = n;} else if (i == 4) { + color[0] = n, color[1] = 1, color[2] = 0; + } else if (i >= 5) {color[0] = 1, color[1] = n, color[2] = 0;} return color; } // Get interpolated color value. -Ogre::ColourValue GridMapVisual::getInterpolatedColor(float intensity, Ogre::ColourValue min_color, - Ogre::ColourValue max_color) +Ogre::ColourValue GridMapVisual::getInterpolatedColor( + float intensity, Ogre::ColourValue min_color, + Ogre::ColourValue max_color) { intensity = std::min(intensity, 1.0f); intensity = std::max(intensity, 0.0f); @@ -286,4 +304,4 @@ Ogre::ColourValue GridMapVisual::getInterpolatedColor(float intensity, Ogre::Col return color; } -} // namespace +} // namespace grid_map_rviz_plugin diff --git a/grid_map_sdf/CHANGELOG.rst b/grid_map_sdf/CHANGELOG.rst index 96b334ecc..3584915d2 100644 --- a/grid_map_sdf/CHANGELOG.rst +++ b/grid_map_sdf/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package grid_map_sdf ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Contributors: Ryan, Ryan Friedman + +2.1.0 (2022-11-08) +------------------ + +2.0.0 (2022-09-13) +------------------ +* fix: mark Eigen library as SYSTEM +* fix: build error on jammy +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map_sdf/CMakeLists.txt b/grid_map_sdf/CMakeLists.txt index 252696b10..9294fb3e3 100644 --- a/grid_map_sdf/CMakeLists.txt +++ b/grid_map_sdf/CMakeLists.txt @@ -1,52 +1,49 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(grid_map_sdf) -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - grid_map_core - pcl_ros -) +## Find ament_cmake macros and libraries +find_package(ament_cmake REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - grid_map_core - pcl_ros - DEPENDS -) +## System dependencies are found with CMake's conventions +#find_package(Eigen3 REQUIRED) +# Solution to find Eigen3 with Saucy. +find_package(Eigen3 QUIET) +if(NOT EIGEN3_FOUND) +find_package(PkgConfig REQUIRED) +pkg_check_modules(EIGEN3 REQUIRED eigen3) +set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) +endif() -########### -## Build ## -########### +grid_map_package() ## Specify additional locations of header files include_directories( include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} + SYSTEM + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} +) + +set(dependencies + grid_map_core + PCL ) +########### +## Build ## +########### + ## Declare a cpp library add_library(${PROJECT_NAME} src/SignedDistanceField.cpp ) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME} SYSTEM + ${dependencies} ) ############# @@ -56,15 +53,15 @@ target_link_libraries(${PROJECT_NAME} # Mark executables and/or libraries for installation install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) # Mark cpp header files for installation install( DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} FILES_MATCHING PATTERN "*.hpp" ) @@ -72,15 +69,48 @@ install( ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") -## Add gtest based cpp test target and link libraries -catkin_add_gtest(${PROJECT_NAME}-test - test/SignedDistanceFieldTest.cpp - test/test_grid_map_sdf.cpp) +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright -build/include_order + ) + endif() + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") + ament_add_gtest(${PROJECT_NAME}-test + test/SignedDistanceFieldTest.cpp + test/test_grid_map_sdf.cpp) + + target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) + endif() if(TARGET ${PROJECT_NAME}-test) target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) - endif() + +ament_export_include_directories(include ${EIGEN3_INCLUDE_DIR}) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_package() diff --git a/grid_map_sdf/include/grid_map_sdf/SignedDistanceField.hpp b/grid_map_sdf/include/grid_map_sdf/SignedDistanceField.hpp index f1105da55..d254d74b9 100644 --- a/grid_map_sdf/include/grid_map_sdf/SignedDistanceField.hpp +++ b/grid_map_sdf/include/grid_map_sdf/SignedDistanceField.hpp @@ -6,40 +6,50 @@ * Institute: ETH Zurich, ANYbotics */ +#ifndef GRID_MAP_SDF__SIGNEDDISTANCEFIELD_HPP_ +#define GRID_MAP_SDF__SIGNEDDISTANCEFIELD_HPP_ + #pragma once #include +#include #include -#include #include #include -namespace grid_map { +namespace grid_map +{ class SignedDistanceField { - public: +public: SignedDistanceField(); virtual ~SignedDistanceField(); - void calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer, const double heightClearance); - double getDistanceAt(const Position3& position) const; - Vector3 getDistanceGradientAt(const Position3& position) const; - double getInterpolatedDistanceAt(const Position3& position) const; - void convertToPointCloud(pcl::PointCloud& points) const; + void calculateSignedDistanceField( + const GridMap & gridMap, const std::string & layer, + const double heightClearance); + double getDistanceAt(const Position3 & position) const; + Vector3 getDistanceGradientAt(const Position3 & position) const; + double getInterpolatedDistanceAt(const Position3 & position) const; + void convertToPointCloud(pcl::PointCloud & points) const; - private: - Matrix getPlanarSignedDistanceField(Eigen::Matrix& data) const; +private: + Matrix getPlanarSignedDistanceField( + Eigen::Matrix & data) const; - double resolution_; Size size_; Position position_; std::vector data_; - float zIndexStartHeight_; float maxDistance_; + float zIndexStartHeight_; + double resolution_; const float lowestHeight_; }; -} /* namespace */ +} // namespace grid_map + +#endif // GRID_MAP_SDF__SIGNEDDISTANCEFIELD_HPP_ diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/dt.h b/grid_map_sdf/include/grid_map_sdf/distance_transform/dt.h deleted file mode 100755 index 7a00eab49..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/dt.h +++ /dev/null @@ -1,117 +0,0 @@ -/* -Copyright (C) 2006 Pedro Felzenszwalb - -This program is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ - -/* distance transform */ - -#pragma once - -#include - -#include "image.h" -#include "imconv.h" - -namespace distance_transform -{ - const double INF = 1E20; - - /* dt of 1d function using squared distance */ - static float *dt(float *f, int n) { - float *d = new float[n]; - int *v = new int[n]; - float *z = new float[n+1]; - int k = 0; - v[0] = 0; - z[0] = -INF; - z[1] = +INF; - for (int q = 1; q <= n-1; q++) { - float s = ((f[q]+square(q))-(f[v[k]]+square(v[k])))/(2*q-2*v[k]); - while (s <= z[k]) { - k--; - s = ((f[q]+square(q))-(f[v[k]]+square(v[k])))/(2*q-2*v[k]); - } - k++; - v[k] = q; - z[k] = s; - z[k+1] = +INF; - } - - k = 0; - for (int q = 0; q <= n-1; q++) { - while (z[k+1] < q) - k++; - d[q] = square(q-v[k]) + f[v[k]]; - } - - delete [] v; - delete [] z; - return d; - } - - /* dt of 2d function using squared distance */ - static void dt(image *im) { - int width = im->width(); - int height = im->height(); - float *f = new float[std::max(width,height)]; - - // transform along columns - for (int x = 0; x < width; x++) { - for (int y = 0; y < height; y++) { - f[y] = imRef(im, x, y); - } - float *d = dt(f, height); - for (int y = 0; y < height; y++) { - imRef(im, x, y) = d[y]; - } - delete [] d; - } - - // transform along rows - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - f[x] = imRef(im, x, y); - } - float *d = dt(f, width); - for (int x = 0; x < width; x++) { - imRef(im, x, y) = d[x]; - } - delete [] d; - } - - delete [] f; - } - - /* dt of binary image using squared distance */ - static image *dt(image *im, uchar on = 1) { - int width = im->width(); - int height = im->height(); - - image *out = new image(width, height, false); - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - if (imRef(im, x, y) == on) - imRef(out, x, y) = 0; - else - imRef(out, x, y) = INF; - } - } - - dt(out); - return out; - } - -} // namespace diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/dt.hpp b/grid_map_sdf/include/grid_map_sdf/distance_transform/dt.hpp new file mode 100755 index 000000000..00275fd9e --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/distance_transform/dt.hpp @@ -0,0 +1,125 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* distance transform */ + +#ifndef GRID_MAP_SDF__DISTANCE_TRANSFORM__DT_HPP_ +#define GRID_MAP_SDF__DISTANCE_TRANSFORM__DT_HPP_ + +#include + +#include "grid_map_sdf/distance_transform/image.hpp" +#include "grid_map_sdf/distance_transform/imconv.hpp" + +namespace distance_transform +{ +const double INF = 1E20; + +/* dt of 1d function using squared distance */ +static float * dt(float * f, int n) +{ + float * d = new float[n]; + int * v = new int[n]; + float * z = new float[n + 1]; + int k = 0; + v[0] = 0; + z[0] = -INF; + z[1] = +INF; + for (int q = 1; q <= n - 1; q++) { + float s = ((f[q] + square(q)) - (f[v[k]] + square(v[k]))) / (2 * q - 2 * v[k]); + while (s <= z[k]) { + k--; + s = ((f[q] + square(q)) - (f[v[k]] + square(v[k]))) / (2 * q - 2 * v[k]); + } + k++; + v[k] = q; + z[k] = s; + z[k + 1] = +INF; + } + + k = 0; + for (int q = 0; q <= n - 1; q++) { + while (z[k + 1] < q) { + k++; + } + d[q] = square(q - v[k]) + f[v[k]]; + } + + delete[] v; + delete[] z; + return d; +} + +/* dt of 2d function using squared distance */ +static void dt(image * im) +{ + int width = im->width(); + int height = im->height(); + float * f = new float[std::max(width, height)]; + + // transform along columns + for (int x = 0; x < width; x++) { + for (int y = 0; y < height; y++) { + f[y] = imRef(im, x, y); + } + float * d = dt(f, height); + for (int y = 0; y < height; y++) { + imRef(im, x, y) = d[y]; + } + delete[] d; + } + + // transform along rows + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + f[x] = imRef(im, x, y); + } + float * d = dt(f, width); + for (int x = 0; x < width; x++) { + imRef(im, x, y) = d[x]; + } + delete[] d; + } + + delete[] f; +} + +/* dt of binary image using squared distance */ +static image * dt(image * im, uchar on = 1) +{ + int width = im->width(); + int height = im->height(); + + image * out = new image(width, height, false); + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + if (imRef(im, x, y) == on) { + imRef(out, x, y) = 0; + } else { + imRef(out, x, y) = INF; + } + } + } + + dt(out); + return out; +} + +} // namespace distance_transform + +#endif // GRID_MAP_SDF__DISTANCE_TRANSFORM__DT_HPP_ diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/image.h b/grid_map_sdf/include/grid_map_sdf/distance_transform/image.h deleted file mode 100755 index 47cada090..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/image.h +++ /dev/null @@ -1,101 +0,0 @@ -/* -Copyright (C) 2006 Pedro Felzenszwalb - -This program is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ - -/* a simple image class */ - -#pragma once - -#include - -namespace distance_transform -{ - - template - class image { - public: - /* create an image */ - image(const int width, const int height, const bool init = true); - - /* delete an image */ - ~image(); - - /* init an image */ - void init(const T &val); - - /* copy an image */ - image *copy() const; - - /* get the width of an image. */ - int width() const { return w; } - - /* get the height of an image. */ - int height() const { return h; } - - /* image data. */ - T *data; - - /* row pointers. */ - T **access; - - private: - int w, h; - }; - - /* use imRef to access image data. */ -#define imRef(im, x, y) (im->access[y][x]) - - /* use imPtr to get pointer to image data. */ -#define imPtr(im, x, y) &(im->access[y][x]) - - template - image::image(const int width, const int height, const bool init) { - w = width; - h = height; - data = new T[w * h]; // allocate space for image data - access = new T*[h]; // allocate space for row pointers - - // initialize row pointers - for (int i = 0; i < h; i++) - access[i] = data + (i * w); - - if (init) - memset(data, 0, w * h * sizeof(T)); - } - - template - image::~image() { - delete [] data; - delete [] access; - } - - template - void image::init(const T &val) { - T *ptr = imPtr(this, 0, 0); - T *end = imPtr(this, w-1, h-1); - while (ptr <= end) - *ptr++ = val; - } - - template - image *image::copy() const { - image *im = new image(w, h, false); - memcpy(im->data, data, w * h * sizeof(T)); - return im; - } - -} // namespace diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/image.hpp b/grid_map_sdf/include/grid_map_sdf/distance_transform/image.hpp new file mode 100755 index 000000000..bb43db79c --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/distance_transform/image.hpp @@ -0,0 +1,112 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* a simple image class */ + +#ifndef GRID_MAP_SDF__DISTANCE_TRANSFORM__IMAGE_HPP_ +#define GRID_MAP_SDF__DISTANCE_TRANSFORM__IMAGE_HPP_ + +#include + +namespace distance_transform +{ + +template +class image // NOLINT +{ +public: + /* create an image */ + image(const int width, const int height, const bool init = true); + + /* delete an image */ + ~image(); + + /* init an image */ + void init(const T & val); + + /* copy an image */ + image * copy() const; + + /* get the width of an image. */ + int width() const {return w;} + + /* get the height of an image. */ + int height() const {return h;} + + /* image data. */ + T * data; + + /* row pointers. */ + T ** access; + +private: + int w, h; +}; + +/* use imRef to access image data. */ +#define imRef(im, x, y) (im->access[y][x]) + +/* use imPtr to get pointer to image data. */ +#define imPtr(im, x, y) & (im->access[y][x]) + +template +image::image(const int width, const int height, const bool init) +{ + w = width; + h = height; + data = new T[w * h]; // allocate space for image data + access = new T *[h]; // allocate space for row pointers + + // initialize row pointers + for (int i = 0; i < h; i++) { + access[i] = data + (i * w); + } + + if (init) { + memset(data, 0, w * h * sizeof(T)); + } +} + +template +image::~image() +{ + delete[] data; + delete[] access; +} + +template +void image::init(const T & val) +{ + T * ptr = imPtr(this, 0, 0); + T * end = imPtr(this, w - 1, h - 1); + while (ptr <= end) { + *ptr++ = val; + } +} + +template +image * image::copy() const +{ + image * im = new image(w, h, false); + memcpy(im->data, data, w * h * sizeof(T)); + return im; +} + +} // namespace distance_transform + +#endif // GRID_MAP_SDF__DISTANCE_TRANSFORM__IMAGE_HPP_ diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/imconv.h b/grid_map_sdf/include/grid_map_sdf/distance_transform/imconv.h deleted file mode 100755 index b4f27ff36..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/imconv.h +++ /dev/null @@ -1,179 +0,0 @@ -/* -Copyright (C) 2006 Pedro Felzenszwalb - -This program is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ - -/* image conversion */ - -#pragma once - -#include - -#include "image.h" -#include "imutil.h" -#include "misc.h" - -namespace distance_transform -{ - const double RED_WEIGHT = 0.299; - const double GREEN_WEIGHT = 0.584; - const double BLUE_WEIGHT = 0.114; - - static image *imageRGBtoGRAY(image *input) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - imRef(output, x, y) = (uchar) - (imRef(input, x, y).r * RED_WEIGHT + - imRef(input, x, y).g * GREEN_WEIGHT + - imRef(input, x, y).b * BLUE_WEIGHT); - } - } - return output; - } - - static image *imageGRAYtoRGB(image *input) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - imRef(output, x, y).r = imRef(input, x, y); - imRef(output, x, y).g = imRef(input, x, y); - imRef(output, x, y).b = imRef(input, x, y); - } - } - return output; - } - - static image *imageUCHARtoFLOAT(image *input) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - imRef(output, x, y) = imRef(input, x, y); - } - } - return output; - } - - static image *imageINTtoFLOAT(image *input) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - imRef(output, x, y) = imRef(input, x, y); - } - } - return output; - } - - static image *imageFLOATtoUCHAR(image *input, - float min, float max) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - if (max == min) - return output; - - float scale = UCHAR_MAX / (max - min); - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - uchar val = (uchar)((imRef(input, x, y) - min) * scale); - imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX); - } - } - return output; - } - - static image *imageFLOATtoUCHAR(image *input) { - float min, max; - min_max(input, &min, &max); - return imageFLOATtoUCHAR(input, min, max); - } - - static image *imageUCHARtoLONG(image *input) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - imRef(output, x, y) = imRef(input, x, y); - } - } - return output; - } - - static image *imageLONGtoUCHAR(image *input, long min, long max) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - if (max == min) - return output; - - float scale = UCHAR_MAX / (float)(max - min); - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - uchar val = (uchar)((imRef(input, x, y) - min) * scale); - imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX); - } - } - return output; - } - - static image *imageLONGtoUCHAR(image *input) { - long min, max; - min_max(input, &min, &max); - return imageLONGtoUCHAR(input, min, max); - } - - static image *imageSHORTtoUCHAR(image *input, - short min, short max) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - if (max == min) - return output; - - float scale = UCHAR_MAX / (float)(max - min); - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - uchar val = (uchar)((imRef(input, x, y) - min) * scale); - imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX); - } - } - return output; - } - - static image *imageSHORTtoUCHAR(image *input) { - short min, max; - min_max(input, &min, &max); - return imageSHORTtoUCHAR(input, min, max); - } - -} // namespace diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/imconv.hpp b/grid_map_sdf/include/grid_map_sdf/distance_transform/imconv.hpp new file mode 100755 index 000000000..f466b98aa --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/distance_transform/imconv.hpp @@ -0,0 +1,199 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* image conversion */ + +#ifndef GRID_MAP_SDF__DISTANCE_TRANSFORM__IMCONV_HPP_ +#define GRID_MAP_SDF__DISTANCE_TRANSFORM__IMCONV_HPP_ + +#include + +#include "grid_map_sdf/distance_transform/image.hpp" +#include "grid_map_sdf/distance_transform/imutil.hpp" +#include "grid_map_sdf/distance_transform/misc.hpp" + +namespace distance_transform +{ +const double RED_WEIGHT = 0.299; +const double GREEN_WEIGHT = 0.584; +const double BLUE_WEIGHT = 0.114; + +static inline image * imageRGBtoGRAY(image * input) +{ + int width = input->width(); + int height = input->height(); + image * output = new image(width, height, false); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(output, x, y) = (uchar) + (imRef(input, x, y).r * RED_WEIGHT + + imRef(input, x, y).g * GREEN_WEIGHT + + imRef(input, x, y).b * BLUE_WEIGHT); + } + } + return output; +} + +static inline image * imageGRAYtoRGB(image * input) +{ + int width = input->width(); + int height = input->height(); + image * output = new image(width, height, false); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(output, x, y).r = imRef(input, x, y); + imRef(output, x, y).g = imRef(input, x, y); + imRef(output, x, y).b = imRef(input, x, y); + } + } + return output; +} + +static inline image * imageUCHARtoFLOAT(image * input) +{ + int width = input->width(); + int height = input->height(); + image * output = new image(width, height, false); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(output, x, y) = imRef(input, x, y); + } + } + return output; +} + +static inline image * imageINTtoFLOAT(image * input) +{ + int width = input->width(); + int height = input->height(); + image * output = new image(width, height, false); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(output, x, y) = imRef(input, x, y); + } + } + return output; +} + +static inline image * imageFLOATtoUCHAR( + image * input, + float min, float max) +{ + int width = input->width(); + int height = input->height(); + image * output = new image(width, height, false); + + if (max == min) { + return output; + } + + float scale = UCHAR_MAX / (max - min); + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + uchar val = (uchar)((imRef(input, x, y) - min) * scale); + imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX); + } + } + return output; +} + +static inline image * imageFLOATtoUCHAR(image * input) +{ + float min, max; + min_max(input, &min, &max); + return imageFLOATtoUCHAR(input, min, max); +} + +static inline image * imageUCHARtoLONG(image * input) +{ + int width = input->width(); + int height = input->height(); + image * output = new image(width, height, false); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(output, x, y) = imRef(input, x, y); + } + } + return output; +} + +static inline image * imageLONGtoUCHAR( + image * input, int64_t min, int64_t max) +{ + int width = input->width(); + int height = input->height(); + image * output = new image(width, height, false); + + if (max == min) { + return output; + } + + float scale = UCHAR_MAX / static_cast(max - min); + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + uchar val = (uchar)((imRef(input, x, y) - min) * scale); + imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX); + } + } + return output; +} + +static inline image * imageLONGtoUCHAR(image * input) +{ + int64_t min, max; + min_max(input, &min, &max); + return imageLONGtoUCHAR(input, min, max); +} + +static inline image * imageSHORTtoUCHAR( + image * input, + int16_t min, int16_t max) +{ + int width = input->width(); + int height = input->height(); + image * output = new image(width, height, false); + + if (max == min) { + return output; + } + + float scale = UCHAR_MAX / static_cast(max - min); + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + uchar val = (uchar)((imRef(input, x, y) - min) * scale); + imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX); + } + } + return output; +} + +static inline image * imageSHORTtoUCHAR(image * input) +{ + int16_t min, max; + min_max(input, &min, &max); + return imageSHORTtoUCHAR(input, min, max); +} + +} // namespace distance_transform + +#endif // GRID_MAP_SDF__DISTANCE_TRANSFORM__IMCONV_HPP_ diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/imutil.h b/grid_map_sdf/include/grid_map_sdf/distance_transform/imutil.h deleted file mode 100755 index ad09d1f8e..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/imutil.h +++ /dev/null @@ -1,67 +0,0 @@ -/* -Copyright (C) 2006 Pedro Felzenszwalb - -This program is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ - -/* some image utilities */ - -#pragma once - -#include "image.h" -#include "misc.h" - -namespace distance_transform -{ - - /* compute minimum and maximum value in an image */ - template - void min_max(image *im, T *ret_min, T *ret_max) { - int width = im->width(); - int height = im->height(); - - T min = imRef(im, 0, 0); - T max = imRef(im, 0, 0); - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - T val = imRef(im, x, y); - if (min > val) - min = val; - if (max < val) - max = val; - } - } - - *ret_min = min; - *ret_max = max; - } - - /* threshold image */ - template - image *threshold(image *src, int t) { - int width = src->width(); - int height = src->height(); - image *dst = new image(width, height); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - imRef(dst, x, y) = (imRef(src, x, y) >= t); - } - } - - return dst; - } - -} // namespace diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/imutil.hpp b/grid_map_sdf/include/grid_map_sdf/distance_transform/imutil.hpp new file mode 100755 index 000000000..f6324eaeb --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/distance_transform/imutil.hpp @@ -0,0 +1,74 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* some image utilities */ + +#ifndef GRID_MAP_SDF__DISTANCE_TRANSFORM__IMUTIL_HPP_ +#define GRID_MAP_SDF__DISTANCE_TRANSFORM__IMUTIL_HPP_ + +#include "grid_map_sdf/distance_transform/image.hpp" +#include "grid_map_sdf/distance_transform/misc.hpp" + +namespace distance_transform +{ + +/* compute minimum and maximum value in an image */ +template +void min_max(image * im, T * ret_min, T * ret_max) +{ + int width = im->width(); + int height = im->height(); + + T min = imRef(im, 0, 0); + T max = imRef(im, 0, 0); + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + T val = imRef(im, x, y); + if (min > val) { + min = val; + } + if (max < val) { + max = val; + } + } + } + + *ret_min = min; + *ret_max = max; +} + +/* threshold image */ +template +image * threshold(image * src, int t) +{ + int width = src->width(); + int height = src->height(); + image * dst = new image(width, height); + + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + imRef(dst, x, y) = (imRef(src, x, y) >= t); + } + } + + return dst; +} + +} // namespace distance_transform + +#endif // GRID_MAP_SDF__DISTANCE_TRANSFORM__IMUTIL_HPP_ diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/misc.h b/grid_map_sdf/include/grid_map_sdf/distance_transform/misc.h deleted file mode 100755 index a9514d1f4..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/misc.h +++ /dev/null @@ -1,62 +0,0 @@ -/* -Copyright (C) 2006 Pedro Felzenszwalb - -This program is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ - -/* random stuff */ - -#pragma once - -#include - -namespace distance_transform -{ - typedef unsigned char uchar; - - typedef struct { uchar r, g, b; } rgb; - - inline bool operator==(const rgb &a, const rgb &b) { - return ((a.r == b.r) && (a.g == b.g) && (a.b == b.b)); - } - - template - inline T abs(const T &x) { return (x > 0 ? x : -x); }; - - template - inline int sign(const T &x) { return (x >= 0 ? 1 : -1); }; - - template - inline T square(const T &x) { return x*x; }; - - template - inline T bound(const T &x, const T &min, const T &max) { - return (x < min ? min : (x > max ? max : x)); - } - - template - inline bool check_bound(const T &x, const T&min, const T &max) { - return ((x < min) || (x > max)); - } - - inline int vlib_round(float x) { return (int)(x + 0.5F); } - - inline int vlib_round(double x) { return (int)(x + 0.5); } - - inline double gaussian(double val, double sigma) { - return exp(-square(val/sigma)/2)/(sqrt(2*M_PI)*sigma); - } - -} // namespace diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/misc.hpp b/grid_map_sdf/include/grid_map_sdf/distance_transform/misc.hpp new file mode 100755 index 000000000..96682531c --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/distance_transform/misc.hpp @@ -0,0 +1,69 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* random stuff */ + +#ifndef GRID_MAP_SDF__DISTANCE_TRANSFORM__MISC_HPP_ +#define GRID_MAP_SDF__DISTANCE_TRANSFORM__MISC_HPP_ + +#include + +namespace distance_transform +{ +typedef unsigned char uchar; + +typedef struct { uchar r, g, b; } rgb; + +inline bool operator==(const rgb & a, const rgb & b) +{ + return (a.r == b.r) && (a.g == b.g) && (a.b == b.b); +} + +template +inline T abs(const T & x) {return x > 0 ? x : -x;} + +template +inline int sign(const T & x) {return x >= 0 ? 1 : -1;} + +template +inline T square(const T & x) {return x * x;} + +template +inline T bound(const T & x, const T & min, const T & max) +{ + return x < min ? min : (x > max ? max : x); +} + +template +inline bool check_bound(const T & x, const T & min, const T & max) +{ + return (x < min) || (x > max); +} + +inline int vlib_round(float x) {return static_cast(x + 0.5F);} + +inline int vlib_round(double x) {return static_cast(x + 0.5);} + +inline double gaussian(double val, double sigma) +{ + return exp(-square(val / sigma) / 2) / (sqrt(2 * M_PI) * sigma); +} + +} // namespace distance_transform + +#endif // GRID_MAP_SDF__DISTANCE_TRANSFORM__MISC_HPP_ diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/pnmfile.h b/grid_map_sdf/include/grid_map_sdf/distance_transform/pnmfile.h deleted file mode 100755 index 282843284..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/pnmfile.h +++ /dev/null @@ -1,214 +0,0 @@ -/* -Copyright (C) 2006 Pedro Felzenszwalb - -This program is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ - -/* basic image I/O */ - -#pragma once - -#include -#include -#include -#include - -#include "image.h" -#include "misc.h" - -namespace distance_transform -{ - - const short BUF_SIZE = 256; - - class pnm_error { }; - - static void read_packed(unsigned char *data, int size, std::ifstream &f) { - unsigned char c = 0; - - int bitshift = -1; - for (int pos = 0; pos < size; pos++) { - if (bitshift == -1) { - c = f.get(); - bitshift = 7; - } - data[pos] = (c >> bitshift) & 1; - bitshift--; - } - } - - static void write_packed(unsigned char *data, int size, std::ofstream &f) { - unsigned char c = 0; - - int bitshift = 7; - for (int pos = 0; pos < size; pos++) { - c = c | (data[pos] << bitshift); - bitshift--; - if ((bitshift == -1) || (pos == size-1)) { - f.put(c); - bitshift = 7; - c = 0; - } - } - } - - /* read PNM field, skipping comments */ - static void pnm_read(std::ifstream &file, char *buf) { - char doc[BUF_SIZE]; - char c; - - file >> c; - while (c == '#') { - file.getline(doc, BUF_SIZE); - file >> c; - } - file.putback(c); - - file.width(BUF_SIZE); - file >> buf; - file.ignore(); - } - - static image *loadPBM(const char *name) { - char buf[BUF_SIZE]; - - /* read header */ - std::ifstream file(name, std::ios::in | std::ios::binary); - pnm_read(file, buf); - if (strncmp(buf, "P4", 2)) - throw pnm_error(); - - pnm_read(file, buf); - int width = atoi(buf); - pnm_read(file, buf); - int height = atoi(buf); - - /* read data */ - image *im = new image(width, height); - for (int i = 0; i < height; i++) - read_packed(imPtr(im, 0, i), width, file); - - return im; - } - - static void savePBM(image *im, const char *name) { - int width = im->width(); - int height = im->height(); - std::ofstream file(name, std::ios::out | std::ios::binary); - - file << "P4\n" << width << " " << height << "\n"; - for (int i = 0; i < height; i++) - write_packed(imPtr(im, 0, i), width, file); - } - - static image *loadPGM(const char *name) { - char buf[BUF_SIZE]; - - /* read header */ - std::ifstream file(name, std::ios::in | std::ios::binary); - pnm_read(file, buf); - if (strncmp(buf, "P5", 2)) - throw pnm_error(); - - pnm_read(file, buf); - int width = atoi(buf); - pnm_read(file, buf); - int height = atoi(buf); - - pnm_read(file, buf); - if (atoi(buf) > UCHAR_MAX) - throw pnm_error(); - - /* read data */ - image *im = new image(width, height); - file.read((char *)imPtr(im, 0, 0), width * height * sizeof(uchar)); - - return im; - } - - static void savePGM(image *im, const char *name) { - int width = im->width(); - int height = im->height(); - std::ofstream file(name, std::ios::out | std::ios::binary); - - file << "P5\n" << width << " " << height << "\n" << UCHAR_MAX << "\n"; - file.write((char *)imPtr(im, 0, 0), width * height * sizeof(uchar)); - } - - static image *loadPPM(const char *name) { - char buf[BUF_SIZE], doc[BUF_SIZE]; - - /* read header */ - std::ifstream file(name, std::ios::in | std::ios::binary); - pnm_read(file, buf); - if (strncmp(buf, "P6", 2)) - throw pnm_error(); - - pnm_read(file, buf); - int width = atoi(buf); - pnm_read(file, buf); - int height = atoi(buf); - - pnm_read(file, buf); - if (atoi(buf) > UCHAR_MAX) - throw pnm_error(); - - /* read data */ - image *im = new image(width, height); - file.read((char *)imPtr(im, 0, 0), width * height * sizeof(rgb)); - - return im; - } - - static void savePPM(image *im, const char *name) { - int width = im->width(); - int height = im->height(); - std::ofstream file(name, std::ios::out | std::ios::binary); - - file << "P6\n" << width << " " << height << "\n" << UCHAR_MAX << "\n"; - file.write((char *)imPtr(im, 0, 0), width * height * sizeof(rgb)); - } - - template - void load_image(image **im, const char *name) { - char buf[BUF_SIZE]; - - /* read header */ - std::ifstream file(name, std::ios::in | std::ios::binary); - pnm_read(file, buf); - if (strncmp(buf, "VLIB", 9)) - throw pnm_error(); - - pnm_read(file, buf); - int width = atoi(buf); - pnm_read(file, buf); - int height = atoi(buf); - - /* read data */ - *im = new image(width, height); - file.read((char *)imPtr((*im), 0, 0), width * height * sizeof(T)); - } - - template - void save_image(image *im, const char *name) { - int width = im->width(); - int height = im->height(); - std::ofstream file(name, std::ios::out | std::ios::binary); - - file << "VLIB\n" << width << " " << height << "\n"; - file.write((char *)imPtr(im, 0, 0), width * height * sizeof(T)); - } - -} // namespace diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/pnmfile.hpp b/grid_map_sdf/include/grid_map_sdf/distance_transform/pnmfile.hpp new file mode 100755 index 000000000..45b5bee48 --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/distance_transform/pnmfile.hpp @@ -0,0 +1,236 @@ +/* +Copyright (C) 2006 Pedro Felzenszwalb + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* basic image I/O */ + +#ifndef GRID_MAP_SDF__DISTANCE_TRANSFORM__PNMFILE_HPP_ +#define GRID_MAP_SDF__DISTANCE_TRANSFORM__PNMFILE_HPP_ + +#include +#include +#include +#include + +#include "grid_map_sdf/distance_transform/image.hpp" +#include "grid_map_sdf/distance_transform/misc.hpp" + +namespace distance_transform +{ + +const int16_t BUF_SIZE = 256; + +class pnm_error {}; + +static void read_packed(unsigned char * data, int size, std::ifstream & f) +{ + unsigned char c = 0; + + int bitshift = -1; + for (int pos = 0; pos < size; pos++) { + if (bitshift == -1) { + c = f.get(); + bitshift = 7; + } + data[pos] = (c >> bitshift) & 1; + bitshift--; + } +} + +static void write_packed(unsigned char * data, int size, std::ofstream & f) +{ + unsigned char c = 0; + + int bitshift = 7; + for (int pos = 0; pos < size; pos++) { + c = c | (data[pos] << bitshift); + bitshift--; + if ((bitshift == -1) || (pos == size - 1)) { + f.put(c); + bitshift = 7; + c = 0; + } + } +} + +/* read PNM field, skipping comments */ +static void pnm_read(std::ifstream & file, char * buf) +{ + char doc[BUF_SIZE]; + char c; + + file >> c; + while (c == '#') { + file.getline(doc, BUF_SIZE); + file >> c; + } + file.putback(c); + + file.width(BUF_SIZE); + file >> buf; + file.ignore(); +} + +static image * loadPBM(const char * name) +{ + char buf[BUF_SIZE]; + + /* read header */ + std::ifstream file(name, std::ios::in | std::ios::binary); + pnm_read(file, buf); + if (strncmp(buf, "P4", 2)) { + throw pnm_error(); + } + + pnm_read(file, buf); + int width = atoi(buf); + pnm_read(file, buf); + int height = atoi(buf); + + /* read data */ + image * im = new image(width, height); + for (int i = 0; i < height; i++) { + read_packed(imPtr(im, 0, i), width, file); + } + + return im; +} + +static void savePBM(image * im, const char * name) +{ + int width = im->width(); + int height = im->height(); + std::ofstream file(name, std::ios::out | std::ios::binary); + + file << "P4\n" << width << " " << height << "\n"; + for (int i = 0; i < height; i++) { + write_packed(imPtr(im, 0, i), width, file); + } +} + +static image * loadPGM(const char * name) +{ + char buf[BUF_SIZE]; + + /* read header */ + std::ifstream file(name, std::ios::in | std::ios::binary); + pnm_read(file, buf); + if (strncmp(buf, "P5", 2)) { + throw pnm_error(); + } + + pnm_read(file, buf); + int width = atoi(buf); + pnm_read(file, buf); + int height = atoi(buf); + + pnm_read(file, buf); + if (atoi(buf) > UCHAR_MAX) { + throw pnm_error(); + } + + /* read data */ + image * im = new image(width, height); + file.read(static_cast(imPtr(im, 0, 0)), width * height * sizeof(uchar)); + + return im; +} + +static void savePGM(image * im, const char * name) +{ + int width = im->width(); + int height = im->height(); + std::ofstream file(name, std::ios::out | std::ios::binary); + + file << "P5\n" << width << " " << height << "\n" << UCHAR_MAX << "\n"; + file.write(static_cast(imPtr(im, 0, 0)), width * height * sizeof(uchar)); +} + +static image * loadPPM(const char * name) +{ + char buf[BUF_SIZE], doc[BUF_SIZE]; + + /* read header */ + std::ifstream file(name, std::ios::in | std::ios::binary); + pnm_read(file, buf); + if (strncmp(buf, "P6", 2)) { + throw pnm_error(); + } + + pnm_read(file, buf); + int width = atoi(buf); + pnm_read(file, buf); + int height = atoi(buf); + + pnm_read(file, buf); + if (atoi(buf) > UCHAR_MAX) { + throw pnm_error(); + } + + /* read data */ + image * im = new image(width, height); + file.read(static_cast(imPtr(im, 0, 0)), width * height * sizeof(rgb)); + + return im; +} + +static void savePPM(image * im, const char * name) +{ + int width = im->width(); + int height = im->height(); + std::ofstream file(name, std::ios::out | std::ios::binary); + + file << "P6\n" << width << " " << height << "\n" << UCHAR_MAX << "\n"; + file.write(static_cast(imPtr(im, 0, 0)), width * height * sizeof(rgb)); +} + +template +void load_image(image ** im, const char * name) +{ + char buf[BUF_SIZE]; + + /* read header */ + std::ifstream file(name, std::ios::in | std::ios::binary); + pnm_read(file, buf); + if (strncmp(buf, "VLIB", 9)) { + throw pnm_error(); + } + + pnm_read(file, buf); + int width = atoi(buf); + pnm_read(file, buf); + int height = atoi(buf); + + /* read data */ + *im = new image(width, height); + file.read(static_cast(imPtr(*im, 0, 0)), width * height * sizeof(T)); +} + +template +void save_image(image * im, const char * name) +{ + int width = im->width(); + int height = im->height(); + std::ofstream file(name, std::ios::out | std::ios::binary); + + file << "VLIB\n" << width << " " << height << "\n"; + file.write(static_cast(imPtr(im, 0, 0)), width * height * sizeof(T)); +} + +} // namespace distance_transform + +#endif // GRID_MAP_SDF__DISTANCE_TRANSFORM__PNMFILE_HPP_ diff --git a/grid_map_sdf/package.xml b/grid_map_sdf/package.xml index 011eed6b3..a45b97055 100644 --- a/grid_map_sdf/package.xml +++ b/grid_map_sdf/package.xml @@ -1,16 +1,29 @@ - + + grid_map_sdf - 1.6.2 + 2.2.2 Generates signed distance fields from grid maps. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Takahiro Miki Péter Fankhauser - catkin + + ament_cmake + grid_map_cmake_helpers + grid_map_core - pcl_ros + libpcl-all-dev + + ament_cmake_gtest + ament_lint_common + ament_lint_auto + + + ament_cmake + diff --git a/grid_map_sdf/src/SignedDistanceField.cpp b/grid_map_sdf/src/SignedDistanceField.cpp index 163dc3a2b..33a9f2dbe 100644 --- a/grid_map_sdf/src/SignedDistanceField.cpp +++ b/grid_map_sdf/src/SignedDistanceField.cpp @@ -6,22 +6,29 @@ * Institute: ETH Zurich, ANYbotics */ -#include -#include +#include +#include +#include +#include +#include +#include -#include +#include "grid_map_sdf/SignedDistanceField.hpp" +#include "grid_map_sdf/distance_transform/dt.hpp" + +#include "grid_map_core/GridMap.hpp" -#include -using namespace distance_transform; +namespace grid_map +{ -namespace grid_map { +using namespace distance_transform; // NOLINT SignedDistanceField::SignedDistanceField() - : maxDistance_(std::numeric_limits::max()), - zIndexStartHeight_(0.0), - resolution_(0.0), - lowestHeight_(-1e5) // We need some precision. +: maxDistance_(std::numeric_limits::max()), + zIndexStartHeight_(0.0), + resolution_(0.0), + lowestHeight_(-1e5) // We need some precision. { } @@ -29,23 +36,25 @@ SignedDistanceField::~SignedDistanceField() { } -void SignedDistanceField::calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer, - const double heightClearance) +void SignedDistanceField::calculateSignedDistanceField( + const GridMap & gridMap, const std::string & layer, + const double heightClearance) { data_.clear(); resolution_ = gridMap.getResolution(); position_ = gridMap.getPosition(); size_ = gridMap.getSize(); - Matrix map = gridMap.get(layer); // Copy! + Matrix map = gridMap.get(layer); // Copy! float minHeight = map.minCoeffOfFinites(); - if (!std::isfinite(minHeight)) minHeight = lowestHeight_; + if (!std::isfinite(minHeight)) {minHeight = lowestHeight_;} float maxHeight = map.maxCoeffOfFinites(); - if (!std::isfinite(maxHeight)) maxHeight = lowestHeight_; + if (!std::isfinite(maxHeight)) {maxHeight = lowestHeight_;} - const float valueForEmptyCells = lowestHeight_; // maxHeight, minHeight (TODO Make this an option). - for (size_t i = 0; i < map.size(); ++i) { - if (std::isnan(map(i))) map(i) = valueForEmptyCells; + // maxHeight, minHeight (TODO Make this an option). + const float valueForEmptyCells = lowestHeight_; + for (int i = 0; i < map.size(); ++i) { + if (std::isnan(map(i))) {map(i) = valueForEmptyCells;} } // Height range of the signed distance field is higher than the max height. @@ -59,29 +68,38 @@ void SignedDistanceField::calculateSignedDistanceField(const GridMap& gridMap, c // Calculate signed distance field from bottom. for (float h = minHeight; h < maxHeight; h += resolution_) { Eigen::Matrix obstacleFreeField = map.array() < h; - Eigen::Matrix obstacleField = obstacleFreeField.array() < 1; + Eigen::Matrix obstacleField = obstacleFreeField.array() < 1; Matrix sdfObstacle = getPlanarSignedDistanceField(obstacleField); Matrix sdfObstacleFree = getPlanarSignedDistanceField(obstacleFreeField); Matrix sdf2d; // If 2d sdfObstacleFree calculation failed, neglect this SDF // to avoid extreme small distances (-INF). - if ((sdfObstacleFree.array() >= INF).any()) sdf2d = sdfObstacle; - else sdf2d = sdfObstacle - sdfObstacleFree; + if ((sdfObstacleFree.array() >= distance_transform::INF).any()) {sdf2d = sdfObstacle;} else { + sdf2d = sdfObstacle - sdfObstacleFree; + } sdf2d *= resolution_; - for (size_t i = 0; i < sdfElevationAbove.size(); ++i) { - if(sdfElevationAbove(i) == maxDistance_ && map(i) <= h) sdfElevationAbove(i) = h - map(i); - else if(sdfElevationAbove(i) != maxDistance_ && map(i) <= h) sdfElevationAbove(i) = sdfLayer(i) + resolution_; - if (sdf2d(i) == 0) sdfLayer(i) = h - map(i); - else if (sdf2d(i) < 0) sdfLayer(i) = -std::min(fabs(sdf2d(i)), fabs(map(i) - h)); - else sdfLayer(i) = std::min(sdf2d(i), sdfElevationAbove(i)); + for (int i = 0; i < sdfElevationAbove.size(); ++i) { + if (sdfElevationAbove(i) == maxDistance_ && map(i) <= h) { + sdfElevationAbove(i) = h - map(i); + } else if (sdfElevationAbove(i) != maxDistance_ && map(i) <= h) { + sdfElevationAbove(i) = sdfLayer(i) + resolution_; + } + if (sdf2d(i) == 0) {sdfLayer(i) = h - map(i);} else if (sdf2d(i) < 0) { + sdfLayer(i) = -std::min(fabs(sdf2d(i)), fabs(map(i) - h)); + } else {sdfLayer(i) = std::min(sdf2d(i), sdfElevationAbove(i));} } data_.push_back(sdfLayer); } } -grid_map::Matrix SignedDistanceField::getPlanarSignedDistanceField(Eigen::Matrix& data) const +grid_map::Matrix SignedDistanceField::getPlanarSignedDistanceField( + Eigen::Matrix & data) const { - image *input = new image(data.rows(), data.cols(), true); + image inputImage(data.rows(), data.cols(), true); + auto * input = &inputImage; for (int y = 0; y < input->height(); y++) { for (int x = 0; x < input->width(); x++) { @@ -90,20 +108,20 @@ grid_map::Matrix SignedDistanceField::getPlanarSignedDistanceField(Eigen::Matrix } // Compute dt. - image *out = dt(input); + std::unique_ptr> out(dt(input)); Matrix result(data.rows(), data.cols()); // Take square roots. for (int y = 0; y < out->height(); y++) { for (int x = 0; x < out->width(); x++) { - result(x, y) = sqrt(imRef(out, x, y)); + result(x, y) = std::sqrt(imRef(out.get(), x, y)); } } return result; } -double SignedDistanceField::getDistanceAt(const Position3& position) const +double SignedDistanceField::getDistanceAt(const Position3 & position) const { double xCenter = size_.x() / 2.0; double yCenter = size_.y() / 2.0; @@ -115,11 +133,11 @@ double SignedDistanceField::getDistanceAt(const Position3& position) const j = std::max(j, 0); j = std::min(j, size_.y() - 1); k = std::max(k, 0); - k = std::min(k, (int)data_.size() - 1); + k = std::min(k, static_cast(data_.size()) - 1); return data_[k](i, j); } -double SignedDistanceField::getInterpolatedDistanceAt(const Position3& position) const +double SignedDistanceField::getInterpolatedDistanceAt(const Position3 & position) const { double xCenter = size_.x() / 2.0; double yCenter = size_.y() / 2.0; @@ -131,7 +149,7 @@ double SignedDistanceField::getInterpolatedDistanceAt(const Position3& position) j = std::max(j, 0); j = std::min(j, size_.y() - 1); k = std::max(k, 0); - k = std::min(k, (int)data_.size() - 1); + k = std::min(k, static_cast(data_.size()) - 1); Vector3 gradient = getDistanceGradientAt(position); double xp = position_.x() + ((size_.x() - i) - xCenter) * resolution_; double yp = position_.y() + ((size_.y() - j) - yCenter) * resolution_; @@ -140,7 +158,7 @@ double SignedDistanceField::getInterpolatedDistanceAt(const Position3& position) return data_[k](i, j) + gradient.dot(error); } -Vector3 SignedDistanceField::getDistanceGradientAt(const Position3& position) const +Vector3 SignedDistanceField::getDistanceGradientAt(const Position3 & position) const { double xCenter = size_.x() / 2.0; double yCenter = size_.y() / 2.0; @@ -152,20 +170,20 @@ Vector3 SignedDistanceField::getDistanceGradientAt(const Position3& position) co j = std::max(j, 1); j = std::min(j, size_.y() - 2); k = std::max(k, 1); - k = std::min(k, (int)data_.size() - 2); + k = std::min(k, static_cast(data_.size()) - 2); double dx = (data_[k](i - 1, j) - data_[k](i + 1, j)) / (2 * resolution_); double dy = (data_[k](i, j - 1) - data_[k](i, j + 1)) / (2 * resolution_); double dz = (data_[k + 1](i, j) - data_[k - 1](i, j)) / (2 * resolution_); return Vector3(dx, dy, dz); } -void SignedDistanceField::convertToPointCloud(pcl::PointCloud& points) const +void SignedDistanceField::convertToPointCloud(pcl::PointCloud & points) const { double xCenter = size_.x() / 2.0; double yCenter = size_.y() / 2.0; - for (int z = 0; z < data_.size(); z++){ - for (int y = 0; y < size_.y(); y++) { - for (int x = 0; x < size_.x(); x++) { + for (int z = 0; z < static_cast(data_.size()); z++) { + for (int y = 0; y < static_cast(size_.y()); y++) { + for (int x = 0; x < static_cast(size_.x()); x++) { double xp = position_.x() + ((size_.x() - x) - xCenter) * resolution_; double yp = position_.y() + ((size_.y() - y) - yCenter) * resolution_; double zp = zIndexStartHeight_ + z * resolution_; @@ -178,7 +196,6 @@ void SignedDistanceField::convertToPointCloud(pcl::PointCloud& p } } } - return; } -} /* namespace */ +} // namespace grid_map diff --git a/grid_map_sdf/test/SignedDistanceFieldTest.cpp b/grid_map_sdf/test/SignedDistanceFieldTest.cpp index c66adbcc0..eeaaaff60 100644 --- a/grid_map_sdf/test/SignedDistanceFieldTest.cpp +++ b/grid_map_sdf/test/SignedDistanceFieldTest.cpp @@ -6,14 +6,14 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_core/GridMap.hpp" -#include "grid_map_sdf/SignedDistanceField.hpp" - #include #include -using namespace std; -using namespace grid_map; +#include "grid_map_core/GridMap.hpp" +#include "grid_map_sdf/SignedDistanceField.hpp" + +using namespace std; // NOLINT +using namespace grid_map; // NOLINT TEST(SignedDistanceField, EmptyMap) { @@ -34,7 +34,7 @@ TEST(SignedDistanceField, GetDistanceFlat) GridMap map({"layer"}); map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2)); map["layer"].setConstant(1.0); - map.at("layer", Index(0,0)) = -1.0; + map.at("layer", Index(0, 0)) = -1.0; SignedDistanceField sdf; sdf.calculateSignedDistanceField(map, "layer", 2.5); @@ -76,21 +76,21 @@ TEST(SignedDistanceField, GetDistance) map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25)); map["layer"].setConstant(1.0); - map.at("layer", Index(3,4)) = 2.0; - map.at("layer", Index(3,5)) = 2.0; - map.at("layer", Index(3,6)) = 2.0; - map.at("layer", Index(4,4)) = 2.0; - map.at("layer", Index(4,5)) = 2.0; - map.at("layer", Index(4,6)) = 2.0; - map.at("layer", Index(5,4)) = 2.0; - map.at("layer", Index(5,5)) = 2.0; - map.at("layer", Index(5,6)) = 2.0; - map.at("layer", Index(6,4)) = 2.0; - map.at("layer", Index(6,5)) = 2.0; - map.at("layer", Index(6,6)) = 2.0; - map.at("layer", Index(7,4)) = 2.0; - map.at("layer", Index(7,5)) = 2.0; - map.at("layer", Index(7,6)) = 2.0; + map.at("layer", Index(3, 4)) = 2.0; + map.at("layer", Index(3, 5)) = 2.0; + map.at("layer", Index(3, 6)) = 2.0; + map.at("layer", Index(4, 4)) = 2.0; + map.at("layer", Index(4, 5)) = 2.0; + map.at("layer", Index(4, 6)) = 2.0; + map.at("layer", Index(5, 4)) = 2.0; + map.at("layer", Index(5, 5)) = 2.0; + map.at("layer", Index(5, 6)) = 2.0; + map.at("layer", Index(6, 4)) = 2.0; + map.at("layer", Index(6, 5)) = 2.0; + map.at("layer", Index(6, 6)) = 2.0; + map.at("layer", Index(7, 4)) = 2.0; + map.at("layer", Index(7, 5)) = 2.0; + map.at("layer", Index(7, 6)) = 2.0; SignedDistanceField sdf; sdf.calculateSignedDistanceField(map, "layer", 1.5); @@ -145,21 +145,21 @@ TEST(SignedDistanceField, GetDistanceGradient) map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25)); map["layer"].setConstant(1.0); - map.at("layer", Index(3,4)) = 2.0; - map.at("layer", Index(3,5)) = 2.0; - map.at("layer", Index(3,6)) = 2.0; - map.at("layer", Index(4,4)) = 2.0; - map.at("layer", Index(4,5)) = 2.0; - map.at("layer", Index(4,6)) = 2.0; - map.at("layer", Index(5,4)) = 2.0; - map.at("layer", Index(5,5)) = 2.0; - map.at("layer", Index(5,6)) = 2.0; - map.at("layer", Index(6,4)) = 2.0; - map.at("layer", Index(6,5)) = 2.0; - map.at("layer", Index(6,6)) = 2.0; - map.at("layer", Index(7,4)) = 2.0; - map.at("layer", Index(7,5)) = 2.0; - map.at("layer", Index(7,6)) = 2.0; + map.at("layer", Index(3, 4)) = 2.0; + map.at("layer", Index(3, 5)) = 2.0; + map.at("layer", Index(3, 6)) = 2.0; + map.at("layer", Index(4, 4)) = 2.0; + map.at("layer", Index(4, 5)) = 2.0; + map.at("layer", Index(4, 6)) = 2.0; + map.at("layer", Index(5, 4)) = 2.0; + map.at("layer", Index(5, 5)) = 2.0; + map.at("layer", Index(5, 6)) = 2.0; + map.at("layer", Index(6, 4)) = 2.0; + map.at("layer", Index(6, 5)) = 2.0; + map.at("layer", Index(6, 6)) = 2.0; + map.at("layer", Index(7, 4)) = 2.0; + map.at("layer", Index(7, 5)) = 2.0; + map.at("layer", Index(7, 6)) = 2.0; SignedDistanceField sdf; sdf.calculateSignedDistanceField(map, "layer", 1.5); @@ -213,31 +213,31 @@ TEST(SignedDistanceField, GetInterpolatedDistance) map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25)); map["layer"].setConstant(1.0); - map.at("layer", Index(3,3)) = 2.0; - map.at("layer", Index(3,4)) = 2.0; - map.at("layer", Index(3,5)) = 2.0; - map.at("layer", Index(3,6)) = 2.0; - map.at("layer", Index(3,7)) = 2.0; - map.at("layer", Index(4,3)) = 2.0; - map.at("layer", Index(4,4)) = 2.0; - map.at("layer", Index(4,5)) = 2.0; - map.at("layer", Index(4,6)) = 2.0; - map.at("layer", Index(4,7)) = 2.0; - map.at("layer", Index(5,3)) = 2.0; - map.at("layer", Index(5,4)) = 2.0; - map.at("layer", Index(5,5)) = 2.0; - map.at("layer", Index(5,6)) = 2.0; - map.at("layer", Index(5,7)) = 2.0; - map.at("layer", Index(6,3)) = 2.0; - map.at("layer", Index(6,4)) = 2.0; - map.at("layer", Index(6,5)) = 2.0; - map.at("layer", Index(6,6)) = 2.0; - map.at("layer", Index(6,7)) = 2.0; - map.at("layer", Index(7,3)) = 2.0; - map.at("layer", Index(7,4)) = 2.0; - map.at("layer", Index(7,5)) = 2.0; - map.at("layer", Index(7,6)) = 2.0; - map.at("layer", Index(7,7)) = 2.0; + map.at("layer", Index(3, 3)) = 2.0; + map.at("layer", Index(3, 4)) = 2.0; + map.at("layer", Index(3, 5)) = 2.0; + map.at("layer", Index(3, 6)) = 2.0; + map.at("layer", Index(3, 7)) = 2.0; + map.at("layer", Index(4, 3)) = 2.0; + map.at("layer", Index(4, 4)) = 2.0; + map.at("layer", Index(4, 5)) = 2.0; + map.at("layer", Index(4, 6)) = 2.0; + map.at("layer", Index(4, 7)) = 2.0; + map.at("layer", Index(5, 3)) = 2.0; + map.at("layer", Index(5, 4)) = 2.0; + map.at("layer", Index(5, 5)) = 2.0; + map.at("layer", Index(5, 6)) = 2.0; + map.at("layer", Index(5, 7)) = 2.0; + map.at("layer", Index(6, 3)) = 2.0; + map.at("layer", Index(6, 4)) = 2.0; + map.at("layer", Index(6, 5)) = 2.0; + map.at("layer", Index(6, 6)) = 2.0; + map.at("layer", Index(6, 7)) = 2.0; + map.at("layer", Index(7, 3)) = 2.0; + map.at("layer", Index(7, 4)) = 2.0; + map.at("layer", Index(7, 5)) = 2.0; + map.at("layer", Index(7, 6)) = 2.0; + map.at("layer", Index(7, 7)) = 2.0; SignedDistanceField sdf; sdf.calculateSignedDistanceField(map, "layer", 1.5); diff --git a/grid_map_sdf/test/test_grid_map_sdf.cpp b/grid_map_sdf/test/test_grid_map_sdf.cpp index 9c3349dfc..f7b7162d2 100644 --- a/grid_map_sdf/test/test_grid_map_sdf.cpp +++ b/grid_map_sdf/test/test_grid_map_sdf.cpp @@ -10,9 +10,9 @@ #include // Run all the tests that were declared with TEST() -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - srand((int)time(0)); + srand(static_cast(time(0))); return RUN_ALL_TESTS(); } diff --git a/grid_map_visualization/CHANGELOG.rst b/grid_map_visualization/CHANGELOG.rst index 40798aa98..31e55b0dc 100644 --- a/grid_map_visualization/CHANGELOG.rst +++ b/grid_map_visualization/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package grid_map_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.2 (2025-09-23) +------------------ + +2.2.1 (2025-01-18) +------------------ + +2.2.0 (2024-07-23) +------------------ +* Merge pull request `#443 `_ from Ryanf55/update-maintainers + Add Ryan as maintainer, remove Steve +* Add Ryan as maintainer, remove Steve +* Contributors: Ryan, Ryan Friedman + +2.1.0 (2022-11-08) +------------------ + +2.0.0 (2022-09-13) +------------------ +* fix: mark Eigen library as SYSTEM +* fix: build error on jammy +* Initial ROS2 port +* Contributors: Maximilian Wulf, Steve Macenski + 1.6.2 (2019-10-14) ------------------ diff --git a/grid_map_visualization/CMakeLists.txt b/grid_map_visualization/CMakeLists.txt index 6b999d1d9..f46704c14 100644 --- a/grid_map_visualization/CMakeLists.txt +++ b/grid_map_visualization/CMakeLists.txt @@ -1,45 +1,28 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(grid_map_visualization) -set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") +## Find libraries +find_package(ament_cmake REQUIRED) +find_package(grid_map_cmake_helpers REQUIRED) +find_package(grid_map_core REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(Eigen3 REQUIRED) -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roscpp - tf +grid_map_package() + +set(dependencies grid_map_core - grid_map_ros grid_map_msgs - visualization_msgs - sensor_msgs + grid_map_ros nav_msgs -) - -## System dependencies are found with CMake's conventions -#find_package(Eigen3 REQUIRED) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS - include -# LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS - roscpp - tf - grid_map_core - grid_map_ros - grid_map_msgs - visualization_msgs - sensor_msgs - nav_msgs + rclcpp + sensor_msgs + visualization_msgs ) ########### @@ -47,42 +30,42 @@ catkin_package( ########### ## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} + SYSTEM + ${EIGEN3_INCLUDE_DIR} ) -## Declare a cpp library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/grid_map_visualization_node.cpp -# ) +set(library_name ${PROJECT_NAME}_core) -## Declare a cpp executable -add_executable( - ${PROJECT_NAME} - src/grid_map_visualization_node.cpp +## Declare a cpp library +add_library(${library_name} SHARED src/GridMapVisualization.cpp src/GridMapVisualizationHelpers.cpp - src/visualizations/VisualizationBase.cpp - src/visualizations/VisualizationFactory.cpp - src/visualizations/PointCloudVisualization.cpp src/visualizations/FlatPointCloudVisualization.cpp - src/visualizations/VectorVisualization.cpp - src/visualizations/OccupancyGridVisualization.cpp src/visualizations/GridCellsVisualization.cpp src/visualizations/MapRegionVisualization.cpp + src/visualizations/OccupancyGridVisualization.cpp + src/visualizations/PointCloudVisualization.cpp + src/visualizations/VectorVisualization.cpp + src/visualizations/VisualizationFactory.cpp + src/visualizations/VisualizationBase.cpp ) -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(grid_map_visualization_node grid_map_visualization_generate_messages_cpp) +ament_target_dependencies(${library_name} SYSTEM + ${dependencies} +) -## Specify libraries to link a library or executable target against -target_link_libraries( - ${PROJECT_NAME} - ${catkin_LIBRARIES} +# Declare cpp node executable +add_executable(${PROJECT_NAME} + src/grid_map_visualization_node.cpp +) + +## Specify libraries to link executable target against +target_link_libraries(${PROJECT_NAME} ${library_name}) + +ament_target_dependencies(${PROJECT_NAME} SYSTEM + ${dependencies} ) ############# @@ -90,15 +73,53 @@ target_link_libraries( ############# # Mark executables and/or libraries for installation -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} ) # Mark other files for installation install( DIRECTORY doc - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) + +############# +## Testing ## +############# + +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright -build/include_order + ) + endif() + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp index 4504d6668..674c7ba91 100644 --- a/grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/GridMapVisualization.hpp @@ -7,36 +7,38 @@ * */ -#pragma once +#ifndef GRID_MAP_VISUALIZATION__GRIDMAPVISUALIZATION_HPP_ +#define GRID_MAP_VISUALIZATION__GRIDMAPVISUALIZATION_HPP_ -#include -#include +#include #include +#include #include #include -#include +#include // ROS -#include +#include // STD -#include #include +#include +#include -namespace grid_map_visualization { +namespace grid_map_visualization +{ /*! * Visualizes a grid map by publishing different topics that can be viewed in Rviz. */ class GridMapVisualization { - public: - +public: /*! * Constructor. - * @param nodeHandle the ROS node handle. + * @param parameterName The config parameter name. */ - GridMapVisualization(ros::NodeHandle& nodeHandle, const std::string& parameterName); + explicit GridMapVisualization(const std::string & parameterName); /*! * Destructor. @@ -47,10 +49,12 @@ class GridMapVisualization * Callback function for the grid map. * @param message the grid map message to be visualized. */ - void callback(const grid_map_msgs::GridMap& message); + void callback(const grid_map_msgs::msg::GridMap::SharedPtr message); - private: + //! ROS node shared pointer + rclcpp::Node::SharedPtr nodePtr; +private: /*! * Read parameters from ROS. * @return true if successful. @@ -66,19 +70,16 @@ class GridMapVisualization /*! * Check if visualizations are active (subscribed to), * and accordingly cancels/activates the subscription to the - * grid map to safe bandwidth. + * grid map to save bandwidth. * @param timerEvent the timer event. */ - void updateSubscriptionCallback(const ros::TimerEvent& timerEvent); - - //! ROS nodehandle. - ros::NodeHandle& nodeHandle_; + void updateSubscriptionCallback(); //! Parameter name of the visualizer configuration list. std::string visualizationsParameter_; //! ROS subscriber to the grid map. - ros::Subscriber mapSubscriber_; + rclcpp::Subscription::SharedPtr mapSubscriber_; //! Topic name of the grid map to be visualized. std::string mapTopic_; @@ -87,16 +88,20 @@ class GridMapVisualization std::vector> visualizations_; //! Visualization factory. - VisualizationFactory factory_; + std::shared_ptr factory_; //! Timer to check the activity of the visualizations. - ros::Timer activityCheckTimer_; + rclcpp::TimerBase::SharedPtr activityCheckTimer_; - //! Duration of checking the activity of the visualizations. - ros::Duration activityCheckDuration_; + //! Rate of checking the activity of the visualizations. + double activityCheckRate_; //! If the grid map visualization is subscribed to the grid map. bool isSubscribed_; + + //! If the grid map subscriber is Transient local. + bool isGridMapSubLatched_; }; -} /* namespace */ +} // namespace grid_map_visualization +#endif // GRID_MAP_VISUALIZATION__GRIDMAPVISUALIZATION_HPP_ diff --git a/grid_map_visualization/include/grid_map_visualization/GridMapVisualizationHelpers.hpp b/grid_map_visualization/include/grid_map_visualization/GridMapVisualizationHelpers.hpp index 39d9551f5..618cee8fb 100644 --- a/grid_map_visualization/include/grid_map_visualization/GridMapVisualizationHelpers.hpp +++ b/grid_map_visualization/include/grid_map_visualization/GridMapVisualizationHelpers.hpp @@ -6,17 +6,19 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_VISUALIZATION__GRIDMAPVISUALIZATIONHELPERS_HPP_ +#define GRID_MAP_VISUALIZATION__GRIDMAPVISUALIZATIONHELPERS_HPP_ // ROS -#include -#include -#include +#include +#include +#include // Eigen #include -namespace grid_map_visualization { +namespace grid_map_visualization +{ /*! * Create a color message from a color vector. @@ -24,14 +26,19 @@ namespace grid_map_visualization { * @param[in] colorVector the color vector * @param[in] resetTransparency if transparency should be reset (to fully visible) or left at current value. */ -void getColorMessageFromColorVector(std_msgs::ColorRGBA& colorMessage, const Eigen::Vector3f& colorVector, bool resetTransparency = true); +void getColorMessageFromColorVector( + std_msgs::msg::ColorRGBA & colorMessage, + const Eigen::Vector3f & colorVector, + bool resetTransparency = true); /*! * Create a color vector from a color message. * @param[out] colorVector the color vector. * @param[in] colorMessage the color message. */ -void getColorVectorFromColorMessage(Eigen::Vector3f& colorVector, const std_msgs::ColorRGBA& colorMessage); +void getColorVectorFromColorMessage( + Eigen::Vector3f & colorVector, + const std_msgs::msg::ColorRGBA & colorMessage); /*! * Sets a color message from a color value. @@ -39,7 +46,9 @@ void getColorVectorFromColorMessage(Eigen::Vector3f& colorVector, const std_msgs * @param[in] colorValue the color value. * @param[in] resetTransparency if transparency should be reset (to fully visible) or left at current value. */ -void setColorFromColorValue(std_msgs::ColorRGBA& color, const unsigned long& colorValue, bool resetTransparency = true); +void setColorFromColorValue( + std_msgs::msg::ColorRGBA & color, const uint64_t & colorValue, + bool resetTransparency = true); /*! * Set the color channel from a scalar value. @@ -51,9 +60,11 @@ void setColorFromColorValue(std_msgs::ColorRGBA& color, const unsigned long& col * @param[in] colorChannelLowerValue the lower value for the color channel. * @param[in] colorChannelUpperValue the upper value for the color channel. */ -void setColorChannelFromValue(float& colorChannel, const double value, const double lowerValueBound, - const double upperValueBound, const bool invert = false, const double colorChannelLowerValue = 0.0, - const double colorChannelUpperValue = 1.0); +void setColorChannelFromValue( + float & colorChannel, const double value, const double lowerValueBound, + const double upperValueBound, const bool invert = false, + const double colorChannelLowerValue = 0.0, + const double colorChannelUpperValue = 1.0); /*! * Sets the color to the interpolation between two colors based on a scalar value. @@ -64,9 +75,10 @@ void setColorChannelFromValue(float& colorChannel, const double value, const dou * @param[in] lowerValueBound the lower boundary of the value. * @param[in] upperValueBound the upper boundary of the value. */ -void interpolateBetweenColors(std_msgs::ColorRGBA& color, const std_msgs::ColorRGBA& colorForLowerValue, - const std_msgs::ColorRGBA& colorForUpperValue, const double value, - const double lowerValueBound, const double upperValueBound); +void interpolateBetweenColors( + std_msgs::msg::ColorRGBA & color, const std_msgs::msg::ColorRGBA & colorForLowerValue, + const std_msgs::msg::ColorRGBA & colorForUpperValue, const double value, + const double lowerValueBound, const double upperValueBound); /*! * Sets the saturation of the color from a scalar value. @@ -77,8 +89,9 @@ void interpolateBetweenColors(std_msgs::ColorRGBA& color, const std_msgs::ColorR * @param[in] maxSaturation the maximum saturation. * @param[in] minSaturation the minimum saturation. */ -void setSaturationFromValue(std_msgs::ColorRGBA& color, const double value, const double lowerValueBound, - const double upperValueBound, const double maxSaturation, const double minSaturation); +void setSaturationFromValue( + std_msgs::msg::ColorRGBA & color, const double value, const double lowerValueBound, + const double upperValueBound, const double maxSaturation, const double minSaturation); /*! * Set the color from the rainbow color spectrum based on scalar value. @@ -87,8 +100,9 @@ void setSaturationFromValue(std_msgs::ColorRGBA& color, const double value, cons * @param[in] lowerValueBound the lower boundary of the value. * @param[in] upperValueBound the upper boundary of the value. */ -void setColorFromValue(std_msgs::ColorRGBA& color, const double value, const double lowerValueBound, - const double upperValueBound); +void setColorFromValue( + std_msgs::msg::ColorRGBA & color, const double value, const double lowerValueBound, + const double upperValueBound); /*! * Computes a linear mapping for a query from the source and to the map. @@ -100,7 +114,8 @@ void setColorFromValue(std_msgs::ColorRGBA& color, const double value, const dou * @return the query mapped to the map. */ double computeLinearMapping( - const double& sourceValue, const double& sourceLowerValue, const double& sourceUpperValue, - const double& mapLowerValue, const double& mapUpperValue); + const double & sourceValue, const double & sourceLowerValue, const double & sourceUpperValue, + const double & mapLowerValue, const double & mapUpperValue); -} /* namespace */ +} // namespace grid_map_visualization +#endif // GRID_MAP_VISUALIZATION__GRIDMAPVISUALIZATIONHELPERS_HPP_ diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/FlatPointCloudVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/FlatPointCloudVisualization.hpp index c2cbe0e4e..2440bc6df 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/FlatPointCloudVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/FlatPointCloudVisualization.hpp @@ -6,29 +6,32 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_VISUALIZATION__VISUALIZATIONS__FLATPOINTCLOUDVISUALIZATION_HPP_ +#define GRID_MAP_VISUALIZATION__VISUALIZATIONS__FLATPOINTCLOUDVISUALIZATION_HPP_ -#include #include // ROS -#include +#include -namespace grid_map_visualization { +#include + +#include "grid_map_visualization/visualizations/VisualizationBase.hpp" + +namespace grid_map_visualization +{ /*! * Visualization of the grid map as a flat point cloud. */ class FlatPointCloudVisualization : public VisualizationBase { - public: - +public: /*! * Constructor. - * @param nodeHandle the ROS node handle. * @param name the name of the visualization. */ - FlatPointCloudVisualization(ros::NodeHandle& nodeHandle, const std::string& name); + explicit FlatPointCloudVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. @@ -40,26 +43,30 @@ class FlatPointCloudVisualization : public VisualizationBase * @param config the parameters as XML. * @return true if successful. */ - bool readParameters(XmlRpc::XmlRpcValue& config); + bool readParameters() override; /*! * Initialization. */ - bool initialize(); + bool initialize() override; /*! * Generates the visualization. * @param map the grid map to visualize. * @return true if successful. */ - bool visualize(const grid_map::GridMap& map); + bool visualize(const grid_map::GridMap & map) override; - private: +private: //! Type that is transformed to points. std::string layer_; //! Height of the z-coordinate at which the flat point cloud is visualized. double height_; + + //! ROS publisher. + rclcpp::Publisher::SharedPtr publisher_; }; -} /* namespace */ +} // namespace grid_map_visualization +#endif // GRID_MAP_VISUALIZATION__VISUALIZATIONS__FLATPOINTCLOUDVISUALIZATION_HPP_ diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/GridCellsVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/GridCellsVisualization.hpp index d697ccbdf..4ca0a9b3c 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/GridCellsVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/GridCellsVisualization.hpp @@ -6,26 +6,31 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_VISUALIZATION__VISUALIZATIONS__GRIDCELLSVISUALIZATION_HPP_ +#define GRID_MAP_VISUALIZATION__VISUALIZATIONS__GRIDCELLSVISUALIZATION_HPP_ -#include #include +#include // ROS -#include +#include -namespace grid_map_visualization { +#include -class GridCellsVisualization : public VisualizationBase +#include "grid_map_visualization/visualizations/VisualizationBase.hpp" + + +namespace grid_map_visualization { - public: +class GridCellsVisualization : public VisualizationBase +{ +public: /*! * Constructor. - * @param nodeHandle the ROS node handle. * @param name the name of the visualization. */ - GridCellsVisualization(ros::NodeHandle& nodeHandle, const std::string& name); + explicit GridCellsVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. @@ -37,27 +42,30 @@ class GridCellsVisualization : public VisualizationBase * @param config the parameters as XML. * @return true if successful. */ - bool readParameters(XmlRpc::XmlRpcValue& config); + bool readParameters() override; /*! * Initialization. */ - bool initialize(); + bool initialize() override; /*! * Generates the visualization. * @param map the grid map to visualize. * @return true if successful. */ - bool visualize(const grid_map::GridMap& map); - - private: + bool visualize(const grid_map::GridMap & map) override; +private: //! Type that is transformed to the occupancy grid. std::string layer_; //! Values that are between lower and upper threshold are shown. float lowerThreshold_, upperThreshold_; + + //! ROS publisher. + rclcpp::Publisher::SharedPtr publisher_; }; -} /* namespace */ +} // namespace grid_map_visualization +#endif // GRID_MAP_VISUALIZATION__VISUALIZATIONS__GRIDCELLSVISUALIZATION_HPP_ diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/MapRegionVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/MapRegionVisualization.hpp index 2a501d05e..1f48ea8cd 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/MapRegionVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/MapRegionVisualization.hpp @@ -6,31 +6,34 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_VISUALIZATION__VISUALIZATIONS__MAPREGIONVISUALIZATION_HPP_ +#define GRID_MAP_VISUALIZATION__VISUALIZATIONS__MAPREGIONVISUALIZATION_HPP_ -#include #include // ROS -#include -#include -#include +#include +#include +#include -namespace grid_map_visualization { +#include + +#include "grid_map_visualization/visualizations/VisualizationBase.hpp" + +namespace grid_map_visualization +{ /*! * Visualization of the region of the grid map as border line. */ class MapRegionVisualization : public VisualizationBase { - public: - +public: /*! * Constructor. - * @param nodeHandle the ROS node handle. * @param name the name of the visualization. */ - MapRegionVisualization(ros::NodeHandle& nodeHandle, const std::string& name); + explicit MapRegionVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. @@ -42,34 +45,36 @@ class MapRegionVisualization : public VisualizationBase * @param config the parameters as XML. * @return true if successful. */ - bool readParameters(XmlRpc::XmlRpcValue& config); + bool readParameters() override; /*! * Initialization. */ - bool initialize(); + bool initialize() override; /*! * Generates the visualization. * @param map the grid map to visualize. * @return true if successful. */ - bool visualize(const grid_map::GridMap& map); - - private: + bool visualize(const grid_map::GridMap & map) override; +private: //! Marker to be published. - visualization_msgs::Marker marker_; + visualization_msgs::msg::Marker marker_; //! Number of vertices of the map region visualization. const unsigned int nVertices_; //! Color of the map region visualization. - std_msgs::ColorRGBA color_; + std_msgs::msg::ColorRGBA color_; //! Line width of the map region marker [m]. double lineWidth_; + //! ROS publisher. + rclcpp::Publisher::SharedPtr publisher_; }; -} /* namespace */ +} // namespace grid_map_visualization +#endif // GRID_MAP_VISUALIZATION__VISUALIZATIONS__MAPREGIONVISUALIZATION_HPP_ diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/OccupancyGridVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/OccupancyGridVisualization.hpp index 7cbad00ca..94552bb53 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/OccupancyGridVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/OccupancyGridVisualization.hpp @@ -6,26 +6,27 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_VISUALIZATION__VISUALIZATIONS__OCCUPANCYGRIDVISUALIZATION_HPP_ +#define GRID_MAP_VISUALIZATION__VISUALIZATIONS__OCCUPANCYGRIDVISUALIZATION_HPP_ -#include #include +#include -// ROS -#include +#include -namespace grid_map_visualization { +#include "grid_map_visualization/visualizations/VisualizationBase.hpp" -class OccupancyGridVisualization : public VisualizationBase +namespace grid_map_visualization { - public: +class OccupancyGridVisualization : public VisualizationBase +{ +public: /*! * Constructor. - * @param nodeHandle the ROS node handle. * @param name the name of the visualization. */ - OccupancyGridVisualization(ros::NodeHandle& nodeHandle, const std::string& name); + explicit OccupancyGridVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. @@ -37,27 +38,31 @@ class OccupancyGridVisualization : public VisualizationBase * @param config the parameters as XML. * @return true if successful. */ - bool readParameters(XmlRpc::XmlRpcValue& config); + bool readParameters() override; /*! * Initialization. */ - bool initialize(); + bool initialize() override; /*! * Generates the visualization. * @param map the grid map to visualize. * @return true if successful. */ - bool visualize(const grid_map::GridMap& map); - - private: + bool visualize(const grid_map::GridMap & map) override; +private: //! Type that is transformed to the occupancy grid. std::string layer_; - //! Minimum and maximum value of the grid map data (used to normalize the cell data in [min, max]). + //! Minimum and maximum value of the grid map data + // (used to normalize the cell data in [min, max]). float dataMin_, dataMax_; + + //! ROS publisher. + rclcpp::Publisher::SharedPtr publisher_; }; -} /* namespace */ +} // namespace grid_map_visualization +#endif // GRID_MAP_VISUALIZATION__VISUALIZATIONS__OCCUPANCYGRIDVISUALIZATION_HPP_ diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/PointCloudVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/PointCloudVisualization.hpp index 77c64a282..27ee6fa9e 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/PointCloudVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/PointCloudVisualization.hpp @@ -6,29 +6,33 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_VISUALIZATION__VISUALIZATIONS__POINTCLOUDVISUALIZATION_HPP_ +#define GRID_MAP_VISUALIZATION__VISUALIZATIONS__POINTCLOUDVISUALIZATION_HPP_ -#include #include +#include // ROS -#include +#include -namespace grid_map_visualization { +#include + +#include "grid_map_visualization/visualizations/VisualizationBase.hpp" + +namespace grid_map_visualization +{ /*! * Visualization of the grid map as a point cloud. */ class PointCloudVisualization : public VisualizationBase { - public: - +public: /*! * Constructor. - * @param nodeHandle the ROS node handle. * @param name the name of the visualization. */ - PointCloudVisualization(ros::NodeHandle& nodeHandle, const std::string& name); + explicit PointCloudVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. @@ -40,23 +44,27 @@ class PointCloudVisualization : public VisualizationBase * @param config the parameters as XML. * @return true if successful. */ - bool readParameters(XmlRpc::XmlRpcValue& config); + bool readParameters() override; /*! * Initialization. */ - bool initialize(); + bool initialize() override; /*! * Generates the visualization. * @param map the grid map to visualize. * @return true if successful. */ - bool visualize(const grid_map::GridMap& map); + bool visualize(const grid_map::GridMap & map) override; - private: +private: //! Type that is transformed to points. std::string layer_; + + //! ROS publisher. + rclcpp::Publisher::SharedPtr publisher_; }; -} /* namespace */ +} // namespace grid_map_visualization +#endif // GRID_MAP_VISUALIZATION__VISUALIZATIONS__POINTCLOUDVISUALIZATION_HPP_ diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/VectorVisualization.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/VectorVisualization.hpp index 45ed18f42..f0f84a43a 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/VectorVisualization.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/VectorVisualization.hpp @@ -6,34 +6,36 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_VISUALIZATION__VISUALIZATIONS__VECTORVISUALIZATION_HPP_ +#define GRID_MAP_VISUALIZATION__VISUALIZATIONS__VECTORVISUALIZATION_HPP_ -#include #include // ROS -#include -#include -#include +#include +#include +#include // STD +#include #include -namespace grid_map_visualization { +#include "grid_map_visualization/visualizations/VisualizationBase.hpp" + +namespace grid_map_visualization +{ /*! * Visualization a combination of three layers of the grid map as a vector field. */ class VectorVisualization : public VisualizationBase { - public: - +public: /*! * Constructor. - * @param nodeHandle the ROS node handle. * @param name the name of the visualization. */ - VectorVisualization(ros::NodeHandle& nodeHandle, const std::string& name); + explicit VectorVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. @@ -45,24 +47,23 @@ class VectorVisualization : public VisualizationBase * @param config the parameters as XML. * @return true if successful. */ - bool readParameters(XmlRpc::XmlRpcValue& config); + bool readParameters() override; /*! * Initialization. */ - bool initialize(); + bool initialize() override; /*! * Generates the visualization. * @param map the grid map to visualize. * @return true if successful. */ - bool visualize(const grid_map::GridMap& map); - - private: + bool visualize(const grid_map::GridMap & map) override; +private: //! Marker to be published. - visualization_msgs::Marker marker_; + visualization_msgs::msg::Marker marker_; //! Types that are transformed to vectors. std::vector types_; @@ -77,7 +78,11 @@ class VectorVisualization : public VisualizationBase double lineWidth_; //! Color of the vectors. - std_msgs::ColorRGBA color_; + std_msgs::msg::ColorRGBA color_; + + //! ROS publisher. + rclcpp::Publisher::SharedPtr publisher_; }; -} /* namespace */ +} // namespace grid_map_visualization +#endif // GRID_MAP_VISUALIZATION__VISUALIZATIONS__VECTORVISUALIZATION_HPP_ diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationBase.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationBase.hpp index 6e725d6b0..1f729890a 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationBase.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationBase.hpp @@ -6,27 +6,29 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_VISUALIZATION__VISUALIZATIONS__VISUALIZATIONBASE_HPP_ +#define GRID_MAP_VISUALIZATION__VISUALIZATIONS__VISUALIZATIONBASE_HPP_ + #include // ROS -#include +#include -namespace grid_map_visualization { +#include +#include -typedef std::map StringMap; +namespace grid_map_visualization +{ class VisualizationBase { - public: - +public: /*! * Constructor. - * @param nodeHandle the ROS node handle. * @param name the name of the visualization. */ - VisualizationBase(ros::NodeHandle& nodeHandle, const std::string& name); + explicit VisualizationBase(const std::string & name, rclcpp::Node::SharedPtr nodePtr); /*! * Destructor. @@ -35,10 +37,9 @@ class VisualizationBase /*! * Read parameters from ROS. - * @param config the parameters as XML. * @return true if successful. */ - virtual bool readParameters(XmlRpc::XmlRpcValue& config); + virtual bool readParameters() = 0; /*! * Initialization. @@ -50,7 +51,7 @@ class VisualizationBase * @param map the grid map to visualize. * @return true if successful. */ - virtual bool visualize(const grid_map::GridMap& map) = 0; + virtual bool visualize(const grid_map::GridMap & map) = 0; /*! * Checks if visualization is active (if somebody has actually subscribed). @@ -58,59 +59,13 @@ class VisualizationBase */ bool isActive() const; - protected: - - /*! - * Get a visualization parameter as a string. - * @param[in] name the name of the parameter - * @param[out] value the string to set with the value. - * @return true if parameter was found, false otherwise. - */ - bool getParam(const std::string& name, std::string& value); - - /*! - * Get a visualization parameter as a double. - * @param[in] name the name of the parameter - * @param[out] value the double to set with the value. - * @return true if parameter was found, false otherwise. - */ - bool getParam(const std::string& name, double& value); - - /*! - * Get a visualization parameter as a float. - * @param[in] name the name of the parameter - * @param[out] value the float to set with the value. - * @return true if parameter was found, false otherwise. - */ - bool getParam(const std::string& name, float& value); - - /*! - * Get a visualization parameter as an integer. - * @param[in] name the name of the parameter - * @param[out] value the int to set with the value. - * @return true if parameter was found, false otherwise. - */ - bool getParam(const std::string&name, int& value); - - /*! - * Get a visualization parameter as a boolean. - * @param[in] name the name of the parameter - * @param[out] value the boolean to set with the value. - * @return true if parameter was found, false otherwise. - */ - bool getParam(const std::string& name, bool& value); - - //! ROS nodehandle. - ros::NodeHandle& nodeHandle_; - - //! Name of the visualization. +protected: + //! visualization name std::string name_; - //! Storage of the parsed XML parameters. - StringMap parameters_; - - //! ROS publisher of the occupancy grid. - ros::Publisher publisher_; + //! ROS node shared pointer + rclcpp::Node::SharedPtr nodePtr_; }; -} /* namespace */ +} // namespace grid_map_visualization +#endif // GRID_MAP_VISUALIZATION__VISUALIZATIONS__VISUALIZATIONBASE_HPP_ diff --git a/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationFactory.hpp b/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationFactory.hpp index a10bf4a36..f31c165bb 100644 --- a/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationFactory.hpp +++ b/grid_map_visualization/include/grid_map_visualization/visualizations/VisualizationFactory.hpp @@ -6,27 +6,34 @@ * Institute: ETH Zurich, ANYbotics */ -#pragma once +#ifndef GRID_MAP_VISUALIZATION__VISUALIZATIONS__VISUALIZATIONFACTORY_HPP_ +#define GRID_MAP_VISUALIZATION__VISUALIZATIONS__VISUALIZATIONFACTORY_HPP_ -#include -#include -#include #include +#include +#include -namespace grid_map_visualization { +#include "grid_map_visualization/visualizations/VisualizationBase.hpp" + +namespace grid_map_visualization +{ class VisualizationFactory { - public: - VisualizationFactory(ros::NodeHandle& nodeHandle); +public: + explicit VisualizationFactory(rclcpp::Node::SharedPtr nodePtr); virtual ~VisualizationFactory(); - bool isValidType(const std::string& type); - std::shared_ptr getInstance(const std::string& type, const std::string& name); + bool isValidType(const std::string & type); + + std::shared_ptr getInstance( + const std::string & type, + const std::string & name); - private: - ros::NodeHandle& nodeHandle_; +private: std::vector types_; + rclcpp::Node::SharedPtr nodePtr_; }; -} /* namespace */ +} // namespace grid_map_visualization +#endif // GRID_MAP_VISUALIZATION__VISUALIZATIONS__VISUALIZATIONFACTORY_HPP_ diff --git a/grid_map_visualization/package.xml b/grid_map_visualization/package.xml index a208a8107..03ecff9b8 100644 --- a/grid_map_visualization/package.xml +++ b/grid_map_visualization/package.xml @@ -1,21 +1,33 @@ - + + grid_map_visualization - 1.6.2 + 2.2.2 Configurable tool to visualize grid maps in RViz. Maximilian Wulf Yoshua Nava + Ryan Friedman BSD http://github.com/anybotics/grid_map http://github.com/anybotics/grid_map/issues Péter Fankhauser - catkin - roscpp + + ament_cmake + grid_map_cmake_helpers + grid_map_core - grid_map_ros grid_map_msgs - tf - visualization_msgs - sensor_msgs + grid_map_ros nav_msgs + rclcpp + sensor_msgs + visualization_msgs + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + diff --git a/grid_map_visualization/src/GridMapVisualization.cpp b/grid_map_visualization/src/GridMapVisualization.cpp index 2301e9818..91a57f65b 100644 --- a/grid_map_visualization/src/GridMapVisualization.cpp +++ b/grid_map_visualization/src/GridMapVisualization.cpp @@ -6,27 +6,33 @@ * Institute: ETH Zurich, ANYbotics */ -#include "grid_map_visualization/GridMapVisualization.hpp" #include #include -using namespace std; -using namespace ros; +#include +#include +#include +#include +#include + +#include "grid_map_visualization/GridMapVisualization.hpp" -namespace grid_map_visualization { +namespace grid_map_visualization +{ -GridMapVisualization::GridMapVisualization(ros::NodeHandle& nodeHandle, - const std::string& parameterName) - : nodeHandle_(nodeHandle), - visualizationsParameter_(parameterName), - factory_(nodeHandle_), - isSubscribed_(false) +GridMapVisualization::GridMapVisualization(const std::string & parameterName) +: visualizationsParameter_(parameterName), + isSubscribed_(false) { - ROS_INFO("Grid map visualization node started."); + nodePtr = std::make_shared("grid_map_visualization"); + factory_ = std::make_shared(nodePtr); + + RCLCPP_INFO(nodePtr->get_logger(), "Grid map visualization nodePtr started."); readParameters(); - activityCheckTimer_ = nodeHandle_.createTimer(activityCheckDuration_, - &GridMapVisualization::updateSubscriptionCallback, - this); + + activityCheckTimer_ = nodePtr->create_wall_timer( + std::chrono::duration(1.0 / activityCheckRate_), + std::bind(&GridMapVisualization::updateSubscriptionCallback, this)); initialize(); } @@ -36,107 +42,97 @@ GridMapVisualization::~GridMapVisualization() bool GridMapVisualization::readParameters() { - nodeHandle_.param("grid_map_topic", mapTopic_, string("/grid_map")); + nodePtr->declare_parameter("grid_map_topic", std::string("/grid_map")); + nodePtr->declare_parameter("activity_check_rate", 2.0); + nodePtr->declare_parameter(visualizationsParameter_, std::vector()); + nodePtr->declare_parameter("transient_local", rclcpp::ParameterValue(false)); - double activityCheckRate; - nodeHandle_.param("activity_check_rate", activityCheckRate, 2.0); - activityCheckDuration_.fromSec(1.0 / activityCheckRate); - ROS_ASSERT(!activityCheckDuration_.isZero()); + nodePtr->get_parameter("grid_map_topic", mapTopic_); + nodePtr->get_parameter("activity_check_rate", activityCheckRate_); + nodePtr->get_parameter("transient_local", isGridMapSubLatched_); + + assert(activityCheckRate_); // Configure the visualizations from a configuration stored on the parameter server. - XmlRpc::XmlRpcValue config; - if (!nodeHandle_.getParam(visualizationsParameter_, config)) { - ROS_WARN( - "Could not load the visualizations configuration from parameter %s,are you sure it" - "was pushed to the parameter server? Assuming that you meant to leave it empty.", - visualizationsParameter_.c_str()); + std::vector config; + if (!nodePtr->get_parameter(visualizationsParameter_, config)) { + RCLCPP_WARN( + nodePtr->get_logger(), + "Could not load the visualizations configuration from parameter %s,are you sure it" + "was pushed to the parameter server? Assuming that you meant to leave it empty.", + visualizationsParameter_.c_str()); return false; } - // Verify proper naming and structure, - if (config.getType() != XmlRpc::XmlRpcValue::TypeArray) { - ROS_ERROR("%s: The visualization specification must be a list, but it is of XmlRpcType %d", - visualizationsParameter_.c_str(), config.getType()); - ROS_ERROR("The XML passed in is formatted as follows:\n %s", config.toXml().c_str()); - return false; - } + std::unordered_set config_check; // Iterate over all visualizations (may be just one), - for (unsigned int i = 0; i < config.size(); ++i) { - if (config[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("%s: Visualizations must be specified as maps, but they are XmlRpcType:%d", - visualizationsParameter_.c_str(), config[i].getType()); - return false; - } else if (!config[i].hasMember("type")) { - ROS_ERROR("%s: Could not add a visualization because no type was given", - visualizationsParameter_.c_str()); - return false; - } else if (!config[i].hasMember("name")) { - ROS_ERROR("%s: Could not add a visualization because no name was given", - visualizationsParameter_.c_str()); - return false; + for (auto name : config) { + std::string type; + + // Check for name collisions within the list itself. + if (config_check.find(name) == config_check.end()) { + config_check.insert(name); } else { - //Check for name collisions within the list itself. - for (int j = i + 1; j < config.size(); ++j) { - if (config[j].getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("%s: Visualizations must be specified as maps, but they are XmlRpcType:%d", - visualizationsParameter_.c_str(), config[j].getType()); - return false; - } - - if (!config[j].hasMember("name") - || config[i]["name"].getType() != XmlRpc::XmlRpcValue::TypeString - || config[j]["name"].getType() != XmlRpc::XmlRpcValue::TypeString) { - ROS_ERROR("%s: Visualizations names must be strings, but they are XmlRpcTypes:%d and %d", - visualizationsParameter_.c_str(), config[i].getType(), config[j].getType()); - return false; - } - - std::string namei = config[i]["name"]; - std::string namej = config[j]["name"]; - if (namei == namej) { - ROS_ERROR("%s: A visualization with the name '%s' already exists.", - visualizationsParameter_.c_str(), namei.c_str()); - return false; - } + RCLCPP_ERROR( + nodePtr->get_logger(), + "%s: A visualization with the name '%s' already exists.", + visualizationsParameter_.c_str(), name.c_str()); + return false; + } + + nodePtr->declare_parameter(name + ".type"); + try { + if (!nodePtr->get_parameter(name + ".type", type)) { + RCLCPP_ERROR( + nodePtr->get_logger(), + "%s: Could not add a visualization because no type was given", + name.c_str()); + return false; } + } catch (const rclcpp::ParameterTypeException & e) { + RCLCPP_ERROR( + nodePtr->get_logger(), + "Could not add %s visualization, because the %s.type parameter is not a string.", + name.c_str(), name.c_str()); + return false; } // Make sure the visualization has a valid type. - if (!factory_.isValidType(config[i]["type"])) { - ROS_ERROR("Could not find visualization of type '%s'.", std::string(config[i]["type"]).c_str()); + if (!factory_->isValidType(type)) { + RCLCPP_ERROR( + nodePtr->get_logger(), + "Could not add %s visualization, no visualization of type '%s' found.", + name.c_str(), type.c_str()); return false; } - } - for (int i = 0; i < config.size(); ++i) { - std::string type = config[i]["type"]; - std::string name = config[i]["name"]; - auto visualization = factory_.getInstance(type, name); - visualization->readParameters(config[i]); + auto visualization = factory_->getInstance(type, name); + visualization->readParameters(); visualizations_.push_back(visualization); - ROS_INFO("%s: Configured visualization of type '%s' with name '%s'.", - visualizationsParameter_.c_str(), type.c_str(), name.c_str()); + RCLCPP_INFO( + nodePtr->get_logger(), "%s: Configured visualization of type '%s' with name '%s'.", + visualizationsParameter_.c_str(), type.c_str(), name.c_str()); } - return true; } bool GridMapVisualization::initialize() { - for (auto& visualization : visualizations_) { + for (auto & visualization : visualizations_) { visualization->initialize(); } - updateSubscriptionCallback(ros::TimerEvent()); - ROS_INFO("Grid map visualization initialized."); + + updateSubscriptionCallback(); + RCLCPP_INFO(nodePtr->get_logger(), "Grid map visualization initialized."); return true; } -void GridMapVisualization::updateSubscriptionCallback(const ros::TimerEvent&) +void GridMapVisualization::updateSubscriptionCallback() { bool isActive = false; - for (auto& visualization : visualizations_) { + for (auto & visualization : visualizations_) { if (visualization->isActive()) { isActive = true; break; @@ -144,27 +140,38 @@ void GridMapVisualization::updateSubscriptionCallback(const ros::TimerEvent&) } if (!isSubscribed_ && isActive) { - mapSubscriber_ = nodeHandle_.subscribe(mapTopic_, 1, &GridMapVisualization::callback, this); + rclcpp::QoS qos_setting = rclcpp::SystemDefaultsQoS(); + + if (isGridMapSubLatched_) { + qos_setting = rclcpp::QoS(1).transient_local(); + } + + mapSubscriber_ = nodePtr->create_subscription( + mapTopic_, qos_setting, + std::bind(&GridMapVisualization::callback, this, std::placeholders::_1)); + isSubscribed_ = true; - ROS_DEBUG("Subscribed to grid map at '%s'.", mapTopic_.c_str()); + RCLCPP_DEBUG(nodePtr->get_logger(), "Subscribed to grid map at '%s'.", mapTopic_.c_str()); } if (isSubscribed_ && !isActive) { - mapSubscriber_.shutdown(); + mapSubscriber_.reset(); isSubscribed_ = false; - ROS_DEBUG("Cancelled subscription to grid map."); + RCLCPP_DEBUG(nodePtr->get_logger(), "Cancelled subscription to grid map."); } } -void GridMapVisualization::callback(const grid_map_msgs::GridMap& message) +void GridMapVisualization::callback(const grid_map_msgs::msg::GridMap::SharedPtr message) { - ROS_DEBUG("Grid map visualization received a map (timestamp %f) for visualization.", - message.info.header.stamp.toSec()); + RCLCPP_DEBUG( + nodePtr->get_logger(), + "Grid map visualization received a map (timestamp %f) for visualization.", + rclcpp::Time(message->header.stamp).seconds()); grid_map::GridMap map; - grid_map::GridMapRosConverter::fromMessage(message, map); + grid_map::GridMapRosConverter::fromMessage(*message, map); - for (auto& visualization : visualizations_) { + for (auto & visualization : visualizations_) { visualization->visualize(map); } } -} /* namespace */ +} // namespace grid_map_visualization diff --git a/grid_map_visualization/src/GridMapVisualizationHelpers.cpp b/grid_map_visualization/src/GridMapVisualizationHelpers.cpp index 9432bad2b..d21b24c40 100644 --- a/grid_map_visualization/src/GridMapVisualizationHelpers.cpp +++ b/grid_map_visualization/src/GridMapVisualizationHelpers.cpp @@ -8,85 +8,114 @@ #include "grid_map_visualization/GridMapVisualizationHelpers.hpp" -#include #include +#include -using namespace Eigen; +#include -namespace grid_map_visualization { +namespace grid_map_visualization +{ -void getColorMessageFromColorVector(std_msgs::ColorRGBA& colorMessage, const Eigen::Vector3f& colorVector, bool resetTransparency) +void getColorMessageFromColorVector( + std_msgs::msg::ColorRGBA & colorMessage, + const Eigen::Vector3f & colorVector, bool resetTransparency) { colorMessage.r = colorVector(0); colorMessage.g = colorVector(1); colorMessage.b = colorVector(2); - if (resetTransparency) colorMessage.a = 1.0; + if (resetTransparency) {colorMessage.a = 1.0;} } -void getColorVectorFromColorMessage(Eigen::Vector3f& colorVector, const std_msgs::ColorRGBA& colorMessage) +void getColorVectorFromColorMessage( + Eigen::Vector3f & colorVector, + const std_msgs::msg::ColorRGBA & colorMessage) { colorVector << colorMessage.r, colorMessage.g, colorMessage.b; } -void setColorFromColorValue(std_msgs::ColorRGBA& color, const unsigned long& colorValue, bool resetTransparency) +void setColorFromColorValue( + std_msgs::msg::ColorRGBA & color, const uint64_t & colorValue, + bool resetTransparency) { - Vector3f colorVector; + Eigen::Vector3f colorVector; grid_map::colorValueToVector(colorValue, colorVector); getColorMessageFromColorVector(color, colorVector, resetTransparency); } -void setColorChannelFromValue(float& colorChannel, const double value, const double lowerValueBound, - const double upperValueBound, const bool invert, const double colorChannelLowerValue, - const double colorChannelUpperValue) +void setColorChannelFromValue( + float & colorChannel, const double value, const double lowerValueBound, + const double upperValueBound, const bool invert, const double colorChannelLowerValue, + const double colorChannelUpperValue) { float tempColorChannelLowerValue = colorChannelLowerValue; float tempColorChannelUpperValue = colorChannelUpperValue; - if (invert) - { + if (invert) { tempColorChannelLowerValue = colorChannelUpperValue; tempColorChannelUpperValue = colorChannelLowerValue; } - colorChannel = static_cast(computeLinearMapping(value, lowerValueBound, upperValueBound, tempColorChannelLowerValue, tempColorChannelUpperValue)); + colorChannel = + static_cast(computeLinearMapping( + value, lowerValueBound, upperValueBound, + tempColorChannelLowerValue, tempColorChannelUpperValue)); } -void interpolateBetweenColors(std_msgs::ColorRGBA& color, const std_msgs::ColorRGBA& colorForLowerValue, - const std_msgs::ColorRGBA& colorForUpperValue, const double value, - const double lowerValueBound, const double upperValueBound) +void interpolateBetweenColors( + std_msgs::msg::ColorRGBA & color, const std_msgs::msg::ColorRGBA & colorForLowerValue, + const std_msgs::msg::ColorRGBA & colorForUpperValue, const double value, + const double lowerValueBound, const double upperValueBound) { - setColorChannelFromValue(color.r, value, lowerValueBound, upperValueBound, false, colorForLowerValue.r, colorForUpperValue.r); - setColorChannelFromValue(color.g, value, lowerValueBound, upperValueBound, false, colorForLowerValue.g, colorForUpperValue.g); - setColorChannelFromValue(color.b, value, lowerValueBound, upperValueBound, false, colorForLowerValue.b, colorForUpperValue.b); + setColorChannelFromValue( + color.r, value, lowerValueBound, upperValueBound, false, + colorForLowerValue.r, colorForUpperValue.r); + setColorChannelFromValue( + color.g, value, lowerValueBound, upperValueBound, false, + colorForLowerValue.g, colorForUpperValue.g); + setColorChannelFromValue( + color.b, value, lowerValueBound, upperValueBound, false, + colorForLowerValue.b, colorForUpperValue.b); } -void setSaturationFromValue(std_msgs::ColorRGBA& color, const double value, const double lowerValueBound, - const double upperValueBound, const double maxSaturation, const double minSaturation) +void setSaturationFromValue( + std_msgs::msg::ColorRGBA & color, const double value, const double lowerValueBound, + const double upperValueBound, const double maxSaturation, const double minSaturation) { + (void) lowerValueBound; // unused argument // Based on "changeSaturation" function by Darel Rex Finley. - const Eigen::Array3f HspFactors(.299, .587, .114); // see http://alienryderflex.com/hsp.html - float saturationChange = static_cast(computeLinearMapping(value, value, upperValueBound, maxSaturation, minSaturation)); - Vector3f colorVector; + const Eigen::Array3f HspFactors(.299, .587, .114); // see http://alienryderflex.com/hsp.html + float saturationChange = + static_cast(computeLinearMapping( + value, value, upperValueBound, maxSaturation, + minSaturation)); + Eigen::Vector3f colorVector; getColorVectorFromColorMessage(colorVector, color); float perceivedBrightness = sqrt((colorVector.array().square() * HspFactors).sum()); - colorVector = perceivedBrightness + saturationChange * (colorVector.array() - perceivedBrightness); - colorVector = (colorVector.array().min(Array3f::Ones())).matrix(); + colorVector = perceivedBrightness + saturationChange * + (colorVector.array() - perceivedBrightness); + colorVector = (colorVector.array().min(Eigen::Array3f::Ones())).matrix(); getColorMessageFromColorVector(color, colorVector, false); } -void setColorFromValue(std_msgs::ColorRGBA& color, const double value, const double lowerValueBound, const double upperValueBound) +void setColorFromValue( + std_msgs::msg::ColorRGBA & color, const double value, + const double lowerValueBound, const double upperValueBound) { - Vector3f hsl; // Hue: [0, 2 Pi], Saturation and Lightness: [0, 1] - Vector3f rgb; + Eigen::Vector3f hsl; // Hue: [0, 2 Pi], Saturation and Lightness: [0, 1] + Eigen::Vector3f rgb; - hsl[0] = static_cast(computeLinearMapping(value, lowerValueBound, upperValueBound, 0.0, 2.0 * M_PI)); + hsl[0] = + static_cast(computeLinearMapping( + value, lowerValueBound, upperValueBound, 0.0, + 2.0 * M_PI)); hsl[1] = 1.0; hsl[2] = 1.0; float offset = 2.0 / 3.0 * M_PI; - Array3f rgbOffset(0, -offset, offset); - rgb = ((rgbOffset + hsl[0]).cos() + 0.5).min(Array3f::Ones()).max(Array3f::Zero()) * hsl[2]; - float white = Vector3f(0.3, 0.59, 0.11).transpose() * rgb; + Eigen::Array3f rgbOffset(0, -offset, offset); + rgb = ((rgbOffset + hsl[0]).cos() + 0.5).min(Eigen::Array3f::Ones()).max(Eigen::Array3f::Zero()) * + hsl[2]; + float white = Eigen::Vector3f(0.3, 0.59, 0.11).transpose() * rgb; float saturation = 1.0 - hsl[1]; rgb = rgb + ((-rgb.array() + white) * saturation).matrix(); @@ -94,23 +123,20 @@ void setColorFromValue(std_msgs::ColorRGBA& color, const double value, const dou } double computeLinearMapping( - const double& sourceValue, const double& sourceLowerValue, const double& sourceUpperValue, - const double& mapLowerValue, const double& mapUpperValue) + const double & sourceValue, const double & sourceLowerValue, const double & sourceUpperValue, + const double & mapLowerValue, const double & mapUpperValue) { double m = (mapLowerValue - mapUpperValue) / (sourceLowerValue - sourceUpperValue); double b = mapUpperValue - m * sourceUpperValue; double mapValue = m * sourceValue + b; - if (mapLowerValue < mapUpperValue) - { + if (mapLowerValue < mapUpperValue) { mapValue = std::max(mapValue, mapLowerValue); mapValue = std::min(mapValue, mapUpperValue); - } - else - { + } else { mapValue = std::min(mapValue, mapLowerValue); mapValue = std::max(mapValue, mapUpperValue); } return mapValue; } -} /* namespace */ +} // namespace grid_map_visualization diff --git a/grid_map_visualization/src/grid_map_visualization_node.cpp b/grid_map_visualization/src/grid_map_visualization_node.cpp index 3ffc97e76..743b8026e 100644 --- a/grid_map_visualization/src/grid_map_visualization_node.cpp +++ b/grid_map_visualization/src/grid_map_visualization_node.cpp @@ -6,17 +6,17 @@ * Institute: ETH Zurich, ANYbotics */ -#include -#include "grid_map_visualization/GridMapVisualization.hpp" +#include +#include -int main(int argc, char** argv) -{ - ros::init(argc, argv, "grid_map_visualization"); - - ros::NodeHandle nodeHandle("~"); +#include - grid_map_visualization::GridMapVisualization gridMapVisualization(nodeHandle, "grid_map_visualizations"); - - ros::spin(); +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared( + "grid_map_visualizations"); + rclcpp::spin(node->nodePtr); + rclcpp::shutdown(); return 0; } diff --git a/grid_map_visualization/src/visualizations/FlatPointCloudVisualization.cpp b/grid_map_visualization/src/visualizations/FlatPointCloudVisualization.cpp index cfb7ca7d5..b5dced9f8 100644 --- a/grid_map_visualization/src/visualizations/FlatPointCloudVisualization.cpp +++ b/grid_map_visualization/src/visualizations/FlatPointCloudVisualization.cpp @@ -6,16 +6,22 @@ * Institute: ETH Zurich, ANYbotics */ -#include #include -#include +#include -namespace grid_map_visualization { +#include -FlatPointCloudVisualization::FlatPointCloudVisualization(ros::NodeHandle& nodeHandle, const std::string& name) - : VisualizationBase(nodeHandle, name), - height_(0.0) +#include "grid_map_visualization/visualizations/FlatPointCloudVisualization.hpp" + +namespace grid_map_visualization +{ + +FlatPointCloudVisualization::FlatPointCloudVisualization( + const std::string & name, + rclcpp::Node::SharedPtr nodePtr) +: VisualizationBase(name, nodePtr), + height_(0.0) { } @@ -23,13 +29,15 @@ FlatPointCloudVisualization::~FlatPointCloudVisualization() { } -bool FlatPointCloudVisualization::readParameters(XmlRpc::XmlRpcValue& config) +bool FlatPointCloudVisualization::readParameters() { - VisualizationBase::readParameters(config); - height_ = 0.0; - if (!getParam("height", height_)) { - ROS_INFO("FlatPointCloudVisualization with name '%s' did not find a 'height' parameter. Using default.", name_.c_str()); + nodePtr_->declare_parameter(name_ + ".params.height", 0.0); + if (!nodePtr_->get_parameter(name_ + ".params.height", height_)) { + RCLCPP_INFO( + nodePtr_->get_logger(), + "FlatPointCloudVisualization with name '%s' " + "did not find a 'height' parameter. Using default.", name_.c_str()); } return true; @@ -37,21 +45,23 @@ bool FlatPointCloudVisualization::readParameters(XmlRpc::XmlRpcValue& config) bool FlatPointCloudVisualization::initialize() { - publisher_ = nodeHandle_.advertise(name_, 1, true); + publisher_ = nodePtr_->create_publisher( + name_, + rclcpp::QoS(1).transient_local()); return true; } -bool FlatPointCloudVisualization::visualize(const grid_map::GridMap& map) +bool FlatPointCloudVisualization::visualize(const grid_map::GridMap & map) { - if (!isActive()) return true; - sensor_msgs::PointCloud2 pointCloud; + if (!isActive()) {return false;} + sensor_msgs::msg::PointCloud2 pointCloud; grid_map::GridMap mapCopy(map); mapCopy.add("flat", height_); grid_map::GridMapRosConverter::toPointCloud(mapCopy, "flat", pointCloud); - publisher_.publish(pointCloud); + publisher_->publish(pointCloud); return true; } -} /* namespace */ +} // namespace grid_map_visualization diff --git a/grid_map_visualization/src/visualizations/GridCellsVisualization.cpp b/grid_map_visualization/src/visualizations/GridCellsVisualization.cpp index e47736ccd..484ec3ec2 100644 --- a/grid_map_visualization/src/visualizations/GridCellsVisualization.cpp +++ b/grid_map_visualization/src/visualizations/GridCellsVisualization.cpp @@ -8,15 +8,19 @@ #include "grid_map_visualization/visualizations/GridCellsVisualization.hpp" #include -#include +#include // STD #include +#include -namespace grid_map_visualization { +namespace grid_map_visualization +{ -GridCellsVisualization::GridCellsVisualization(ros::NodeHandle& nodeHandle, const std::string& name) -: VisualizationBase(nodeHandle, name), +GridCellsVisualization::GridCellsVisualization( + const std::string & name, + rclcpp::Node::SharedPtr nodePtr) +: VisualizationBase(name, nodePtr), lowerThreshold_(-std::numeric_limits::infinity()), upperThreshold_(std::numeric_limits::infinity()) { @@ -26,21 +30,36 @@ GridCellsVisualization::~GridCellsVisualization() { } -bool GridCellsVisualization::readParameters(XmlRpc::XmlRpcValue& config) +bool GridCellsVisualization::readParameters() { - VisualizationBase::readParameters(config); + nodePtr_->declare_parameter( + name_ + ".params.layer", + std::string("elevation")); + nodePtr_->declare_parameter(name_ + ".params.lower_threshold", 5.0); + nodePtr_->declare_parameter(name_ + ".params.upper_threshold", -5.0); - if (!getParam("layer", layer_)) { - ROS_ERROR("GridCellsVisualization with name '%s' did not find a 'layer' parameter.", name_.c_str()); + if (!nodePtr_->get_parameter(name_ + ".params.layer", layer_)) { + RCLCPP_ERROR( + nodePtr_->get_logger(), + "GridCellsVisualization with name '%s' did not find a 'layer' parameter.", + name_.c_str()); return false; } - if (!getParam("lower_threshold", lowerThreshold_)) { - ROS_INFO("GridCellsVisualization with name '%s' did not find a 'lower_threshold' parameter. Using negative infinity.", name_.c_str()); + if (!nodePtr_->get_parameter(name_ + ".params.lower_threshold", lowerThreshold_)) { + RCLCPP_INFO( + nodePtr_->get_logger(), + "GridCellsVisualization with name '%s' " + "did not find a 'lower_threshold' parameter. Using negative infinity.", + name_.c_str()); } - if (!getParam("upper_threshold", upperThreshold_)) { - ROS_INFO("GridCellsVisualization with name '%s' did not find a 'upper_threshold' parameter. Using infinity.", name_.c_str()); + if (!nodePtr_->get_parameter(name_ + ".params.upper_threshold", upperThreshold_)) { + RCLCPP_INFO( + nodePtr_->get_logger(), + "GridCellsVisualization with name '%s' " + "did not find a 'upper_threshold' parameter. Using infinity.", + name_.c_str()); } return true; @@ -48,21 +67,27 @@ bool GridCellsVisualization::readParameters(XmlRpc::XmlRpcValue& config) bool GridCellsVisualization::initialize() { - publisher_ = nodeHandle_.advertise(name_, 1, true); + publisher_ = nodePtr_->create_publisher( + name_, + rclcpp::QoS(1).transient_local()); return true; } -bool GridCellsVisualization::visualize(const grid_map::GridMap& map) +bool GridCellsVisualization::visualize(const grid_map::GridMap & map) { - if (!isActive()) return true; + if (!isActive()) {return false;} if (!map.exists(layer_)) { - ROS_WARN_STREAM("GridCellsVisualization::visualize: No grid map layer with name '" << layer_ << "' found."); + RCLCPP_WARN_STREAM( + nodePtr_->get_logger(), + "GridCellsVisualization::visualize: No grid map layer with name '" << layer_ << "' found."); return false; } - nav_msgs::GridCells gridCells; - grid_map::GridMapRosConverter::toGridCells(map, layer_, lowerThreshold_, upperThreshold_, gridCells); - publisher_.publish(gridCells); + nav_msgs::msg::GridCells gridCells; + grid_map::GridMapRosConverter::toGridCells( + map, layer_, lowerThreshold_, upperThreshold_, + gridCells); + publisher_->publish(gridCells); return true; } -} /* namespace */ +} // namespace grid_map_visualization diff --git a/grid_map_visualization/src/visualizations/MapRegionVisualization.cpp b/grid_map_visualization/src/visualizations/MapRegionVisualization.cpp index 1b9c0fbd7..bef81f2c1 100644 --- a/grid_map_visualization/src/visualizations/MapRegionVisualization.cpp +++ b/grid_map_visualization/src/visualizations/MapRegionVisualization.cpp @@ -10,14 +10,21 @@ #include // ROS -#include +#include -namespace grid_map_visualization { +#include -MapRegionVisualization::MapRegionVisualization(ros::NodeHandle& nodeHandle, const std::string& name) - : VisualizationBase(nodeHandle, name), - nVertices_(5), - lineWidth_(0.01) +using namespace std::chrono_literals; + +namespace grid_map_visualization +{ + +MapRegionVisualization::MapRegionVisualization( + const std::string & name, + rclcpp::Node::SharedPtr nodePtr) +: VisualizationBase(name, nodePtr), + nVertices_(5), + lineWidth_(0.01) { } @@ -25,18 +32,25 @@ MapRegionVisualization::~MapRegionVisualization() { } -bool MapRegionVisualization::readParameters(XmlRpc::XmlRpcValue& config) +bool MapRegionVisualization::readParameters() { - VisualizationBase::readParameters(config); - + nodePtr_->declare_parameter(name_ + ".params.line_width", 0.003); + nodePtr_->declare_parameter(name_ + ".params.color", 16777215); lineWidth_ = 0.003; - if (!getParam("line_width", lineWidth_)) { - ROS_INFO("MapRegionVisualization with name '%s' did not find a 'line_width' parameter. Using default.", name_.c_str()); + if (!nodePtr_->get_parameter(name_ + ".params.line_width", lineWidth_)) { + RCLCPP_INFO( + nodePtr_->get_logger(), + "MapRegionVisualization with name '%s' did not find a 'line_width' parameter. Using default.", + name_.c_str()); + return false; } - int colorValue = 16777215; // white, http://www.wolframalpha.com/input/?i=BitOr%5BBitShiftLeft%5Br%2C16%5D%2C+BitShiftLeft%5Bg%2C8%5D%2C+b%5D+where+%7Br%3D20%2C+g%3D50%2C+b%3D230%7D - if (!getParam("color", colorValue)) { - ROS_INFO("MapRegionVisualization with name '%s' did not find a 'color' parameter. Using default.", name_.c_str()); + int colorValue = 16777215; // white, http://www.wolframalpha.com/input/?i=BitOr%5BBitShiftLeft%5Br%2C16%5D%2C+BitShiftLeft%5Bg%2C8%5D%2C+b%5D+where+%7Br%3D20%2C+g%3D50%2C+b%3D230%7D // NOLINT + if (!nodePtr_->get_parameter(name_ + ".params.color", colorValue)) { + RCLCPP_INFO( + nodePtr_->get_logger(), + "MapRegionVisualization with name '%s' did not find a 'color' parameter. Using default.", + name_.c_str()); } setColorFromColorValue(color_, colorValue, true); @@ -46,25 +60,27 @@ bool MapRegionVisualization::readParameters(XmlRpc::XmlRpcValue& config) bool MapRegionVisualization::initialize() { marker_.ns = "map_region"; - marker_.lifetime = ros::Duration(); - marker_.action = visualization_msgs::Marker::ADD; - marker_.type = visualization_msgs::Marker::LINE_STRIP; + marker_.lifetime = rclcpp::Duration(0ns); // Setting lifetime to forever + marker_.action = visualization_msgs::msg::Marker::ADD; + marker_.type = visualization_msgs::msg::Marker::LINE_STRIP; marker_.scale.x = lineWidth_; - marker_.points.resize(nVertices_); // Initialized to [0.0, 0.0, 0.0] + marker_.points.resize(nVertices_); // Initialized to [0.0, 0.0, 0.0] marker_.colors.resize(nVertices_, color_); - publisher_ = nodeHandle_.advertise(name_, 1, true); + publisher_ = nodePtr_->create_publisher( + name_, + rclcpp::QoS(1).transient_local()); return true; } -bool MapRegionVisualization::visualize(const grid_map::GridMap& map) +bool MapRegionVisualization::visualize(const grid_map::GridMap & map) { - if (!isActive()) return true; + if (!isActive()) {return false;} - // TODO Replace this with ploygon? + // TODO(needs_assignment): Replace this with ploygon? // Set marker info. marker_.header.frame_id = map.getFrameId(); - marker_.header.stamp.fromNSec(map.getTimestamp()); + marker_.header.stamp = rclcpp::Time(map.getTimestamp()); // Adapt positions of markers. float halfLengthX = map.getLength().x() / 2.0; @@ -81,8 +97,8 @@ bool MapRegionVisualization::visualize(const grid_map::GridMap& map) marker_.points[4].x = marker_.points[0].x; marker_.points[4].y = marker_.points[0].y; - publisher_.publish(marker_); + publisher_->publish(marker_); return true; } -} /* namespace */ +} // namespace grid_map_visualization diff --git a/grid_map_visualization/src/visualizations/OccupancyGridVisualization.cpp b/grid_map_visualization/src/visualizations/OccupancyGridVisualization.cpp index 7aeb45fed..a4b19c1ff 100644 --- a/grid_map_visualization/src/visualizations/OccupancyGridVisualization.cpp +++ b/grid_map_visualization/src/visualizations/OccupancyGridVisualization.cpp @@ -8,12 +8,17 @@ #include "grid_map_visualization/visualizations/OccupancyGridVisualization.hpp" #include -#include +#include -namespace grid_map_visualization { +#include -OccupancyGridVisualization::OccupancyGridVisualization(ros::NodeHandle& nodeHandle, const std::string& name) -: VisualizationBase(nodeHandle, name), +namespace grid_map_visualization +{ + +OccupancyGridVisualization::OccupancyGridVisualization( + const std::string & name, + rclcpp::Node::SharedPtr nodePtr) +: VisualizationBase(name, nodePtr), dataMin_(0.0), dataMax_(1.0) { @@ -23,22 +28,33 @@ OccupancyGridVisualization::~OccupancyGridVisualization() { } -bool OccupancyGridVisualization::readParameters(XmlRpc::XmlRpcValue& config) +bool OccupancyGridVisualization::readParameters() { - VisualizationBase::readParameters(config); + nodePtr_->declare_parameter(name_ + ".params.layer", std::string("elevation")); + nodePtr_->declare_parameter(name_ + ".params.data_min", 0.0); + nodePtr_->declare_parameter(name_ + ".params.data_max", 1.0); - if (!getParam("layer", layer_)) { - ROS_ERROR("OccupancyGridVisualization with name '%s' did not find a 'layer' parameter.", name_.c_str()); + if (!nodePtr_->get_parameter(name_ + ".params.layer", layer_)) { + RCLCPP_ERROR( + nodePtr_->get_logger(), + "OccupancyGridVisualization with name '%s' did not find a 'layer' parameter.", + name_.c_str()); return false; } - if (!getParam("data_min", dataMin_)) { - ROS_ERROR("OccupancyGridVisualization with name '%s' did not find a 'data_min' parameter.", name_.c_str()); + if (!nodePtr_->get_parameter(name_ + ".params.data_min", dataMin_)) { + RCLCPP_ERROR( + nodePtr_->get_logger(), + "OccupancyGridVisualization with name '%s' did not find a 'data_min' parameter.", + name_.c_str()); return false; } - if (!getParam("data_max", dataMax_)) { - ROS_ERROR("OccupancyGridVisualization with name '%s' did not find a 'data_max' parameter.", name_.c_str()); + if (!nodePtr_->get_parameter(name_ + ".params.data_max", dataMax_)) { + RCLCPP_ERROR( + nodePtr_->get_logger(), + "OccupancyGridVisualization with name '%s' did not find a 'data_max' parameter.", + name_.c_str()); return false; } @@ -47,21 +63,26 @@ bool OccupancyGridVisualization::readParameters(XmlRpc::XmlRpcValue& config) bool OccupancyGridVisualization::initialize() { - publisher_ = nodeHandle_.advertise(name_, 1, true); + publisher_ = nodePtr_->create_publisher( + name_, + rclcpp::QoS(1).transient_local()); return true; } -bool OccupancyGridVisualization::visualize(const grid_map::GridMap& map) +bool OccupancyGridVisualization::visualize(const grid_map::GridMap & map) { - if (!isActive()) return true; + if (!isActive()) {return false;} if (!map.exists(layer_)) { - ROS_WARN_STREAM("OccupancyGridVisualization::visualize: No grid map layer with name '" << layer_ << "' found."); + RCLCPP_WARN_STREAM( + nodePtr_->get_logger(), + "OccupancyGridVisualization::visualize: No grid map layer with name '" << layer_ << + "' found."); return false; } - nav_msgs::OccupancyGrid occupancyGrid; + nav_msgs::msg::OccupancyGrid occupancyGrid; grid_map::GridMapRosConverter::toOccupancyGrid(map, layer_, dataMin_, dataMax_, occupancyGrid); - publisher_.publish(occupancyGrid); + publisher_->publish(occupancyGrid); return true; } -} /* namespace */ +} // namespace grid_map_visualization diff --git a/grid_map_visualization/src/visualizations/PointCloudVisualization.cpp b/grid_map_visualization/src/visualizations/PointCloudVisualization.cpp index 53a968518..83b89e544 100644 --- a/grid_map_visualization/src/visualizations/PointCloudVisualization.cpp +++ b/grid_map_visualization/src/visualizations/PointCloudVisualization.cpp @@ -6,15 +6,18 @@ * Institute: ETH Zurich, ANYbotics */ -#include +#include "grid_map_visualization/visualizations/PointCloudVisualization.hpp" #include -#include +#include -namespace grid_map_visualization { +namespace grid_map_visualization +{ -PointCloudVisualization::PointCloudVisualization(ros::NodeHandle& nodeHandle, const std::string& name) - : VisualizationBase(nodeHandle, name) +PointCloudVisualization::PointCloudVisualization( + const std::string & name, + rclcpp::Node::SharedPtr nodePtr) +: VisualizationBase(name, nodePtr) { } @@ -22,11 +25,14 @@ PointCloudVisualization::~PointCloudVisualization() { } -bool PointCloudVisualization::readParameters(XmlRpc::XmlRpcValue& config) +bool PointCloudVisualization::readParameters() { - VisualizationBase::readParameters(config); - if (!getParam("layer", layer_)) { - ROS_ERROR("PointCloudVisualization with name '%s' did not find a 'layer' parameter.", name_.c_str()); + nodePtr_->declare_parameter(name_ + ".params.layer", std::string("elevation")); + if (!nodePtr_->get_parameter(name_ + ".params.layer", layer_)) { + RCLCPP_ERROR( + nodePtr_->get_logger(), + "PointCloudVisualization with name '%s' did not find a 'layer' parameter.", + name_.c_str()); return false; } return true; @@ -34,21 +40,26 @@ bool PointCloudVisualization::readParameters(XmlRpc::XmlRpcValue& config) bool PointCloudVisualization::initialize() { - publisher_ = nodeHandle_.advertise(name_, 1, true); + publisher_ = nodePtr_->create_publisher( + name_, + rclcpp::QoS(1).transient_local()); return true; } -bool PointCloudVisualization::visualize(const grid_map::GridMap& map) +bool PointCloudVisualization::visualize(const grid_map::GridMap & map) { - if (!isActive()) return true; + if (!isActive()) {return false;} if (!map.exists(layer_)) { - ROS_WARN_STREAM("PointCloudVisualization::visualize: No grid map layer with name '" << layer_ << "' found."); + RCLCPP_WARN_STREAM( + nodePtr_->get_logger(), + "PointCloudVisualization::visualize: No grid map layer with name '" << layer_ << + "' found."); return false; } - sensor_msgs::PointCloud2 pointCloud; + sensor_msgs::msg::PointCloud2 pointCloud; grid_map::GridMapRosConverter::toPointCloud(map, layer_, pointCloud); - publisher_.publish(pointCloud); + publisher_->publish(pointCloud); return true; } -} /* namespace */ +} // namespace grid_map_visualization diff --git a/grid_map_visualization/src/visualizations/VectorVisualization.cpp b/grid_map_visualization/src/visualizations/VectorVisualization.cpp index 947b5e4d3..a90bafaa0 100644 --- a/grid_map_visualization/src/visualizations/VectorVisualization.cpp +++ b/grid_map_visualization/src/visualizations/VectorVisualization.cpp @@ -8,20 +8,25 @@ #include "grid_map_visualization/visualizations/VectorVisualization.hpp" -// Color conversion -#include - // Iterator #include +// Color conversion +#include + // ROS -#include -#include +#include +#include -namespace grid_map_visualization { +#include -VectorVisualization::VectorVisualization(ros::NodeHandle& nodeHandle, const std::string& name) - : VisualizationBase(nodeHandle, name) +using namespace std::chrono_literals; + +namespace grid_map_visualization +{ + +VectorVisualization::VectorVisualization(const std::string & name, rclcpp::Node::SharedPtr nodePtr) +: VisualizationBase(name, nodePtr) { } @@ -29,37 +34,59 @@ VectorVisualization::~VectorVisualization() { } -bool VectorVisualization::readParameters(XmlRpc::XmlRpcValue& config) +bool VectorVisualization::readParameters() { - VisualizationBase::readParameters(config); + nodePtr_->declare_parameter(name_ + ".params.layer_prefix", std::string("")); + nodePtr_->declare_parameter(name_ + ".params.position_layer", std::string("")); + nodePtr_->declare_parameter(name_ + ".params.scale", 1.0); + nodePtr_->declare_parameter(name_ + ".params.line_width", 0.003); + nodePtr_->declare_parameter(name_ + ".params.color", 65280); std::string typePrefix; - if (!getParam("layer_prefix", typePrefix)) { - ROS_ERROR("VectorVisualization with name '%s' did not find a 'layer_prefix' parameter.", name_.c_str()); + if (!nodePtr_->get_parameter(name_ + ".params.layer_prefix", typePrefix)) { + RCLCPP_ERROR( + nodePtr_->get_logger(), + "VectorVisualization with name '%s' did not find a 'layer_prefix' parameter.", + name_.c_str()); return false; } types_.push_back(typePrefix + "x"); types_.push_back(typePrefix + "y"); types_.push_back(typePrefix + "z"); - if (!getParam("position_layer", positionLayer_)) { - ROS_ERROR("VectorVisualization with name '%s' did not find a 'position_layer' parameter.", name_.c_str()); + if (!nodePtr_->get_parameter( + name_ + ".params.position_layer", + positionLayer_)) + { + RCLCPP_ERROR( + nodePtr_->get_logger(), + "VectorVisualization with name '%s' did not find a 'position_layer' parameter.", + name_.c_str()); return false; } scale_ = 1.0; - if (!getParam("scale", scale_)) { - ROS_INFO("VectorVisualization with name '%s' did not find a 'scale' parameter. Using default.", name_.c_str()); + if (!nodePtr_->get_parameter(name_ + ".params.scale", scale_)) { + RCLCPP_INFO( + nodePtr_->get_logger(), + "VectorVisualization with name '%s' did not find a 'scale' parameter. Using default.", + name_.c_str()); } lineWidth_ = 0.003; - if (!getParam("line_width", lineWidth_)) { - ROS_INFO("VectorVisualization with name '%s' did not find a 'line_width' parameter. Using default.", name_.c_str()); + if (!nodePtr_->get_parameter(name_ + ".params.line_width", lineWidth_)) { + RCLCPP_INFO( + nodePtr_->get_logger(), + "VectorVisualization with name '%s' did not find a 'line_width' parameter. Using default.", + name_.c_str()); } - int colorValue = 65280; // green - if (!getParam("color", colorValue)) { - ROS_INFO("VectorVisualization with name '%s' did not find a 'color' parameter. Using default.", name_.c_str()); + int colorValue = 65280; // green + if (!nodePtr_->get_parameter(name_ + ".params.color", colorValue)) { + RCLCPP_INFO( + nodePtr_->get_logger(), + "VectorVisualization with name '%s' did not find a 'color' parameter. Using default.", + name_.c_str()); } setColorFromColorValue(color_, colorValue, true); @@ -69,62 +96,63 @@ bool VectorVisualization::readParameters(XmlRpc::XmlRpcValue& config) bool VectorVisualization::initialize() { marker_.ns = "vector"; - marker_.lifetime = ros::Duration(); - marker_.action = visualization_msgs::Marker::ADD; - marker_.type = visualization_msgs::Marker::LINE_LIST; + marker_.lifetime = rclcpp::Duration(0ns); // setting lifetime forever + marker_.action = visualization_msgs::msg::Marker::ADD; + marker_.type = visualization_msgs::msg::Marker::LINE_LIST; marker_.scale.x = lineWidth_; - publisher_ = nodeHandle_.advertise(name_, 1, true); + publisher_ = nodePtr_->create_publisher( + name_, + rclcpp::QoS(1).transient_local()); return true; } -bool VectorVisualization::visualize(const grid_map::GridMap& map) +bool VectorVisualization::visualize(const grid_map::GridMap & map) { - if (!isActive()) return true; + if (!isActive()) {return false;} - for (const auto& type : types_) { + for (const auto & type : types_) { if (!map.exists(type)) { - ROS_WARN_STREAM( - "VectorVisualization::visualize: No grid map layer with name '" << type << "' found."); + RCLCPP_WARN_STREAM( + nodePtr_->get_logger(), + "VectorVisualization::visualize: No grid map layer with name '" << type << "' found."); return false; } } // Set marker info. marker_.header.frame_id = map.getFrameId(); - marker_.header.stamp.fromNSec(map.getTimestamp()); + marker_.header.stamp = rclcpp::Time(map.getTimestamp()); // Clear points. marker_.points.clear(); marker_.colors.clear(); - for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) - { - if (!map.isValid(*iterator, positionLayer_) || !map.isValid(*iterator, types_)) continue; - geometry_msgs::Vector3 vector; + for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { + if (!map.isValid(*iterator, positionLayer_) || !map.isValid(*iterator, types_)) {continue;} + geometry_msgs::msg::Vector3 vector; vector.x = map.at(types_[0], *iterator); vector.y = map.at(types_[1], *iterator); vector.z = map.at(types_[2], *iterator); Eigen::Vector3d position; map.getPosition3(positionLayer_, *iterator, position); - geometry_msgs::Point startPoint; + geometry_msgs::msg::Point startPoint; startPoint.x = position.x(); startPoint.y = position.y(); startPoint.z = position.z(); marker_.points.push_back(startPoint); - geometry_msgs::Point endPoint; + geometry_msgs::msg::Point endPoint; endPoint.x = startPoint.x + scale_ * vector.x; endPoint.y = startPoint.y + scale_ * vector.y; endPoint.z = startPoint.z + scale_ * vector.z; marker_.points.push_back(endPoint); - marker_.colors.push_back(color_); // Each vertex needs a color. + marker_.colors.push_back(color_); // Each vertex needs a color. marker_.colors.push_back(color_); } - publisher_.publish(marker_); + publisher_->publish(marker_); return true; } - -} /* namespace */ +} // namespace grid_map_visualization diff --git a/grid_map_visualization/src/visualizations/VisualizationBase.cpp b/grid_map_visualization/src/visualizations/VisualizationBase.cpp index 073a5dad6..cb09765a9 100644 --- a/grid_map_visualization/src/visualizations/VisualizationBase.cpp +++ b/grid_map_visualization/src/visualizations/VisualizationBase.cpp @@ -8,92 +8,23 @@ #include "grid_map_visualization/visualizations/VisualizationBase.hpp" -namespace grid_map_visualization { +#include -VisualizationBase::VisualizationBase(ros::NodeHandle& nodeHandle, const std::string& name) - : nodeHandle_(nodeHandle), - name_(name) +namespace grid_map_visualization { -} - -VisualizationBase::~VisualizationBase() -{ -} - -bool VisualizationBase::isActive() const -{ - if (publisher_.getNumSubscribers() > 0) return true; - return false; -} -bool VisualizationBase::readParameters(XmlRpc::XmlRpcValue& config) +VisualizationBase::VisualizationBase(const std::string & name, rclcpp::Node::SharedPtr nodePtr) +: name_(name), + nodePtr_(nodePtr) { - if (config.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("A filter configuration must be a map with fields name, type, and params."); - return false; - } - - // Check to see if we have parameters in our list. - if (config.hasMember("params")) { - XmlRpc::XmlRpcValue params = config["params"]; - if (params.getType() != XmlRpc::XmlRpcValue::TypeStruct) { - ROS_ERROR("Params must be a map."); - return false; - } else { - for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) { - ROS_DEBUG("Loading param %s\n", it->first.c_str()); - parameters_[it->first] = it->second; - } - } - } - - return true; } -bool VisualizationBase::getParam(const std::string& name, std::string& value) -{ - StringMap::iterator it = parameters_.find(name); - if (it == parameters_.end()) return false; - if (it->second.getType() != XmlRpc::XmlRpcValue::TypeString) return false; - value = std::string(it->second); - return true; -} - -bool VisualizationBase::getParam(const std::string& name, double& value) -{ - StringMap::iterator it = parameters_.find(name); - if (it == parameters_.end()) return false; - if (it->second.getType() != XmlRpc::XmlRpcValue::TypeDouble - && it->second.getType() != XmlRpc::XmlRpcValue::TypeInt) return false; - value = it->second.getType() == XmlRpc::XmlRpcValue::TypeInt ? - (int) (it->second) : (double) (it->second); - return true; -} - -bool VisualizationBase::getParam(const std::string& name, float& value) -{ - double valueDouble; - bool isSuccess = getParam(name, valueDouble); - if (isSuccess) value = static_cast(valueDouble); - return isSuccess; -} - -bool VisualizationBase::getParam(const std::string& name, bool& value) +VisualizationBase::~VisualizationBase() { - StringMap::iterator it = parameters_.find(name); - if (it == parameters_.end()) return false; - if (it->second.getType() != XmlRpc::XmlRpcValue::TypeBoolean) return false; - value = it->second; - return true; } -bool VisualizationBase::getParam(const std::string&name, int& value) +bool VisualizationBase::isActive() const { - StringMap::iterator it = parameters_.find(name); - if (it == parameters_.end()) return false; - if(it->second.getType() != XmlRpc::XmlRpcValue::TypeInt) return false; - value = it->second; - return true; + return static_cast(nodePtr_->count_subscribers(name_)); } - -} /* namespace */ +} // namespace grid_map_visualization diff --git a/grid_map_visualization/src/visualizations/VisualizationFactory.cpp b/grid_map_visualization/src/visualizations/VisualizationFactory.cpp index edfc63df1..3736ad10b 100644 --- a/grid_map_visualization/src/visualizations/VisualizationFactory.cpp +++ b/grid_map_visualization/src/visualizations/VisualizationFactory.cpp @@ -6,21 +6,24 @@ * Institute: ETH Zurich, ANYbotics */ -#include -#include -#include -#include -#include -#include -#include - // STL #include +#include +#include + +#include "grid_map_visualization/visualizations/VisualizationFactory.hpp" +#include "grid_map_visualization/visualizations/PointCloudVisualization.hpp" +#include "grid_map_visualization/visualizations/FlatPointCloudVisualization.hpp" +#include "grid_map_visualization/visualizations/VectorVisualization.hpp" +#include "grid_map_visualization/visualizations/OccupancyGridVisualization.hpp" +#include "grid_map_visualization/visualizations/GridCellsVisualization.hpp" +#include "grid_map_visualization/visualizations/MapRegionVisualization.hpp" -namespace grid_map_visualization { +namespace grid_map_visualization +{ -VisualizationFactory::VisualizationFactory(ros::NodeHandle& nodeHandle) - : nodeHandle_(nodeHandle) +VisualizationFactory::VisualizationFactory(rclcpp::Node::SharedPtr nodePtr) +: nodePtr_(nodePtr) { types_.push_back("point_cloud"); types_.push_back("flat_point_cloud"); @@ -34,22 +37,36 @@ VisualizationFactory::~VisualizationFactory() { } -bool VisualizationFactory::isValidType(const std::string& type) +bool VisualizationFactory::isValidType(const std::string & type) { return end(types_) != std::find(begin(types_), end(types_), type); } -std::shared_ptr VisualizationFactory::getInstance(const std::string& type, - const std::string& name) +std::shared_ptr VisualizationFactory::getInstance( + const std::string & type, + const std::string & name) { - // TODO: Make this nicer: http://stackoverflow.com/questions/9975672/c-automatic-factory-registration-of-derived-types - if (type == "point_cloud") return std::shared_ptr(new PointCloudVisualization(nodeHandle_, name)); - if (type == "flat_point_cloud") return std::shared_ptr(new FlatPointCloudVisualization(nodeHandle_, name)); - if (type == "vectors") return std::shared_ptr(new VectorVisualization(nodeHandle_, name)); - if (type == "occupancy_grid") return std::shared_ptr(new OccupancyGridVisualization(nodeHandle_, name)); - if (type == "grid_cells") return std::shared_ptr(new GridCellsVisualization(nodeHandle_, name)); - if (type == "map_region") return std::shared_ptr(new MapRegionVisualization(nodeHandle_, name)); + // TODO(needs_assignment): + // Make this nicer: http://stackoverflow.com/questions/9975672/c-automatic-factory-registration-of-derived-types + if (type == "point_cloud") { + return std::shared_ptr(new PointCloudVisualization(name, nodePtr_)); + } + if (type == "flat_point_cloud") { + return std::shared_ptr(new FlatPointCloudVisualization(name, nodePtr_)); + } + if (type == "vectors") { + return std::shared_ptr(new VectorVisualization(name, nodePtr_)); + } + if (type == "occupancy_grid") { + return std::shared_ptr(new OccupancyGridVisualization(name, nodePtr_)); + } + if (type == "grid_cells") { + return std::shared_ptr(new GridCellsVisualization(name, nodePtr_)); + } + if (type == "map_region") { + return std::shared_ptr(new MapRegionVisualization(name, nodePtr_)); + } return std::shared_ptr(); } -} /* namespace grid_map_visualization */ +} // namespace grid_map_visualization diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos new file mode 100644 index 000000000..23d88992a --- /dev/null +++ b/tools/ros2_dependencies.repos @@ -0,0 +1,6 @@ +repositories: + ros-planning/navigation2: + type: git + # back to upstream main after https://github.com/ros-navigation/navigation2/pull/4298 is merged + url: https://github.com/tonynajjar/navigation2.git + version: fix-devcontainer