diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 790de84..b872da0 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -7,23 +7,17 @@ on: push: branches: - humble - schedule: - - cron: '0 0 * * 6' jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-22.04] - fail-fast: false + runs-on: ubuntu-22.04 + container: + image: ubuntu:jammy steps: - uses: actions/checkout@v4 with: ref: humble - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: humble - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/.github/workflows/humble_cron.yaml b/.github/workflows/humble_cron.yaml new file mode 100644 index 0000000..061a43d --- /dev/null +++ b/.github/workflows/humble_cron.yaml @@ -0,0 +1,38 @@ +name: humble + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ubuntu-22.04 + container: + image: ubuntu:jammy + steps: + - uses: actions/checkout@v4 + with: + ref: humble + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin + target-ros2-distro: humble + ref: humble + colcon-defaults: | + { + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-name: coverage-gcc + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 4c6cc39..558b75c 100644 --- a/.github/workflows/jazzy.yaml +++ b/.github/workflows/jazzy.yaml @@ -7,23 +7,17 @@ on: push: branches: - jazzy - schedule: - - cron: '0 0 * * 6' jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - uses: actions/checkout@v4 with: ref: jazzy - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: jazzy - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml new file mode 100644 index 0000000..85c2a30 --- /dev/null +++ b/.github/workflows/jazzy_cron.yaml @@ -0,0 +1,38 @@ +name: jazzy + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble + steps: + - uses: actions/checkout@v4 + with: + ref: jazzy + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin + target-ros2-distro: jazzy + ref: jazzy + colcon-defaults: | + { + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-name: coverage-gcc + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index 503f6e7..c5b75e6 100644 --- a/.github/workflows/kilted.yaml +++ b/.github/workflows/kilted.yaml @@ -7,23 +7,17 @@ on: push: branches: - kilted - schedule: - - cron: '0 0 * * 6' jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble steps: - uses: actions/checkout@v4 with: ref: kilted - name: Setup ROS 2 uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: kilted - name: build and test uses: ros-tooling/action-ros-ci@0.4.5 with: diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml new file mode 100644 index 0000000..df98c2f --- /dev/null +++ b/.github/workflows/kilted_cron.yaml @@ -0,0 +1,38 @@ +name: kilted + +on: + schedule: + - cron: '0 0 * * 6' +jobs: + build-and-test: + runs-on: ubuntu-24.04 + container: + image: ubuntu:noble + steps: + - uses: actions/checkout@v4 + with: + ref: kilted + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@0.7.15 + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.5 + with: + package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin + target-ros2-distro: kilted + ref: kilted + colcon-defaults: | + { + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-name: coverage-gcc + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/lyrical.yaml b/.github/workflows/lyrical.yaml new file mode 100644 index 0000000..60f9109 --- /dev/null +++ b/.github/workflows/lyrical.yaml @@ -0,0 +1,42 @@ +name: lyrical + +on: + pull_request: + branches: + - lyrical + push: + branches: + - lyrical + schedule: + - cron: '0 0 * * 6' + workflow_dispatch: +jobs: + build-and-test: + runs-on: ubuntu-26.04 + container: + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-resolute-ros-lyrical-ros-base:master + steps: + - uses: actions/checkout@v6 + with: + ref: lyrical + - name: build and test + uses: ros-tooling/action-ros-ci@0.4.8 + with: + package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin + target-ros2-distro: lyrical + colcon-defaults: | + { + "test": { + "parallel-workers" : 1 + } + } + colcon-mixin-name: coverage-gcc + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - name: Codecov + uses: codecov/codecov-action@v5.4.0 + with: + files: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + # yml: ./codecov.yml + fail_ci_if_error: false diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index bfcbdaf..72473cd 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -8,24 +8,19 @@ on: branches: - rolling schedule: - - cron: '0 0 * * 6' + - cron: '0 0 * * 6' + workflow_dispatch: jobs: build-and-test: - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-24.04] - fail-fast: false + runs-on: ubuntu-26.04 + container: + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-resolute-ros-rolling-ros-base:master steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@v6 with: ref: rolling - - name: Setup ROS 2 - uses: ros-tooling/setup-ros@0.7.15 - with: - required-ros-distributions: rolling - name: build and test - uses: ros-tooling/action-ros-ci@0.4.5 + uses: ros-tooling/action-ros-ci@0.4.8 with: package-name: navmap_core navmap_ros navmap_ros_interfaces navmap_rviz_plugin target-ros2-distro: rolling diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..46e553e --- /dev/null +++ b/.gitignore @@ -0,0 +1,8 @@ +# VS Code stuff +/.vscode/** +**/__pycache__/ + +# ROS 2 build files +build/ +install/ +log/ diff --git a/README.md b/README.md index 46105df..30bd70a 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ # NavMap [![Doxygen Deployment](https://github.com/EasyNavigation/NavMap/actions/workflows/doxygen-doc.yml/badge.svg)](https://github.com/EasyNavigation/NavMap/actions/workflows/doxygen-doc.yml) [![rolling](https://github.com/EasyNavigation/NavMap/actions/workflows/rolling.yaml/badge.svg?branch=rolling)](https://github.com/EasyNavigation/NavMap/actions/workflows/rolling.yaml) +[![lyrical](https://github.com/EasyNavigation/NavMap/actions/workflows/lyrical.yaml/badge.svg?branch=lyrical)](https://github.com/EasyNavigation/NavMap/actions/workflows/lyrical.yaml) [![kilted](https://github.com/EasyNavigation/NavMap/actions/workflows/kilted.yaml/badge.svg?branch=kilted)](https://github.com/EasyNavigation/NavMap/actions/workflows/kilted.yaml) [![jazzy](https://github.com/EasyNavigation/NavMap/actions/workflows/jazzy.yaml/badge.svg?branch=jazzy)](https://github.com/EasyNavigation/NavMap/actions/workflows/jazzy.yaml) [![humble](https://github.com/EasyNavigation/NavMap/actions/workflows/humble.yaml/badge.svg?branch=humble)](https://github.com/EasyNavigation/NavMap/actions/workflows/humble.yaml) diff --git a/navmap_core/CHANGELOG.rst b/navmap_core/CHANGELOG.rst index ad00f61..1c49cf0 100644 --- a/navmap_core/CHANGELOG.rst +++ b/navmap_core/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package navmap_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.5.0 (2026-07-25) +------------------ +* Update version +* Speedup the navcel location +* Cleanup unused headers +* Fix potential linker error and warning +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + +0.4.0 (2025-11-24) +------------------ +* Speedup the navcel location +* Cleanup unused headers +* Fix potential linker error and warning +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.2.5 (2025-10-17) ------------------ diff --git a/navmap_core/include/navmap_core/Geometry.hpp b/navmap_core/include/navmap_core/Geometry.hpp index 1b7d03f..8ffa1d7 100644 --- a/navmap_core/include/navmap_core/Geometry.hpp +++ b/navmap_core/include/navmap_core/Geometry.hpp @@ -34,7 +34,6 @@ #include #include #include -#include namespace navmap { diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index 45bf6f2..cb837a8 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -45,10 +45,8 @@ #include #include #include -#include #include #include -#include #include #include #include @@ -199,6 +197,40 @@ struct LayerView : LayerViewBase ///@} }; +/** @cond INTERNAL */ +namespace detail +{ +inline std::uint64_t fnv1a64_bytes( + const void * data, std::size_t n, + std::uint64_t seed = 1469598103934665603ULL) +{ + const auto * p = static_cast(data); + std::uint64_t h = seed; + for (std::size_t i = 0; i < n; ++i) { + h ^= p[i]; h *= 1099511628211ULL; + } + return h; +} +} // namespace detail +/** @endcond */ + +template +std::uint64_t LayerView::content_hash() const +{ + if (!hash_dirty_) {return hash_cache_;} + const std::size_t n = data_.size(); + std::uint64_t h = navmap::detail::fnv1a64_bytes(&n, sizeof(n)); + if (n) { + static_assert( + std::is_trivially_copyable::value, + "LayerView requires trivially copyable T."); + h = navmap::detail::fnv1a64_bytes(data_.data(), n * sizeof(T), h); + } + hash_cache_ = h; + hash_dirty_ = false; + return hash_cache_; +} + /** * \brief Registry of named layers (per-NavCel). * diff --git a/navmap_core/package.xml b/navmap_core/package.xml index e47a17b..19c1667 100644 --- a/navmap_core/package.xml +++ b/navmap_core/package.xml @@ -2,7 +2,7 @@ navmap_core - 0.2.5 + 0.5.0 Core C++ library for NavMap. Francisco Martín Rico diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index 504ee39..627c8bd 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -15,6 +15,7 @@ #include "navmap_core/NavMap.hpp" +#include #include #include #include @@ -25,40 +26,6 @@ namespace navmap { -/** @cond INTERNAL */ -namespace navmap -{namespace detail -{ -inline std::uint64_t fnv1a64_bytes( - const void * data, std::size_t n, - std::uint64_t seed = 1469598103934665603ULL) -{ - const auto * p = static_cast(data); - std::uint64_t h = seed; - for (std::size_t i = 0; i < n; ++i) { - h ^= p[i]; h *= 1099511628211ULL; - } - return h; -} -}} // namespaces -/** @endcond */ - -template -std::uint64_t LayerView::content_hash() const -{ - if (!hash_dirty_) {return hash_cache_;} - const std::size_t n = data_.size(); - std::uint64_t h = navmap::detail::fnv1a64_bytes(&n, sizeof(n)); - if (n) { - static_assert( - std::is_trivially_copyable::value, - "LayerView requires trivially copyable T."); - h = navmap::detail::fnv1a64_bytes(data_.data(), n * sizeof(T), h); - } - hash_cache_ = h; - hash_dirty_ = false; - return hash_cache_; -} namespace { @@ -474,7 +441,7 @@ bool NavMap::raycast( { bool any = false; float best_t = std::numeric_limits::infinity(); - Vec3 best_p; + Vec3 best_p = Vec3::Zero(); NavCelId best_cid = 0; for (const auto & s : surfaces) { @@ -581,7 +548,7 @@ bool NavMap::locate_by_walking( Vec3 * hit_pt, float planar_eps) const { - const int kMaxSteps = 64; + const int kMaxSteps = 16; NavCelId cid = start_cid; for (int step = 0; step < kMaxSteps; ++step) { @@ -634,26 +601,71 @@ bool NavMap::locate_navcel_core( Vec3 * hit_pt, const LocateOpts & opts) const { - // 1) Try walking if there is a valid hint. + // 0) Fast path: directly test the hinted triangle, if any. if (opts.hint_cid.has_value()) { - if (locate_by_walking( - opts.hint_cid.value(), - p_world, - cid, - bary, - hit_pt, - opts.planar_eps)) - { - for (size_t s = 0; s < surfaces.size(); ++s) { - const auto & surf = surfaces[s]; - if (std::find(surf.navcels.begin(), surf.navcels.end(), cid) != - surf.navcels.end()) + const NavCelId hint = *opts.hint_cid; + if (hint < navcels.size()) { + const auto & c = navcels[hint]; + const Vec3 a = positions.at(c.v[0]); + const Vec3 b = positions.at(c.v[1]); + const Vec3 d = positions.at(c.v[2]); + const Vec3 & n = c.normal; + + const float dist = n.dot(p_world - a); + const Vec3 q = p_world - dist * n; + + Vec3 bary_hint; + if (point_in_triangle_bary(q, a, b, d, bary_hint, opts.planar_eps) && + std::fabs(dist) <= opts.height_eps) + { + cid = hint; + bary = bary_hint; + if (hit_pt) { + *hit_pt = q; + } + + // Find the surface that owns this navcel. + for (size_t s = 0; s < surfaces.size(); ++s) { + const auto & surf = surfaces[s]; + if (std::find(surf.navcels.begin(), surf.navcels.end(), cid) != surf.navcels.end()) { + surface_idx = s; + return true; + } + } + // If no surface owns the hinted navcel, fall through to the generic search. + } + } + } + + // 1) Try walking if there is a valid hint and we are not far from its plane. + if (opts.hint_cid.has_value()) { + const NavCelId start = *opts.hint_cid; + if (start < navcels.size()) { + const auto & c0 = navcels[start]; + const Vec3 a0 = positions.at(c0.v[0]); + const Vec3 & n0 = c0.normal; + const float dist0 = n0.dot(p_world - a0); + + // Do not walk if the query point is clearly off the hinted plane. + if (std::fabs(dist0) <= opts.height_eps) { + if (locate_by_walking( + start, + p_world, + cid, + bary, + hit_pt, + opts.planar_eps)) { - surface_idx = s; - return true; + for (size_t s = 0; s < surfaces.size(); ++s) { + const auto & surf = surfaces[s]; + if (std::find(surf.navcels.begin(), surf.navcels.end(), cid) != surf.navcels.end()) { + surface_idx = s; + return true; + } + } + // Fall through if surface not found. } } - // Fall through if surface not found. } } diff --git a/navmap_core/tests/test_geometry.cpp b/navmap_core/tests/test_geometry.cpp index 2e63771..492a435 100644 --- a/navmap_core/tests/test_geometry.cpp +++ b/navmap_core/tests/test_geometry.cpp @@ -15,7 +15,6 @@ #include #include -#include #include "navmap_core/Geometry.hpp" using namespace navmap; diff --git a/navmap_core/tests/test_navmap_uniform_and_closest.cpp b/navmap_core/tests/test_navmap_uniform_and_closest.cpp index 446a3cc..9e49c29 100644 --- a/navmap_core/tests/test_navmap_uniform_and_closest.cpp +++ b/navmap_core/tests/test_navmap_uniform_and_closest.cpp @@ -15,7 +15,6 @@ #include #include -#include #include "navmap_core/NavMap.hpp" using namespace navmap; diff --git a/navmap_examples/CHANGELOG.rst b/navmap_examples/CHANGELOG.rst index e37829d..0b55802 100644 --- a/navmap_examples/CHANGELOG.rst +++ b/navmap_examples/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package navmap_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.5.0 (2026-07-25) +------------------ +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + +0.4.0 (2025-11-24) +------------------ +* Cleanup unused headers +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.2.5 (2025-10-17) ------------------ diff --git a/navmap_examples/package.xml b/navmap_examples/package.xml index 7bd3095..adbc164 100644 --- a/navmap_examples/package.xml +++ b/navmap_examples/package.xml @@ -2,7 +2,7 @@ navmap_examples - 0.2.5 + 0.5.0 Examples related to navmap_core y navmap_ros. Francisco Martín Rico diff --git a/navmap_examples/src/01_flat_plane.cpp b/navmap_examples/src/01_flat_plane.cpp index 130bba3..50a2404 100644 --- a/navmap_examples/src/01_flat_plane.cpp +++ b/navmap_examples/src/01_flat_plane.cpp @@ -17,16 +17,12 @@ #include #include #include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; using navmap::LayerType; using Eigen::Vector3f; using std::cout; using std::cerr; using std::endl; @@ -54,7 +50,7 @@ int main() NavMap nm; make_flat_square(nm); auto occ = nm.layers.add_or_get("occupancy", nm.navcels.size(), LayerType::U8); - if(!occ) {cerr << "Cannot create 'occupancy'\n"; return 1;} + if (!occ) {cerr << "Cannot create 'occupancy'\n"; return 1;} (*occ)[0] = 0; (*occ)[1] = 254; Vector3f p(0.75f, 0.75f, 0.4f); @@ -62,7 +58,7 @@ int main() bool ok = nm.locate_navcel(p, sidx, cid, bary, &hit); cout << "locate=" << ok << " sidx=" << sidx << " cid=" << cid << " hit=(" << hit.x() << "," << hit.y() << "," << hit.z() << ")\n"; - if(ok) { + if (ok) { cout << "occ at cid: " << (int)nm.navcel_value(cid, *occ) << endl; } return 0; diff --git a/navmap_examples/src/02_two_floors.cpp b/navmap_examples/src/02_two_floors.cpp index e3ccd1b..ac89f44 100644 --- a/navmap_examples/src/02_two_floors.cpp +++ b/navmap_examples/src/02_two_floors.cpp @@ -16,20 +16,14 @@ #include #include -#include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; +using std::cout; using std::endl; // 02_two_floors: two stacked floors, locate & closest_navcel static void make_two_floors(NavMap & nm, float z0, float z1) diff --git a/navmap_examples/src/03_slope_surface.cpp b/navmap_examples/src/03_slope_surface.cpp index c4cbcb6..f479cb9 100644 --- a/navmap_examples/src/03_slope_surface.cpp +++ b/navmap_examples/src/03_slope_surface.cpp @@ -16,20 +16,14 @@ #include #include -#include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; +using std::cout; using std::endl; // 03_slope_surface: sloped z, sample_layer_at int main() diff --git a/navmap_examples/src/04_layers.cpp b/navmap_examples/src/04_layers.cpp index 5edfffe..c376bc4 100644 --- a/navmap_examples/src/04_layers.cpp +++ b/navmap_examples/src/04_layers.cpp @@ -15,21 +15,15 @@ #include -#include #include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; +using std::cout; using std::endl; // 04_layers: add/list/set/get int main() @@ -50,11 +44,12 @@ int main() nm.layer_set("cost", c0, 5.5f); auto names = nm.list_layers(); - cout << "Layers:"; for(auto & n:names) { + cout << "Layers:"; for (auto & n:names) { cout << " " << n; } cout << endl; - cout << "occ=" << (int)nm.layer_get("occ", c0, + cout << "occ=" << (int)nm.layer_get( + "occ", c0, 0) << ", cost=" << nm.layer_get("cost", c0, -1.0) << endl; return 0; } diff --git a/navmap_examples/src/05_neighbors_and_centroids.cpp b/navmap_examples/src/05_neighbors_and_centroids.cpp index 7f11065..5d99476 100644 --- a/navmap_examples/src/05_neighbors_and_centroids.cpp +++ b/navmap_examples/src/05_neighbors_and_centroids.cpp @@ -16,20 +16,14 @@ #include #include -#include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; +using std::cout; using std::endl; // 05_neighbors_and_centroids static void make_flat_square(NavMap & nm) @@ -55,7 +49,7 @@ int main() auto neigh = nm.navcel_neighbors(c0); cout << "centroid0=(" << cc0.x() << "," << cc0.y() << "," << cc0.z() << ")" << endl; cout << "centroid1=(" << cc1.x() << "," << cc1.y() << "," << cc1.z() << ")" << endl; - cout << "neighbors of c0:"; for(auto n:neigh) { + cout << "neighbors of c0:"; for (auto n:neigh) { cout << " " << n; } cout << endl; diff --git a/navmap_examples/src/06_area_marking.cpp b/navmap_examples/src/06_area_marking.cpp index fdda86c..bef573e 100644 --- a/navmap_examples/src/06_area_marking.cpp +++ b/navmap_examples/src/06_area_marking.cpp @@ -15,21 +15,15 @@ #include -#include #include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; +using std::cout; using std::endl; // 06_area_marking: set_area CIRCULAR y RECTANGULAR sobre una malla 1x1 de 2 tris #include @@ -50,14 +44,17 @@ int main() nm.add_layer("obstacles", "occupancy obstacles", "%", 0); // Circular in the center radius 0.3 → marks both centroids - bool ok1 = nm.set_area(Vector3f(0.5f, 0.5f, 10.0f), (uint8_t)254, - "obstacles", navmap::AreaShape::CIRCULAR, 0.3f); + bool ok1 = nm.set_area( + Vector3f(0.5f, 0.5f, 10.0f), (uint8_t)254, + "obstacles", navmap::AreaShape::CIRCULAR, 0.3f); // Rectangular near (0.8,0.2) side 0.35 → mark one - bool ok2 = nm.set_area(Vector3f(0.80f, 0.20f, -5.0f), (uint8_t)200, - "obstacles", navmap::AreaShape::RECTANGULAR, 0.35f); + bool ok2 = nm.set_area( + Vector3f(0.80f, 0.20f, -5.0f), (uint8_t)200, + "obstacles", navmap::AreaShape::RECTANGULAR, 0.35f); cout << "set_area circle=" << ok1 << " rect=" << ok2 << endl; - cout << "c0=" << (int)nm.layer_get("obstacles", c0, - 0) << " c1=" << (int)nm.layer_get("obstacles", c1, 0) << endl; + cout << "c0=" << (int)nm.layer_get( + "obstacles", c0, + 0) << " c1=" << (int)nm.layer_get("obstacles", c1, 0) << endl; return 0; } diff --git a/navmap_examples/src/07_raycast.cpp b/navmap_examples/src/07_raycast.cpp index ce97653..4ff7d8d 100644 --- a/navmap_examples/src/07_raycast.cpp +++ b/navmap_examples/src/07_raycast.cpp @@ -16,20 +16,13 @@ #include #include -#include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; // 07_raycast: simple y batch (raycast_many) int main() diff --git a/navmap_examples/src/08_copy_and_assign.cpp b/navmap_examples/src/08_copy_and_assign.cpp index 0b84de4..691d1de 100644 --- a/navmap_examples/src/08_copy_and_assign.cpp +++ b/navmap_examples/src/08_copy_and_assign.cpp @@ -17,19 +17,13 @@ #include #include #include -#include -#include #include #include "navmap_core/NavMap.hpp" using navmap::NavMap; using navmap::NavCelId; -using navmap::Surface; -using navmap::LayerView; -using navmap::LayerType; using Eigen::Vector3f; -using std::cout; using std::cerr; using std::endl; // 08_copy_and_assign: muestra operator= optimizado (igual geometría) y completo (distinta) static void fill_one_tri_map(navmap::NavMap & m) @@ -56,7 +50,7 @@ int main() dst = src; auto names_after = dst.list_layers(); std::cout << "assign equal geom ok; layers after:"; - for(auto & n:names_after) { + for (auto & n:names_after) { std::cout << " " << n; } std::cout << "\n"; diff --git a/navmap_examples/src_ros2/01_from_occgrid.cpp b/navmap_examples/src_ros2/01_from_occgrid.cpp index eda2662..1a64de1 100644 --- a/navmap_examples/src_ros2/01_from_occgrid.cpp +++ b/navmap_examples/src_ros2/01_from_occgrid.cpp @@ -21,7 +21,8 @@ using std::placeholders::_1; -class GridToNavMapNode : public rclcpp::Node { +class GridToNavMapNode : public rclcpp::Node +{ public: GridToNavMapNode() : Node("navmap_from_occgrid") @@ -36,7 +37,7 @@ class GridToNavMapNode : public rclcpp::Node { navmap::NavMap nm = navmap_ros::from_occupancy_grid(*msg); size_t sidx{}; navmap::NavCelId cid{}; Eigen::Vector3f bary, hit; - if(nm.locate_navcel(Eigen::Vector3f(0.5f, 0.5f, 0.5f), sidx, cid, bary, &hit)) { + if (nm.locate_navcel(Eigen::Vector3f(0.5f, 0.5f, 0.5f), sidx, cid, bary, &hit)) { RCLCPP_INFO(this->get_logger(), "hit on surface %zu cell %u", sidx, (unsigned)cid); } } diff --git a/navmap_examples/src_ros2/02_to_occgrid.cpp b/navmap_examples/src_ros2/02_to_occgrid.cpp index 71d68c9..c72fc44 100644 --- a/navmap_examples/src_ros2/02_to_occgrid.cpp +++ b/navmap_examples/src_ros2/02_to_occgrid.cpp @@ -19,13 +19,15 @@ #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" -class NavMapToGridNode : public rclcpp::Node { +class NavMapToGridNode : public rclcpp::Node +{ public: NavMapToGridNode() : Node("navmap_to_occgrid") { pub_ = this->create_publisher("navmap_grid", 10); - timer_ = this->create_wall_timer(std::chrono::seconds(1), + timer_ = this->create_wall_timer( + std::chrono::seconds(1), std::bind(&NavMapToGridNode::tick, this)); } @@ -34,7 +36,7 @@ class NavMapToGridNode : public rclcpp::Node { { static bool init = false; static navmap::NavMap nm; - if(!init) { + if (!init) { auto v0 = nm.add_vertex({0, 0, 0}); auto v1 = nm.add_vertex({1, 0, 0}); auto v2 = nm.add_vertex({1, 1, 0}); diff --git a/navmap_examples/src_ros2/03_save_load.cpp b/navmap_examples/src_ros2/03_save_load.cpp index fa43e54..b87587a 100644 --- a/navmap_examples/src_ros2/03_save_load.cpp +++ b/navmap_examples/src_ros2/03_save_load.cpp @@ -27,37 +27,37 @@ static void save_json(const navmap::NavMap & nm, const std::string & path) json j; j["x"] = nm.positions.x; j["y"] = nm.positions.y; j["z"] = nm.positions.z; j["tris"] = json::array(); - for(const auto & c: nm.navcels) { + for (const auto & c: nm.navcels) { j["tris"].push_back({c.v[0], c.v[1], c.v[2]}); } // Solo capa "occupancy" si existe auto occ_any = nm.layers.get("occupancy"); - if(occ_any) { + if (occ_any) { auto occ = std::dynamic_pointer_cast>(occ_any); - if(occ) {j["occupancy"] = occ->data();} + if (occ) {j["occupancy"] = occ->data();} } std::ofstream ofs(path); ofs << j.dump(2); } static bool load_json(navmap::NavMap & nm, const std::string & path) { - std::ifstream ifs(path); if(!ifs) {return false;} + std::ifstream ifs(path); if (!ifs) {return false;} json j; ifs >> j; nm.positions.x = j["x"].get>(); nm.positions.y = j["y"].get>(); nm.positions.z = j["z"].get>(); nm.navcels.resize(j["tris"].size()); - for(size_t i = 0; i < nm.navcels.size(); ++i) { + for (size_t i = 0; i < nm.navcels.size(); ++i) { auto t = j["tris"][i]; nm.navcels[i].v[0] = t[0]; nm.navcels[i].v[1] = t[1]; nm.navcels[i].v[2] = t[2]; } nm.surfaces.clear(); auto s = nm.create_surface("map"); - for(size_t i = 0; i < nm.navcels.size(); ++i) { + for (size_t i = 0; i < nm.navcels.size(); ++i) { nm.add_navcel_to_surface(s, (navmap::NavCelId)i); } nm.rebuild_geometry_accels(); - if(j.contains("occupancy")) { + if (j.contains("occupancy")) { auto occ = nm.add_layer("occupancy", "occ", "%", 0); auto & v = occ->mutable_data(); v = j["occupancy"].get>(); @@ -65,7 +65,8 @@ static bool load_json(navmap::NavMap & nm, const std::string & path) return true; } -class SaveLoadNode : public rclcpp::Node { +class SaveLoadNode : public rclcpp::Node +{ public: SaveLoadNode() : Node("navmap_save_load") @@ -86,8 +87,9 @@ class SaveLoadNode : public rclcpp::Node { navmap::NavMap re; (void)load_json(re, "/tmp/navmap.json"); - RCLCPP_INFO(get_logger(), "Loaded back: vertices=%zu tris=%zu", - re.positions.size(), re.navcels.size()); + RCLCPP_INFO( + get_logger(), "Loaded back: vertices=%zu tris=%zu", + re.positions.size(), re.navcels.size()); } }; diff --git a/navmap_ros/CHANGELOG.rst b/navmap_ros/CHANGELOG.rst index 676c47b..e3801d6 100644 --- a/navmap_ros/CHANGELOG.rst +++ b/navmap_ros/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package navmap_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.5.0 (2026-07-25) +------------------ +* Fix test compilation error with rosidl::Buffer +* PCL private linkage: avoid Qt5/6 conflicts +* Fix doc in header +* Add headers in conversions +* Cleanup unused headers +* Add occupancy grid constants +* Acelerated respecting floors +* Working slow with many points +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, estherag + +0.4.0 (2025-11-24) +------------------ +* Cleanup unused headers +* Occupancy works +* Add occupancy grid constants +* Fix surface creation from points +* Remove unused field +* Remove some comments +* Final working version +* Acelerated respecting floors +* Working slow with many points +* Initial working version +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.2.5 (2025-10-17) ------------------ * Fix pcl_conversions build diff --git a/navmap_ros/CMakeLists.txt b/navmap_ros/CMakeLists.txt index 727269d..b4466c6 100644 --- a/navmap_ros/CMakeLists.txt +++ b/navmap_ros/CMakeLists.txt @@ -27,9 +27,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $ ${PCL_INCLUDE_DIRS} - ${pcl_conversions_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME} PUBLIC + +target_link_libraries(${PROJECT_NAME} rclcpp::rclcpp navmap_core::navmap_core ${navmap_ros_interfaces_TARGETS} @@ -38,7 +38,6 @@ target_link_libraries(${PROJECT_NAME} PUBLIC ${sensor_msgs_TARGETS} ${std_srvs_TARGETS} ${PCL_LIBRARIES} - ${pcl_conversions_LIBRARIES} ) add_executable(slam_server_app src/slam_server_app.cpp @@ -70,6 +69,10 @@ if(BUILD_TESTING) add_subdirectory(tests) endif() +ament_target_dependencies(${PROJECT_NAME} + pcl_conversions +) + ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(${PROJECT_NAME}) ament_export_targets(export_${PROJECT_NAME}) @@ -77,10 +80,10 @@ ament_export_dependencies( rclcpp navmap_core navmap_ros_interfaces + nav_msgs geometry_msgs sensor_msgs std_srvs - PCL pcl_conversions ) ament_package() \ No newline at end of file diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index 068f56f..bbe9303 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -37,12 +37,11 @@ */ #include -#include -#include #include #include #include +#include "std_msgs/msg/header.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "navmap_ros_interfaces/msg/nav_map.hpp" #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" @@ -57,12 +56,33 @@ namespace navmap_ros { +/** + * @name Costmap value semantics + * @brief Standardized occupancy/cost values used when projecting NavMap layers + * onto a 2D grid (compatible with `costmap_2d` conventions). + * + * These constants follow the same meaning as in `costmap_2d`: + * - `NO_INFORMATION` (255): Unknown or unobserved area. + * - `LETHAL_OBSTACLE` (254): Non-traversable obstacle. + * - `INSCRIBED_INFLATED_OBSTACLE` (253): Inside the robot’s inscribed radius. + * - `MAX_NON_OBSTACLE` (252): Highest cost still considered traversable. + * - `FREE_SPACE` (0): Known free space. + * @{ + */ +constexpr uint8_t NO_INFORMATION = 255; +constexpr uint8_t LETHAL_OBSTACLE = 254; +constexpr uint8_t INSCRIBED_INFLATED_OBSTACLE = 253; +constexpr uint8_t MAX_NON_OBSTACLE = 252; +constexpr uint8_t FREE_SPACE = 0; +/** @} */ // end of Costmap value semantics group + // --------- NavMap <-> ROS message --------- /** * @brief Convert a core `navmap::NavMap` into its compact ROS transport message. * * @param[in] nm Core NavMap to be serialized into a ROS message. + * @param[in] header Header to assign to the resulting message. * @return A `navmap_ros_interfaces::msg::NavMap` containing geometry (vertices, triangles), * surfaces metadata and user-defined layers. * @@ -73,12 +93,20 @@ namespace navmap_ros * @note This function does not perform IO; it only builds the message in-memory. */ navmap_ros_interfaces::msg::NavMap to_msg( - const navmap::NavMap & nm); + const navmap::NavMap & nm, const std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (no header provided). + * + * The returned message header will be default-constructed. + */ +navmap_ros_interfaces::msg::NavMap to_msg(const navmap::NavMap & nm); /** * @brief Reconstruct a core `navmap::NavMap` from the ROS transport message. * * @param[in] msg Input `navmap_ros_interfaces::msg::NavMap` message. + * @param[out] header Header extracted from the message. * @return A core `navmap::NavMap` equivalent to the content of @p msg. * * @details @@ -87,6 +115,13 @@ navmap_ros_interfaces::msg::NavMap to_msg( * * @throw std::runtime_error If the message describes inconsistent geometry or layer sizes. */ +navmap::NavMap from_msg( + const navmap_ros_interfaces::msg::NavMap & msg, + std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (ignores message header). + */ navmap::NavMap from_msg(const navmap_ros_interfaces::msg::NavMap & msg); /** @@ -94,6 +129,7 @@ navmap::NavMap from_msg(const navmap_ros_interfaces::msg::NavMap & msg); * * @param[in] nm Input NavMap. * @param[in] layer Name of the layer to export. + * @param[in] header Header to assign to the resulting message. * @return A NavMapLayer message containing the layer values and metadata. * * @details @@ -103,6 +139,14 @@ navmap::NavMap from_msg(const navmap_ros_interfaces::msg::NavMap & msg); * * @throw std::runtime_error If the layer does not exist or has an unsupported type. */ +navmap_ros_interfaces::msg::NavMapLayer to_msg( + const navmap::NavMap & nm, + const std::string & layer, + const std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (no header provided). + */ navmap_ros_interfaces::msg::NavMapLayer to_msg( const navmap::NavMap & nm, const std::string & layer); @@ -115,6 +159,7 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( * * @param[in] msg Input NavMapLayer message. * @param[in,out] nm Destination NavMap (must already have navcels sized correctly). + * @param[out] header Header extracted from the message. * * @details * - The function verifies that the length of the populated data array matches @@ -123,6 +168,14 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( * * @throw std::runtime_error If sizes are inconsistent or the message is ill-formed. */ +void from_msg( + const navmap_ros_interfaces::msg::NavMapLayer & msg, + navmap::NavMap & nm, + std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (ignores message header). + */ void from_msg( const navmap_ros_interfaces::msg::NavMapLayer & msg, navmap::NavMap & nm); @@ -134,6 +187,7 @@ void from_msg( * using a regular triangular surface with shared vertices. * * @param[in] grid Input ROS OccupancyGrid (row-major, width×height, resolution and origin). + * @param[out] header Header to assign to the resulting message. * @return A core `navmap::NavMap` with: * - Vertices: `(W+1) * (H+1)` laid on the grid plane, with `Z = grid.info.origin.position.z`. * - Triangles: `2 * W * H` (two per cell), using diagonal pattern = 0. @@ -149,6 +203,13 @@ void from_msg( * @note The grid origin pose may contain a rotation. The vertex Z is taken from the origin Z; * handling of non-zero yaw/roll/pitch (if any) is implementation-defined in the builder. */ +navmap::NavMap from_occupancy_grid( + const nav_msgs::msg::OccupancyGrid & grid, + std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (ignores grid header). + */ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid); /** @@ -174,6 +235,13 @@ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid); * @warning If the map does not carry grid metadata or the `"occupancy"` layer is missing, * the result may be incomplete or implementation-defined. */ +nav_msgs::msg::OccupancyGrid to_occupancy_grid( + const navmap::NavMap & nm, + const std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload. + */ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm); /** @@ -200,13 +268,13 @@ struct BuildParams float neighbor_radius = 2.0f; // search radius /** @brief Alternative to radius: number of nearest neighbors (k-NN). */ - int k_neighbors = 20; // k-NN alternative to radius + int k_neighbors = 20; // k-NN alternative to radius /** @brief Minimum triangle area (square meters) to reject degenerate faces. */ float min_area = 1e-6f; // minimum triangle area to avoid degenerates /** @brief If true, use radius-based neighborhoods; otherwise use k-NN. */ - bool use_radius = true; + bool use_radius = true; /** @brief Minimum interior angle (degrees) to avoid sliver triangles. */ float min_angle_deg = 20.0f; // minimum interior angle (deg) to avoid sliver triangles @@ -238,7 +306,7 @@ struct BuildParams * @throw std::runtime_error If meshing fails due to inconsistent parameters or empty input. */ navmap::NavMap from_points( - const pcl::PointCloud & input_points, + const pcl::PointCloud & input_points, navmap_ros_interfaces::msg::NavMap & out_msg, BuildParams params); diff --git a/navmap_ros/include/navmap_ros/navmap_io.hpp b/navmap_ros/include/navmap_ros/navmap_io.hpp index a2b8d60..1500b28 100644 --- a/navmap_ros/include/navmap_ros/navmap_io.hpp +++ b/navmap_ros/include/navmap_ros/navmap_io.hpp @@ -41,7 +41,6 @@ #include #include "navmap_core/NavMap.hpp" -#include "navmap_ros/conversions.hpp" #include "navmap_ros_interfaces/msg/nav_map.hpp" diff --git a/navmap_ros/package.xml b/navmap_ros/package.xml index 4f0ec59..791cd1e 100644 --- a/navmap_ros/package.xml +++ b/navmap_ros/package.xml @@ -1,7 +1,7 @@ navmap_ros - 0.2.5 + 0.5.0 Conversions between navmap_core and ROS messages Francisco Martín Rico diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index d04e0b6..7565125 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -25,21 +25,17 @@ #include #include "geometry_msgs/msg/pose.hpp" -#include +#include "std_msgs/msg/header.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "navmap_ros_interfaces/msg/nav_map.hpp" #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" -#include "navmap_core/Geometry.hpp" - #include "pcl_conversions/pcl_conversions.h" -#include "pcl/point_types_conversion.h" +#include "pcl/common/point_tests.h" -#include "pcl/common/transforms.h" #include "pcl/point_cloud.h" #include "pcl/point_types.h" -#include "pcl/PointIndices.h" #include "pcl/kdtree/kdtree_flann.h" namespace navmap_ros @@ -53,15 +49,15 @@ using navmap_ros_interfaces::msg::NavMapSurface; static inline uint8_t occ_to_u8(int8_t v) { - if (v < 0) {return 255u;} - if (v >= 100) {return 254u;} - return static_cast(std::lround((v / 100.0) * 254.0)); + if (v < 0) {return NO_INFORMATION;} + if (v >= 100) {return LETHAL_OBSTACLE;} + return static_cast(std::lround((v / 100.0) * static_cast(LETHAL_OBSTACLE))); } static inline int8_t u8_to_occ(uint8_t u) { - if (u == 255u) {return -1;} - return static_cast(std::lround((u / 254.0) * 100.0)); + if (u == NO_INFORMATION) {return -1;} + return static_cast(std::lround((u / static_cast(LETHAL_OBSTACLE)) * 100.0)); } static inline navmap::NavCelId tri_index_for_cell(uint32_t i, uint32_t j, uint32_t W) @@ -71,9 +67,10 @@ static inline navmap::NavCelId tri_index_for_cell(uint32_t i, uint32_t j, uint32 // ----------------- NavMap <-> ROS message ----------------- -NavMap to_msg(const navmap::NavMap & nm) +NavMap to_msg(const navmap::NavMap & nm, const std_msgs::msg::Header & header) { NavMap out; + out.header = header; // positions out.positions_x.assign(nm.positions.x.begin(), nm.positions.x.end()); @@ -139,8 +136,14 @@ NavMap to_msg(const navmap::NavMap & nm) return out; } -navmap::NavMap from_msg(const NavMap & msg) +NavMap to_msg(const navmap::NavMap & nm) +{ + return to_msg(nm, std_msgs::msg::Header()); +} + +navmap::NavMap from_msg(const NavMap & msg, std_msgs::msg::Header & header) { + header = msg.header; navmap::NavMap nm; // positions @@ -171,8 +174,9 @@ navmap::NavMap from_msg(const NavMap & msg) nm.surfaces.resize(msg.surfaces.size()); for (size_t i = 0; i < msg.surfaces.size(); ++i) { nm.surfaces[i].frame_id = msg.surfaces[i].frame_id; - nm.surfaces[i].navcels.assign(msg.surfaces[i].navcels.begin(), - msg.surfaces[i].navcels.end()); + nm.surfaces[i].navcels.assign( + msg.surfaces[i].navcels.begin(), + msg.surfaces[i].navcels.end()); } // Fallback: create a single surface if none provided and triangles exist. @@ -207,11 +211,19 @@ navmap::NavMap from_msg(const NavMap & msg) return nm; } +navmap::NavMap from_msg(const NavMap & msg) +{ + std_msgs::msg::Header unused; + return from_msg(msg, unused); +} + navmap_ros_interfaces::msg::NavMapLayer to_msg( const navmap::NavMap & nm, - const std::string & layer_name) + const std::string & layer_name, + const std_msgs::msg::Header & header) { navmap_ros_interfaces::msg::NavMapLayer msg; + msg.header = header; msg.name = layer_name; auto base = nm.layers.get(layer_name); @@ -245,13 +257,22 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( return msg; } +navmap_ros_interfaces::msg::NavMapLayer to_msg( + const navmap::NavMap & nm, + const std::string & layer_name) +{ + return to_msg(nm, layer_name, std_msgs::msg::Header()); +} + void from_msg( const navmap_ros_interfaces::msg::NavMapLayer & msg, - navmap::NavMap & nm) + navmap::NavMap & nm, + std_msgs::msg::Header & header) { + header = msg.header; switch (msg.type) { case navmap_ros_interfaces::msg::NavMapLayer::U8: { - auto dst = nm.add_layer(msg.name, /*desc*/"", /*unit*/"", uint8_t{}); + auto dst = nm.add_layer(msg.name, /*desc*/ "", /*unit*/ "", uint8_t{}); if (dst->data().size() != msg.data_u8.size()) { dst->data().resize(msg.data_u8.size()); } @@ -259,7 +280,7 @@ void from_msg( break; } case navmap_ros_interfaces::msg::NavMapLayer::F32: { - auto dst = nm.add_layer(msg.name, /*desc*/"", /*unit*/"", 0.0f); + auto dst = nm.add_layer(msg.name, /*desc*/ "", /*unit*/ "", 0.0f); if (dst->data().size() != msg.data_f32.size()) { dst->data().resize(msg.data_f32.size()); } @@ -267,7 +288,7 @@ void from_msg( break; } case navmap_ros_interfaces::msg::NavMapLayer::F64: { - auto dst = nm.add_layer(msg.name, /*desc*/"", /*unit*/"", 0.0); + auto dst = nm.add_layer(msg.name, /*desc*/ "", /*unit*/ "", 0.0); if (dst->data().size() != msg.data_f64.size()) { dst->data().resize(msg.data_f64.size()); } @@ -275,15 +296,27 @@ void from_msg( break; } default: - throw std::runtime_error("from_msg(NavMapLayer): unsupported type value " + - std::to_string(msg.type)); + throw std::runtime_error( + "from_msg(NavMapLayer): unsupported type value " + + std::to_string(msg.type)); } } +void from_msg( + const navmap_ros_interfaces::msg::NavMapLayer & msg, + navmap::NavMap & nm) +{ + std_msgs::msg::Header unused; + from_msg(msg, nm, unused); +} + // ----------------- OccupancyGrid <-> NavMap ----------------- -navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) +navmap::NavMap from_occupancy_grid( + const nav_msgs::msg::OccupancyGrid & grid, + std_msgs::msg::Header & header) { + header = grid.header; navmap::NavMap nm; const uint32_t W = grid.info.width; @@ -353,10 +386,21 @@ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) return nm; } -nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) +navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) +{ + std_msgs::msg::Header unused; + return from_occupancy_grid(grid, unused); +} + +nav_msgs::msg::OccupancyGrid to_occupancy_grid( + const navmap::NavMap & nm, + const std_msgs::msg::Header & header) { nav_msgs::msg::OccupancyGrid g; - g.header.frame_id = (nm.surfaces.empty() ? std::string() : nm.surfaces[0].frame_id); + g.header = header; + if (g.header.frame_id.empty() && !nm.surfaces.empty()) { + g.header.frame_id = nm.surfaces[0].frame_id; + } auto base = nm.layers.get("occupancy"); if (!base || base->type() != navmap::LayerType::U8) { @@ -440,8 +484,9 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) Eigen::Vector3f closest; float sq = 0.0f; - if (nm.closest_navcel({cx, cy, static_cast(g.info.origin.position.z)}, - sidx, cid, closest, sq)) + if (nm.closest_navcel( + {cx, cy, static_cast(g.info.origin.position.z)}, + sidx, cid, closest, sq)) { const uint8_t u8 = (*occ)[cid]; g.data[idx_cell(i, j)] = u8_to_occ(u8); @@ -453,6 +498,13 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) return g; } +nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) +{ + std_msgs::msg::Header h; + h.frame_id = (nm.surfaces.empty() ? std::string() : nm.surfaces[0].frame_id); + return to_occupancy_grid(nm, h); +} + bool build_navmap_from_mesh( const pcl::PointCloud & cloud, const std::vector & triangles, @@ -587,7 +639,7 @@ struct TriHasher std::size_t operator()(const TriKey & t) const noexcept { std::size_t h = 1469598103934665603ull; - auto mix = [&](int k){ + auto mix = [&](int k) { h ^= static_cast(k) + 0x9e3779b97f4a7c15ull + (h << 6) + (h >> 2); }; mix(t.a); mix(t.b); mix(t.c); @@ -700,15 +752,16 @@ downsample_voxelize_topZ_layered( auto & idxs = kv.second; if (idxs.empty()) {continue;} - std::sort(idxs.begin(), idxs.end(), - [&](int a, int b){return input_points[a].z < input_points[b].z;}); + std::sort( + idxs.begin(), idxs.end(), + [&](int a, int b) {return input_points[a].z < input_points[b].z;}); double sum_x = 0.0, sum_y = 0.0; - float z_max = -std::numeric_limits::infinity(); - int count = 0; - float last_z = input_points[idxs.front()].z; + float z_max = -std::numeric_limits::infinity(); + int count = 0; + float last_z = input_points[idxs.front()].z; - auto flush_cluster = [&](){ + auto flush_cluster = [&]() { if (count <= 0) {return;} const float cx = static_cast(sum_x / count); const float cy = static_cast(sum_y / count); @@ -821,9 +874,10 @@ static void keep_top_surfaces_by_size(navmap_ros_interfaces::msg::NavMap & msg, std::vector ids(msg.surfaces.size()); std::iota(ids.begin(), ids.end(), 0); - std::sort(ids.begin(), ids.end(), [&](size_t a, size_t b){ + std::sort( + ids.begin(), ids.end(), [&](size_t a, size_t b) { return msg.surfaces[a].navcels.size() > msg.surfaces[b].navcels.size(); - }); + }); std::vector kept; kept.reserve(static_cast(max_surfaces)); @@ -931,8 +985,9 @@ static void rebuild_surfaces_by_connectivity(navmap_ros_interfaces::msg::NavMap for (auto & kv : comp) { comps.push_back(std::move(kv.second)); } - std::sort(comps.begin(), comps.end(), - [](const auto & A, const auto & B){return A.size() > B.size();}); + std::sort( + comps.begin(), comps.end(), + [](const auto & A, const auto & B) {return A.size() > B.size();}); const std::string fid = msg.header.frame_id; std::vector out; @@ -983,8 +1038,9 @@ navmap::NavMap from_points( // Seeds in ascending Z std::vector order(N); std::iota(order.begin(), order.end(), 0); - std::sort(order.begin(), order.end(), - [&](int a, int b){return cloud[a].z < cloud[b].z;}); + std::sort( + order.begin(), order.end(), + [&](int a, int b) {return cloud[a].z < cloud[b].z;}); // Global state std::unordered_set tri_set_global; @@ -1011,11 +1067,11 @@ navmap::NavMap from_points( } }; - auto dist3f = [](const pcl::PointXYZ & A, const pcl::PointXYZ & B){ + auto dist3f = [](const pcl::PointXYZ & A, const pcl::PointXYZ & B) { const float dx = A.x - B.x, dy = A.y - B.y, dz = A.z - B.z; return std::sqrt(dx * dx + dy * dy + dz * dz); }; - auto distXY = [](const pcl::PointXYZ & A, const pcl::PointXYZ & B){ + auto distXY = [](const pcl::PointXYZ & A, const pcl::PointXYZ & B) { const float dx = A.x - B.x, dy = A.y - B.y; return std::sqrt(dx * dx + dy * dy); }; @@ -1092,18 +1148,22 @@ navmap::NavMap from_points( // Quick filters if (P.max_edge_len > 0.0f) { - neigh_seed.erase(std::remove_if(neigh_seed.begin(), neigh_seed.end(), - [&](int j){ - const auto & Q = cloud[j]; if (!pcl::isFinite(Q)) { - return true; - } - return dist3f(cloud[seed_idx], Q) > P.max_edge_len; - }), + neigh_seed.erase( + std::remove_if( + neigh_seed.begin(), neigh_seed.end(), + [&](int j) { + const auto & Q = cloud[j]; if (!pcl::isFinite(Q)) { + return true; + } + return dist3f(cloud[seed_idx], Q) > P.max_edge_len; + }), neigh_seed.end()); } { - neigh_seed.erase(std::remove_if(neigh_seed.begin(), neigh_seed.end(), - [&](int j){return std::fabs(cloud[j].z - cloud[seed_idx].z) > z_window_seed;}), + neigh_seed.erase( + std::remove_if( + neigh_seed.begin(), neigh_seed.end(), + [&](int j) {return std::fabs(cloud[j].z - cloud[seed_idx].z) > z_window_seed;}), neigh_seed.end()); } @@ -1115,11 +1175,12 @@ navmap::NavMap from_points( // Angular sort in XY { const auto & Cc = cloud[seed_idx]; - std::sort(neigh_seed.begin(), neigh_seed.end(), [&](int a, int b){ + std::sort( + neigh_seed.begin(), neigh_seed.end(), [&](int a, int b) { const float ax = cloud[a].x - Cc.x, ay = cloud[a].y - Cc.y; const float bx = cloud[b].x - Cc.x, by = cloud[b].y - Cc.y; return std::atan2(ay, ax) < std::atan2(by, bx); - }); + }); } const size_t tri_off = triangles.size(); @@ -1155,7 +1216,8 @@ navmap::NavMap from_points( const int k = neigh_seed.front(); bool dup = false; if (precheck(seed_idx, j, k, Phase::FAN, comp_rej, dup)) { - if (try_add_triangle(seed_idx, j, k, cloud, P, tri_set_global, edge_set_global, + if (try_add_triangle( + seed_idx, j, k, cloud, P, tri_set_global, edge_set_global, triangles)) { ++comp_fan_accept; diff --git a/navmap_ros/src/slam_server_app.cpp b/navmap_ros/src/slam_server_app.cpp index ed28550..803e633 100644 --- a/navmap_ros/src/slam_server_app.cpp +++ b/navmap_ros/src/slam_server_app.cpp @@ -32,7 +32,8 @@ class SLAMServerNode : public rclcpp::Node SLAMServerNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : Node("slam_server_node", options) { - navmap_pub_ = create_publisher("navmap", + navmap_pub_ = create_publisher( + "navmap", rclcpp::QoS(1).transient_local().reliable()); incoming_occ_map_sub_ = create_subscription( @@ -64,17 +65,18 @@ class SLAMServerNode : public rclcpp::Node navmap_pub_->publish(navmap_msg_); }); - savemap_srv_ = create_service("savemap", - [this]( - const std::shared_ptr request, - std::shared_ptr response) - { - (void)request; - (void)response; - RCLCPP_INFO(get_logger(), "Saving NavMap from /tmp/map.navmap"); - - navmap_ros::io::save_to_file(navmap_, "/tmp/map.navmap"); - }); + savemap_srv_ = create_service( + "savemap", + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + (void)request; + (void)response; + RCLCPP_INFO(get_logger(), "Saving NavMap from /tmp/map.navmap"); + + navmap_ros::io::save_to_file(navmap_, "/tmp/map.navmap"); + }); } private: diff --git a/navmap_ros/tests/test_conversions.cpp b/navmap_ros/tests/test_conversions.cpp index 54bc3f0..266fe6e 100644 --- a/navmap_ros/tests/test_conversions.cpp +++ b/navmap_ros/tests/test_conversions.cpp @@ -17,7 +17,7 @@ #include #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" -#include "navmap_ros_interfaces/msg/nav_map.hpp" +#include "std_msgs/msg/header.hpp" #include "navmap_ros/conversions.hpp" #include "navmap_core/NavMap.hpp" @@ -125,8 +125,17 @@ static inline navmap::NavCelId tri_index_for_cell(uint32_t i, uint32_t j, uint32 TEST(NavMap_FullConversions, RoundTrip_All) { navmap::NavMap a; build_square_with_layers(a); - auto msg = navmap_ros::to_msg(a); - navmap::NavMap b = navmap_ros::from_msg(msg); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 123; + h.stamp.nanosec = 456; + auto msg = navmap_ros::to_msg(a, h); + std_msgs::msg::Header h2; + navmap::NavMap b = navmap_ros::from_msg(msg, h2); + + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); ASSERT_EQ(a.positions.size(), b.positions.size()); for (size_t i = 0; i < a.positions.size(); ++i) { @@ -171,8 +180,16 @@ TEST(NavMap_FullConversions, RoundTrip_All) TEST(NavMap_FullConversions, EmptyMap_RoundTrip) { navmap::NavMap a; - auto msg = navmap_ros::to_msg(a); - navmap::NavMap b = navmap_ros::from_msg(msg); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 1; + h.stamp.nanosec = 2; + auto msg = navmap_ros::to_msg(a, h); + std_msgs::msg::Header h2; + navmap::NavMap b = navmap_ros::from_msg(msg, h2); + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); EXPECT_EQ(b.positions.size(), 0u); EXPECT_EQ(b.navcels.size(), 0u); EXPECT_EQ(b.surfaces.size(), 0u); @@ -185,7 +202,14 @@ TEST(NavMap_LayerConversions, U8_RoundTrip) auto occ = nm.add_layer("occupancy", "occ", "", uint8_t(0)); occ->data()[0] = 10u; occ->data()[1] = 250u; - auto msg = navmap_ros::to_msg(nm, "occupancy"); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 10; + h.stamp.nanosec = 20; + auto msg = navmap_ros::to_msg(nm, "occupancy", h); + EXPECT_EQ(msg.header.frame_id, h.frame_id); + EXPECT_EQ(msg.header.stamp.sec, h.stamp.sec); + EXPECT_EQ(msg.header.stamp.nanosec, h.stamp.nanosec); EXPECT_EQ(msg.name, "occupancy"); EXPECT_EQ(msg.type, 0u); // 0=U8 ASSERT_EQ(msg.data_u8.size(), 2u); @@ -193,7 +217,11 @@ TEST(NavMap_LayerConversions, U8_RoundTrip) EXPECT_EQ(msg.data_u8[1], 250u); navmap::NavMap nm2; make_flat_square(nm2); - navmap_ros::from_msg(msg, nm2); + std_msgs::msg::Header h2; + navmap_ros::from_msg(msg, nm2, h2); + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); ASSERT_TRUE(nm2.has_layer("occupancy")); EXPECT_NEAR(nm2.layer_get("occupancy", 0), 10.0, 1e-6); EXPECT_NEAR(nm2.layer_get("occupancy", 1), 250.0, 1e-6); @@ -205,14 +233,25 @@ TEST(NavMap_LayerConversions, F32_RoundTrip) auto cost = nm.add_layer("cost", "cost", "", 0.0f); cost->data()[0] = 1.25f; cost->data()[1] = 9.5f; - auto msg = navmap_ros::to_msg(nm, "cost"); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 11; + h.stamp.nanosec = 22; + auto msg = navmap_ros::to_msg(nm, "cost", h); + EXPECT_EQ(msg.header.frame_id, h.frame_id); + EXPECT_EQ(msg.header.stamp.sec, h.stamp.sec); + EXPECT_EQ(msg.header.stamp.nanosec, h.stamp.nanosec); EXPECT_EQ(msg.type, 1u); // 1=F32 ASSERT_EQ(msg.data_f32.size(), 2u); EXPECT_NEAR(msg.data_f32[0], 1.25f, 1e-6); EXPECT_NEAR(msg.data_f32[1], 9.5f, 1e-6); navmap::NavMap nm2; make_flat_square(nm2); - navmap_ros::from_msg(msg, nm2); + std_msgs::msg::Header h2; + navmap_ros::from_msg(msg, nm2, h2); + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); ASSERT_TRUE(nm2.has_layer("cost")); EXPECT_NEAR(nm2.layer_get("cost", 0), 1.25, 1e-6); EXPECT_NEAR(nm2.layer_get("cost", 1), 9.5, 1e-6); @@ -225,14 +264,25 @@ TEST(NavMap_LayerConversions, F64_RoundTrip) elev->data()[0] = 12.345; elev->data()[1] = -2.5; - auto msg = navmap_ros::to_msg(nm, "elevation"); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 12; + h.stamp.nanosec = 24; + auto msg = navmap_ros::to_msg(nm, "elevation", h); + EXPECT_EQ(msg.header.frame_id, h.frame_id); + EXPECT_EQ(msg.header.stamp.sec, h.stamp.sec); + EXPECT_EQ(msg.header.stamp.nanosec, h.stamp.nanosec); EXPECT_EQ(msg.type, 2u); // 2=F64 ASSERT_EQ(msg.data_f64.size(), 2u); EXPECT_NEAR(msg.data_f64[0], 12.345, 1e-9); EXPECT_NEAR(msg.data_f64[1], -2.5, 1e-9); navmap::NavMap nm2; make_flat_square(nm2); - navmap_ros::from_msg(msg, nm2); + std_msgs::msg::Header h2; + navmap_ros::from_msg(msg, nm2, h2); + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); ASSERT_TRUE(nm2.has_layer("elevation")); EXPECT_NEAR(nm2.layer_get("elevation", 0), 12.345, 1e-9); EXPECT_NEAR(nm2.layer_get("elevation", 1), -2.5, 1e-9); @@ -248,9 +298,23 @@ TEST(TestConversions, RoundTrip_ExactEquality_4m_0p1) { const int W = 40, H = 40; auto g = make_grid_4m_0p1(); - - auto nm = from_occupancy_grid(g); - auto gout = to_occupancy_grid(nm); + g.header.stamp.sec = 111; + g.header.stamp.nanosec = 222; + + std_msgs::msg::Header h_in; + auto nm = from_occupancy_grid(g, h_in); + EXPECT_EQ(h_in.frame_id, g.header.frame_id); + EXPECT_EQ(h_in.stamp.sec, g.header.stamp.sec); + EXPECT_EQ(h_in.stamp.nanosec, g.header.stamp.nanosec); + + std_msgs::msg::Header h_out; + h_out.frame_id = "map"; + h_out.stamp.sec = 333; + h_out.stamp.nanosec = 444; + auto gout = to_occupancy_grid(nm, h_out); + EXPECT_EQ(gout.header.frame_id, h_out.frame_id); + EXPECT_EQ(gout.header.stamp.sec, h_out.stamp.sec); + EXPECT_EQ(gout.header.stamp.nanosec, h_out.stamp.nanosec); ASSERT_EQ(gout.info.width, g.info.width); ASSERT_EQ(gout.info.height, g.info.height); @@ -294,7 +358,8 @@ TEST(TestConversions, TriangleIndicesFollowPattern0) { const int W = 40; auto g = make_grid_4m_0p1(); - auto nm = from_occupancy_grid(g); + std_msgs::msg::Header unused; + auto nm = from_occupancy_grid(g, unused); // Pick a cell and verify its triangles reference the expected 4 vertices. auto v_id = [W](uint32_t i, uint32_t j) -> navmap::PointId { diff --git a/navmap_ros/tests/test_navmap_io.cpp b/navmap_ros/tests/test_navmap_io.cpp index 07a6209..72a6746 100644 --- a/navmap_ros/tests/test_navmap_io.cpp +++ b/navmap_ros/tests/test_navmap_io.cpp @@ -18,12 +18,13 @@ #include #include -#include #include #include #include #include +#include "std_msgs/msg/header.hpp" + #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" #include "navmap_ros/navmap_io.hpp" @@ -60,7 +61,7 @@ void fill_basic_header(navmap_ros_interfaces::msg::NavMap & msg, const std::stri // --- helpers: semantic comparison for messages --- template -static void ExpectVecEq(const std::vector & a, const std::vector & b, const char * what) +static void ExpectVecEq(const T & a, const T & b, const char * what) { ASSERT_EQ(a.size(), b.size()) << what << " size mismatch"; for (size_t i = 0; i < a.size(); ++i) { @@ -92,7 +93,7 @@ static void ExpectNavMapMsgEqualSemantic( const navmap_ros_interfaces::msg::NavMap & A, const navmap_ros_interfaces::msg::NavMap & B) { - // Header: frame must match; stamp puede variar → lo ignoramos + // Header: frame must match; stamp may change -> we ignore it EXPECT_EQ(A.header.frame_id, B.header.frame_id); // Geometry @@ -291,24 +292,28 @@ TEST(NavMapIoCore, RoundtripViaCoreAndMsgCompare) msg.layers = {u8, f32}; // msg -> core -navmap::NavMap core = navmap_ros::from_msg(msg); +std_msgs::msg::Header h_in; +navmap::NavMap core = navmap_ros::from_msg(msg, h_in); +EXPECT_EQ(h_in.frame_id, msg.header.frame_id); // save(core) -> load(core) -std::string path = (std::filesystem::temp_directory_path() / + std::string path = (std::filesystem::temp_directory_path() / ("core_roundtrip_" + std::to_string(::getpid()) + ".navmap")).string(); -std::error_code ec; -ASSERT_TRUE(navmap_ros::io::save_to_file(core, path, {}, &ec)) << ec.message(); + std::error_code ec; + ASSERT_TRUE(navmap_ros::io::save_to_file(core, path, {}, &ec)) << ec.message(); -navmap::NavMap core_loaded; -ASSERT_TRUE(navmap_ros::io::load_from_file(path, core_loaded, &ec)) << ec.message(); + navmap::NavMap core_loaded; + ASSERT_TRUE(navmap_ros::io::load_from_file(path, core_loaded, &ec)) << ec.message(); // core -> msg -auto msg_from_core = navmap_ros::to_msg(core); -auto msg_from_core_loaded = navmap_ros::to_msg(core_loaded); +std_msgs::msg::Header h_out; +h_out.frame_id = msg.header.frame_id; +auto msg_from_core = navmap_ros::to_msg(core, h_out); +auto msg_from_core_loaded = navmap_ros::to_msg(core_loaded, h_out); -// Comparación semántica (tolerante a orden y FP) -ExpectNavMapMsgEqualSemantic(msg_from_core, msg_from_core_loaded); +// Semantic comparison (order and FP tolerant) + ExpectNavMapMsgEqualSemantic(msg_from_core, msg_from_core_loaded); -std::filesystem::remove(path); + std::filesystem::remove(path); std::filesystem::remove(path); } diff --git a/navmap_ros_interfaces/CHANGELOG.rst b/navmap_ros_interfaces/CHANGELOG.rst index cda5d5f..c47690d 100644 --- a/navmap_ros_interfaces/CHANGELOG.rst +++ b/navmap_ros_interfaces/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package navmap_ros_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.5.0 (2026-07-25) +------------------ +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico + +0.4.0 (2025-11-24) +------------------ +* Merge branch 'rolling' into kilted +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico + 0.2.5 (2025-10-17) ------------------ diff --git a/navmap_ros_interfaces/package.xml b/navmap_ros_interfaces/package.xml index 209368a..4940da8 100644 --- a/navmap_ros_interfaces/package.xml +++ b/navmap_ros_interfaces/package.xml @@ -1,7 +1,7 @@ navmap_ros_interfaces - 0.2.5 + 0.5.0 ROS 2 interfaces for NavMap (messages for visualization and layers) Francisco Martín Rico Apache License, Version 2.0 diff --git a/navmap_rviz_plugin/CHANGELOG.rst b/navmap_rviz_plugin/CHANGELOG.rst index 8b5da4d..d97987f 100644 --- a/navmap_rviz_plugin/CHANGELOG.rst +++ b/navmap_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,25 @@ Changelog for package navmap_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.5.0 (2026-07-25) +------------------ +* Fully commit to Qt6 only and cleanup CMake +* Set Qt6 references and moc to proper plugin export +* PCL private linkage: avoid Qt5/6 conflicts +* NavMap Goal Pose +* FREE_SPACE as white +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, estherag + +0.4.0 (2025-11-24) +------------------ +* Cleanup unused headers +* NavMap Goal Pose +* Occupancy works +* FREE_SPACE as white +* Merge branch 'jazzy' into rolling +* Contributors: Francisco Martín Rico, Francisco Miguel Moreno + 0.2.5 (2025-10-17) ------------------ diff --git a/navmap_rviz_plugin/CMakeLists.txt b/navmap_rviz_plugin/CMakeLists.txt index 5ceccfe..d910fce 100644 --- a/navmap_rviz_plugin/CMakeLists.txt +++ b/navmap_rviz_plugin/CMakeLists.txt @@ -1,39 +1,44 @@ cmake_minimum_required(VERSION 3.10) project(navmap_rviz_plugin) -# 1) Qt y AUTOMOC -find_package(Qt5 REQUIRED COMPONENTS Core Widgets) set(CMAKE_AUTOMOC ON) -add_definitions(-DQT_NO_KEYWORDS) + +find_package(Qt6 QUIET COMPONENTS Widgets) +if(Qt6_FOUND) + set(QT_DEPENDENCY Qt6) + set(QT_WIDGETS_TARGET Qt6::Widgets) +else() + find_package(Qt5 REQUIRED COMPONENTS Widgets) + set(QT_DEPENDENCY Qt5) + set(QT_WIDGETS_TARGET Qt5::Widgets) +endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) +find_package(rviz_default_plugins REQUIRED) find_package(pluginlib REQUIRED) find_package(rosidl_default_runtime REQUIRED) find_package(navmap_ros_interfaces REQUIRED) - -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -qt5_wrap_cpp(NAVMAP_MOC_SRCS - include/navmap_rviz_plugin/NavMapDisplay.hpp -) +find_package(navmap_ros REQUIRED) +find_package(navmap_core REQUIRED) add_library(${PROJECT_NAME} SHARED - src/NavMapDisplay.cpp - include/navmap_rviz_plugin/NavMapDisplay.hpp - ${NAVMAP_MOC_SRCS} + src/navmap_rviz_plugin/NavMapDisplay.cpp + src/navmap_rviz_plugin/navmap_goal_tool.cpp + src/navmap_rviz_plugin/navmap_pose_tool.cpp ) target_link_libraries(navmap_rviz_plugin PUBLIC ${navmap_ros_interfaces_TARGETS} + ${QT_WIDGETS_TARGET} + navmap_ros::navmap_ros + navmap_core::navmap_core pluginlib::pluginlib rclcpp::rclcpp rviz_common::rviz_common rviz_rendering::rviz_rendering - Qt5::Core - Qt5::Widgets + rviz_default_plugins::rviz_default_plugins ) target_include_directories(${PROJECT_NAME} PUBLIC $ @@ -50,6 +55,11 @@ install(TARGETS RUNTIME DESTINATION lib/${PROJECT_NAME} ) +install( + DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" + DESTINATION "share/${PROJECT_NAME}" +) + install(FILES resource/navmap_rviz_plugin_description.xml DESTINATION share/${PROJECT_NAME} ) @@ -64,14 +74,14 @@ endif() ament_export_libraries(${PROJECT_NAME}) ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies( + ${QT_DEPENDENCY} rclcpp pluginlib rviz_common rviz_rendering rviz_default_plugins - Qt5 navmap_ros_interfaces geometry_msgs - std_msg + std_msgs ) -ament_package() \ No newline at end of file +ament_package() diff --git a/navmap_rviz_plugin/icons/classes/NavMapSetGoal.png b/navmap_rviz_plugin/icons/classes/NavMapSetGoal.png new file mode 100644 index 0000000..91de9c0 Binary files /dev/null and b/navmap_rviz_plugin/icons/classes/NavMapSetGoal.png differ diff --git a/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp b/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp index d42830f..24b9e5a 100644 --- a/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp +++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp @@ -18,10 +18,8 @@ #define NAVMAP_RVIZ_PLUGIN__NAVMAP_DISPLAY_HPP_ #include -#include #include #include -#include #include @@ -40,6 +38,8 @@ #include #include +#include "navmap_core/NavMap.hpp" + #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ #define NAVMAP_RVIZ_PLUGIN_EXPORT __attribute__ ((dllexport)) @@ -56,9 +56,9 @@ #define NAVMAP_RVIZ_PLUGIN_PUBLIC_TYPE NAVMAP_RVIZ_PLUGIN_PUBLIC #define NAVMAP_RVIZ_PLUGIN_LOCAL #else - #define NAVMAP_RVIZ_PLUGIN_PUBLIC __attribute__ ((visibility ("default"))) + #define NAVMAP_RVIZ_PLUGIN_PUBLIC __attribute__ ((visibility("default"))) #define NAVMAP_RVIZ_PLUGIN_PUBLIC_TYPE - #define NAVMAP_RVIZ_PLUGIN_LOCAL __attribute__ ((visibility ("hidden"))) + #define NAVMAP_RVIZ_PLUGIN_LOCAL __attribute__ ((visibility("hidden"))) #endif // Forward declarations to avoid hard coupling here @@ -74,6 +74,8 @@ class HardwareVertexBuffer; namespace navmap_rviz_plugin { +inline navmap::NavMap received_navmap; + class NAVMAP_RVIZ_PLUGIN_PUBLIC NavMapDisplay : public rviz_common::MessageFilterDisplay { @@ -144,8 +146,8 @@ private Q_SLOTS: // ---- Status counters ---- std::uint64_t navmap_msg_count_{0}; std::uint64_t layer_update_count_{0}; - rclcpp::Time last_navmap_stamp_; - rclcpp::Time last_layer_stamp_; + rclcpp::Time last_navmap_stamp_; + rclcpp::Time last_layer_stamp_; // ---- Data state ---- NavMapMsg::SharedPtr last_msg_; diff --git a/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp new file mode 100644 index 0000000..7a4562d --- /dev/null +++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp @@ -0,0 +1,69 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 NAVMAP_RVIZ_PLUGIN__NAVMAP_GOAL_TOOL_HPP_ +#define NAVMAP_RVIZ_PLUGIN__NAVMAP_GOAL_TOOL_HPP_ + +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "navmap_rviz_plugin/navmap_pose_tool.hpp" +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_common +{ +class DisplayContext; +namespace properties +{ +class StringProperty; +class QosProfileProperty; +} // namespace properties +} // namespace rviz_common + +namespace navmap_rviz_plugin +{ +class RVIZ_DEFAULT_PLUGINS_PUBLIC NavMapGoalTool : public NavMapPoseTool +{ + Q_OBJECT + +public: + NavMapGoalTool(); + + ~NavMapGoalTool() override; + + void onInitialize() override; + +protected: + void onPoseSet(double x, double y, double z, double theta) override; + +private Q_SLOTS: + void updateTopic(); + +private: + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Clock::SharedPtr clock_; + + rviz_common::properties::StringProperty * topic_property_; + rviz_common::properties::QosProfileProperty * qos_profile_property_; + + rclcpp::QoS qos_profile_; +}; + +} // namespace navmap_rviz_plugin + +#endif // NAVMAP_RVIZ_PLUGIN__NAVMAP_GOAL_TOOL_HPP_ diff --git a/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp new file mode 100644 index 0000000..0cdae6a --- /dev/null +++ b/navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp @@ -0,0 +1,93 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 NAVMAP_RVIZ_PLUGIN__NAVMAP_POSE_TOOL_HPP_ +#define NAVMAP_RVIZ_PLUGIN__NAVMAP_POSE_TOOL_HPP_ + +#include +#include +#include + +#include + +#include // NOLINT cpplint cannot handle include order here + +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/quaternion.hpp" + +#include "rviz_common/tool.hpp" +#include "rviz_rendering/viewport_projection_finder.hpp" +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_rendering +{ +class Arrow; +} // namespace rviz_rendering + +namespace navmap_rviz_plugin +{ + +class RVIZ_DEFAULT_PLUGINS_PUBLIC NavMapPoseTool : public rviz_common::Tool +{ +public: + NavMapPoseTool(); + + ~NavMapPoseTool() override; + + void onInitialize() override; + + void activate() override; + + void deactivate() override; + + int processMouseEvent(rviz_common::ViewportMouseEvent & event) override; + +protected: + virtual void onPoseSet(double x, double y, double z, double theta) = 0; + + geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle); + + void logPose( + std::string designation, + geometry_msgs::msg::Point position, + geometry_msgs::msg::Quaternion orientation, + double angle, + std::string frame); + + std::shared_ptr arrow_; + + enum State + { + Position, + Orientation + }; + State state_; + double angle_; + + Ogre::Vector3 arrow_position_; + std::shared_ptr projection_finder_; + +private: + int processMouseLeftButtonPressed(std::pair xy_plane_intersection); + int processMouseMoved(std::pair xy_plane_intersection); + int processMouseLeftButtonReleased(); + void makeArrowVisibleAndSetOrientation(double angle); + double calculateAngle(Ogre::Vector3 start_point, Ogre::Vector3 end_point); +}; + +} // namespace navmap_rviz_plugin + +#endif // NAVMAP_RVIZ_PLUGIN__NAVMAP_POSE_TOOL_HPP_ diff --git a/navmap_rviz_plugin/package.xml b/navmap_rviz_plugin/package.xml index 3f17dc4..1638776 100644 --- a/navmap_rviz_plugin/package.xml +++ b/navmap_rviz_plugin/package.xml @@ -3,7 +3,7 @@ navmap_rviz_plugin - 0.2.5 + 0.5.0 RViz2 display plugin for NavMap surfaces and layers. Francisco Martín Rico @@ -17,6 +17,8 @@ rviz_rendering rviz_default_plugins navmap_ros_interfaces + navmap_ros + navmap_core geometry_msgs std_msgs sensor_msgs diff --git a/navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml b/navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml index 7e0ef71..c4984f6 100644 --- a/navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml +++ b/navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml @@ -5,4 +5,15 @@ base_class_type="rviz_common::Display"> Visualize NavMap triangles and layers, with optional normals and alpha. + + + + Publish a goal pose for the robot. After one use, reverts to default tool. + + + diff --git a/navmap_rviz_plugin/src/NavMapDisplay.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp similarity index 93% rename from navmap_rviz_plugin/src/NavMapDisplay.cpp rename to navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp index f377532..0085beb 100644 --- a/navmap_rviz_plugin/src/NavMapDisplay.cpp +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp @@ -14,35 +14,22 @@ // limitations under the License. -#include "navmap_rviz_plugin/NavMapDisplay.hpp" +#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 +#include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" +#include "navmap_rviz_plugin/NavMapDisplay.hpp" namespace { @@ -53,7 +40,7 @@ inline void hsv2rgb(float H, float S, float V, float & R, float & G, float & B) const float m = V - C; float r1 = 0.f, g1 = 0.f, b1 = 0.f; - if (H < 60.f) {r1 = C; g1 = X; b1 = 0.f;} else if (H < 120.f) { + if (H < 60.f) {r1 = C; g1 = X; b1 = 0.f;} else if (H < 120.f) { r1 = X; g1 = C; b1 = 0.f; } else if (H < 180.f) {r1 = 0.f; g1 = C; b1 = X;} else if (H < 240.f) { r1 = 0.f; g1 = X; b1 = C; @@ -76,8 +63,8 @@ inline Ogre::ColourValue colorFromRainbow(float value, float max_value, float al inline Ogre::ColourValue colorFromU8(uint8_t v, float alpha) { - if (v == 0) {return Ogre::ColourValue(0.5f, 0.5f, 0.5f, alpha);} - if (v == 255) {return Ogre::ColourValue(0.0f, 0.39f, 0.0f, alpha);} + if (v == 0) {return Ogre::ColourValue(1.0f, 1.0f, 1.0f, alpha);} + if (v == 255) {return Ogre::ColourValue(0.25f, 0.25f, 0.25f, alpha);} if (v == 254) {return Ogre::ColourValue(0.0f, 0.0f, 0.0f, alpha);} float occ = static_cast(v) / 253.0f; float c = 1.0f - occ; @@ -212,10 +199,11 @@ void NavMapDisplay::processMessage(const NavMapMsg::ConstSharedPtr msg) geometry_msgs::msg::Pose identity; if (!context_->getFrameManager()->transform( - msg->header, identity, position, orientation)) + msg->header, identity, position, orientation)) { - setStatus(rviz_common::properties::StatusProperty::Error, - "TF", "Unable to transform " + QString::fromStdString(msg->header.frame_id)); + setStatus( + rviz_common::properties::StatusProperty::Error, + "TF", "Unable to transform " + QString::fromStdString(msg->header.frame_id)); return; } @@ -223,6 +211,7 @@ void NavMapDisplay::processMessage(const NavMapMsg::ConstSharedPtr msg) root_node_->setOrientation(orientation); last_msg_ = std::make_shared(*msg); + received_navmap = navmap_ros::from_msg(*last_msg_); ++navmap_msg_count_; last_navmap_stamp_ = rviz_ros_node_.lock()->get_raw_node()->now(); @@ -280,24 +269,27 @@ void NavMapDisplay::subscribeToLayerTopic() s << "Some layer messages were lost. New lost: " << info.total_count_change << " | Total lost: " << info.total_count; - setStatus(rviz_common::properties::StatusProperty::Warn, "Layer Update Topic", + setStatus( + rviz_common::properties::StatusProperty::Warn, "Layer Update Topic", s.str().c_str()); }; layer_subscription_ = node->create_subscription( - layer_topic_property_->getTopicStd(), - layer_profile_, + layer_topic_property_->getTopicStd(), + layer_profile_, [this](NavMapLayerMsg::ConstSharedPtr msg) {incomingLayer(msg);}, - sub_opts); + sub_opts); layer_subscription_start_time_ = node->now(); setStatus(rviz_common::properties::StatusProperty::Ok, "Layer Update Topic", "OK"); } catch (const rclcpp::exceptions::InvalidTopicNameError & e) { - setStatus(rviz_common::properties::StatusProperty::Error, + setStatus( + rviz_common::properties::StatusProperty::Error, "Layer Update Topic", QString("Invalid topic: ") + e.what()); } catch (const std::exception & e) { - setStatus(rviz_common::properties::StatusProperty::Error, + setStatus( + rviz_common::properties::StatusProperty::Error, "Layer Update Topic", QString("Failed to subscribe: ") + e.what()); } } @@ -331,13 +323,15 @@ void NavMapDisplay::incomingLayer(const NavMapLayerMsg::ConstSharedPtr & msg) const int non_empty = (n_u8 ? 1 : 0) + (n_f32 ? 1 : 0) + (n_f64 ? 1 : 0); if (non_empty != 1) { - setStatus(rviz_common::properties::StatusProperty::Error, "Layer Update", + setStatus( + rviz_common::properties::StatusProperty::Error, "Layer Update", "Exactly one of data_u8 / data_f32 / data_f64 must be non-empty."); return; } const size_t eff_len = n_u8 ? n_u8 : (n_f32 ? n_f32 : n_f64); if (eff_len != n_tris) { - setStatus(rviz_common::properties::StatusProperty::Error, "Layer Update", + setStatus( + rviz_common::properties::StatusProperty::Error, "Layer Update", QString("Layer size (%1) does not match number of triangles (%2)") .arg(eff_len).arg(n_tris)); return; @@ -362,8 +356,9 @@ void NavMapDisplay::incomingLayer(const NavMapLayerMsg::ConstSharedPtr & msg) .arg(QString::fromStdString(msg->name)) .arg(type_str) .arg(qulonglong(len)); - setStatus(rviz_common::properties::StatusProperty::Ok, - "Layer Update Topic", line); + setStatus( + rviz_common::properties::StatusProperty::Ok, + "Layer Update Topic", line); if (currentSelectedLayer_() == msg->name) { updateColorSchemeOptions_(); @@ -563,13 +558,13 @@ void NavMapDisplay::ensureMeshBuilt_() // Create the dynamic colour buffer (one 32-bit colour per vertex) const Ogre::VertexElementType col_type = Ogre::VET_COLOUR_ARGB; // we'll pack ARGB - decl->addElement(COLOR_SRC, /*offset=*/0, col_type, Ogre::VES_DIFFUSE); + decl->addElement(COLOR_SRC, /*offset=*/ 0, col_type, Ogre::VES_DIFFUSE); Ogre::HardwareVertexBufferSharedPtr colour_vbuf = Ogre::HardwareBufferManager::getSingleton().createVertexBuffer( - Ogre::VertexElement::getTypeSize(col_type), // should be 4 - vertex_count, - Ogre::HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY_DISCARDABLE); + Ogre::VertexElement::getTypeSize(col_type), // should be 4 + vertex_count, + Ogre::HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY_DISCARDABLE); bind->setBinding(COLOR_SRC, colour_vbuf); @@ -670,7 +665,7 @@ void NavMapDisplay::updateColorsOnly_() uint32_t * p = reinterpret_cast(base); auto packARGB = [&](const Ogre::ColourValue & c) -> uint32_t { - // Pack into ARGB to match VET_COLOUR_ARGB used at creation + // Pack into ARGB to match VET_COLOUR_ARGB used at creation return Ogre::VertexElement::convertColourValue(c, Ogre::VET_COLOUR_ARGB); }; @@ -708,7 +703,7 @@ void NavMapDisplay::updateColorsOnly_() for (size_t t = 0; t < V0.size(); ++t) { const Ogre::ColourValue col = use_rainbow ? colorFromRainbow(selected_layer->data_f32[t], max_val, alpha) : - colorFromHeat (selected_layer->data_f32[t], max_val, alpha); + colorFromHeat(selected_layer->data_f32[t], max_val, alpha); const uint32_t packed = packARGB(col); *p++ = packed; *p++ = packed; *p++ = packed; } @@ -717,7 +712,7 @@ void NavMapDisplay::updateColorsOnly_() const float v = static_cast(selected_layer->data_f64[t]); const Ogre::ColourValue col = use_rainbow ? colorFromRainbow(v, max_val, alpha) : - colorFromHeat (v, max_val, alpha); + colorFromHeat(v, max_val, alpha); const uint32_t packed = packARGB(col); *p++ = packed; *p++ = packed; *p++ = packed; } diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp new file mode 100644 index 0000000..1d1aa85 --- /dev/null +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp @@ -0,0 +1,87 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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. + + +#include "navmap_rviz_plugin/navmap_goal_tool.hpp" + +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/properties/string_property.hpp" +#include "rviz_common/properties/qos_profile_property.hpp" + +namespace navmap_rviz_plugin +{ + +NavMapGoalTool::NavMapGoalTool() +: navmap_rviz_plugin::NavMapPoseTool(), qos_profile_(5) +{ + shortcut_key_ = 'g'; + + topic_property_ = new rviz_common::properties::StringProperty( + "Topic", "goal_pose", + "The topic on which to publish goals.", + getPropertyContainer(), SLOT(updateTopic()), this); + + qos_profile_property_ = new rviz_common::properties::QosProfileProperty( + topic_property_, qos_profile_); +} + +NavMapGoalTool::~NavMapGoalTool() = default; + +void NavMapGoalTool::onInitialize() +{ + NavMapPoseTool::onInitialize(); + qos_profile_property_->initialize( + [this](rclcpp::QoS profile) {this->qos_profile_ = profile;}); + setName("NavMap Goal Pose"); + updateTopic(); +} + +void NavMapGoalTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = + context_->getRosNodeAbstraction().lock()->get_raw_node(); + publisher_ = raw_node-> + template create_publisher( + topic_property_->getStdString(), qos_profile_); + clock_ = raw_node->get_clock(); +} + +void NavMapGoalTool::onPoseSet(double x, double y, double z, double theta) +{ + std::string fixed_frame = context_->getFixedFrame().toStdString(); + + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = clock_->now(); + goal.header.frame_id = fixed_frame; + + goal.pose.position.x = x; + goal.pose.position.y = y; + goal.pose.position.z = z; + + goal.pose.orientation = orientationAroundZAxis(theta); + + logPose("goal", goal.pose.position, goal.pose.orientation, theta, fixed_frame); + + publisher_->publish(goal); +} + +} // namespace navmap_rviz_plugin + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(navmap_rviz_plugin::NavMapGoalTool, rviz_common::Tool) diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp new file mode 100644 index 0000000..398f5bd --- /dev/null +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_pose_tool.cpp @@ -0,0 +1,220 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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. + + +#include "navmap_rviz_plugin/navmap_pose_tool.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "rviz_rendering/geometry.hpp" +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/render_window.hpp" + +#include "rviz_common/logging.hpp" +#include "rviz_common/display_context.hpp" +#include "rviz_common/render_panel.hpp" +#include "rviz_common/viewport_mouse_event.hpp" +#include "rviz_common/view_manager.hpp" +#include "rviz_common/view_controller.hpp" + +#include "navmap_core/NavMap.hpp" +#include "navmap_rviz_plugin/NavMapDisplay.hpp" + +namespace navmap_rviz_plugin +{ + +NavMapPoseTool::NavMapPoseTool() +: rviz_common::Tool(), arrow_(nullptr), angle_(0) +{ + projection_finder_ = std::make_shared(); +} + +NavMapPoseTool::~NavMapPoseTool() = default; + +void NavMapPoseTool::onInitialize() +{ + arrow_ = std::make_shared( + scene_manager_, nullptr, 2.0f, 0.2f, 0.5f, 0.35f); + arrow_->setColor(0.0f, 1.0f, 0.0f, 1.0f); + arrow_->getSceneNode()->setVisible(false); +} + +void NavMapPoseTool::activate() +{ + setStatus("Click and drag mouse to set position/orientation."); + state_ = Position; +} + +void NavMapPoseTool::deactivate() +{ + arrow_->getSceneNode()->setVisible(false); +} + +static std::pair +rayHitOnNavMap( + rviz_common::ViewportMouseEvent & event, + rviz_common::DisplayContext * context) +{ + auto * view_controller = context->getViewManager()->getCurrent(); + if (!view_controller) { + return {false, Ogre::Vector3::ZERO}; + } + + // 2) Cámara Ogre directamente + Ogre::Camera * cam = view_controller->getCamera(); + if (!cam || !cam->getViewport()) { + return {false, Ogre::Vector3::ZERO}; + } + + Ogre::Viewport * vp = cam->getViewport(); + + const float nx = static_cast(event.x) / static_cast(vp->getActualWidth()); + const float ny = static_cast(event.y) / static_cast(vp->getActualHeight()); + const Ogre::Ray ray = cam->getCameraToViewportRay(nx, ny); + + Eigen::Vector3f o(ray.getOrigin().x, ray.getOrigin().y, ray.getOrigin().z); + Eigen::Vector3f d(ray.getDirection().x, ray.getDirection().y, ray.getDirection().z); + if (d.squaredNorm() == 0.0f) { + return {false, Ogre::Vector3::ZERO}; + } + d.normalize(); + + ::navmap::NavCelId cid = 0; + float t = 0.0f; + Eigen::Vector3f hit; + const bool ok = received_navmap.raycast(o, d, cid, t, hit); + if (!ok) { + return {false, Ogre::Vector3::ZERO}; + } + + return {true, Ogre::Vector3(hit.x(), hit.y(), hit.z())}; +} + +int NavMapPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + std::pair hit = {false, Ogre::Vector3::ZERO}; + + if (!received_navmap.surfaces.empty()) { + hit = rayHitOnNavMap(event, context_); + } + + if (!hit.first) { // Fallback: If there is not an intersection with navmap, intersect with z = 0 + hit = projection_finder_->getViewportPointProjectionOnXYPlane( + event.panel->getRenderWindow(), event.x, event.y); + } + + if (event.leftDown()) { + return processMouseLeftButtonPressed(hit); // espera pair + } else if (event.type == QEvent::MouseMove && event.left()) { + return processMouseMoved(hit); + } else if (event.leftUp()) { + return processMouseLeftButtonReleased(); + } + return 0; +} + +int NavMapPoseTool::processMouseLeftButtonPressed( + std::pair xy_plane_intersection) +{ + int flags = 0; + assert(state_ == Position); + if (xy_plane_intersection.first) { + arrow_position_ = xy_plane_intersection.second; + arrow_->setPosition(arrow_position_); + + state_ = Orientation; + flags |= Render; + } + return flags; +} + +int NavMapPoseTool::processMouseMoved(std::pair xy_plane_intersection) +{ + int flags = 0; + if (state_ == Orientation) { + // compute angle in x-y plane + if (xy_plane_intersection.first) { + angle_ = calculateAngle(xy_plane_intersection.second, arrow_position_); + makeArrowVisibleAndSetOrientation(angle_); + + flags |= Render; + } + } + + return flags; +} + +void NavMapPoseTool::makeArrowVisibleAndSetOrientation(double angle) +{ + arrow_->getSceneNode()->setVisible(true); + + // we need base_orient, since the arrow goes along the -z axis by default + // (for historical reasons) + Ogre::Quaternion orient_x = Ogre::Quaternion( + Ogre::Radian(-Ogre::Math::HALF_PI), + Ogre::Vector3::UNIT_Y); + + arrow_->setOrientation(Ogre::Quaternion(Ogre::Radian(angle), Ogre::Vector3::UNIT_Z) * orient_x); +} + +int NavMapPoseTool::processMouseLeftButtonReleased() +{ + int flags = 0; + if (state_ == Orientation) { + onPoseSet(arrow_position_.x, arrow_position_.y, arrow_position_.z, angle_); + flags |= (Finished | Render); + } + + return flags; +} + +double NavMapPoseTool::calculateAngle(Ogre::Vector3 start_point, Ogre::Vector3 end_point) +{ + return atan2(start_point.y - end_point.y, start_point.x - end_point.x); +} + +geometry_msgs::msg::Quaternion NavMapPoseTool::orientationAroundZAxis(double angle) +{ + auto orientation = geometry_msgs::msg::Quaternion(); + orientation.x = 0.0; + orientation.y = 0.0; + orientation.z = sin(angle) / (2 * cos(angle / 2)); + orientation.w = cos(angle / 2); + return orientation; +} + +void NavMapPoseTool::logPose( + std::string designation, geometry_msgs::msg::Point position, + geometry_msgs::msg::Quaternion orientation, double angle, std::string frame) +{ + RVIZ_COMMON_LOG_INFO_STREAM( + "Setting " << designation << " pose: Frame:" << frame << ", Position(" << position.x << ", " << + position.y << ", " << position.z << "), Orientation(" << orientation.x << ", " << + orientation.y << ", " << orientation.z << ", " << orientation.w << + ") = Angle: " << angle); +} + +} // namespace navmap_rviz_plugin