Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions navmap_core/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,10 @@ install(TARGETS

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
# Humble's ament_copyright does not recognize the license with the EasyNav line in the header
if("$ENV{ROS_DISTRO}" STREQUAL "humble")
list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright)
endif()
ament_lint_auto_find_test_dependencies()

find_package(ament_cmake_gtest REQUIRED)
Expand Down
7 changes: 4 additions & 3 deletions navmap_core/include/navmap_core/Geometry.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,14 +26,15 @@
* - Closest point on triangle and point–triangle distance.
*/

#ifndef NAVMAP_CORE__GEOMETRY_HPP
#define NAVMAP_CORE__GEOMETRY_HPP
#ifndef NAVMAP_CORE__GEOMETRY_HPP_
#define NAVMAP_CORE__GEOMETRY_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>
#include <cmath>
#include <algorithm>
#include <utility>

namespace navmap
{
Expand Down Expand Up @@ -376,4 +377,4 @@ inline float point_triangle_squared_distance(

} // namespace navmap

#endif // NAVMAP_CORE__GEOMETRY_HPP
#endif // NAVMAP_CORE__GEOMETRY_HPP_
36 changes: 16 additions & 20 deletions navmap_core/include/navmap_core/NavMap.hpp
Original file line number Diff line number Diff line change
@@ -1,25 +1,20 @@
// Copyright 2025 Intelligent Robotics Lab
//
// This file is part of the project Easy Navigation (EasyNav in short)
// licensed under the GNU General Public License v3.0.
// See <http://www.gnu.org/licenses/> for details.
// 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
//
// Easy Navigation program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3.0, or
// (at your option) any later version.
// http://www.apache.org/licenses/LICENSE-2.0
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

// 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_CORE__NAVMAP_HPP
#define NAVMAP_CORE__NAVMAP_HPP
#ifndef NAVMAP_CORE__NAVMAP_HPP_
#define NAVMAP_CORE__NAVMAP_HPP_

/**
* \file
Expand Down Expand Up @@ -50,8 +45,9 @@
#include <type_traits>
#include <deque>
#include <algorithm>
#include <utility>

#include <Eigen/Core>
#include "Eigen/Core"
#include "navmap_core/Geometry.hpp"

namespace navmap
Expand Down Expand Up @@ -881,7 +877,7 @@ class NavMap
auto src_base = layers.get(src);
auto src_view = std::dynamic_pointer_cast<LayerView<T>>(src_base);
if (!src_view) {
return false; // source layer missing or wrong type
return false; // source layer missing or wrong type
}

// Ensure destination exists with correct length (navcels.size())
Expand All @@ -906,7 +902,7 @@ class NavMap
// Hash-based skip: O(1) if cached, O(n) only on first compute (then cached)
const bool same_hash = (src_view->content_hash() == dst_view->content_hash());
if (same_hash) {
return true; // identical content → avoid copy
return true; // identical content → avoid copy
}

// Copy only when different. set_data() marks hash as dirty for dst.
Expand Down Expand Up @@ -1193,4 +1189,4 @@ inline bool NavMap::locate_navcel(

} // namespace navmap

#endif // NAVMAP_CORE__NAVMAP_HPP
#endif // NAVMAP_CORE__NAVMAP_HPP_
4 changes: 2 additions & 2 deletions navmap_core/src/navmap_core/NavMap.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ inline std::uint64_t fnv1a64(
}
return h;
}
} // namespace
} // namespace

NavMap::NavMap()
{
Expand Down Expand Up @@ -696,7 +696,7 @@ bool NavMap::locate_navcel_core(
const Vec3 c = positions.at(tri.v[2]);
Vec3 bary_tmp;
if (!point_in_triangle_bary(h.p, a, b, c, bary_tmp, opts.planar_eps)) {
continue; // numerical guard
continue; // numerical guard
}
if (dz < best_dz) {
best_dz = dz;
Expand Down
6 changes: 5 additions & 1 deletion navmap_core/tests/test_geometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,11 @@
#include <Eigen/Core>
#include "navmap_core/Geometry.hpp"

using namespace navmap;
using navmap::AABB;
using navmap::Vec3;
using navmap::ray_triangle_intersect;
using navmap::triangle_area;
using navmap::triangle_normal;

static constexpr float kEps = 1e-6f;

Expand Down
6 changes: 4 additions & 2 deletions navmap_core/tests/test_locate_grid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,9 @@
#include <gtest/gtest.h>
#include "navmap_core/NavMap.hpp"

using namespace navmap;
using navmap::NavCelId;
using navmap::NavMap;
using navmap::Surface;

TEST(NavMap_LocateGrid, MultiFloorsChoosesClosestByDz)
{
Expand Down Expand Up @@ -46,6 +48,6 @@ TEST(NavMap_LocateGrid, MultiFloorsChoosesClosestByDz)
size_t sidx; NavCelId cid; Eigen::Vector3f bary; Eigen::Vector3f hit;
bool ok = nm.locate_navcel(p, sidx, cid, bary, &hit);
ASSERT_TRUE(ok);
EXPECT_EQ(sidx, 1u); // should pick upper floor
EXPECT_EQ(sidx, 1u); // should pick upper floor
EXPECT_NEAR(hit.z(), 3.0f, 1e-6);
}
6 changes: 5 additions & 1 deletion navmap_core/tests/test_navmap.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,11 @@

#include "navmap_core/NavMap.hpp"

using namespace navmap;
using navmap::LayerType;
using navmap::LayerView;
using navmap::NavCelId;
using navmap::NavMap;
using navmap::PointId;

namespace
{
Expand Down
37 changes: 17 additions & 20 deletions navmap_core/tests/test_navmap_assign.cpp
Original file line number Diff line number Diff line change
@@ -1,22 +1,17 @@
// Copyright 2025 Intelligent Robotics Lab
//
// This file is part of the project Easy Navigation (EasyNav in short)
// licensed under the GNU General Public License v3.0.
// See <http://www.gnu.org/licenses/> for details.
// 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
//
// Easy Navigation program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3.0, or
// (at your option) any later version.
// http://www.apache.org/licenses/LICENSE-2.0
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

// 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 <gtest/gtest.h>
#include <cstdint>
Expand All @@ -25,7 +20,9 @@

#include "navmap_core/NavMap.hpp"

using namespace navmap;
using navmap::LayerView;
using navmap::NavMap;
using navmap::layer_type_tag;

namespace
{
Expand All @@ -44,7 +41,7 @@ void fill_one_tri_map(NavMap & m)
auto occ = m.layers.add_or_get<uint8_t>("occ", 1, layer_type_tag<uint8_t>());
auto & v = occ->mutable_data();
v.resize(1);
v[0] = 254; // occupied (black)
v[0] = 254; // occupied (black)
m.layer_meta["occ"].description = "occupancy";
m.layer_meta["occ"].unit = "";
m.layer_meta["occ"].per_cell = true;
Expand All @@ -69,7 +66,7 @@ void fill_one_tri_map_variant(NavMap & m)

if (auto occ = std::dynamic_pointer_cast<LayerView<uint8_t>>(m.layers.get("occ"))) {
auto & v = occ->mutable_data();
v[0] = 0; // free (white)
v[0] = 0; // free (white)
}
if (auto cost = std::dynamic_pointer_cast<LayerView<float>>(m.layers.get("cost"))) {
auto & v = cost->mutable_data();
Expand All @@ -88,12 +85,12 @@ void fill_one_tri_map_variant(NavMap & m)
}
}

} // namespace
} // namespace

TEST(NavMapAssign, CopiesOnlyLayersWhenGeometryEqualAndRemovesExtras)
{
NavMap src; fill_one_tri_map(src);
NavMap dst; fill_one_tri_map_variant(dst); // has extra layer and different values
NavMap dst; fill_one_tri_map_variant(dst); // has extra layer and different values

// Keep destination geometry pointers to verify geometry is not rebuilt
const float * px = dst.positions.x.data();
Expand Down Expand Up @@ -206,7 +203,7 @@ TEST(NavMapAssign, HashSkipsCopyWhenLayerIdentical)
auto occ_dst_after = std::dynamic_pointer_cast<LayerView<uint8_t>>(dst.layers.get("occ"));
ASSERT_TRUE(occ_dst_after);
const uint8_t * occ_ptr_after = occ_dst_after->data().data();
EXPECT_EQ(occ_ptr_before, occ_ptr_after); // same backing storage
EXPECT_EQ(occ_ptr_before, occ_ptr_after); // same backing storage

// Content must still be equal
auto occ_src = std::dynamic_pointer_cast<LayerView<uint8_t>>(src.layers.get("occ"));
Expand Down
4 changes: 2 additions & 2 deletions navmap_core/tests/test_navmap_easy.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -226,7 +226,7 @@ TEST(NavMap_EasyAPI, AddSurfaceMoveOverload)
Surface s = nm.create_surface_obj("map");
s.navcels.clear();
std::size_t idx1 = nm.add_surface(s); // by copy
std::size_t idx2 = nm.add_surface(std::move(s)); // by move
std::size_t idx2 = nm.add_surface(std::move(s)); // by move
EXPECT_EQ(idx1, 0u);
EXPECT_EQ(idx2, 1u);
EXPECT_EQ(nm.surfaces.size(), 2u);
Expand Down Expand Up @@ -363,7 +363,7 @@ TEST(NavMap_SetArea, TypeMismatchReturnsFalseAndDoesNotModifyData)
static void make_grid(NavMap & nm, int nx, int ny, float z = 0.0f)
{
// Build a [0,1]x[0,1] plane subdivided in nx*ny quads, 2 triangles per quad
nm.create_surface("map"); // index 0
nm.create_surface("map"); // index 0
auto addv = [&](float x, float y) {
return nm.add_vertex(Eigen::Vector3f(x, y, z));
};
Expand Down
4 changes: 3 additions & 1 deletion navmap_core/tests/test_navmap_uniform_and_closest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,9 @@
#include <Eigen/Core>
#include "navmap_core/NavMap.hpp"

using namespace navmap;
using navmap::NavCelId;
using navmap::NavMap;
using navmap::PointId;

namespace
{
Expand Down
4 changes: 3 additions & 1 deletion navmap_core/tests/test_raycast_many.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,9 @@
#include <gtest/gtest.h>
#include "navmap_core/NavMap.hpp"

using namespace navmap;
using navmap::NavMap;
using navmap::Ray;
using navmap::RayHit;

TEST(NavMap_RaycastMany, FlatFloorBatchHits)
{
Expand Down
6 changes: 4 additions & 2 deletions navmap_examples/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,10 @@ endif()

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
# Humble's ament_copyright does not recognize the license with the EasyNav line in the header
if("$ENV{ROS_DISTRO}" STREQUAL "humble")
list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright)
endif()
ament_lint_auto_find_test_dependencies()
endif()

Expand Down
15 changes: 15 additions & 0 deletions navmap_examples/launch/navmap_to_occgrid.launch.py
Original file line number Diff line number Diff line change
@@ -1,3 +1,18 @@
# 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.

from launch import LaunchDescription
from launch_ros.actions import Node

Expand Down
4 changes: 2 additions & 2 deletions navmap_examples/src/01_flat_plane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@
#include <iostream>
#include <vector>
#include <cstdint>
#include <Eigen/Core>

#include "Eigen/Core"
#include "navmap_core/NavMap.hpp"

using navmap::NavMap;
Expand Down Expand Up @@ -59,7 +59,7 @@ int main()
cout << "locate=" << ok << " sidx=" << sidx << " cid=" << cid << " hit=(" << hit.x() << "," <<
hit.y() << "," << hit.z() << ")\n";
if (ok) {
cout << "occ at cid: " << (int)nm.navcel_value<uint8_t>(cid, *occ) << endl;
cout << "occ at cid: " << static_cast<int>(nm.navcel_value<uint8_t>(cid, *occ)) << endl;
}
return 0;
}
2 changes: 1 addition & 1 deletion navmap_examples/src/02_two_floors.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@

#include <iostream>
#include <vector>
#include <Eigen/Core>

#include "Eigen/Core"
#include "navmap_core/NavMap.hpp"

using navmap::NavMap;
Expand Down
2 changes: 1 addition & 1 deletion navmap_examples/src/03_slope_surface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@

#include <iostream>
#include <vector>
#include <Eigen/Core>

#include "Eigen/Core"
#include "navmap_core/NavMap.hpp"

using navmap::NavMap;
Expand Down
4 changes: 2 additions & 2 deletions navmap_examples/src/04_layers.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@

#include <iostream>
#include <cstdint>
#include <Eigen/Core>

#include "Eigen/Core"
#include "navmap_core/NavMap.hpp"

using navmap::NavMap;
Expand All @@ -44,7 +44,7 @@ int main()
nm.layer_set<float>("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;
Expand Down
Loading
Loading