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/rolling.yaml b/.github/workflows/rolling.yaml index bfcbdaf..8d5ec03 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -7,23 +7,17 @@ on: push: branches: - rolling - 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: 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 with: diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml new file mode 100644 index 0000000..4134591 --- /dev/null +++ b/.github/workflows/rolling_cron.yaml @@ -0,0 +1,38 @@ +name: rolling + +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: rolling + - 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: rolling + ref: rolling + 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/.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/navmap_core/include/navmap_core/Geometry.hpp b/navmap_core/include/navmap_core/Geometry.hpp index 8b63286..c251d03 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 7a155d0..b380c72 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,39 @@ 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/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index 6a47303..d7c4e48 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,39 +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 { @@ -472,7 +440,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) { @@ -579,7 +547,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) { @@ -632,25 +600,70 @@ 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/src/01_flat_plane.cpp b/navmap_examples/src/01_flat_plane.cpp index 130bba3..326d8a0 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; 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..efb2617 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() diff --git a/navmap_examples/src/05_neighbors_and_centroids.cpp b/navmap_examples/src/05_neighbors_and_centroids.cpp index 7f11065..954c595 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) diff --git a/navmap_examples/src/06_area_marking.cpp b/navmap_examples/src/06_area_marking.cpp index fdda86c..603bc95 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 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..a11affb 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) diff --git a/navmap_ros/CMakeLists.txt b/navmap_ros/CMakeLists.txt index e2b6a3e..cb2e721 100644 --- a/navmap_ros/CMakeLists.txt +++ b/navmap_ros/CMakeLists.txt @@ -28,16 +28,18 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ ${PCL_INCLUDE_DIRS} ) +target_link_libraries(${PROJECT_NAME} PRIVATE + pcl_conversions::pcl_conversions + ${PCL_LIBRARIES} +) target_link_libraries(${PROJECT_NAME} PUBLIC rclcpp::rclcpp navmap_core::navmap_core - pcl_conversions::pcl_conversions ${navmap_ros_interfaces_TARGETS} ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} ${sensor_msgs_TARGETS} ${std_srvs_TARGETS} - ${PCL_LIBRARIES} ) add_executable(slam_server_app src/slam_server_app.cpp @@ -76,6 +78,7 @@ ament_export_dependencies( rclcpp navmap_core navmap_ros_interfaces + nav_msgs geometry_msgs sensor_msgs std_srvs diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index 068f56f..b5843a2 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); /** 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/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index d04e0b6..e77d552 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 @@ -207,11 +210,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,10 +256,19 @@ 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{}); @@ -280,10 +300,21 @@ void from_msg( } } +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 +384,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) { @@ -453,6 +495,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, 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..8168125 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" @@ -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,7 +292,9 @@ 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() / @@ -303,10 +306,12 @@ 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) +// Semantic comparison (order and FP tolerant) ExpectNavMapMsgEqualSemantic(msg_from_core, msg_from_core_loaded); std::filesystem::remove(path); diff --git a/navmap_rviz_plugin/CMakeLists.txt b/navmap_rviz_plugin/CMakeLists.txt index 5ceccfe..ed73758 100644 --- a/navmap_rviz_plugin/CMakeLists.txt +++ b/navmap_rviz_plugin/CMakeLists.txt @@ -1,39 +1,40 @@ 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 REQUIRED COMPONENTS Widgets) 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) +find_package(navmap_ros REQUIRED) +find_package(navmap_core REQUIRED) -set(CMAKE_CXX_STANDARD 23) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -qt5_wrap_cpp(NAVMAP_MOC_SRCS - include/navmap_rviz_plugin/NavMapDisplay.hpp +qt_wrap_cpp(NAVMAP_MOC_SRCS + include/navmap_rviz_plugin/NavMapDisplay.hpp + include/navmap_rviz_plugin/navmap_goal_tool.hpp ) add_library(${PROJECT_NAME} SHARED - src/NavMapDisplay.cpp - include/navmap_rviz_plugin/NavMapDisplay.hpp + src/navmap_rviz_plugin/NavMapDisplay.cpp + src/navmap_rviz_plugin/navmap_goal_tool.cpp + src/navmap_rviz_plugin/navmap_pose_tool.cpp ${NAVMAP_MOC_SRCS} ) target_link_libraries(navmap_rviz_plugin PUBLIC ${navmap_ros_interfaces_TARGETS} + Qt6::Widgets + 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 +51,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 +70,14 @@ endif() ament_export_libraries(${PROJECT_NAME}) ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies( + Qt6 rclcpp pluginlib rviz_common rviz_rendering rviz_default_plugins - Qt5 navmap_ros_interfaces geometry_msgs std_msg ) -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..1e62d8f 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)) @@ -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 { 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..0f3ad3f 100644 --- a/navmap_rviz_plugin/package.xml +++ b/navmap_rviz_plugin/package.xml @@ -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 97% rename from navmap_rviz_plugin/src/NavMapDisplay.cpp rename to navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp index f377532..ad64a76 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 { @@ -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; @@ -223,6 +210,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(); 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