-
Notifications
You must be signed in to change notification settings - Fork 20
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
### Proposed changes NDT map visualization publisher. It uses ellipsoids to represent obstacles placed using the mean and the covariance matrix of each cell.  >Obstacle representation made with green ellipsoids (scale factor is used). #### Type of change - [ ] 🐛 Bugfix (change which fixes an issue) - [x] 🚀 Feature (change which adds functionality) - [ ] 📚 Documentation (change which fixes or extends documentation) ### Checklist - [ ] Lint and unit tests (if any) pass locally with my changes - [ ] I have added tests that prove my fix is effective or that my feature works - [x] I have added necessary documentation (if appropriate) - [x] All commits have been signed for [DCO](https://developercertificate.org/) --------- Signed-off-by: Fernando <sanz.fernando172@gmail.com> Signed-off-by: Fernando-Sanz <sanz.fernando172@gmail.com> Signed-off-by: Marcos Huck <marcoshuck@ekumenlabs.com>
- Loading branch information
1 parent
f035e0f
commit 7d213f8
Showing
6 changed files
with
285 additions
and
12 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,110 @@ | ||
| // Copyright 2025 Ekumen, Inc. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef BELUGA_ROS_NDT_ELLIPSOID_HPP | ||
| #define BELUGA_ROS_NDT_ELLIPSOID_HPP | ||
|
|
||
| #include <beluga_ros/messages.hpp> | ||
|
|
||
| #include <Eigen/Core> | ||
| #include <beluga/eigen_compatibility.hpp> | ||
|
|
||
| #include <range/v3/view/enumerate.hpp> | ||
|
|
||
| #include <beluga/sensor/data/ndt_cell.hpp> | ||
| #include <beluga/sensor/data/sparse_value_grid.hpp> | ||
|
|
||
| /** | ||
| * \file | ||
| * \brief Utilities for NDT I/O over ROS interfaces. | ||
| */ | ||
|
|
||
| namespace beluga_ros { | ||
|
|
||
| namespace detail { | ||
|
|
||
| /// Create an ellipoid based on NDT cell data (eigenvalues and eigenvectors) contained in the received marker. | ||
| /** | ||
| * \param eigen_solver Precreated eigen solver of the NDT cell. | ||
| * \param cell NDT cell. | ||
| * \param marker Marker that will contain the message. | ||
| * \return A boolean set to false if the ellipsoid could not be created (because the covariances are | ||
| * non-diagonalizable). | ||
| */ | ||
| bool use_mean_covariance( | ||
| Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>>& eigen_solver, | ||
| beluga::NDTCell<3> cell, | ||
| beluga_ros::msg::Marker& marker); | ||
|
|
||
| /// Create a cube contained in the received marker. | ||
| /** | ||
| * \param position Center of the cube. | ||
| * \param size Size of the edges. | ||
| * \param marker Marker that will contain the message. | ||
| */ | ||
| void use_cell_size(const Eigen::Vector<int, 3>& position, double size, beluga_ros::msg::Marker& marker); | ||
|
|
||
| } // namespace detail | ||
|
|
||
| /// Assign an ellipsoid to each cell of a SparseValueGrid. A cube is used instead if the distribution of the | ||
| /// cell is not suitable for the rotation matrix creation. | ||
| /** | ||
| * \param grid A SparseValueGrid that contains cells representing obstacles. | ||
| * \param[out] message A MarkerArray that will contain the shapes | ||
| * \return A std::pair with the MarkerArray containing the shapes and a boolean indicating if at least one cube was | ||
| * generated. \tparam MapType Container that maps from Eigen::Vector<int, NDim> to the type of the cell. See | ||
| * [SparseValueGrid] | ||
| * (https://ekumen-os.github.io/beluga/packages/beluga/docs/_doxygen/generated/reference/html/classbeluga_1_1SparseValueGrid.html). | ||
| * \tparam NDim Dimension of the grid. | ||
| */ | ||
| template <typename MapType, int NDim> | ||
| std::pair<beluga_ros::msg::MarkerArray, bool> assign_obstacle_map( | ||
| const beluga::SparseValueGrid<MapType, NDim>& grid, | ||
| beluga_ros::msg::MarkerArray& message) { | ||
| bool cubes_generated = false; | ||
| // Get data from the grid | ||
| auto& map = grid.data(); | ||
|
|
||
| // Clean up the message | ||
| beluga_ros::msg::Marker marker; | ||
| marker.ns = "obstacles"; | ||
| marker.action = beluga_ros::msg::Marker::DELETEALL; | ||
| message.markers.push_back(marker); | ||
|
|
||
| // Add the markers | ||
| Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>> eigen_solver; | ||
| for (auto [index, entry] : ranges::views::enumerate(map)) { | ||
| const auto& [cell_center, cell] = entry; | ||
| beluga_ros::msg::Marker marker; | ||
| marker.header.frame_id = "map"; | ||
| marker.id = index + 1; | ||
| marker.ns = "obstacles"; | ||
|
|
||
| // Try to create an ellipsoid with values of the cell | ||
| if (!beluga_ros::detail::use_mean_covariance(eigen_solver, cell, marker)) { | ||
| // Create a cube based on the resolution of the grid | ||
| cubes_generated = true; | ||
| beluga_ros::detail::use_cell_size(cell_center, grid.resolution(), marker); | ||
| } | ||
|
|
||
| // Add the marker | ||
| message.markers.push_back(marker); | ||
| } | ||
|
|
||
| return {message, cubes_generated}; | ||
| } | ||
|
|
||
| } // namespace beluga_ros | ||
|
|
||
| #endif // BELUGA_ROS_NDT_ELLIPSOID_HPP |
Oops, something went wrong.