From d92b7b667402a49ea69e9d3150efcc8ba6f80933 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Wed, 19 Aug 2026 10:17:37 +0200 Subject: [PATCH] Tool to generate simulated world/NavMap from satellite imaginary MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/include/navmap_ros/conversions.hpp | 32 ++ navmap_ros/src/navmap_ros/conversions.cpp | 193 ++++++-- navmap_ros/tests/test_conversions.cpp | 174 +++++++ .../src/navmap_rviz_plugin/NavMapDisplay.cpp | 14 +- navmap_tools/CMakeLists.txt | 64 +++ navmap_tools/navmap_tools/__init__.py | 16 + navmap_tools/navmap_tools/cli.py | 315 ++++++++++++ navmap_tools/navmap_tools/geo/__init__.py | 16 + navmap_tools/navmap_tools/geo/cache.py | 77 +++ navmap_tools/navmap_tools/geo/dem.py | 164 +++++++ navmap_tools/navmap_tools/geo/google.py | 195 ++++++++ navmap_tools/navmap_tools/geo/imagery.py | 242 +++++++++ navmap_tools/navmap_tools/geo/net.py | 60 +++ navmap_tools/navmap_tools/geo/pnoa.py | 176 +++++++ navmap_tools/navmap_tools/geo/projection.py | 111 +++++ navmap_tools/navmap_tools/mesh_export.py | 270 +++++++++++ navmap_tools/navmap_tools/pcd_writer.py | 101 ++++ navmap_tools/navmap_tools/scaffold.py | 458 ++++++++++++++++++ navmap_tools/navmap_tools/terrain.py | 126 +++++ navmap_tools/package.xml | 45 ++ navmap_tools/scripts/navmap_gis_tool | 22 + navmap_tools/src/navmap_publisher.cpp | 135 ++++++ navmap_tools/src/pointcloud_to_navmap.cpp | 209 ++++++++ navmap_tools/test/__init__.py | 0 navmap_tools/test/test_cache.py | 129 +++++ navmap_tools/test/test_cli.py | 299 ++++++++++++ navmap_tools/test/test_copyright.py | 23 + navmap_tools/test/test_dem.py | 160 ++++++ navmap_tools/test/test_flake8.py | 25 + navmap_tools/test/test_google.py | 294 +++++++++++ navmap_tools/test/test_imagery.py | 297 ++++++++++++ navmap_tools/test/test_mesh_export.py | 360 ++++++++++++++ navmap_tools/test/test_net.py | 136 ++++++ navmap_tools/test/test_pcd_writer.py | 164 +++++++ navmap_tools/test/test_pep257.py | 23 + navmap_tools/test/test_pnoa.py | 190 ++++++++ navmap_tools/test/test_projection.py | 145 ++++++ navmap_tools/test/test_scaffold.py | 174 +++++++ navmap_tools/test/test_terrain.py | 152 ++++++ navmap_tools/test/test_xmllint.py | 23 + 40 files changed, 5774 insertions(+), 35 deletions(-) create mode 100644 navmap_tools/CMakeLists.txt create mode 100644 navmap_tools/navmap_tools/__init__.py create mode 100644 navmap_tools/navmap_tools/cli.py create mode 100644 navmap_tools/navmap_tools/geo/__init__.py create mode 100644 navmap_tools/navmap_tools/geo/cache.py create mode 100644 navmap_tools/navmap_tools/geo/dem.py create mode 100644 navmap_tools/navmap_tools/geo/google.py create mode 100644 navmap_tools/navmap_tools/geo/imagery.py create mode 100644 navmap_tools/navmap_tools/geo/net.py create mode 100644 navmap_tools/navmap_tools/geo/pnoa.py create mode 100644 navmap_tools/navmap_tools/geo/projection.py create mode 100644 navmap_tools/navmap_tools/mesh_export.py create mode 100644 navmap_tools/navmap_tools/pcd_writer.py create mode 100644 navmap_tools/navmap_tools/scaffold.py create mode 100644 navmap_tools/navmap_tools/terrain.py create mode 100644 navmap_tools/package.xml create mode 100755 navmap_tools/scripts/navmap_gis_tool create mode 100644 navmap_tools/src/navmap_publisher.cpp create mode 100644 navmap_tools/src/pointcloud_to_navmap.cpp create mode 100644 navmap_tools/test/__init__.py create mode 100644 navmap_tools/test/test_cache.py create mode 100644 navmap_tools/test/test_cli.py create mode 100644 navmap_tools/test/test_copyright.py create mode 100644 navmap_tools/test/test_dem.py create mode 100644 navmap_tools/test/test_flake8.py create mode 100644 navmap_tools/test/test_google.py create mode 100644 navmap_tools/test/test_imagery.py create mode 100644 navmap_tools/test/test_mesh_export.py create mode 100644 navmap_tools/test/test_net.py create mode 100644 navmap_tools/test/test_pcd_writer.py create mode 100644 navmap_tools/test/test_pep257.py create mode 100644 navmap_tools/test/test_pnoa.py create mode 100644 navmap_tools/test/test_projection.py create mode 100644 navmap_tools/test/test_scaffold.py create mode 100644 navmap_tools/test/test_terrain.py create mode 100644 navmap_tools/test/test_xmllint.py diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index b5843a2..253f9a5 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -331,6 +331,38 @@ navmap::NavMap from_pointcloud2( navmap_ros_interfaces::msg::NavMap & out_msg, BuildParams params); +/** + * @brief Build a NavMap surface from an *organized* (grid-shaped) PCL point cloud. + * + * Unlike ::navmap_ros::from_points -- which discovers connectivity by neighbor + * search over an otherwise-unstructured point set, a heuristic that needs + * either a generous @p params.neighbor_radius (at real cost in redundant, + * overlapping triangles) or leaves gaps on an evenly-sampled surface even + * when every cell is individually navigable -- this overload takes a cloud + * whose grid connectivity is *already known* (row-major, `cloud.width` x + * `cloud.height`, exactly as PCL's own "organized point cloud" convention) + * and builds the two triangles per grid cell directly from the (i, j) + * indices, the same deterministic scheme ::navmap_ros::from_occupancy_grid + * already uses. No neighbor search, no possibility of a meshing-artifact + * gap: the only triangles skipped are ones that fail the slope filter + * (@p params.max_slope_deg) or reference a non-finite vertex, i.e. every + * remaining hole is a real "not navigable here" gap, not a search-radius + * artifact. + * + * @param[in] grid_points Organized point set (`cloud.height` must be > 1); + * row j, column i is at index `j * cloud.width + i`. + * @param[out] out_msg Output transport message mirroring the created NavMap. + * @param[in] params Only @p params.max_slope_deg and @p params.max_surfaces + * apply here; the neighbor-search/edge-length/angle fields + * are meaningless without a search step and are ignored. + * @return The constructed `navmap::NavMap`; empty if @p grid_points is not + * organized (`height <= 1`) or is smaller than 2x2. + */ +navmap::NavMap from_regular_grid( + const pcl::PointCloud & grid_points, + navmap_ros_interfaces::msg::NavMap & out_msg, + BuildParams params); + } // namespace navmap_ros #endif // NAVMAP_ROS__CONVERSIONS_HPP_ diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index e77d552..1d84b88 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -793,6 +793,29 @@ downsample_voxelize_topZ_layered( // Includes: max edge length, max |Δz|, XY neighbor radius, min area, // min internal angle, and slope limit via n·z. +// max_edge_len is meant as a broad sanity cap on 3D edge length, separate +// from neighbor_radius (the search radius used to find candidate points) +// and max_dz (the per-edge vertical cap). When max_edge_len is set close to +// neighbor_radius -- as in BuildParams' own defaults, both 2.0 m -- it stops +// being a broad cap and starts spuriously rejecting valid edges: the +// candidate search is a 3D sphere of radius neighbor_radius, so any real +// terrain roughness (a few cm of Z noise, far under max_slope_deg/max_dz) +// shifts which points land near that sphere's boundary and can nudge a +// perfectly legitimate edge's 3D length a hair past max_edge_len, leaving a +// hole exactly where the terrain is *not* too steep to be navigable. The +// true worst-case length of an edge that already satisfies both the XY +// search radius and the vertical max_dz cap is the hypotenuse of the two, +// so max_edge_len should never be tighter than that combination -- this +// widens it to match whenever the caller's value would otherwise clip +// geometrically valid edges. See easynav_gis_tool.md for the reproduction. +inline float effective_max_edge_len(const BuildParams & P) +{ + if (P.max_edge_len <= 0.0f || P.neighbor_radius <= 0.0f) {return P.max_edge_len;} + const float floor = std::sqrt( + P.neighbor_radius * P.neighbor_radius + P.max_dz * P.max_dz); + return std::max(P.max_edge_len, floor); +} + inline bool try_add_triangle( int i, int j, int k, const pcl::PointCloud & cloud, @@ -813,8 +836,9 @@ inline bool try_add_triangle( auto dAB = dist3(A, B); auto dBC = dist3(B, C); auto dCA = dist3(C, A); - if (P.max_edge_len > 0.0f && - (dAB > P.max_edge_len || dBC > P.max_edge_len || dCA > P.max_edge_len)) {return false;} + const float max_edge_len = effective_max_edge_len(P); + if (max_edge_len > 0.0f && + (dAB > max_edge_len || dBC > max_edge_len || dCA > max_edge_len)) {return false;} // Max |Δz| per edge if (P.max_dz > 0.0f) { @@ -1071,6 +1095,7 @@ navmap::NavMap from_points( enum class Phase { FAN, BFS }; struct RejectCounts { size_t edge_len = 0, dz = 0, xy = 0, dup = 0, geom = 0, zwin_seed = 0, zwin_bfs = 0; }; + const float eff_max_edge_len = effective_max_edge_len(P); auto precheck = [&](int i, int j, int k, Phase phase, RejectCounts & rej, bool & dup_out)->bool { TriKey tk = make_tri(i, j, k); if (tri_set_global.find(tk) != tri_set_global.end()) { @@ -1080,9 +1105,9 @@ navmap::NavMap from_points( const auto & A = cloud[i], & B = cloud[j], & C = cloud[k]; - if (P.max_edge_len > 0.0f) { + if (eff_max_edge_len > 0.0f) { const float lAB = dist3f(A, B), lBC = dist3f(B, C), lCA = dist3f(C, A); - if (lAB > P.max_edge_len || lBC > P.max_edge_len || lCA > P.max_edge_len) { + if (lAB > eff_max_edge_len || lBC > eff_max_edge_len || lCA > eff_max_edge_len) { rej.edge_len++; return false; } } @@ -1140,14 +1165,14 @@ navmap::NavMap from_points( } // Quick filters - if (P.max_edge_len > 0.0f) { + if (eff_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; - }), + return dist3f(cloud[seed_idx], Q) > eff_max_edge_len; + }), neigh_seed.end()); } { @@ -1250,39 +1275,47 @@ navmap::NavMap from_points( const float z_mid = 0.5f * (cloud[e.a].z + cloud[e.b].z); - // Neighbors of e.a (filtered by radius and Z window vs e.b) - std::vector neigh_a; + // Candidate completion points: union of the neighbors of *both* + // e.a and e.b, not just e.a. Searching only from e.a misses any + // point that is close to e.b but just outside radius of e.a -- which + // happens routinely once the edge itself is close to + // neighbor_radius long (the common case for a diagonal grid edge), + // and was a real source of unexplained holes on otherwise-navigable + // terrain: it forced neighbor_radius to be inflated well past the + // true point spacing just to paper over this asymmetry. Geometric + // validity of the resulting triangle is still fully enforced below + // by precheck()/try_add_triangle(), so widening the candidate pool + // here does not admit any edge/angle/slope violation. See + // easynav_gis_tool.md. + std::vector neigh_candidates; { - std::vector inds; std::vector dists; - if (P.neighbor_radius > 0.0f) { - if (kdtree.radiusSearch(cloud[e.a], P.neighbor_radius, inds, dists) > 0) { - for (int id : inds) { - if (id != e.a) { - neigh_a.push_back(id); + std::unordered_set seen; + auto collect = [&](const pcl::PointXYZ & from) { + std::vector inds; std::vector dists; + if (P.neighbor_radius > 0.0f) { + if (kdtree.radiusSearch(from, P.neighbor_radius, inds, dists) > 0) { + for (int id : inds) { + if (id != e.a && id != e.b && seen.insert(id).second) { + neigh_candidates.push_back(id); + } + } } - } - } - } else { - const int K = std::max(8, P.k_neighbors); - if (kdtree.nearestKSearch(cloud[e.a], K, inds, dists) > 0) { - for (int id : inds) { - if (id != e.a) { - neigh_a.push_back(id); + } else { + const int K = std::max(8, P.k_neighbors); + if (kdtree.nearestKSearch(from, K, inds, dists) > 0) { + for (int id : inds) { + if (id != e.a && id != e.b && seen.insert(id).second) { + neigh_candidates.push_back(id); + } + } } } - } - } + }; + collect(cloud[e.a]); + collect(cloud[e.b]); } - for (int c : neigh_a) { - if (c == e.a || c == e.b) {continue;} - - if (P.neighbor_radius > 0.0f) { - const float dx = cloud[c].x - cloud[e.b].x; - const float dy = cloud[c].y - cloud[e.b].y; - if ((dx * dx + dy * dy) > P.neighbor_radius * P.neighbor_radius) {continue;} - } - + for (int c : neigh_candidates) { const float z_half = std::max(P.max_dz, 0.35f); const float dz = cloud[c].z - z_mid; if (std::fabs(dz) > z_half) {++global_rej.zwin_bfs; continue;} @@ -1359,6 +1392,98 @@ navmap::NavMap from_points( return from_msg(out_msg); } +// ----------------- Regular-grid builder (deterministic, gap-free) ----------------- + +navmap::NavMap from_regular_grid( + const pcl::PointCloud & grid_points, + navmap_ros_interfaces::msg::NavMap & out_msg, + BuildParams P) +{ + out_msg = navmap_ros_interfaces::msg::NavMap(); + + const uint32_t W = grid_points.width; + const uint32_t H = grid_points.height; + if (H <= 1 || W < 2 || H < 2 || grid_points.size() != static_cast(W) * H) { + return navmap::NavMap(); + } + + auto v_id = [W](uint32_t i, uint32_t j) {return static_cast(j * W + i);}; + const float cos_max_slope = + std::cos(P.max_slope_deg * static_cast(M_PI) / 180.0f); + + std::vector triangles; + triangles.reserve(static_cast(2) * (W - 1) * (H - 1)); + + // One grid cell -> up to two triangles, split along the same diagonal + // from_occupancy_grid uses. No neighbor search: connectivity is already + // fully known from the grid indices, so the only reason to skip a + // triangle is a non-finite vertex or a real slope violation -- never a + // search-radius/angle heuristic, so this cannot leave a meshing-artifact + // hole on an otherwise-navigable, evenly-sampled surface. + auto try_cell_triangle = [&](int i0, int i1, int i2) { + const auto & A = grid_points[i0]; + const auto & B = grid_points[i1]; + const auto & C = grid_points[i2]; + if (!pcl::isFinite(A) || !pcl::isFinite(B) || !pcl::isFinite(C)) {return;} + + Eigen::Vector3f a(A.x, A.y, A.z), b(B.x, B.y, B.z), c(C.x, C.y, C.z); + Eigen::Vector3f n = (b - a).cross(c - a); + const float nn = n.norm(); + if (nn < 1e-9f) {return;} + n /= nn; + if (n.dot(Eigen::Vector3f::UnitZ()) < cos_max_slope) {return;} + + // Canonical orientation (normal facing +Z), matching try_add_triangle. + if (n.dot(Eigen::Vector3f::UnitZ()) < 0.0f) { + triangles.emplace_back(i0, i2, i1); + } else { + triangles.emplace_back(i0, i1, i2); + } + }; + + for (uint32_t j = 0; j + 1 < H; ++j) { + for (uint32_t i = 0; i + 1 < W; ++i) { + const int id00 = v_id(i, j); + const int id10 = v_id(i + 1, j); + const int id11 = v_id(i + 1, j + 1); + const int id01 = v_id(i, j + 1); + try_cell_triangle(id00, id10, id11); + try_cell_triangle(id00, id11, id01); + } + } + + if (triangles.empty()) { + return navmap::NavMap(); + } + + const std::string frame_id = "map"; + navmap_ros_interfaces::msg::NavMap msg_tmp; + navmap::NavMap core; + if (!build_navmap_from_mesh(grid_points, triangles, frame_id, msg_tmp, &core)) { + return navmap::NavMap(); + } + + // Single surface covering every accepted triangle; rebuild_surfaces_by_ + // connectivity then splits it by real adjacency, so terrain separated by + // a too-steep band ends up as distinct surfaces, same as from_points. + msg_tmp.surfaces.clear(); + { + navmap_ros_interfaces::msg::NavMapSurface s; + s.frame_id = frame_id; + s.navcels.resize(triangles.size()); + for (size_t k = 0; k < triangles.size(); ++k) { + s.navcels[k] = static_cast(k); + } + msg_tmp.surfaces.push_back(std::move(s)); + } + + rebuild_surfaces_by_connectivity(msg_tmp); + keep_top_surfaces_by_size(msg_tmp, P.max_surfaces); + + out_msg = std::move(msg_tmp); + return from_msg(out_msg); +} + // ----------------- ROS PointCloud2 entry ----------------- navmap::NavMap from_pointcloud2( diff --git a/navmap_ros/tests/test_conversions.cpp b/navmap_ros/tests/test_conversions.cpp index 266fe6e..bcdd9f5 100644 --- a/navmap_ros/tests/test_conversions.cpp +++ b/navmap_ros/tests/test_conversions.cpp @@ -13,8 +13,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include + #include #include +#include +#include #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" #include "std_msgs/msg/header.hpp" @@ -24,6 +30,7 @@ using navmap_ros::from_occupancy_grid; using navmap_ros::to_occupancy_grid; +using navmap_ros::from_regular_grid; static void build_square_with_layers(navmap::NavMap & nm) @@ -383,3 +390,170 @@ TEST(TestConversions, TriangleIndicesFollowPattern0) EXPECT_EQ(b.v[1], v_id(ci + 1, cj + 1)); EXPECT_EQ(b.v[2], v_id(ci + 0, cj + 1)); } + +// ----------------- from_regular_grid ----------------- +// +// Deterministic grid mesher (no neighbor search): every cell of an +// organized (width x height, row-major) point cloud produces exactly two +// triangles unless one of its vertices is non-finite or the cell's slope +// exceeds max_slope_deg -- see easynav_gis_tool.md for why from_points' +// neighbor-search heuristic could leave gaps on an evenly-sampled, fully +// navigable grid and why this deterministic path was added instead. + +static pcl::PointCloud make_organized_grid( + int W, int H, double spacing, + const std::function & z_fn = [](int, int) {return 0.0f;}) +{ + pcl::PointCloud cloud; + cloud.width = static_cast(W); + cloud.height = static_cast(H); + cloud.is_dense = false; + cloud.points.resize(static_cast(W) * H); + for (int j = 0; j < H; ++j) { + for (int i = 0; i < W; ++i) { + cloud.points[j * W + i] = pcl::PointXYZ( + static_cast(i * spacing), static_cast(j * spacing), z_fn(i, j)); + } + } + return cloud; +} + +TEST(FromRegularGrid, FlatGridProducesFullCoverageNoOverlap) +{ + const int W = 10, H = 10; + auto cloud = make_organized_grid(W, H, 1.0); + + navmap_ros::BuildParams p; + p.max_slope_deg = 30.0f; + navmap_ros_interfaces::msg::NavMap out_msg; + auto nm = from_regular_grid(cloud, out_msg, p); + + EXPECT_EQ(nm.navcels.size(), static_cast(2 * (W - 1) * (H - 1))); + EXPECT_EQ(nm.positions.x.size(), static_cast(W * H)); + ASSERT_EQ(nm.surfaces.size(), 1u); + EXPECT_EQ(nm.surfaces[0].navcels.size(), nm.navcels.size()); +} + +TEST(FromRegularGrid, MatchesFromOccupancyGridWindingPattern) +{ + const int W = 5, H = 5; + auto cloud = make_organized_grid(W, H, 1.0); + navmap_ros::BuildParams p; + p.max_slope_deg = 30.0f; + navmap_ros_interfaces::msg::NavMap out_msg; + auto nm = from_regular_grid(cloud, out_msg, p); + + auto v_id = [W](uint32_t i, uint32_t j) {return j * W + i;}; + const uint32_t ci = 2, cj = 1; + const uint32_t cell = cj * (W - 1) + ci; + const auto & a = nm.navcels[2 * cell]; + const auto & b = nm.navcels[2 * cell + 1]; + + EXPECT_EQ(a.v[0], v_id(ci + 0, cj + 0)); + EXPECT_EQ(a.v[1], v_id(ci + 1, cj + 0)); + EXPECT_EQ(a.v[2], v_id(ci + 1, cj + 1)); + + EXPECT_EQ(b.v[0], v_id(ci + 0, cj + 0)); + EXPECT_EQ(b.v[1], v_id(ci + 1, cj + 1)); + EXPECT_EQ(b.v[2], v_id(ci + 0, cj + 1)); +} + +TEST(FromRegularGrid, GentleSlopeUnderLimitStaysFullyCovered) +{ + const int W = 10, H = 10; + // A uniform ramp: 10 deg slope over the whole grid, well under 30 deg. + const float slope_rad = 10.0f * static_cast(M_PI) / 180.0f; + auto cloud = make_organized_grid( + W, H, 1.0, [&](int i, int) {return i * std::tan(slope_rad);}); + + navmap_ros::BuildParams p; + p.max_slope_deg = 30.0f; + navmap_ros_interfaces::msg::NavMap out_msg; + auto nm = from_regular_grid(cloud, out_msg, p); + + EXPECT_EQ(nm.navcels.size(), static_cast(2 * (W - 1) * (H - 1))); + ASSERT_EQ(nm.surfaces.size(), 1u); +} + +TEST(FromRegularGrid, TooSteepCellsAreSkippedNotTheWholeGrid) +{ + const int W = 10, H = 10; + // A step in the middle column: any cell touching column 5 is a near-vertical + // wall (well above 30 deg), everything else is flat. + auto cloud = make_organized_grid( + W, H, 1.0, [](int i, int) {return i >= 5 ? 5.0f : 0.0f;}); + + navmap_ros::BuildParams p; + p.max_slope_deg = 30.0f; + navmap_ros_interfaces::msg::NavMap out_msg; + auto nm = from_regular_grid(cloud, out_msg, p); + + // Flat region left of the step (columns 0..4) and right of it (columns + // 5..9) each mesh fully; only the column-4/5 boundary cells (touching the + // cliff) are rejected by the slope filter. + const size_t expected_flat_tris = static_cast(2) * 4 * (H - 1) * 2; + EXPECT_EQ(nm.navcels.size(), expected_flat_tris); + // Split into (at least) two disconnected navigable islands by the cliff. + EXPECT_GE(nm.surfaces.size(), 2u); +} + +TEST(FromRegularGrid, NonFiniteVertexOnlySkipsItsOwnTriangles) +{ + const int W = 5, H = 5; + auto cloud = make_organized_grid(W, H, 1.0); + const uint32_t nan_idx = 2 * W + 2; // vertex (i=2, j=2) + cloud.points[nan_idx].z = std::numeric_limits::quiet_NaN(); + + navmap_ros::BuildParams p; + p.max_slope_deg = 30.0f; + navmap_ros_interfaces::msg::NavMap out_msg; + auto nm = from_regular_grid(cloud, out_msg, p); + + // Of the 4 cells touching vertex (2,2), it is a shared "diagonal" corner + // (id00/id11, present in both of that cell's triangles) for 2 of them and + // an "off-diagonal" corner (id10/id01, present in only one triangle) for + // the other 2 -- 2+2+1+1 = 6 triangles lost, not a flat "4 cells x 2". + const size_t expected = static_cast(2 * (W - 1) * (H - 1) - 6); + EXPECT_EQ(nm.navcels.size(), expected); + + // No surviving triangle references the NaN vertex at all. + for (const auto & c : nm.navcels) { + EXPECT_NE(c.v[0], nan_idx); + EXPECT_NE(c.v[1], nan_idx); + EXPECT_NE(c.v[2], nan_idx); + } + + // A cell far from the NaN vertex still meshes normally (2 triangles). + auto v_id = [W](uint32_t i, uint32_t j) {return j * W + i;}; + int cell00_tris = 0; + for (const auto & c : nm.navcels) { + const bool touches_cell00 = + (c.v[0] == v_id(0, 0) || c.v[1] == v_id(0, 0) || c.v[2] == v_id(0, 0)) && + (c.v[0] == v_id(1, 1) || c.v[1] == v_id(1, 1) || c.v[2] == v_id(1, 1)); + if (touches_cell00) {++cell00_tris;} + } + EXPECT_EQ(cell00_tris, 2); +} + +TEST(FromRegularGrid, UnorganizedCloudReturnsEmpty) +{ + pcl::PointCloud cloud; + cloud.width = 25; cloud.height = 1; + cloud.points.resize(25, pcl::PointXYZ(0.f, 0.f, 0.f)); + + navmap_ros::BuildParams p; + navmap_ros_interfaces::msg::NavMap out_msg; + auto nm = from_regular_grid(cloud, out_msg, p); + + EXPECT_TRUE(nm.navcels.empty()); + EXPECT_TRUE(nm.positions.x.empty()); +} + +TEST(FromRegularGrid, TooSmallGridReturnsEmpty) +{ + auto cloud = make_organized_grid(1, 5, 1.0); + navmap_ros::BuildParams p; + navmap_ros_interfaces::msg::NavMap out_msg; + auto nm = from_regular_grid(cloud, out_msg, p); + EXPECT_TRUE(nm.navcels.empty()); +} diff --git a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp index ad64a76..6a6618d 100644 --- a/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp +++ b/navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp @@ -33,6 +33,11 @@ namespace { +// Shared between repopulateLayerEnum_() (offers it in the "Layer" dropdown) +// and updateColorsOnly_() (recognizes it once selected) -- kept as a single +// constant so the two can't drift apart. +const char * const kVertexColorLayerName = "Color (vertex RGBA)"; + inline void hsv2rgb(float H, float S, float V, float & R, float & G, float & B) { const float C = V * S; @@ -390,6 +395,13 @@ void NavMapDisplay::repopulateLayerEnum_() for (const auto & L : last_msg_->layers) { layer_property_->addOption(QString::fromStdString(L.name)); } + if (last_msg_->has_vertex_rgba && + last_msg_->colors_r.size() == last_msg_->positions_x.size() && + last_msg_->colors_g.size() == last_msg_->positions_x.size() && + last_msg_->colors_b.size() == last_msg_->positions_x.size()) + { + layer_property_->addOption(kVertexColorLayerName); + } } if (!prev.empty()) { layer_property_->setString(prev.c_str()); @@ -605,7 +617,7 @@ void NavMapDisplay::updateColorsOnly_() auto it = layers_by_name_.find(sel); if (it != layers_by_name_.end()) { selected_layer = it->second; - } else if (sel == "Color (vertex RGBA)") { + } else if (sel == kVertexColorLayerName) { vertex_color_mode = last_msg_->has_vertex_rgba && last_msg_->colors_r.size() == last_msg_->positions_x.size() && last_msg_->colors_g.size() == last_msg_->positions_x.size() && diff --git a/navmap_tools/CMakeLists.txt b/navmap_tools/CMakeLists.txt new file mode 100644 index 0000000..9ceae06 --- /dev/null +++ b/navmap_tools/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.16) +project(navmap_tools) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclcpp REQUIRED) +find_package(navmap_core REQUIRED) +find_package(navmap_ros REQUIRED) +find_package(navmap_ros_interfaces REQUIRED) +find_package(PCL REQUIRED COMPONENTS common io kdtree search) + +# ---------------- Python package + CLI entry point ---------------- +ament_python_install_package(${PROJECT_NAME}) + +install(PROGRAMS + scripts/navmap_gis_tool + DESTINATION lib/${PROJECT_NAME} +) + +# ---------------- C++ tools ---------------- +add_executable(pointcloud_to_navmap src/pointcloud_to_navmap.cpp) +target_include_directories(pointcloud_to_navmap PRIVATE ${PCL_INCLUDE_DIRS}) +target_link_libraries(pointcloud_to_navmap + navmap_core::navmap_core + navmap_ros::navmap_ros + ${navmap_ros_interfaces_TARGETS} + ${PCL_LIBRARIES} +) + +add_executable(navmap_publisher src/navmap_publisher.cpp) +target_link_libraries(navmap_publisher + rclcpp::rclcpp + navmap_core::navmap_core + navmap_ros::navmap_ros + ${navmap_ros_interfaces_TARGETS} +) + +install(TARGETS + pointcloud_to_navmap + navmap_publisher + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_pytest REQUIRED) + ament_add_pytest_test(navmap_tools_pytest test + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR} + ) +endif() + +ament_package() diff --git a/navmap_tools/navmap_tools/__init__.py b/navmap_tools/navmap_tools/__init__.py new file mode 100644 index 0000000..b4d3336 --- /dev/null +++ b/navmap_tools/navmap_tools/__init__.py @@ -0,0 +1,16 @@ +# Copyright 2026 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. + +"""navmap_tools: GIS-to-Gazebo/NavMap generation and other NavMap utilities.""" diff --git a/navmap_tools/navmap_tools/cli.py b/navmap_tools/navmap_tools/cli.py new file mode 100644 index 0000000..34103f3 --- /dev/null +++ b/navmap_tools/navmap_tools/cli.py @@ -0,0 +1,315 @@ +# Copyright 2026 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. + +""" +navmap_gis_tool: build a Gazebo world and/or a NavMap from real GIS data. + +Orchestrates the shared pipeline described in easynav_gis_tool.md: project a +GPS center + size into a WGS84 bbox, fetch/cache DEM + imagery over it, +sample both onto one local-ENU TerrainGrid, then hand that same grid to the +--gazebo mesh exporter and/or the --navmap point-cloud + pointcloud_to_navmap +path -- which is why the two outputs line up even run separately. +""" + +import argparse +import subprocess +import sys + +import numpy as np + +from .geo.cache import default_cache_dir, DiskCache +from .geo.dem import load_dem_grid +from .geo.google import ( + API_KEY_ENV_VAR as GOOGLE_API_KEY_ENV_VAR, get_api_key as get_google_api_key, + load_google_mosaic, +) +from .geo.imagery import DEFAULT_ZOOM, load_imagery_mosaic, native_resolution_m_per_px +from .geo.pnoa import DEFAULT_RESOLUTION_M as PNOA_DEFAULT_RESOLUTION_M, load_pnoa_mosaic +from .geo.projection import LocalProjection +from .mesh_export import write_dae, write_stl, write_texture_png +from .pcd_writer import write_colors_csv, write_pcd_xyz +from .scaffold import scaffold_world_package +from .terrain import build_terrain_grid, render_texture + +_METERS_PER_DEGREE = 111320.0 +# Bounds for the auto-sized texture (see _texture_pixels_for below): below +# this, tiny areas don't need huge textures; above this, PNG size/render +# cost grow faster than the extra detail is worth for a robotics sim asset. +_MIN_TEXTURE_PIXELS = 256 +_MAX_TEXTURE_PIXELS = 4096 +# The DEM/imagery sampling loop (terrain.build_terrain_grid) is pure Python, +# one pyproj + bilinear-sample call per grid point, not vectorized. Measured +# ~7.6 us/point (linear), so this many points is ~30 s of pure compute -- +# generous, but still bounded so a huge --size/--resolution combination fails +# fast with a clear message instead of running for a very long time and +# producing an impractically large mesh. See easynav_gis_tool.md. +_MAX_GRID_POINTS = 4_000_000 + + +def _texture_pixels_for(size_m: float, resolution_m_per_px: float) -> int: + """ + Auto-size the baked texture so it actually uses the fetched imagery detail. + + Without this, a fixed low pixel count would throw away resolution that + was just downloaded at real cost (more tiles/chunks, more time). + Source-agnostic: takes the imagery's own ground resolution directly + rather than a zoom level, since not every source has one (PNOA is a WMS + service requested at an explicit meters/pixel, not a tile zoom). + """ + ideal = round(size_m / resolution_m_per_px) + return min(_MAX_TEXTURE_PIXELS, max(_MIN_TEXTURE_PIXELS, ideal)) + + +def _default_package_name(lat: float, lon: float) -> str: + def fmt(v: float) -> str: + s = f'{abs(v):.4f}'.replace('.', 'p') + return ('m' if v < 0 else '') + s + + return f'gis_{fmt(lat)}_{fmt(lon)}_world' + + +def _parse_center(value: str): + parts = value.split(',') + if len(parts) != 2: + raise argparse.ArgumentTypeError(f'--center must be "LAT,LON", got {value!r}') + try: + lat, lon = float(parts[0]), float(parts[1]) + except ValueError as e: + raise argparse.ArgumentTypeError(f'--center must be "LAT,LON": {e}') from e + return lat, lon + + +def build_arg_parser() -> argparse.ArgumentParser: + """Build the argparse parser for navmap_gis_tool.""" + parser = argparse.ArgumentParser( + prog='navmap_gis_tool', + description=( + 'Generate a Gazebo world and/or a NavMap from real elevation + ' + 'imagery data, given a GPS center and a size in meters.' + ), + ) + parser.add_argument( + '--center', required=True, type=_parse_center, metavar='LAT,LON', + help='GPS center of the area, e.g. 40.3314,-3.8356') + parser.add_argument( + '--size', required=True, type=float, metavar='METERS', + help='side length (meters) of the square area centered on --center') + parser.add_argument('--gazebo', action='store_true', help='emit a Gazebo world + models') + parser.add_argument('--navmap', action='store_true', help='emit a .navmap file') + parser.add_argument( + '--resolution', type=float, default=1.0, metavar='METERS', + help=( + 'grid sampling resolution in meters, shared by --gazebo geometry ' + 'and --navmap (default: %(default)s)' + )) + parser.add_argument( + '--max-slope-deg', type=float, default=30.0, metavar='DEGREES', + help='NavMap maximum navigable slope (default: %(default)s)') + parser.add_argument( + '--package', default=None, + help=( + 'output ROS package name, e.g. urjc_excavation_world -- the whole ' + 'colcon package (worlds/, models/, maps/, launch/) is generated ' + 'under this name (default: derived from --center)' + )) + parser.add_argument( + '--output-dir', default=None, + help='directory to create the package in (default: ./)') + parser.add_argument( + '--cache-dir', default=None, + help='GIS download cache directory (default: platformdirs cache dir)') + parser.add_argument( + '--dem-source', choices=['copernicus30'], default='copernicus30', + help='elevation data source (default: %(default)s)') + parser.add_argument( + '--imagery-source', choices=['esri', 'pnoa', 'google'], default='esri', + help=( + 'imagery data source: esri (worldwide, keyless), pnoa (Spain ' + "only -- IGN's national aerial survey, keyless, CC BY 4.0, up to " + '25 cm/px native and often sharper than esri in rural Spain), or ' + 'google (worldwide, paid -- requires a Google Maps Platform API ' + f'key with "Map Tiles API" enabled, in the {GOOGLE_API_KEY_ENV_VAR} ' + 'environment variable; see easynav_gis_tool.md for a ' + 'caching/licensing caveat specific to this source) ' + '(default: %(default)s)' + )) + parser.add_argument( + '--zoom', type=int, default=DEFAULT_ZOOM, + help=( + 'esri/google only: imagery tile zoom level, 0-23; higher = more ' + 'detail but more tiles/download time. For esri, automatically ' + 'stepped down if the requested zoom has no real coverage (see ' + 'find_available_zoom) (default: %(default)s)' + )) + parser.add_argument( + '--texture-pixels', type=int, default=0, metavar='N', + help=( + 'baked texture size in pixels per side; 0 (default) auto-sizes ' + 'it from --zoom/--size so the texture actually uses the fetched ' + f'imagery detail, clamped to [{_MIN_TEXTURE_PIXELS}, {_MAX_TEXTURE_PIXELS}]' + )) + parser.add_argument( + '--force-refresh', action='store_true', help='bypass the GIS download cache') + return parser + + +def run(argv=None) -> int: + """Parse arguments and run the full pipeline; returns a process exit code.""" + parser = build_arg_parser() + args = parser.parse_args(argv) + + if not args.gazebo and not args.navmap: + parser.error('at least one of --gazebo/--navmap is required') + if args.size <= 0: + parser.error('--size must be positive') + if args.resolution <= 0: + parser.error('--resolution must be positive') + if not 0 <= args.zoom <= 23: + parser.error('--zoom must be in [0, 23]') + if args.texture_pixels < 0: + parser.error('--texture-pixels must be >= 0 (0 = auto)') + if args.imagery_source == 'google': + try: + get_google_api_key() + except RuntimeError as e: + parser.error(str(e)) + + n_per_side = round(args.size / args.resolution) + 1 + if n_per_side * n_per_side > _MAX_GRID_POINTS: + parser.error( + f'--size {args.size} / --resolution {args.resolution} would sample ' + f'{n_per_side * n_per_side} grid points (a {n_per_side}x{n_per_side} grid), ' + f'over the {_MAX_GRID_POINTS} limit; increase --resolution or reduce --size ' + '(the DEM/imagery sampling loop is pure Python and does not scale to huge grids)' + ) + + lat, lon = args.center + package_name = args.package or _default_package_name(lat, lon) + output_dir = args.output_dir or f'./{package_name}' + cache_dir = args.cache_dir or default_cache_dir() + cache = DiskCache(cache_dir) + + print(f'[navmap_gis_tool] center=({lat}, {lon}) size={args.size} m package={package_name!r}') + print(f'[navmap_gis_tool] cache: {cache.root}') + + projection = LocalProjection(lat, lon) + bbox = projection.square_bbox(args.size) + + print(f'[navmap_gis_tool] fetching DEM ({args.dem_source}) over {bbox} ...') + dem = load_dem_grid(bbox, cache, force=args.force_refresh) + + if args.imagery_source == 'esri': + print(f'[navmap_gis_tool] fetching imagery (esri, zoom={args.zoom}) ...') + imagery = load_imagery_mosaic(bbox, cache, zoom=args.zoom, force=args.force_refresh) + resolution_m_per_px = native_resolution_m_per_px(imagery.zoom, lat) + imagery_desc = ( + f'Esri World Imagery, zoom {imagery.zoom} ' + f'(~{resolution_m_per_px:.2f} m/px at this latitude)' + ) + imagery_attribution = ( + 'Imagery (c) Esri, Maxar, Earthstar Geographics, and the GIS User Community.' + ) + elif args.imagery_source == 'pnoa': + print( + f'[navmap_gis_tool] fetching imagery (pnoa, ' + f'{PNOA_DEFAULT_RESOLUTION_M} m/px) ...') + imagery = load_pnoa_mosaic( + bbox, cache, resolution_m_per_px=PNOA_DEFAULT_RESOLUTION_M, force=args.force_refresh) + resolution_m_per_px = imagery.pixel_size_m + imagery_desc = f'PNOA (IGN Spain), {resolution_m_per_px:.2f} m/px' + imagery_attribution = ( + 'Contains PNOA orthoimagery (c) Instituto Geografico Nacional de Espana, ' + 'CC BY 4.0.' + ) + else: + print(f'[navmap_gis_tool] fetching imagery (google, zoom={args.zoom}) ...') + imagery = load_google_mosaic(bbox, cache, zoom=args.zoom, force=args.force_refresh) + resolution_m_per_px = native_resolution_m_per_px(imagery.zoom, lat) + imagery_desc = ( + f'Google Maps Platform satellite, zoom {imagery.zoom} ' + f'(~{resolution_m_per_px:.2f} m/px at this latitude)' + ) + imagery_attribution = 'Imagery (c) Google.' + + dem_native_m = dem.pixel_size_lat * _METERS_PER_DEGREE + if args.resolution < dem_native_m / 2: + print( + f'[navmap_gis_tool] warning: --resolution {args.resolution} m is much finer ' + f"than the DEM's native ~{dem_native_m:.1f} m resolution; elevation detail " + 'below that will be smoothly interpolated, not real', file=sys.stderr) + grid = build_terrain_grid(projection, dem, imagery, args.size, args.resolution) + + paths = scaffold_world_package( + output_dir, package_name, lat, lon, args.size, grid.center_elevation_amsl, + imagery_desc, imagery_attribution) + print(f'[navmap_gis_tool] package scaffolded at {paths.root}') + + if args.navmap: + grid_rows, grid_cols = grid.xs.shape + points = np.stack([grid.xs, grid.ys, grid.elevation], axis=-1).reshape(-1, 3) + colors = grid.rgb.reshape(-1, 3) + n = write_pcd_xyz(paths.pcd_path, points, width=grid_cols, height=grid_rows) + write_colors_csv(paths.colors_csv_path, colors) + print(f'[navmap_gis_tool] wrote {paths.pcd_path} ({n} points)') + + exe = _find_tool_executable('pointcloud_to_navmap') + cmd = [ + exe, + '--input', str(paths.pcd_path), + '--output', str(paths.navmap_path), + '--colors', str(paths.colors_csv_path), + '--resolution', str(args.resolution), + '--max-slope-deg', str(args.max_slope_deg), + ] + print(f'[navmap_gis_tool] running: {" ".join(cmd)}') + result = subprocess.run(cmd, capture_output=True, text=True) + sys.stdout.write(result.stdout) + sys.stderr.write(result.stderr) + if result.returncode != 0: + print('[navmap_gis_tool] pointcloud_to_navmap failed', file=sys.stderr) + return result.returncode + print(f'[navmap_gis_tool] wrote {paths.navmap_path}') + + if args.gazebo: + texture_pixels = args.texture_pixels or _texture_pixels_for( + args.size, resolution_m_per_px) + print(f'[navmap_gis_tool] texture: {texture_pixels}x{texture_pixels} px') + texture = render_texture(projection, imagery, args.size, pixels=texture_pixels) + write_texture_png(paths.texture_path, texture) + write_dae(paths.dae_path, grid, args.size, paths.texture_path.name) + write_stl(paths.stl_path, grid) + print(f'[navmap_gis_tool] wrote {paths.dae_path}, {paths.stl_path}') + print(f'[navmap_gis_tool] wrote {paths.world_path}') + + print(f'[navmap_gis_tool] done: {paths.root}') + return 0 + + +def _find_tool_executable(name: str) -> str: + from ament_index_python.packages import get_package_prefix, PackageNotFoundError + + try: + prefix = get_package_prefix('navmap_tools') + except PackageNotFoundError as e: + raise RuntimeError( + 'navmap_tools is not colcon-built/sourced (needed to locate the ' + f'compiled {name!r} tool)' + ) from e + return f'{prefix}/lib/navmap_tools/{name}' + + +def main(argv=None) -> None: + """Console-script entry point.""" + sys.exit(run(argv)) diff --git a/navmap_tools/navmap_tools/geo/__init__.py b/navmap_tools/navmap_tools/geo/__init__.py new file mode 100644 index 0000000..6c4792e --- /dev/null +++ b/navmap_tools/navmap_tools/geo/__init__.py @@ -0,0 +1,16 @@ +# Copyright 2026 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. + +"""GIS data acquisition: local projection, disk cache, DEM and imagery fetch.""" diff --git a/navmap_tools/navmap_tools/geo/cache.py b/navmap_tools/navmap_tools/geo/cache.py new file mode 100644 index 0000000..8dc902c --- /dev/null +++ b/navmap_tools/navmap_tools/geo/cache.py @@ -0,0 +1,77 @@ +# Copyright 2026 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. + +""" +Content-addressed disk cache for downloaded GIS data (DEM/imagery tiles). + +Callers provide a cache key and a callable that knows how to produce the file +the first time; subsequent calls with the same key reuse the file on disk +instead of re-downloading it, satisfying the "cache en disco para no +descargarnos todo otra vez" requirement from easynav_gis_tool.md. +""" + +import hashlib +from pathlib import Path +from typing import Callable + +import platformdirs + + +def default_cache_dir() -> Path: + """Return the default on-disk cache root for navmap_tools GIS downloads.""" + return Path(platformdirs.user_cache_dir('navmap_tools')) / 'gis' + + +class DiskCache: + """A directory-backed cache keyed by an arbitrary string.""" + + def __init__(self, root: Path): + """Create (if needed) and wrap the cache directory at `root`.""" + self.root = Path(root) + self.root.mkdir(parents=True, exist_ok=True) + + def path_for(self, key: str, suffix: str) -> Path: + """Return the deterministic on-disk path for a given cache key.""" + digest = hashlib.sha1(key.encode('utf-8')).hexdigest() + safe_suffix = suffix if suffix.startswith('.') else f'.{suffix}' + return self.root / f'{digest}{safe_suffix}' + + def get_or_fetch( + self, + key: str, + suffix: str, + fetch: Callable[[Path], None], + force: bool = False, + ) -> Path: + """ + Return a cached file for `key`, downloading it via `fetch` if needed. + + `fetch(dest)` must create `dest` (e.g. by streaming an HTTP response to + it). If it raises, no cache entry is left behind: a partial file is + removed so a later retry doesn't see a corrupt cache hit. + """ + dest = self.path_for(key, suffix) + if dest.exists() and not force: + return dest + tmp = dest.with_name(dest.name + '.part') + try: + fetch(tmp) + if not tmp.exists(): + raise RuntimeError(f'fetch() for key {key!r} did not create {tmp}') + tmp.replace(dest) + except BaseException: + tmp.unlink(missing_ok=True) + raise + return dest diff --git a/navmap_tools/navmap_tools/geo/dem.py b/navmap_tools/navmap_tools/geo/dem.py new file mode 100644 index 0000000..f5b76f4 --- /dev/null +++ b/navmap_tools/navmap_tools/geo/dem.py @@ -0,0 +1,164 @@ +# Copyright 2026 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. + +""" +Copernicus DEM GLO-30 (30 m, public AWS Open Data, no API key) access. + +Chosen over blendergis's default (OpenTopography SRTM, which requires a free +but registered API key) specifically so navmap_tools works without any +account setup. DEM tiles are 1x1 degree Cloud-Optimized GeoTIFFs at a fixed, +predictable S3 key; navmap_tools downloads whole covering tile(s) (not +windowed HTTP range reads) and caches them on disk -- simpler and more robust +than partial reads, and the one-time per-degree-tile download is cheap once +cached (see easynav_gis_tool.md for the full rationale). +""" + +from dataclasses import dataclass +import math +from typing import Tuple + +import numpy as np + +import tifffile + +from .cache import DiskCache +from .net import get_to_file +from .projection import BBox + +_BUCKET_URL = 'https://copernicus-dem-30m.s3.amazonaws.com' +_TIMEOUT_S = 120 + + +def tile_key(lat_floor: int, lon_floor: int) -> str: + """Return the Copernicus DEM S3 object-key prefix for a 1x1 degree tile.""" + ns = 'N' if lat_floor >= 0 else 'S' + ew = 'E' if lon_floor >= 0 else 'W' + return ( + f'Copernicus_DSM_COG_10_{ns}{abs(lat_floor):02d}_00_' + f'{ew}{abs(lon_floor):03d}_00_DEM' + ) + + +def tile_url(lat_floor: int, lon_floor: int) -> str: + """Return the full HTTPS URL of the DEM GeoTIFF for a 1x1 degree tile.""" + key = tile_key(lat_floor, lon_floor) + return f'{_BUCKET_URL}/{key}/{key}.tif' + + +def _download(url: str, dest) -> None: + if not get_to_file(url, dest, _TIMEOUT_S, allow_404=True): + raise FileNotFoundError(f'no DEM tile at {url} (likely open ocean)') + + +def fetch_tile_path(lat_floor: int, lon_floor: int, cache: DiskCache, force: bool = False): + """Download (or reuse from cache) the DEM tile covering (lat_floor, lon_floor).""" + url = tile_url(lat_floor, lon_floor) + key = f'dem/copernicus30/{tile_key(lat_floor, lon_floor)}' + return cache.get_or_fetch(key, '.tif', lambda dest: _download(url, dest), force=force) + + +@dataclass +class DemGrid: + """A rectangular elevation grid in plain WGS84 degrees, north edge at row 0.""" + + elevation: np.ndarray + west: float + north: float + pixel_size_lon: float + pixel_size_lat: float + + def sample(self, lon: float, lat: float) -> float: + """Bilinearly sample elevation (meters) at (lon, lat); clamps to the grid edge.""" + rows, cols = self.elevation.shape + col = (lon - self.west) / self.pixel_size_lon + row = (self.north - lat) / self.pixel_size_lat + col = min(max(col, 0.0), cols - 1.0) + row = min(max(row, 0.0), rows - 1.0) + c0, r0 = int(math.floor(col)), int(math.floor(row)) + c1, r1 = min(c0 + 1, cols - 1), min(r0 + 1, rows - 1) + fc, fr = col - c0, row - r0 + v00 = self.elevation[r0, c0] + v01 = self.elevation[r0, c1] + v10 = self.elevation[r1, c0] + v11 = self.elevation[r1, c1] + top = v00 * (1 - fc) + v01 * fc + bot = v10 * (1 - fc) + v11 * fc + return float(top * (1 - fr) + bot * fr) + + +def _read_tile_array(path) -> Tuple[np.ndarray, float, float, float, float]: + tif = tifffile.TiffFile(str(path)) + meta = tif.geotiff_metadata + arr = tif.pages[0].asarray().astype(np.float32) + sx, sy, _ = meta['ModelPixelScale'] + _, _, _, ox, oy, _ = meta['ModelTiepoint'] + return arr, ox, oy, sx, sy + + +def load_dem_grid(bbox: BBox, cache: DiskCache, force: bool = False) -> DemGrid: + """ + Load (downloading/caching as needed) a DEM grid covering `bbox`. + + Mosaics every 1x1 degree tile touching the bbox into a single grid so + `DemGrid.sample()` never has to reason about tile boundaries. Tiles with + no DEM coverage (open ocean) are skipped, contributing zero elevation. + + Raises RuntimeError if the bbox straddles a latitude band where the + Copernicus grid's column count changes (this only happens very close to + the poles) -- an edge case not worth silently mosaicking incorrectly. + """ + lat0, lat1 = int(math.floor(bbox.south)), int(math.floor(bbox.north)) + lon0, lon1 = int(math.floor(bbox.west)), int(math.floor(bbox.east)) + + tiles = {} + for lat_floor in range(lat0, lat1 + 1): + for lon_floor in range(lon0, lon1 + 1): + try: + path = fetch_tile_path(lat_floor, lon_floor, cache, force=force) + except FileNotFoundError: + continue + tiles[(lat_floor, lon_floor)] = _read_tile_array(path) + + if not tiles: + raise RuntimeError(f'no DEM coverage found for bbox {bbox}') + + shapes = {arr.shape for arr, _, _, _, _ in tiles.values()} + if len(shapes) > 1: + raise RuntimeError( + 'the requested area straddles a DEM tile latitude band with a ' + f'different grid resolution ({shapes}); reduce --size or move ' + '--center away from the tile boundary' + ) + rows_per_tile, cols_per_tile = next(iter(shapes)) + _, _, _, sx, sy = next(iter(tiles.values())) + + mosaic_west = float(lon0) + mosaic_north = float(lat1 + 1) + total_rows = (lat1 - lat0 + 1) * rows_per_tile + total_cols = (lon1 - lon0 + 1) * cols_per_tile + mosaic = np.zeros((total_rows, total_cols), dtype=np.float32) + + for (lat_floor, lon_floor), (arr, _ox, _oy, _sx, _sy) in tiles.items(): + row_off = (lat1 - lat_floor) * rows_per_tile + col_off = (lon_floor - lon0) * cols_per_tile + mosaic[row_off:row_off + rows_per_tile, col_off:col_off + cols_per_tile] = arr + + return DemGrid( + elevation=mosaic, + west=mosaic_west, + north=mosaic_north, + pixel_size_lon=sx, + pixel_size_lat=sy, + ) diff --git a/navmap_tools/navmap_tools/geo/google.py b/navmap_tools/navmap_tools/geo/google.py new file mode 100644 index 0000000..395c680 --- /dev/null +++ b/navmap_tools/navmap_tools/geo/google.py @@ -0,0 +1,195 @@ +# Copyright 2026 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. + +""" +Google Maps Platform "Map Tiles API" satellite imagery -- requires an API key. + +Unlike Esri World Imagery (keyless) and PNOA (keyless, CC BY 4.0), Google +Maps Platform is a paid, keyed API, added on explicit user request after +Esri looked noticeably worse/older in their area. IMPORTANT license caveat +(see easynav_gis_tool.md for the full discussion): Google Maps Platform's +terms generally restrict caching/storing its content and forbid building a +derivative "basemap" from it, which is close to what this module's disk +cache + baked-texture pipeline does. That's a compliance question for +whoever runs this tool with a Google key, not something this code can +verify or enforce -- use at your own judgement. + +Reuses geo.imagery's tile-grid math (`lonlat_to_tile`, `ImageryMosaic`, +`native_resolution_m_per_px`): the Map Tiles API serves the same 256x256 +Web Mercator XYZ pyramid Esri does, just gated behind a short-lived session +token instead of being open. Session creation: + + POST https://tile.googleapis.com/v1/createSession?key=API_KEY + {"mapType": "satellite", "language": "en-US", "region": "US"} + -> {"session": "...", "expiry": "", ...} + + GET https://tile.googleapis.com/v1/2dtiles/{z}/{x}/{y}?session=...&key=... + +Session tokens are documented to last a couple of hours; this module +creates one lazily (only on an actual cache miss -- a fully-cached re-run +touches the Google API not at all, so it costs nothing and needs no key) and +transparently refreshes it if it has expired, guarded by a lock since tile +fetches run concurrently. This request/response shape is implemented from +Google's published Map Tiles API documentation; it has not been exercised +against a live key in this repository (unlike Esri/PNOA, both tested +end-to-end this session) -- verify it against your own key and report back +if anything doesn't match. +""" + +from concurrent.futures import as_completed, ThreadPoolExecutor +import math +import os +import sys +import threading +import time +from typing import Optional + +import numpy as np + +from PIL import Image + +import requests + +from .cache import DiskCache +from .imagery import ImageryMosaic, lonlat_to_tile +from .net import get_to_file +from .projection import BBox + +API_KEY_ENV_VAR = 'GOOGLE_MAPS_API_KEY' +_CREATE_SESSION_URL = 'https://tile.googleapis.com/v1/createSession' +_TILE_URL_TEMPLATE = 'https://tile.googleapis.com/v1/2dtiles/{z}/{x}/{y}' +_TILE_SIZE = 256 +_TIMEOUT_S = 60 +_MAX_TILES = 30000 +_MAX_WORKERS = 16 +# Margin subtracted from the session's own reported expiry so a fetch +# in-flight when the check runs doesn't race an expiring token. +_SESSION_EXPIRY_MARGIN_S = 30.0 + + +def get_api_key() -> str: + """Read the Google Maps Platform API key from the environment; raises if unset.""" + key = os.environ.get(API_KEY_ENV_VAR, '').strip() + if not key: + raise RuntimeError( + '--imagery-source google requires a Google Maps Platform API key ' + f'with the "Map Tiles API" enabled; set it in the {API_KEY_ENV_VAR} ' + 'environment variable' + ) + return key + + +class GoogleSession: + """A Map Tiles API session token plus its expiry, as a wall-clock epoch second.""" + + def __init__(self, token: str, expiry_epoch_s: float): + self.token = token + self.expiry_epoch_s = expiry_epoch_s + + def is_valid(self) -> bool: + """Return False once within `_SESSION_EXPIRY_MARGIN_S` of the reported expiry.""" + return time.time() < (self.expiry_epoch_s - _SESSION_EXPIRY_MARGIN_S) + + +def create_session(api_key: str) -> GoogleSession: + """Create a new Map Tiles API session (one HTTP POST); raises on failure.""" + resp = requests.post( + _CREATE_SESSION_URL, + params={'key': api_key}, + json={'mapType': 'satellite', 'language': 'en-US', 'region': 'US'}, + timeout=_TIMEOUT_S, + ) + resp.raise_for_status() + data = resp.json() + return GoogleSession(token=data['session'], expiry_epoch_s=float(data['expiry'])) + + +class _SessionProvider: + """Lazily creates a session on first use and refreshes it once it expires.""" + + def __init__(self, api_key: str): + self._api_key = api_key + self._lock = threading.Lock() + self._session: Optional[GoogleSession] = None + + def get(self) -> GoogleSession: + with self._lock: + if self._session is None or not self._session.is_valid(): + self._session = create_session(self._api_key) + return self._session + + +def _fetch_tile_path( + z: int, x: int, y: int, sessions: _SessionProvider, api_key: str, + cache: DiskCache, force: bool = False, +): + key = f'imagery/google/{z}/{x}/{y}' + + def fetch(dest) -> None: + session = sessions.get() + url = f'{_TILE_URL_TEMPLATE.format(z=z, x=x, y=y)}?session={session.token}&key={api_key}' + get_to_file(url, dest, _TIMEOUT_S) + + # Session creation only happens inside `fetch`, i.e. only on an actual + # cache miss -- a fully-cached re-run never calls the Google API at all. + return cache.get_or_fetch(key, '.jpg', fetch, force=force) + + +def load_google_mosaic( + bbox: BBox, cache: DiskCache, api_key: Optional[str] = None, + zoom: int = 20, force: bool = False, +) -> ImageryMosaic: + """Fetch/mosaic (caching each tile) Google satellite imagery covering `bbox` at `zoom`.""" + if api_key is None: + api_key = get_api_key() + if not 0 <= zoom <= 23: + raise ValueError(f'zoom out of range [0, 23]: {zoom}') + + x0f, y0f = lonlat_to_tile(bbox.west, bbox.north, zoom) + x1f, y1f = lonlat_to_tile(bbox.east, bbox.south, zoom) + tx0, ty0 = int(math.floor(x0f)), int(math.floor(y0f)) + tx1, ty1 = int(math.floor(x1f)), int(math.floor(y1f)) + n_tiles = (tx1 - tx0 + 1) * (ty1 - ty0 + 1) + if n_tiles > _MAX_TILES: + raise ValueError( + f'imagery request would need {n_tiles} tiles at zoom {zoom}; ' + 'reduce --size or --zoom' + ) + + sessions = _SessionProvider(api_key) + cols = (tx1 - tx0 + 1) * _TILE_SIZE + rows = (ty1 - ty0 + 1) * _TILE_SIZE + mosaic = np.zeros((rows, cols, 3), dtype=np.uint8) + + def _fetch_and_place(tx: int, ty: int) -> None: + path = _fetch_tile_path(zoom, tx, ty, sessions, api_key, cache, force=force) + with Image.open(path) as im: + tile_rgb = np.asarray(im.convert('RGB')) + row_off = (ty - ty0) * _TILE_SIZE + col_off = (tx - tx0) * _TILE_SIZE + # Disjoint slice per (tx, ty): safe to write concurrently, no lock needed. + mosaic[row_off:row_off + _TILE_SIZE, col_off:col_off + _TILE_SIZE] = tile_rgb + + tile_coords = [(tx, ty) for ty in range(ty0, ty1 + 1) for tx in range(tx0, tx1 + 1)] + with ThreadPoolExecutor(max_workers=_MAX_WORKERS) as pool: + futures = [pool.submit(_fetch_and_place, tx, ty) for tx, ty in tile_coords] + done = 0 + for future in as_completed(futures): + future.result() # re-raises any exception from the worker + done += 1 + if n_tiles >= 500 and done % max(1, n_tiles // 20) == 0: + print(f'[navmap_gis_tool] imagery: {done}/{n_tiles} tiles', file=sys.stderr) + + return ImageryMosaic(image=mosaic, zoom=zoom, tile_x0=tx0, tile_y0=ty0) diff --git a/navmap_tools/navmap_tools/geo/imagery.py b/navmap_tools/navmap_tools/geo/imagery.py new file mode 100644 index 0000000..2feceba --- /dev/null +++ b/navmap_tools/navmap_tools/geo/imagery.py @@ -0,0 +1,242 @@ +# Copyright 2026 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. + +""" +ESRI World Imagery XYZ tile fetch + mosaic (no API key required). + +Uses the same keyless ArcGIS Online "World_Imagery" service blendergis lists +under its SOURCES["ESRI"]["AERIAL"] entry (core/basemaps/servicesDefs.py). +Tiles are Web Mercator (EPSG:3857) 256x256 slippy-map tiles; navmap_tools +mosaics the tiles covering a bbox and exposes point sampling in (lon, lat) so +callers never need to reason about the Web Mercator tile grid directly. +""" + +from concurrent.futures import as_completed, ThreadPoolExecutor +from dataclasses import dataclass +import hashlib +import math +import sys +from typing import Tuple + +import numpy as np + +from PIL import Image + +from .cache import DiskCache +from .net import get_to_file +from .projection import BBox + +_URL_TEMPLATE = ( + 'https://server.arcgisonline.com/ArcGIS/rest/services/' + 'World_Imagery/MapServer/tile/{z}/{y}/{x}' +) +_TILE_SIZE = 256 +_TIMEOUT_S = 60 +# Generous: at zoom 23 and ~40 deg latitude this covers roughly a 500-600 m +# square (tile count grows ~4x per zoom level and with latitude via +# native_resolution_m_per_px's cos(lat) term). Fetched concurrently (see +# _MAX_WORKERS below) so this stays practical despite the tile count. +_MAX_TILES = 30000 +_MAX_WORKERS = 16 +# The true ceiling for this service. Not every region has real imagery +# detail this fine (some areas re-serve upsampled, not sharper, tiles past +# ~19-20), but defaulting to the max means every run gets the best available +# detail rather than silently leaving resolution on the table; --zoom can +# still be lowered explicitly for faster runs over larger areas. +DEFAULT_ZOOM = 23 +# Lowest zoom that automatic fallback (find_available_zoom, below) will try +# before giving up: below this, imagery is too coarse to be worth using at +# all for a robotics-scale terrain. +_MIN_FALLBACK_ZOOM = 10 + +# Esri serves this exact, byte-for-byte-identical tile (a flat grey square +# reading "Map data not yet available") for coordinates it has no imagery +# for at the requested zoom, instead of a 404 -- discovered when a real +# generated world showed that message repeated across every mesh cell after +# raising the default zoom to 23. It downloads and decodes as a perfectly +# valid JPEG, so nothing in the normal fetch path notices anything wrong; +# only comparing the fetched bytes against this known placeholder catches +# it. See easynav_gis_tool.md for the full diagnosis. +_PLACEHOLDER_MD5_HASHES = frozenset({ + 'f27d9de7f80c13501f470595e327aa6d', +}) + + +def _is_placeholder_tile(path) -> bool: + """Return True if the tile at `path` is Esri's known "no data" placeholder image.""" + digest = hashlib.md5(open(path, 'rb').read()).hexdigest() + return digest in _PLACEHOLDER_MD5_HASHES + + +def find_available_zoom( + lon: float, lat: float, cache: DiskCache, max_zoom: int, + min_zoom: int = _MIN_FALLBACK_ZOOM, force: bool = False, +) -> int: + """ + Probe (lon, lat) from `max_zoom` downward; return the first zoom with real imagery. + + Requesting the highest zoom unconditionally is only useful where Esri + actually has that much detail; many rural/remote areas don't, and + silently mosaic-ing their "not yet available" placeholder tile as if it + were real imagery is worse than just using a lower zoom that does have + real coverage. Falls back to `min_zoom` if nothing better is found. + """ + for zoom in range(max_zoom, min_zoom - 1, -1): + tx, ty = (int(math.floor(v)) for v in lonlat_to_tile(lon, lat, zoom)) + path = _fetch_tile_path(zoom, tx, ty, cache, force=force) + if not _is_placeholder_tile(path): + return zoom + return min_zoom + + +# Web Mercator ground resolution at zoom 0, equator: Earth's equatorial +# circumference / tile size in pixels (40075016.686 m / 256 px). +_EQUATOR_METERS_PER_PIXEL_AT_ZOOM0 = 156543.03392804097 + + +def native_resolution_m_per_px(zoom: int, lat_deg: float) -> float: + """ + Ground resolution (meters/pixel) of Web Mercator tiles at `zoom`/`lat_deg`. + + Used to size the baked output texture to actually use the detail being + downloaded: fetching high-zoom tiles is wasted if the final texture PNG + is baked at a fixed low pixel count regardless of `--zoom`. + """ + return ( + _EQUATOR_METERS_PER_PIXEL_AT_ZOOM0 + * math.cos(math.radians(lat_deg)) + / (2.0 ** zoom) + ) + + +def lonlat_to_tile(lon: float, lat: float, zoom: int) -> Tuple[float, float]: + """Convert (lon, lat) to fractional Web Mercator tile coordinates at `zoom`.""" + lat = min(max(lat, -85.05112878), 85.05112878) + lat_rad = math.radians(lat) + n = 2.0 ** zoom + x = (lon + 180.0) / 360.0 * n + y = (1.0 - math.log(math.tan(lat_rad) + 1.0 / math.cos(lat_rad)) / math.pi) / 2.0 * n + return x, y + + +def _fetch_tile_path(z: int, x: int, y: int, cache: DiskCache, force: bool = False): + url = _URL_TEMPLATE.format(z=z, y=y, x=x) + key = f'imagery/esri/{z}/{x}/{y}' + return cache.get_or_fetch( + key, '.jpg', lambda dest: get_to_file(url, dest, _TIMEOUT_S), force=force) + + +@dataclass +class ImageryMosaic: + """An RGB image mosaic covering a bbox, in Web Mercator (EPSG:3857) tile space.""" + + image: np.ndarray # shape (H, W, 3), uint8 + zoom: int + tile_x0: int + tile_y0: int + + def sample(self, lon: float, lat: float) -> Tuple[int, int, int]: + """Bilinearly sample an RGB color at (lon, lat); clamps to the mosaic edge.""" + gx, gy = lonlat_to_tile(lon, lat, self.zoom) + px = (gx - self.tile_x0) * _TILE_SIZE + py = (gy - self.tile_y0) * _TILE_SIZE + h, w, _ = self.image.shape + px = min(max(px, 0.0), w - 1.0) + py = min(max(py, 0.0), h - 1.0) + x0, y0 = int(math.floor(px)), int(math.floor(py)) + x1, y1 = min(x0 + 1, w - 1), min(y0 + 1, h - 1) + fx, fy = px - x0, py - y0 + c00 = self.image[y0, x0].astype(np.float32) + c01 = self.image[y0, x1].astype(np.float32) + c10 = self.image[y1, x0].astype(np.float32) + c11 = self.image[y1, x1].astype(np.float32) + top = c00 * (1 - fx) + c01 * fx + bot = c10 * (1 - fx) + c11 * fx + rgb = top * (1 - fy) + bot * fy + r, g, b = (int(round(v)) for v in rgb) + return r, g, b + + +def load_imagery_mosaic( + bbox: BBox, cache: DiskCache, zoom: int = DEFAULT_ZOOM, force: bool = False, + auto_fallback: bool = True, +) -> ImageryMosaic: + """ + Fetch/mosaic (caching each tile) the imagery covering `bbox` at `zoom`. + + If `auto_fallback` (the default), first probes `bbox`'s center and steps + `zoom` down until it finds real imagery instead of Esri's "not yet + available" placeholder (see `find_available_zoom`) -- the mosaic is then + fetched at that adjusted zoom, and `ImageryMosaic.zoom` reflects it. + """ + if not 0 <= zoom <= 23: + raise ValueError(f'zoom out of range [0, 23]: {zoom}') + + # Bound the request at the *requested* zoom before doing any network + # activity at all (including the fallback probe below): an absurdly + # large bbox should fail fast regardless of what fallback might later + # pick, and tile count only shrinks if fallback lowers the zoom. + def _tile_range(z): + x0f, y0f = lonlat_to_tile(bbox.west, bbox.north, z) + x1f, y1f = lonlat_to_tile(bbox.east, bbox.south, z) + return ( + int(math.floor(x0f)), int(math.floor(y0f)), + int(math.floor(x1f)), int(math.floor(y1f)), + ) + + tx0, ty0, tx1, ty1 = _tile_range(zoom) + n_tiles = (tx1 - tx0 + 1) * (ty1 - ty0 + 1) + if n_tiles > _MAX_TILES: + raise ValueError( + f'imagery request would need {n_tiles} tiles at zoom {zoom}; ' + 'reduce --size or --zoom' + ) + + if auto_fallback: + center_lon = (bbox.west + bbox.east) / 2.0 + center_lat = (bbox.south + bbox.north) / 2.0 + effective_zoom = find_available_zoom(center_lon, center_lat, cache, zoom, force=force) + if effective_zoom != zoom: + print( + f'[navmap_gis_tool] no imagery at zoom {zoom} here; ' + f'using zoom {effective_zoom} instead', file=sys.stderr) + zoom = effective_zoom + tx0, ty0, tx1, ty1 = _tile_range(zoom) + n_tiles = (tx1 - tx0 + 1) * (ty1 - ty0 + 1) + + cols = (tx1 - tx0 + 1) * _TILE_SIZE + rows = (ty1 - ty0 + 1) * _TILE_SIZE + mosaic = np.zeros((rows, cols, 3), dtype=np.uint8) + + def _fetch_and_place(tx: int, ty: int) -> None: + path = _fetch_tile_path(zoom, tx, ty, cache, force=force) + with Image.open(path) as im: + tile_rgb = np.asarray(im.convert('RGB')) + row_off = (ty - ty0) * _TILE_SIZE + col_off = (tx - tx0) * _TILE_SIZE + # Disjoint slice per (tx, ty): safe to write concurrently, no lock needed. + mosaic[row_off:row_off + _TILE_SIZE, col_off:col_off + _TILE_SIZE] = tile_rgb + + tile_coords = [(tx, ty) for ty in range(ty0, ty1 + 1) for tx in range(tx0, tx1 + 1)] + with ThreadPoolExecutor(max_workers=_MAX_WORKERS) as pool: + futures = [pool.submit(_fetch_and_place, tx, ty) for tx, ty in tile_coords] + done = 0 + for future in as_completed(futures): + future.result() # re-raises any exception from the worker + done += 1 + if n_tiles >= 500 and done % max(1, n_tiles // 20) == 0: + print(f'[navmap_gis_tool] imagery: {done}/{n_tiles} tiles', file=sys.stderr) + + return ImageryMosaic(image=mosaic, zoom=zoom, tile_x0=tx0, tile_y0=ty0) diff --git a/navmap_tools/navmap_tools/geo/net.py b/navmap_tools/navmap_tools/geo/net.py new file mode 100644 index 0000000..9bf6e8b --- /dev/null +++ b/navmap_tools/navmap_tools/geo/net.py @@ -0,0 +1,60 @@ +# Copyright 2026 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. + +""" +Small HTTP GET-to-file helper with retries, shared by geo/dem.py and geo/imagery.py. + +DEM/imagery fetches are many sequential requests against third-party tile +servers (dozens to thousands per run); a single transient connection hiccup +on any one of them should not abort an otherwise-successful run, especially +since the disk cache means a bare re-run would otherwise have to re-fetch +nothing except that one tile anyway. +""" + +import time + +import requests + +_RETRY_BACKOFF_S = (0.5, 1.5, 3.0) + + +def get_to_file(url: str, dest, timeout: float, allow_404: bool = False) -> bool: + """ + GET `url` and stream it to `dest`, retrying transient failures. + + Retries on `requests.exceptions.RequestException` (connection errors, + timeouts, etc.) with a short backoff; does not retry on HTTP error status + codes (`raise_for_status()`) other than a 404 when `allow_404` is set. + + Returns `True` if the download succeeded (`dest` was created). Returns + `False` without creating `dest` if `allow_404` is set and the server + returned 404. Raises the last `RequestException` if every attempt fails. + """ + last_exc = None + for backoff in (0.0,) + _RETRY_BACKOFF_S: + if backoff: + time.sleep(backoff) + try: + with requests.get(url, timeout=timeout, stream=True) as resp: + if allow_404 and resp.status_code == 404: + return False + resp.raise_for_status() + with open(dest, 'wb') as f: + for chunk in resp.iter_content(chunk_size=1 << 20): + f.write(chunk) + return True + except requests.exceptions.RequestException as e: + last_exc = e + raise last_exc diff --git a/navmap_tools/navmap_tools/geo/pnoa.py b/navmap_tools/navmap_tools/geo/pnoa.py new file mode 100644 index 0000000..ad1d9ba --- /dev/null +++ b/navmap_tools/navmap_tools/geo/pnoa.py @@ -0,0 +1,176 @@ +# Copyright 2026 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. + +""" +PNOA (IGN Spain national aerial orthophoto) WMS access -- no API key required. + +Esri World Imagery (geo/imagery.py) has patchy real coverage in rural areas +(see find_available_zoom's docstring and easynav_gis_tool.md for a concrete +case where it fell back to a placeholder tile). PNOA is Spain's own national +aerial survey, flown specifically to cover the whole country including rural +land, at up to 25 cm/px native resolution -- for areas within Spain it is +usually the sharper, more complete option. Published CC BY 4.0, no fees, no +API key (`Fees`/`AccessConstraints` in the service's own GetCapabilities). + +Unlike Esri's pre-tiled XYZ pyramid, this is a WMS service: each request asks +for an arbitrary (bbox, pixel size) image directly, capped by the server at +4096x4096 px per call (its own `MaxWidth`/`MaxHeight`), so a large area is +split into a grid of such requests and mosaicked, mirroring geo/imagery.py's +concurrent-fetch approach. +""" + +from concurrent.futures import as_completed, ThreadPoolExecutor +from dataclasses import dataclass +import math +import sys +from typing import Tuple + +import numpy as np + +from PIL import Image + +from pyproj import Transformer + +from .cache import DiskCache +from .net import get_to_file +from .projection import BBox + +_WMS_URL = 'https://www.ign.es/wms-inspire/pnoa-ma' +# The actual raster imagery layer. OI.MosaicElement (also offered by this +# service) looks like imagery in its GetCapabilities title ("Mosaico") but +# is a vector index of tile footprints + acquisition-date text labels, not +# pixels -- confirmed by requesting it and getting a mostly-blank image with +# a magenta date label instead of a photo, per its own style Abstract +# ("El atributo fecha ... se representa mediante una etiqueta de texto"). +_LAYER = 'OI.OrthoimageCoverage' +_TIMEOUT_S = 60 +# The server's own MaxWidth/MaxHeight (GetCapabilities); requesting more +# than this in one call fails. +_MAX_WMS_PX = 4096 +_MAX_WORKERS = 16 +# Bounds total mosaic size the same way geo/imagery.py bounds tile count: +# fails fast with a clear message instead of an impractically large fetch. +_MAX_TOTAL_PIXELS = 16384 +# PNOA's stated native resolution is "0.25 m or 0.50 m depending on the +# zone" -- requesting the finer of the two is safe even where only 0.50 m +# is really available (the server just resamples up, still valid imagery). +DEFAULT_RESOLUTION_M = 0.25 + +_TO_WEB_MERCATOR = Transformer.from_crs('EPSG:4326', 'EPSG:3857', always_xy=True) + + +@dataclass +class PnoaMosaic: + """An RGB image mosaic covering a bbox, in EPSG:3857 (Web Mercator) meters.""" + + image: np.ndarray # shape (H, W, 3), uint8 + origin_x: float # EPSG:3857 meters, west edge + origin_y: float # EPSG:3857 meters, north edge + pixel_size_m: float + + def sample(self, lon: float, lat: float) -> Tuple[int, int, int]: + """Bilinearly sample an RGB color at (lon, lat); clamps to the mosaic edge.""" + mx, my = _TO_WEB_MERCATOR.transform(lon, lat) + px = (mx - self.origin_x) / self.pixel_size_m + py = (self.origin_y - my) / self.pixel_size_m + h, w, _ = self.image.shape + px = min(max(px, 0.0), w - 1.0) + py = min(max(py, 0.0), h - 1.0) + x0, y0 = int(math.floor(px)), int(math.floor(py)) + x1, y1 = min(x0 + 1, w - 1), min(y0 + 1, h - 1) + fx, fy = px - x0, py - y0 + c00 = self.image[y0, x0].astype(np.float32) + c01 = self.image[y0, x1].astype(np.float32) + c10 = self.image[y1, x0].astype(np.float32) + c11 = self.image[y1, x1].astype(np.float32) + top = c00 * (1 - fx) + c01 * fx + bot = c10 * (1 - fx) + c11 * fx + rgb = top * (1 - fy) + bot * fy + r, g, b = (int(round(v)) for v in rgb) + return r, g, b + + +def _fetch_chunk( + x0: float, y0: float, x1: float, y1: float, width_px: int, height_px: int, + cache: DiskCache, force: bool = False, +): + url = ( + f'{_WMS_URL}?SERVICE=WMS&REQUEST=GetMap&VERSION=1.3.0&LAYERS={_LAYER}' + f'&STYLES=&CRS=EPSG:3857&BBOX={x0:.3f},{y0:.3f},{x1:.3f},{y1:.3f}' + f'&WIDTH={width_px}&HEIGHT={height_px}&FORMAT=image/jpeg' + ) + key = f'imagery/pnoa/{x0:.3f}_{y0:.3f}_{x1:.3f}_{y1:.3f}_{width_px}x{height_px}' + return cache.get_or_fetch( + key, '.jpg', lambda dest: get_to_file(url, dest, _TIMEOUT_S), force=force) + + +def load_pnoa_mosaic( + bbox: BBox, cache: DiskCache, resolution_m_per_px: float = DEFAULT_RESOLUTION_M, + force: bool = False, +) -> PnoaMosaic: + """Fetch/mosaic (caching each WMS chunk) PNOA imagery covering `bbox`.""" + if resolution_m_per_px <= 0: + raise ValueError(f'resolution_m_per_px must be positive: {resolution_m_per_px}') + + x0, y0 = _TO_WEB_MERCATOR.transform(bbox.west, bbox.south) + x1, y1 = _TO_WEB_MERCATOR.transform(bbox.east, bbox.north) + + total_w = max(1, round((x1 - x0) / resolution_m_per_px)) + total_h = max(1, round((y1 - y0) / resolution_m_per_px)) + if total_w > _MAX_TOTAL_PIXELS or total_h > _MAX_TOTAL_PIXELS: + raise ValueError( + f'PNOA request would need a {total_w}x{total_h} px mosaic; ' + 'reduce --size or use a coarser resolution' + ) + + n_cols = math.ceil(total_w / _MAX_WMS_PX) + n_rows = math.ceil(total_h / _MAX_WMS_PX) + chunk_w = math.ceil(total_w / n_cols) + chunk_h = math.ceil(total_h / n_rows) + chunk_size_x_m = chunk_w * resolution_m_per_px + chunk_size_y_m = chunk_h * resolution_m_per_px + + mosaic = np.zeros((total_h, total_w, 3), dtype=np.uint8) + n_chunks = n_cols * n_rows + + def _fetch_and_place(row: int, col: int) -> None: + cx0 = x0 + col * chunk_size_x_m + cx1 = min(x0 + (col + 1) * chunk_size_x_m, x1) + # Row 0 is the northmost strip (top of the mosaic image). + cy1 = y1 - row * chunk_size_y_m + cy0 = max(y1 - (row + 1) * chunk_size_y_m, y0) + w_px = max(1, round((cx1 - cx0) / resolution_m_per_px)) + h_px = max(1, round((cy1 - cy0) / resolution_m_per_px)) + path = _fetch_chunk(cx0, cy0, cx1, cy1, w_px, h_px, cache, force=force) + with Image.open(path) as im: + chunk_rgb = np.asarray(im.convert('RGB')) + row_off = round((y1 - cy1) / resolution_m_per_px) + col_off = round((cx0 - x0) / resolution_m_per_px) + h, w, _ = chunk_rgb.shape + # Disjoint slice per (row, col): safe to write concurrently, no lock needed. + mosaic[row_off:row_off + h, col_off:col_off + w] = chunk_rgb[ + :min(h, total_h - row_off), :min(w, total_w - col_off)] + + coords = [(row, col) for row in range(n_rows) for col in range(n_cols)] + with ThreadPoolExecutor(max_workers=_MAX_WORKERS) as pool: + futures = [pool.submit(_fetch_and_place, row, col) for row, col in coords] + done = 0 + for future in as_completed(futures): + future.result() # re-raises any exception from the worker + done += 1 + if n_chunks >= 20 and done % max(1, n_chunks // 20) == 0: + print(f'[navmap_gis_tool] imagery: {done}/{n_chunks} chunks', file=sys.stderr) + + return PnoaMosaic(image=mosaic, origin_x=x0, origin_y=y1, pixel_size_m=resolution_m_per_px) diff --git a/navmap_tools/navmap_tools/geo/projection.py b/navmap_tools/navmap_tools/geo/projection.py new file mode 100644 index 0000000..f115c6a --- /dev/null +++ b/navmap_tools/navmap_tools/geo/projection.py @@ -0,0 +1,111 @@ +# Copyright 2026 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. + +""" +Local ENU/AEQD projection centered at a GPS point, and bbox math for --size. + +An Azimuthal Equidistant (AEQD) projection centered on the query point is used +because it preserves true distances (in meters) from the center along any +direction -- exactly the property needed to turn a "--size meters square +centered on --center" request into an accurate WGS84 bounding box, and to +convert every downstream local-frame vertex (x, y) back to a real (lon, lat) +to sample DEM/imagery at. +""" + +from dataclasses import dataclass +from typing import Tuple + +from pyproj import Transformer + + +@dataclass(frozen=True) +class BBox: + """A WGS84 bounding box (degrees).""" + + west: float + south: float + east: float + north: float + + +class LocalProjection: + """Maps between WGS84 (lon, lat) and a local ENU plane centered at a point.""" + + def __init__(self, center_lat: float, center_lon: float): + """Build the AEQD projection centered at (center_lat, center_lon).""" + if not -90.0 <= center_lat <= 90.0: + raise ValueError(f'latitude out of range [-90, 90]: {center_lat}') + if not -180.0 <= center_lon <= 180.0: + raise ValueError(f'longitude out of range [-180, 180]: {center_lon}') + self.center_lat = center_lat + self.center_lon = center_lon + proj4 = ( + f'+proj=aeqd +lat_0={center_lat} +lon_0={center_lon} ' + '+datum=WGS84 +units=m +no_defs' + ) + self._to_local = Transformer.from_crs('EPSG:4326', proj4, always_xy=True) + self._to_lonlat = Transformer.from_crs(proj4, 'EPSG:4326', always_xy=True) + + def to_local(self, lon: float, lat: float) -> Tuple[float, float]: + """Project (lon, lat) to local ENU meters (x=east, y=north) from center.""" + x, y = self._to_local.transform(lon, lat) + return x, y + + def to_lonlat(self, x: float, y: float) -> Tuple[float, float]: + """Unproject local ENU meters (x=east, y=north) back to (lon, lat).""" + lon, lat = self._to_lonlat.transform(x, y) + return lon, lat + + def square_bbox( + self, size_m: float, margin_ratio: float = 0.05, samples: int = 64 + ) -> BBox: + """ + WGS84 bbox covering a `size_m` x `size_m` square centered at (0, 0). + + Sampled around the full perimeter (not just the 4 corners) and padded + by `margin_ratio` of the half-size, so downstream DEM/imagery fetches + comfortably cover every point of the square even though AEQD parallels + are not straight lines in (lon, lat) space. + """ + if size_m <= 0: + raise ValueError(f'size_m must be positive: {size_m}') + if not 0.0 <= margin_ratio < 1.0: + raise ValueError(f'margin_ratio must be in [0, 1): {margin_ratio}') + if samples < 4: + raise ValueError(f'samples must be >= 4: {samples}') + + half = size_m / 2.0 + extent = half * (1.0 + margin_ratio) + + lons = [] + lats = [] + for x, y in self._perimeter_points(extent, samples): + lon, lat = self.to_lonlat(x, y) + lons.append(lon) + lats.append(lat) + return BBox(west=min(lons), south=min(lats), east=max(lons), north=max(lats)) + + @staticmethod + def _perimeter_points(half_extent: float, samples: int): + n_per_side = max(1, samples // 4) + step = 2.0 * half_extent / n_per_side + pts = [] + for i in range(n_per_side + 1): + c = -half_extent + step * i + pts.append((c, -half_extent)) + pts.append((c, half_extent)) + pts.append((-half_extent, c)) + pts.append((half_extent, c)) + return pts diff --git a/navmap_tools/navmap_tools/mesh_export.py b/navmap_tools/navmap_tools/mesh_export.py new file mode 100644 index 0000000..fc3f6f5 --- /dev/null +++ b/navmap_tools/navmap_tools/mesh_export.py @@ -0,0 +1,270 @@ +# Copyright 2026 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. + +""" +Grid -> textured mesh export (COLLADA `.dae` visual + `.stl` collision). + +Hand-rolled (no trimesh/open3d dependency): the geometry is always a regular +grid with two triangles per cell, so a small purpose-built writer is simpler +and lighter than pulling in a general mesh library. Mirrors the +STL-collision / DAE-visual split already used by +src/urjc-excavation-world/models/urjc_excavation/model.sdf. + +Both meshes are Z-up (x=east, y=north, z=up), matching SDF's native frame, so +the model.sdf that references them needs no extra rotation. The DAE +explicitly declares `Z_UP` for the same reason: without +it, some COLLADA importers apply an implicit Y-up-to-Z-up rotation that would +put the visual mesh out of alignment with the (unrotated) STL collision mesh. +""" + +from pathlib import Path + +import numpy as np + +from PIL import Image + +from .terrain import TerrainGrid + + +def _grid_to_vertices_faces(grid: TerrainGrid): + """ + Grid (rows=north/y, cols=east/x) -> shared vertices + CCW-from-above triangles. + + Winding must put the "front" face (right-hand-rule normal) up (+Z): a + viewer above the terrain (the normal case) then sees the lit, textured + front face instead of the backface-culled one, which is otherwise + invisible and lets the scene background show through instead -- exactly + the "one side white, other side background-grey" symptom a real + generated world showed in Gazebo before this was caught. Confirmed by + computing the normal of each winding directly: (v00, v10, v11) points + -Z (down, wrong); (v00, v11, v10) points +Z (up, correct). + """ + rows, cols = grid.elevation.shape + verts = np.stack([grid.xs, grid.ys, grid.elevation], axis=-1).reshape(-1, 3) + + def vid(i, j): + return i * cols + j + + faces = [] + for i in range(rows - 1): + for j in range(cols - 1): + v00, v01 = vid(i, j), vid(i, j + 1) + v10, v11 = vid(i + 1, j), vid(i + 1, j + 1) + faces.append((v00, v11, v10)) + faces.append((v00, v01, v11)) + return verts, np.asarray(faces, dtype=np.int64) + + +def _uvs_for_grid(grid: TerrainGrid, size_m: float) -> np.ndarray: + """ + Grid -> COLLADA UVs (v=0 at the BOTTOM of the texture, per the COLLADA spec). + + gz-common's ColladaLoader::LoadTexCoords flips v (`1.0 - v`) on load to + convert from COLLADA's bottom-left origin to Ogre's top-left one. Writing + v already flipped here double-flips it, draping the north-up texture + north-south mirrored in Gazebo (the NavMap side is unaffected: its + per-vertex colors are assigned by 3D nearest-neighbor against the source + imagery, not through this UV/DAE path at all -- see + pointcloud_to_navmap.cpp). North (y=+half) must map to v=1 here so that, + after gz's own flip, it lands on row 0 (the top, i.e. north row) of the + texture PNG written by terrain.render_texture(). + """ + half = size_m / 2.0 + u = (grid.xs + half) / size_m + v = (grid.ys + half) / size_m + return np.stack([u, v], axis=-1).reshape(-1, 2) + + +def _vertex_normals(grid: TerrainGrid) -> np.ndarray: + """ + Per-vertex smooth normals from the heightfield gradient. + + Required, not cosmetic: gz-rendering's Ogre2 PBS pipeline needs a NORMAL + vertex attribute to shade with -- without one, it silently falls back to + flat/unlit rendering and never samples the diffuse texture at all, which + looks exactly like "no texture" even though the material/image data is + completely valid. Confirmed empirically: a generated world stayed plain + white (with a known-good reference texture swapped in, ruling out the + image) until a NORMAL source was added, at which point the texture + appeared immediately. See mesh_export.py's module history / decisions + log in easynav_gis_tool.md for the full diagnostic trail. + """ + dz_dy, dz_dx = np.gradient( + grid.elevation.astype(np.float64), grid.spacing_m, grid.spacing_m) + normals = np.stack([-dz_dx, -dz_dy, np.ones_like(dz_dx)], axis=-1) + normals /= np.linalg.norm(normals, axis=-1, keepdims=True) + return normals.reshape(-1, 3) + + +def write_stl(path, grid: TerrainGrid) -> int: + """Write a binary STL (used as the Gazebo collision mesh); returns triangle count.""" + verts, faces = _grid_to_vertices_faces(grid) + with open(Path(path), 'wb') as f: + f.write(b'\x00' * 80) + f.write(np.uint32(len(faces)).tobytes()) + for a, b, c in faces: + p0, p1, p2 = verts[a], verts[b], verts[c] + normal = np.cross(p1 - p0, p2 - p0) + norm = np.linalg.norm(normal) + if norm > 0: + normal = normal / norm + f.write(np.asarray(normal, dtype=' None: + """Write an (H, W, 3) uint8 array as a PNG texture.""" + if rgb.ndim != 3 or rgb.shape[2] != 3 or rgb.dtype != np.uint8: + raise ValueError( + f'expected an (H, W, 3) uint8 array, got shape {rgb.shape} dtype {rgb.dtype}' + ) + Image.fromarray(rgb, mode='RGB').save(Path(path)) + + +_DAE_TEMPLATE = """ + + + Z_UP + + + + {texture_filename} + + + + + + + terrain_texture + + + terrain_texture_surface + + + + + + + + + + + + + + + + + + + {positions} + + + + + + + + + {normals} + + + + + + + + + {uvs} + + + + + + + + + + + + + +

{indices}

+ + + + + + + + + + + + + + + + + + + + + +""" + + +def write_dae(path, grid: TerrainGrid, size_m: float, texture_filename: str) -> int: + """ + Write a textured COLLADA mesh; returns the triangle count. + + `texture_filename` is written verbatim into and must be a + path relative to the .dae file's directory. + + POSITION and TEXCOORD use distinct offsets (0 and 1) in `

`, each + index repeated per corner even though UVs are computed per grid vertex + (so the two numbers are always equal): gz-common's ColladaLoader sizes + its `

` stride as the *count of declared (semantic, offset) inputs*, + not `max(offset) + 1` -- giving VERTEX and TEXCOORD the same offset="0" + (each its own distinct entry) makes it expect 2 values per corner while + only 1 was written, desyncing the whole index stream into a garbage, + invisible mesh. Confirmed by reading gz-common's ColladaLoader.cc + (`LoadTriangles`, `offsetSize += input.second.size()`) after a generated + world loaded in Gazebo with no visible geometry despite no load errors. + """ + verts, faces = _grid_to_vertices_faces(grid) + uvs = _uvs_for_grid(grid, size_m) + normals = _vertex_normals(grid) + + positions_str = ' '.join(f'{v:.6f}' for v in verts.reshape(-1)) + normals_str = ' '.join(f'{v:.6f}' for v in normals.reshape(-1)) + uvs_str = ' '.join(f'{v:.6f}' for v in uvs.reshape(-1)) + indices_str = ' '.join(f'{i} {i}' for i in faces.reshape(-1)) + xml = _DAE_TEMPLATE.format( + texture_filename=texture_filename, + n_pos=verts.size, + positions=positions_str, + normals=normals_str, + n_verts=len(verts), + n_uv=uvs.size, + uvs=uvs_str, + n_tris=len(faces), + indices=indices_str, + ) + Path(path).write_text(xml) + return len(faces) diff --git a/navmap_tools/navmap_tools/pcd_writer.py b/navmap_tools/navmap_tools/pcd_writer.py new file mode 100644 index 0000000..260fd19 --- /dev/null +++ b/navmap_tools/navmap_tools/pcd_writer.py @@ -0,0 +1,101 @@ +# Copyright 2026 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. + +""" +ASCII PCD (x y z) writer, plus a parallel per-point RGB sidecar. + +The PCD itself matches the header shape of maps/*.pcd in +src/urjc-excavation-world (VERSION 0.7, FIELDS x y z, TYPE F F F, DATA ascii) +so the same `pointcloud_to_navmap` C++ tool that already works for that world +can also consume navmap_tools' output. + +Colors are written as a *separate* same-row-order CSV rather than a packed +`rgb` PCD field: PCL's XYZRGB PCD field is a float that is actually a +bit-reinterpreted packed uint32, an easy thing to get subtly wrong from +Python without a byte-for-byte match to PCL's own (un)packing. A plain CSV of +"r,g,b" per line, read back in C++ as three ordinary uint8s, has no such +pitfall and is trivial to test on both ends. +""" + +from pathlib import Path + +import numpy as np + + +def write_pcd_xyz(path, points, width=None, height=None) -> int: + """ + Write an ASCII XYZ PCD file from an (N, 3) array-like; returns N. + + `width`/`height` mark the cloud as *organized* (PCL's convention for a + grid-shaped point set, row-major: row j/col i at index j*width+i) rather + than the default unorganized WIDTH=N/HEIGHT=1. navmap_tools always + builds `points` this way (see terrain.TerrainGrid), and passing the true + shape here lets `pointcloud_to_navmap` use the dedicated, gap-free + `navmap_ros::from_regular_grid` mesher instead of the generic + neighbor-search one -- see easynav_gis_tool.md for why the generic + mesher could leave holes on an evenly-sampled, fully navigable grid. + """ + pts = np.asarray(points, dtype=np.float64) + if pts.size == 0: + pts = pts.reshape(0, 3) + if pts.ndim != 2 or pts.shape[1] != 3: + raise ValueError(f'points must be an Nx3 array-like, got shape {pts.shape}') + if not np.isfinite(pts).all(): + raise ValueError('points must not contain NaN/Inf values') + + n = pts.shape[0] + if (width is None) != (height is None): + raise ValueError('width and height must be given together') + if width is not None and width * height != n: + raise ValueError(f'width*height ({width}*{height}) must equal the point count ({n})') + out_width, out_height = (width, height) if width is not None else (n, 1) + + with open(Path(path), 'w') as f: + f.write('# .PCD v0.7 - Point Cloud Data file format\n') + f.write('VERSION 0.7\n') + f.write('FIELDS x y z\n') + f.write('SIZE 4 4 4\n') + f.write('TYPE F F F\n') + f.write('COUNT 1 1 1\n') + f.write(f'WIDTH {out_width}\n') + f.write(f'HEIGHT {out_height}\n') + f.write('VIEWPOINT 0 0 0 1 0 0 0\n') + f.write(f'POINTS {n}\n') + f.write('DATA ascii\n') + for x, y, z in pts: + f.write(f'{x:.6f} {y:.6f} {z:.6f}\n') + return n + + +def write_colors_csv(path, colors) -> int: + """ + Write an (N, 3) uint8 array-like of RGB colors as a plain "r,g,b" CSV. + + Row order must match the PCD written by `write_pcd_xyz` for the same + point set: `pointcloud_to_navmap` pairs them up positionally. + """ + cols = np.asarray(colors) + if cols.size == 0: + cols = cols.reshape(0, 3) + if cols.ndim != 2 or cols.shape[1] != 3: + raise ValueError(f'colors must be an Nx3 array-like, got shape {cols.shape}') + if ((cols < 0) | (cols > 255)).any(): + raise ValueError('colors must be in [0, 255]') + + n = cols.shape[0] + with open(Path(path), 'w') as f: + for r, g, b in cols: + f.write(f'{int(r)},{int(g)},{int(b)}\n') + return n diff --git a/navmap_tools/navmap_tools/scaffold.py b/navmap_tools/navmap_tools/scaffold.py new file mode 100644 index 0000000..3fe2995 --- /dev/null +++ b/navmap_tools/navmap_tools/scaffold.py @@ -0,0 +1,458 @@ +# Copyright 2026 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. + +""" +Build a whole `--package` colcon package tree for a navmap_gis_tool run. + +Mirrors src/urjc-excavation-world's structure and file conventions exactly +(worlds/*.world, models//{model.sdf,model.config,meshes/}, maps/, +launch/*.launch.py, env-hooks/*.dsv.in) so the generated package plugs into +this workspace the same way that reference world package does -- see +easynav_gis_tool.md for why this structure was chosen. `--package` is the +exact ROS package name the user wants (e.g. "urjc_excavation_world"); the +shorter `name` used for the model directory and the world/mesh/map file +basenames is that package name with a trailing "_world" stripped, matching +how urjc-excavation-world itself is named ("urjc_excavation_world" package, +"urjc_excavation" model/mesh basename). + +Scaffold files (package.xml, CMakeLists.txt, worlds/, models/, launch/, +env-hooks/, README.md) are always (re)written. `maps/` is left to the +caller (pcd_writer / the pointcloud_to_navmap tool): re-running with only +one of --gazebo/--navmap must not clobber files the other flag already +produced in a previous, separate invocation into the same --output-dir. +""" + +from dataclasses import dataclass +from pathlib import Path + + +def _render(template: str, **kwargs) -> str: + out = template + for key, value in kwargs.items(): + out = out.replace(f'__{key.upper()}__', str(value)) + return out + + +@dataclass +class WorldPaths: + """Resolved paths inside a generated `--package` package.""" + + package_name: str + name: str + root: Path + maps_dir: Path + models_dir: Path + model_dir: Path + meshes_dir: Path + worlds_dir: Path + launch_dir: Path + env_hooks_dir: Path + + @property + def pcd_path(self) -> Path: + """Path of the point cloud consumed/produced for --navmap.""" + return self.maps_dir / f'{self.name}.pcd' + + @property + def navmap_path(self) -> Path: + """Path of the generated .navmap file.""" + return self.maps_dir / f'{self.name}.navmap' + + @property + def colors_csv_path(self) -> Path: + """Path of the per-point RGB sidecar for the point cloud (see pcd_writer.py).""" + return self.maps_dir / f'{self.name}_colors.csv' + + @property + def world_path(self) -> Path: + """Path of the generated .world SDF file.""" + return self.worlds_dir / f'{self.name}.world' + + @property + def dae_path(self) -> Path: + """Path of the generated visual mesh.""" + return self.meshes_dir / f'{self.name}.dae' + + @property + def stl_path(self) -> Path: + """Path of the generated collision mesh.""" + return self.meshes_dir / f'{self.name}.stl' + + @property + def texture_path(self) -> Path: + """Path of the generated diffuse texture.""" + return self.meshes_dir / f'{self.name}_texture.png' + + +def short_name_for(package_name: str) -> str: + """ + Derive the model/mesh/world basename from a package name. + + Strips a trailing "_world" (e.g. "urjc_excavation_world" -> "urjc_excavation"), + matching src/urjc-excavation-world's own naming; packages not following + that convention are used as-is. + """ + suffix = '_world' + if package_name.endswith(suffix) and len(package_name) > len(suffix): + return package_name[:-len(suffix)] + return package_name + + +def world_paths(output_dir, package_name: str) -> WorldPaths: + """Resolve every path inside the `` package for `package_name`.""" + name = short_name_for(package_name) + root = Path(output_dir) + models_dir = root / 'models' + model_dir = models_dir / name + return WorldPaths( + package_name=package_name, + name=name, + root=root, + maps_dir=root / 'maps', + models_dir=models_dir, + model_dir=model_dir, + meshes_dir=model_dir / 'meshes', + worlds_dir=root / 'worlds', + launch_dir=root / 'launch', + env_hooks_dir=root / 'env-hooks', + ) + + +_PACKAGE_XML = """ + + __PACKAGE__ + 1.0.0 + + __PACKAGE__ for Gazebo simulations, generated by navmap_tools' + navmap_gis_tool from GPS center (__LAT__, __LON__) and size __SIZE__ m. + + Apache 2.0 + Generated by navmap_tools (navmap_gis_tool) + Generated by navmap_tools (navmap_gis_tool) + ament_cmake + ros_gz + gz_sim_vendor + gz_plugin_vendor + + + ament_cmake + + + + +""" + +_CMAKELISTS = """cmake_minimum_required(VERSION 3.5) +project(__PACKAGE__) + +find_package(ros_gz_sim REQUIRED) +find_package(ament_cmake_ros REQUIRED) + +install(DIRECTORY launch maps models worlds + DESTINATION share/${PROJECT_NAME} +) + +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/__PACKAGE__.dsv.in") + +ament_export_dependencies(ros_gz_sim) + +ament_package() +""" + +_ENV_HOOK = """set;GZ_VERSION;@GZ_VERSION@ +prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/models +prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/worlds +prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;@CMAKE_INSTALL_PREFIX@/lib + +prepend-non-duplicate;LD_LIBRARY_PATH;@CMAKE_INSTALL_PREFIX@/lib +""" + +_MODEL_CONFIG = """ + + + __NAME__ + 1.0 + model.sdf + + + Generated by navmap_tools (navmap_gis_tool) + fmrico@gmail.com + + + + Terrain generated from GIS data (GPS center __LAT__, __LON__; size __SIZE__ m; + DEM: Copernicus GLO-30; imagery: __IMAGERY_DESC__). + + +""" + +_MODEL_SDF = """ + + + true + + + + + model://__NAME__/meshes/__NAME__.stl + + + + + + 0.9000000 + 0.1000000 + 0.000000 0.000000 1.000000 + + + + 0.000000 + 100000.000000 + + + + 9.000000 + 9.000000 + 10000.000000 + 1.000000 + 100.000000 + 0.001000 + + + + + + + + model://__NAME__/meshes/__NAME__.dae + + + + + + +""" + +_WORLD_SDF = """ + + + + + 0.005 + 1.0 + + + + + + + + + + + + + + ogre2 + + + + + EARTH_WGS84 + ENU + __LAT__ + __LON__ + __ELEVATION__ + 0 + + + 0.0 0.0 -9.8 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + false + false + false + + + + 0 + 0 0 10000 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + model://__NAME__ + + + +""" + +_LAUNCH_PY = '''# Copyright 2026 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. + +"""Launch __PACKAGE__ standalone in Gazebo (gz sim -r -v4), no ROS robot.""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + world_file_name = '__NAME__.world' + package_dir = get_package_share_directory('__PACKAGE__') + world = LaunchConfiguration('world') + + model_path = os.path.join(package_dir, 'models') + + gazebo = ExecuteProcess( + cmd=['gz', 'sim', '-r', '-v4', world], output='screen') + + return LaunchDescription([ + SetEnvironmentVariable('GZ_SIM_RESOURCE_PATH', model_path), + DeclareLaunchArgument( + 'world', + default_value=[os.path.join(package_dir, 'worlds', world_file_name), ''], + description='SDF world file'), + DeclareLaunchArgument(name='use_sim_time', default_value='true'), + gazebo, + ]) + + +if __name__ == '__main__': + generate_launch_description() +''' + +_README = """# __PACKAGE__ + +Gazebo (gz-sim) world generated by `navmap_tools`' `navmap_gis_tool` from +real-world GIS data: + +* Center: __LAT__, __LON__ +* Size: __SIZE__ m x __SIZE__ m +* Elevation source: Copernicus DEM GLO-30 (~30 m, public AWS Open Data, no API key) +* Imagery source: __IMAGERY_DESC__ + +Attribution: contains modified Copernicus DEM data -- see +https://spacedata.copernicus.eu/ for its terms. __IMAGERY_ATTRIBUTION__ + +## Load directly into Gazebo (without ROS) + +```bash +export GZ_SIM_RESOURCE_PATH=`pwd`/models:`pwd`/worlds +gz sim worlds/__NAME__.world +``` + +## ROS launch (standalone, no robot) + +```bash +colcon build --symlink-install --packages-select __PACKAGE__ +source install/setup.bash +ros2 launch __PACKAGE__ __NAME__.launch.py +``` + +## NavMap + +If generated with `--navmap`, `maps/__NAME__.pcd` and `maps/__NAME__.navmap` +sit alongside this world and share the exact same local-ENU origin (this +world's GPS center), so they line up with the terrain above even though they +may have been produced in a separate `navmap_gis_tool` run. Publish it for +RViz2 with: + +```bash +ros2 run navmap_tools navmap_publisher --input maps/__NAME__.navmap +``` +""" + + +def _write(path: Path, content: str) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(content) + + +def scaffold_world_package( + output_dir, + package_name: str, + center_lat: float, + center_lon: float, + size_m: float, + elevation_amsl: float, + imagery_desc: str, + imagery_attribution: str = '', +) -> WorldPaths: + """Create/update the `--package` package tree; returns its resolved paths.""" + if not package_name or not package_name.replace('_', '').isalnum(): + raise ValueError( + f'package name must be a valid ROS package identifier: {package_name!r}') + + paths = world_paths(output_dir, package_name) + paths.meshes_dir.mkdir(parents=True, exist_ok=True) + paths.maps_dir.mkdir(parents=True, exist_ok=True) + paths.worlds_dir.mkdir(parents=True, exist_ok=True) + paths.launch_dir.mkdir(parents=True, exist_ok=True) + paths.env_hooks_dir.mkdir(parents=True, exist_ok=True) + + kwargs = { + 'package': package_name, 'name': paths.name, 'lat': center_lat, 'lon': center_lon, + 'size': size_m, 'elevation': elevation_amsl, 'imagery_desc': imagery_desc, + 'imagery_attribution': imagery_attribution, + } + + _write(paths.root / 'package.xml', _render(_PACKAGE_XML, **kwargs)) + _write(paths.root / 'CMakeLists.txt', _render(_CMAKELISTS, **kwargs)) + _write(paths.env_hooks_dir / f'{package_name}.dsv.in', _ENV_HOOK) + _write(paths.model_dir / 'model.config', _render(_MODEL_CONFIG, **kwargs)) + _write(paths.model_dir / 'model.sdf', _render(_MODEL_SDF, **kwargs)) + _write(paths.world_path, _render(_WORLD_SDF, **kwargs)) + _write(paths.launch_dir / f'{paths.name}.launch.py', _render(_LAUNCH_PY, **kwargs)) + _write(paths.root / 'README.md', _render(_README, **kwargs)) + + return paths diff --git a/navmap_tools/navmap_tools/terrain.py b/navmap_tools/navmap_tools/terrain.py new file mode 100644 index 0000000..df23691 --- /dev/null +++ b/navmap_tools/navmap_tools/terrain.py @@ -0,0 +1,126 @@ +# Copyright 2026 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. + +""" +Combine a DEM grid and an imagery mosaic into local-ENU samples. + +Both --gazebo and --navmap consume the exact same `TerrainGrid` for a given +(center, size) query -- that shared source is *why* the two outputs line up +even when generated in separate invocations (see easynav_gis_tool.md). + +Texture rendering is deliberately decoupled from `TerrainGrid`'s geometry +spacing (which follows the DEM's coarser ~30 m native resolution): the mesh +is coarse, but the draped texture can still use the imagery's much finer +native resolution, exactly like a game/robotics terrain that combines a +low-poly heightfield with a high-resolution diffuse texture. +""" + +from dataclasses import dataclass +import sys + +import numpy as np + +from .geo.dem import DemGrid +from .geo.imagery import ImageryMosaic +from .geo.projection import LocalProjection + + +@dataclass +class TerrainGrid: + """A regular local-ENU grid of (x, y, z) + RGB samples.""" + + xs: np.ndarray + ys: np.ndarray + elevation: np.ndarray + rgb: np.ndarray + spacing_m: float + center_elevation_amsl: float + + +def build_terrain_grid( + projection: LocalProjection, + dem: DemGrid, + imagery: ImageryMosaic, + size_m: float, + spacing_m: float, +) -> TerrainGrid: + """Sample DEM elevation + imagery color on a `spacing_m` grid over `size_m`.""" + if size_m <= 0: + raise ValueError(f'size_m must be positive: {size_m}') + if spacing_m <= 0: + raise ValueError(f'spacing_m must be positive: {spacing_m}') + + half = size_m / 2.0 + n = max(1, int(round(size_m / spacing_m))) + coords = np.linspace(-half, half, n + 1) + + center_elevation = dem.sample(projection.center_lon, projection.center_lat) + + rows = cols = len(coords) + xs = np.zeros((rows, cols), dtype=np.float64) + ys = np.zeros((rows, cols), dtype=np.float64) + elevation = np.zeros((rows, cols), dtype=np.float32) + rgb = np.zeros((rows, cols, 3), dtype=np.uint8) + + for i, y in enumerate(coords): + for j, x in enumerate(coords): + lon, lat = projection.to_lonlat(x, y) + xs[i, j] = x + ys[i, j] = y + elevation[i, j] = dem.sample(lon, lat) - center_elevation + rgb[i, j] = imagery.sample(lon, lat) + + return TerrainGrid( + xs=xs, + ys=ys, + elevation=elevation, + rgb=rgb, + spacing_m=(2 * half) / n, + center_elevation_amsl=center_elevation, + ) + + +def render_texture( + projection: LocalProjection, + imagery: ImageryMosaic, + size_m: float, + pixels: int = 512, +) -> np.ndarray: + """ + Render a `pixels` x `pixels` RGB texture over the `size_m` square. + + Independent of `TerrainGrid`'s (coarser) geometry spacing, so the mesh + can stay low-poly while the draped texture keeps the imagery's detail. + """ + if size_m <= 0: + raise ValueError(f'size_m must be positive: {size_m}') + if pixels < 1: + raise ValueError(f'pixels must be >= 1: {pixels}') + + half = size_m / 2.0 + # Sample at texel centers; texel 0 is the north-west corner (row-major, + # top-to-bottom) to match standard image/UV conventions. + xs = np.linspace(-half, half, pixels, endpoint=False) + (size_m / pixels) / 2.0 + ys = np.linspace(half, -half, pixels, endpoint=False) - (size_m / pixels) / 2.0 + + out = np.zeros((pixels, pixels, 3), dtype=np.uint8) + progress_every = max(1, pixels // 20) + for i, y in enumerate(ys): + for j, x in enumerate(xs): + lon, lat = projection.to_lonlat(x, y) + out[i, j] = imagery.sample(lon, lat) + if pixels >= 1000 and (i + 1) % progress_every == 0: + print(f'[navmap_gis_tool] texture: row {i + 1}/{pixels}', file=sys.stderr) + return out diff --git a/navmap_tools/package.xml b/navmap_tools/package.xml new file mode 100644 index 0000000..5be6843 --- /dev/null +++ b/navmap_tools/package.xml @@ -0,0 +1,45 @@ + + + + navmap_tools + 0.5.0 + + Tools and utilities for building and testing NavMaps: a GIS-to-NavMap/Gazebo + generator (given a GPS center and a size in meters), a PointCloud-to-NavMap + mesh builder, and a NavMap file publisher for RViz2. + + Francisco Martín Rico + + Apache License, Version 2.0 + + ament_cmake + ament_cmake_python + + rclcpp + navmap_core + navmap_ros + navmap_ros_interfaces + pcl_conversions + + python3-numpy + python3-pil + python3-pyproj + python3-tifffile + python3-requests + python3-platformdirs + ament_index_python + + ament_cmake_gtest + ament_cmake_pytest + ament_copyright + ament_flake8 + ament_pep257 + ament_xmllint + ament_lint_auto + ament_lint_common + python3-pytest + + + ament_cmake + + diff --git a/navmap_tools/scripts/navmap_gis_tool b/navmap_tools/scripts/navmap_gis_tool new file mode 100755 index 0000000..54cd537 --- /dev/null +++ b/navmap_tools/scripts/navmap_gis_tool @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 +# Copyright 2026 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. + +"""Console entry point: `ros2 run navmap_tools navmap_gis_tool ...`.""" + +from navmap_tools.cli import main + +if __name__ == '__main__': + main() diff --git a/navmap_tools/src/navmap_publisher.cpp b/navmap_tools/src/navmap_publisher.cpp new file mode 100644 index 0000000..a5e38da --- /dev/null +++ b/navmap_tools/src/navmap_publisher.cpp @@ -0,0 +1,135 @@ +// Copyright 2026 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. + +// Loads a .navmap file from disk and republishes it periodically so +// late-joining subscribers -- e.g. RViz2's NavMapDisplay, added via +// navmap_rviz_plugin -- reliably receive it. +// +// Originally this published once on a transient-local topic (matching +// navmap_ros/src/slam_server_app.cpp's "navmap" publisher) and relied on +// DDS's durability-service replay for late joiners. That reproducibly never +// delivered for this specific message type: confirmed empirically with a +// throwaway rclpy publisher (no C++ of this file involved) that +// NavMap+TRANSIENT_LOCAL never reaches a subscriber even locally, while +// NavMap+VOLATILE with periodic publishing, and TRANSIENT_LOCAL with a +// plain std_msgs/String, both work fine in the same environment -- pointing +// at a rmw/FastDDS quirk specific to this message type's transient-local +// history replay, not a bug in navmap_tools. Periodic republish sidesteps it +// entirely: any subscriber alive for at least one period receives the map, +// with plain, well-exercised QoS. See easynav_gis_tool.md for the full +// diagnostic trail. + +#include +#include +#include + +#include "navmap_ros/navmap_io.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ + +struct Args +{ + std::string input; + std::string topic = "navmap"; + std::string frame_id; + double rate_hz = 1.0; +}; + +bool parse_args(int argc, char * argv[], Args & args) +{ + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + auto next = [&](const char * flag) -> const char * { + if (i + 1 >= argc) { + std::cerr << "missing value for " << flag << "\n"; + return nullptr; + } + return argv[++i]; + }; + if (arg == "--input") { + const char * v = next("--input"); if (!v) {return false;} args.input = v; + } else if (arg == "--topic") { + const char * v = next("--topic"); if (!v) {return false;} args.topic = v; + } else if (arg == "--frame-id") { + const char * v = next("--frame-id"); if (!v) {return false;} args.frame_id = v; + } else if (arg == "--rate") { + const char * v = next("--rate"); if (!v) {return false;} + args.rate_hz = std::stod(v); + } else { + std::cerr << "unknown argument: " << arg << "\n"; + return false; + } + } + if (args.input.empty()) { + std::cerr << "usage: navmap_publisher --input [--topic navmap] " + "[--frame-id map] [--rate 1.0]\n"; + return false; + } + if (args.rate_hz <= 0.0) { + std::cerr << "--rate must be positive\n"; + return false; + } + return true; +} + +} // namespace + +int main(int argc, char * argv[]) +{ + Args args; + if (!parse_args(argc, argv, args)) { + return 1; + } + + navmap_ros_interfaces::msg::NavMap msg; + std::error_code ec; + if (!navmap_ros::io::load_msg_from_file(args.input, msg, &ec)) { + std::cerr << "failed to load " << args.input << ": " << ec.message() << "\n"; + return 1; + } + + // argv has already been fully consumed by parse_args() above and is not + // ROS syntax (no --ros-args), so it is deliberately not passed here -- + // doing so makes rclcpp try (and noisily fail) to lex it as ROS arguments. + rclcpp::init(0, nullptr); + auto node = rclcpp::Node::make_shared("navmap_publisher"); + + if (!args.frame_id.empty()) { + msg.header.frame_id = args.frame_id; + } + + auto pub = node->create_publisher( + args.topic, rclcpp::QoS(1).reliable()); + + RCLCPP_INFO( + node->get_logger(), + "Publishing %s (%zu vertices, %zu triangles) on '%s' at %.2f Hz, frame '%s'", + args.input.c_str(), msg.positions_x.size(), msg.navcels_v0.size(), args.topic.c_str(), + args.rate_hz, msg.header.frame_id.c_str()); + + const auto period = std::chrono::duration(1.0 / args.rate_hz); + auto timer = node->create_wall_timer( + std::chrono::duration_cast(period), + [&node, &pub, &msg]() { + msg.header.stamp = node->now(); + pub->publish(msg); + }); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/navmap_tools/src/pointcloud_to_navmap.cpp b/navmap_tools/src/pointcloud_to_navmap.cpp new file mode 100644 index 0000000..73cce60 --- /dev/null +++ b/navmap_tools/src/pointcloud_to_navmap.cpp @@ -0,0 +1,209 @@ +// Copyright 2026 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. + +// Builds a .navmap file from a point cloud (.pcd) using the existing +// navmap_ros::from_points() mesher -- the resolution/max-slope-deg +// downsampling and filtering already implemented and tested there, not +// reimplemented here. Optionally colors the resulting mesh's vertices +// (the only real "texture/RGBD" hook NavMap has, see navmap_core/NavMap.hpp +// Colors) by nearest-neighbour lookup against a parallel r,g,b CSV that +// navmap_tools' Python side writes alongside the .pcd (see pcd_writer.py for +// why that's a plain CSV and not PCL's packed-float "rgb" PCD field). + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "navmap_ros/conversions.hpp" +#include "navmap_ros/navmap_io.hpp" + +namespace +{ + +struct Args +{ + std::string input; + std::string output; + std::string colors; + std::string frame_id = "map"; + float resolution = 1.0f; + float max_slope_deg = 30.0f; +}; + +bool parse_args(int argc, char * argv[], Args & args) +{ + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + auto next = [&](const char * flag) -> const char * { + if (i + 1 >= argc) { + std::cerr << "missing value for " << flag << "\n"; + return nullptr; + } + return argv[++i]; + }; + if (arg == "--input") { + const char * v = next("--input"); if (!v) {return false;} args.input = v; + } else if (arg == "--output") { + const char * v = next("--output"); if (!v) {return false;} args.output = v; + } else if (arg == "--colors") { + const char * v = next("--colors"); if (!v) {return false;} args.colors = v; + } else if (arg == "--frame-id") { + const char * v = next("--frame-id"); if (!v) {return false;} args.frame_id = v; + } else if (arg == "--resolution") { + const char * v = next("--resolution"); if (!v) {return false;} + args.resolution = std::stof(v); + } else if (arg == "--max-slope-deg") { + const char * v = next("--max-slope-deg"); if (!v) {return false;} + args.max_slope_deg = std::stof(v); + } else { + std::cerr << "unknown argument: " << arg << "\n"; + return false; + } + } + if (args.input.empty() || args.output.empty()) { + std::cerr << "usage: pointcloud_to_navmap --input --output " + "[--colors ] [--resolution 1.0] [--max-slope-deg 30.0] " + "[--frame-id map]\n"; + return false; + } + return true; +} + +// One "r,g,b" triple per line, same row order as the input .pcd. +std::vector> load_colors_csv(const std::string & path) +{ + std::vector> colors; + std::ifstream ifs(path); + if (!ifs) { + throw std::runtime_error("cannot open colors CSV: " + path); + } + std::string line; + while (std::getline(ifs, line)) { + if (line.empty()) {continue;} + std::istringstream iss(line); + std::string r_s, g_s, b_s; + if (!std::getline(iss, r_s, ',') || !std::getline(iss, g_s, ',') || + !std::getline(iss, b_s, ',')) + { + throw std::runtime_error("malformed colors CSV line: " + line); + } + colors.push_back( + {static_cast(std::stoi(r_s)), static_cast(std::stoi(g_s)), + static_cast(std::stoi(b_s))}); + } + return colors; +} + +} // namespace + +int main(int argc, char * argv[]) +{ + Args args; + if (!parse_args(argc, argv, args)) { + return 1; + } + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + if (pcl::io::loadPCDFile(args.input, *cloud) != 0) { + std::cerr << "failed to load PCD: " << args.input << "\n"; + return 1; + } + if (cloud->empty()) { + std::cerr << "input point cloud is empty: " << args.input << "\n"; + return 1; + } + + navmap_ros::BuildParams params; + params.resolution = args.resolution; + params.max_slope_deg = args.max_slope_deg; + + navmap_ros_interfaces::msg::NavMap out_msg; + // navmap_tools always writes its own .pcd as an *organized* (grid-shaped) + // cloud (see pcd_writer.write_pcd_xyz), so the connectivity between + // adjacent samples is already fully known -- from_regular_grid meshes it + // deterministically from the grid indices with no neighbor search, which + // both avoids the search-radius-driven gaps from_points can leave on an + // evenly-sampled, fully navigable grid and produces far fewer redundant + // triangles. Older/foreign .pcd files (unorganized, height == 1, e.g. a + // hand-collected point cloud) still go through the generic from_points + // mesher unchanged. + navmap::NavMap nm = cloud->height > 1 ? + navmap_ros::from_regular_grid(*cloud, out_msg, params) : + navmap_ros::from_points(*cloud, out_msg, params); + (void)nm; + + const size_t n_verts = out_msg.positions_x.size(); + const size_t n_tris = out_msg.navcels_v0.size(); + if (n_verts == 0 || n_tris == 0) { + std::cerr << "meshing produced an empty NavMap (" << n_verts << " vertices, " << + n_tris << " triangles) -- check --resolution/--max-slope-deg against the input\n"; + return 1; + } + + if (!args.colors.empty()) { + std::vector> colors; + try { + colors = load_colors_csv(args.colors); + } catch (const std::exception & e) { + std::cerr << e.what() << "\n"; + return 1; + } + if (colors.size() != cloud->size()) { + std::cerr << "colors CSV has " << colors.size() << " rows but the input cloud has " << + cloud->size() << " points; they must be row-aligned\n"; + return 1; + } + + pcl::search::KdTree kdtree; + kdtree.setInputCloud(cloud); + + out_msg.has_vertex_rgba = true; + out_msg.colors_r.resize(n_verts); + out_msg.colors_g.resize(n_verts); + out_msg.colors_b.resize(n_verts); + out_msg.colors_a.resize(n_verts, 255); + + std::vector idx(1); + std::vector dist(1); + for (size_t i = 0; i < n_verts; ++i) { + pcl::PointXYZ query( + out_msg.positions_x[i], out_msg.positions_y[i], out_msg.positions_z[i]); + if (kdtree.nearestKSearch(query, 1, idx, dist) > 0) { + const auto & c = colors[static_cast(idx[0])]; + out_msg.colors_r[i] = c[0]; + out_msg.colors_g[i] = c[1]; + out_msg.colors_b[i] = c[2]; + } + } + } + + out_msg.header.frame_id = args.frame_id; + + std::error_code ec; + if (!navmap_ros::io::save_msg_to_file(out_msg, args.output, {}, &ec)) { + std::cerr << "failed to save " << args.output << ": " << ec.message() << "\n"; + return 1; + } + + std::cout << "wrote " << args.output << " (" << n_verts << " vertices, " << + n_tris << " triangles" << (args.colors.empty() ? "" : ", colored") << ")\n"; + return 0; +} diff --git a/navmap_tools/test/__init__.py b/navmap_tools/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/navmap_tools/test/test_cache.py b/navmap_tools/test/test_cache.py new file mode 100644 index 0000000..e94c371 --- /dev/null +++ b/navmap_tools/test/test_cache.py @@ -0,0 +1,129 @@ +# Copyright 2026 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. + +"""Unit tests for navmap_tools.geo.cache -- no network involved.""" + +from pathlib import Path + +from navmap_tools.geo.cache import default_cache_dir, DiskCache + +import pytest + + +def test_default_cache_dir_ends_with_navmap_tools_gis(): + path = default_cache_dir() + assert path.parts[-2:] == ('navmap_tools', 'gis') + + +def test_diskcache_creates_root_directory(tmp_path): + root = tmp_path / 'a' / 'b' / 'cache' + assert not root.exists() + DiskCache(root) + assert root.is_dir() + + +def test_path_for_is_deterministic(tmp_path): + cache = DiskCache(tmp_path) + p1 = cache.path_for('key', '.tif') + p2 = cache.path_for('key', '.tif') + assert p1 == p2 + + +def test_path_for_differs_for_different_keys(tmp_path): + cache = DiskCache(tmp_path) + assert cache.path_for('key1', '.tif') != cache.path_for('key2', '.tif') + + +def test_path_for_adds_leading_dot_to_suffix(tmp_path): + cache = DiskCache(tmp_path) + assert cache.path_for('key', 'tif') == cache.path_for('key', '.tif') + + +def test_path_for_lives_under_root(tmp_path): + cache = DiskCache(tmp_path) + p = cache.path_for('key', '.tif') + assert p.parent == tmp_path + + +def test_get_or_fetch_calls_fetch_on_first_call(tmp_path): + cache = DiskCache(tmp_path) + calls = [] + + def fetch(dest): + calls.append(dest) + dest.write_text('data') + + path = cache.get_or_fetch('key', '.txt', fetch) + assert path.read_text() == 'data' + assert len(calls) == 1 + + +def test_get_or_fetch_reuses_cache_on_second_call(tmp_path): + cache = DiskCache(tmp_path) + calls = [] + + def fetch(dest): + calls.append(dest) + dest.write_text('data') + + cache.get_or_fetch('key', '.txt', fetch) + cache.get_or_fetch('key', '.txt', fetch) + assert len(calls) == 1 + + +def test_get_or_fetch_force_refetches(tmp_path): + cache = DiskCache(tmp_path) + calls = [] + + def fetch(dest): + calls.append(dest) + dest.write_text('data') + + cache.get_or_fetch('key', '.txt', fetch) + cache.get_or_fetch('key', '.txt', fetch, force=True) + assert len(calls) == 2 + + +def test_get_or_fetch_propagates_fetch_exception_and_cleans_up(tmp_path): + cache = DiskCache(tmp_path) + + def fetch(dest): + dest.write_text('partial') + raise RuntimeError('boom') + + with pytest.raises(RuntimeError, match='boom'): + cache.get_or_fetch('key', '.txt', fetch) + + dest = cache.path_for('key', '.txt') + assert not dest.exists() + assert not (tmp_path / (dest.name + '.part')).exists() + assert list(tmp_path.iterdir()) == [] + + +def test_get_or_fetch_raises_if_fetch_does_not_create_dest(tmp_path): + cache = DiskCache(tmp_path) + + def fetch(dest): + pass # deliberately does not create dest + + with pytest.raises(RuntimeError, match='did not create'): + cache.get_or_fetch('key', '.txt', fetch) + assert list(tmp_path.iterdir()) == [] + + +def test_get_or_fetch_returns_a_path_instance(tmp_path): + cache = DiskCache(tmp_path) + path = cache.get_or_fetch('key', '.txt', lambda dest: dest.write_text('x')) + assert isinstance(path, Path) diff --git a/navmap_tools/test/test_cli.py b/navmap_tools/test/test_cli.py new file mode 100644 index 0000000..a4a45ed --- /dev/null +++ b/navmap_tools/test/test_cli.py @@ -0,0 +1,299 @@ +# Copyright 2026 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. + +""" +Unit tests for navmap_tools.cli's pure argument-parsing logic. + +Deliberately does not exercise `run()`: that hits the network (DEM/imagery) +and shells out to the compiled `pointcloud_to_navmap`, both covered by the +end-to-end smoke test instead (see easynav_gis_tool.md). +""" + +import argparse + +from navmap_tools.cli import ( + _default_package_name, + _parse_center, + _texture_pixels_for, + build_arg_parser, +) +from navmap_tools.geo.imagery import DEFAULT_ZOOM, native_resolution_m_per_px + +import pytest + + +# --------------------------------------------------------------------------- +# _default_package_name +# --------------------------------------------------------------------------- + +def test_default_package_name_ends_with_world(): + assert _default_package_name(40.3314, -3.8356).endswith('_world') + + +def test_default_package_name_is_a_valid_identifier_segment(): + name = _default_package_name(40.3314, -3.8356) + assert name.replace('_', '').isalnum() + + +def test_default_package_name_encodes_sign_for_negative_values(): + name = _default_package_name(-33.45, -70.66) + assert 'm33p4500' in name + assert 'm70p6600' in name + + +def test_default_package_name_has_no_sign_marker_for_positive_values(): + name = _default_package_name(40.3314, 3.8356) + assert 'm40p3314' not in name + assert 'm3p8356' not in name + + +def test_default_package_name_differs_for_different_coordinates(): + a = _default_package_name(40.0, -3.0) + b = _default_package_name(41.0, -3.0) + assert a != b + + +def test_default_package_name_stable_for_same_coordinates(): + assert _default_package_name(40.0, -3.0) == _default_package_name(40.0, -3.0) + + +def test_default_package_name_handles_zero(): + name = _default_package_name(0.0, 0.0) + assert name == 'gis_0p0000_0p0000_world' + + +# --------------------------------------------------------------------------- +# _parse_center +# --------------------------------------------------------------------------- + +def test_parse_center_valid(): + assert _parse_center('40.3314,-3.8356') == (40.3314, -3.8356) + + +def test_parse_center_with_spaces_around_numbers(): + assert _parse_center('0,0') == (0.0, 0.0) + + +@pytest.mark.parametrize('value', ['40.3314', '40.3314,-3.8356,1.0', '', ',']) +def test_parse_center_rejects_wrong_field_count(value): + with pytest.raises(argparse.ArgumentTypeError): + _parse_center(value) + + +@pytest.mark.parametrize('value', ['abc,1.0', '1.0,xyz']) +def test_parse_center_rejects_non_numeric(value): + with pytest.raises(argparse.ArgumentTypeError): + _parse_center(value) + + +# --------------------------------------------------------------------------- +# build_arg_parser / run() validation +# --------------------------------------------------------------------------- + +def test_center_and_size_are_required(): + parser = build_arg_parser() + with pytest.raises(SystemExit): + parser.parse_args([]) + + +def test_minimal_valid_invocation_parses(): + parser = build_arg_parser() + args = parser.parse_args(['--center', '40.0,-3.0', '--size', '100', '--gazebo']) + assert args.center == (40.0, -3.0) + assert args.size == 100.0 + assert args.gazebo is True + assert args.navmap is False + + +def test_defaults(): + parser = build_arg_parser() + args = parser.parse_args(['--center', '40.0,-3.0', '--size', '100', '--navmap']) + assert args.resolution == 1.0 + assert args.max_slope_deg == 30.0 + assert args.package is None + assert args.output_dir is None + assert args.dem_source == 'copernicus30' + assert args.imagery_source == 'esri' + assert args.force_refresh is False + assert args.zoom == DEFAULT_ZOOM + assert args.texture_pixels == 0 + + +def test_rejects_unknown_dem_source(): + parser = build_arg_parser() + with pytest.raises(SystemExit): + parser.parse_args( + ['--center', '40.0,-3.0', '--size', '100', '--navmap', '--dem-source', 'bogus']) + + +def test_rejects_unknown_imagery_source(): + parser = build_arg_parser() + with pytest.raises(SystemExit): + parser.parse_args( + ['--center', '40.0,-3.0', '--size', '100', '--navmap', '--imagery-source', 'bogus']) + + +def test_accepts_pnoa_imagery_source(): + parser = build_arg_parser() + args = parser.parse_args( + ['--center', '40.0,-3.0', '--size', '100', '--navmap', '--imagery-source', 'pnoa']) + assert args.imagery_source == 'pnoa' + + +def test_accepts_google_imagery_source(): + parser = build_arg_parser() + args = parser.parse_args( + ['--center', '40.0,-3.0', '--size', '100', '--navmap', '--imagery-source', 'google']) + assert args.imagery_source == 'google' + + +def test_google_imagery_source_without_api_key_fails_fast(monkeypatch): + from navmap_tools.cli import run + from navmap_tools.geo.google import API_KEY_ENV_VAR + + monkeypatch.delenv(API_KEY_ENV_VAR, raising=False) + with pytest.raises(SystemExit): + run([ + '--center', '40.0,-3.0', '--size', '100', '--navmap', + '--imagery-source', 'google', + ]) + + +def test_google_imagery_source_with_api_key_passes_validation(monkeypatch): + from navmap_tools import cli as cli_mod + from navmap_tools.geo.google import API_KEY_ENV_VAR + + monkeypatch.setenv(API_KEY_ENV_VAR, 'fake-key') + + def fake_load_dem_grid(bbox, cache, force=False): + raise _Sentinel() + + monkeypatch.setattr(cli_mod, 'load_dem_grid', fake_load_dem_grid) + with pytest.raises(_Sentinel): + cli_mod.run([ + '--center', '40.0,-3.0', '--size', '100', '--gazebo', + '--imagery-source', 'google', + ]) + + +def test_at_least_one_output_flag_required(): + from navmap_tools.cli import run + + with pytest.raises(SystemExit): + run(['--center', '40.0,-3.0', '--size', '100']) + + +def test_size_must_be_positive(): + from navmap_tools.cli import run + + with pytest.raises(SystemExit): + run(['--center', '40.0,-3.0', '--size', '0', '--gazebo']) + with pytest.raises(SystemExit): + run(['--center', '40.0,-3.0', '--size', '-5', '--navmap']) + + +def test_resolution_must_be_positive(): + from navmap_tools.cli import run + + with pytest.raises(SystemExit): + run(['--center', '40.0,-3.0', '--size', '100', '--resolution', '0', '--gazebo']) + with pytest.raises(SystemExit): + run(['--center', '40.0,-3.0', '--size', '100', '--resolution', '-1', '--navmap']) + + +def test_zoom_must_be_in_range(): + from navmap_tools.cli import run + + with pytest.raises(SystemExit): + run(['--center', '40.0,-3.0', '--size', '100', '--gazebo', '--zoom', '-1']) + with pytest.raises(SystemExit): + run(['--center', '40.0,-3.0', '--size', '100', '--gazebo', '--zoom', '24']) + + +def test_texture_pixels_must_not_be_negative(): + from navmap_tools.cli import run + + with pytest.raises(SystemExit): + run(['--center', '40.0,-3.0', '--size', '100', '--gazebo', '--texture-pixels', '-1']) + + +# --------------------------------------------------------------------------- +# _texture_pixels_for +# +# Auto-sizes the baked texture from the actual fetched imagery resolution: +# without this, a fixed pixel count wastes whatever extra detail --zoom paid +# to download (a real user asked for this after finding the texture blurrier +# than the source imagery; see easynav_gis_tool.md). +# --------------------------------------------------------------------------- + +def test_texture_pixels_for_matches_native_resolution_at_equator(): + # At zoom 19, equator: ~0.29808 m/px: 300 m / 0.29808 =~ 1006. + resolution = native_resolution_m_per_px(zoom=19, lat_deg=0.0) + px = _texture_pixels_for(size_m=300.0, resolution_m_per_px=resolution) + assert 900 <= px <= 1100 + + +def test_texture_pixels_for_is_clamped_to_minimum(): + resolution = native_resolution_m_per_px(zoom=1, lat_deg=0.0) + px = _texture_pixels_for(size_m=1.0, resolution_m_per_px=resolution) + assert px == 256 + + +def test_texture_pixels_for_is_clamped_to_maximum(): + resolution = native_resolution_m_per_px(zoom=23, lat_deg=0.0) + px = _texture_pixels_for(size_m=100000.0, resolution_m_per_px=resolution) + assert px == 4096 + + +def test_texture_pixels_for_increases_with_finer_resolution(): + low = _texture_pixels_for( + size_m=300.0, resolution_m_per_px=native_resolution_m_per_px(zoom=15, lat_deg=40.0)) + high = _texture_pixels_for( + size_m=300.0, resolution_m_per_px=native_resolution_m_per_px(zoom=19, lat_deg=40.0)) + assert high > low + + +def test_texture_pixels_for_works_with_pnoa_style_resolution(): + # PNOA gives a plain meters/pixel value directly (no zoom level at all). + px = _texture_pixels_for(size_m=100.0, resolution_m_per_px=0.25) + assert px == 400 + + +def test_huge_grid_is_rejected_before_touching_the_network(): + from navmap_tools.cli import run + + # 100000 m / 1 m resolution -> a 100001x100001 grid, far over the cap; + # must fail fast on validation, not hang trying to fetch DEM/imagery. + with pytest.raises(SystemExit): + run(['--center', '40.0,-3.0', '--size', '100000', '--gazebo']) + + +class _Sentinel(Exception): + """Raised by a stub to prove control reached past argument validation.""" + + +def test_grid_within_the_cap_passes_validation(monkeypatch): + from navmap_tools import cli as cli_mod + + # Stub out the network-touching step right after validation to prove + # validation itself did not reject this combination (this is the exact + # --size 300 --resolution 1.0 combination that a real user hit; see + # easynav_gis_tool.md). + def fake_load_dem_grid(bbox, cache, force=False): + raise _Sentinel() + + monkeypatch.setattr(cli_mod, 'load_dem_grid', fake_load_dem_grid) + with pytest.raises(_Sentinel): + cli_mod.run(['--center', '40.0,-3.0', '--size', '300', '--gazebo']) diff --git a/navmap_tools/test/test_copyright.py b/navmap_tools/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/navmap_tools/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/navmap_tools/test/test_dem.py b/navmap_tools/test/test_dem.py new file mode 100644 index 0000000..e6ba6dd --- /dev/null +++ b/navmap_tools/test/test_dem.py @@ -0,0 +1,160 @@ +# Copyright 2026 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. + +""" +Unit tests for navmap_tools.geo.dem -- no network involved. + +Network-touching functions (`fetch_tile_path`, `load_dem_grid`'s HTTP path) +are exercised via monkeypatching so no real download happens here; the +actual download is covered by the end-to-end smoke test instead. +""" + +from navmap_tools.geo import dem as dem_mod +from navmap_tools.geo.dem import DemGrid, load_dem_grid, tile_key, tile_url +from navmap_tools.geo.projection import BBox + +import numpy as np + +import pytest + + +# --------------------------------------------------------------------------- +# tile_key / tile_url +# --------------------------------------------------------------------------- + +@pytest.mark.parametrize( + 'lat,lon,expected', + [ + (40, -4, 'Copernicus_DSM_COG_10_N40_00_W004_00_DEM'), + (0, 0, 'Copernicus_DSM_COG_10_N00_00_E000_00_DEM'), + (-1, -1, 'Copernicus_DSM_COG_10_S01_00_W001_00_DEM'), + (89, 179, 'Copernicus_DSM_COG_10_N89_00_E179_00_DEM'), + (-90, -180, 'Copernicus_DSM_COG_10_S90_00_W180_00_DEM'), + ], +) +def test_tile_key_matches_copernicus_naming(lat, lon, expected): + assert tile_key(lat, lon) == expected + + +def test_tile_url_embeds_the_key_twice(): + url = tile_url(40, -4) + key = tile_key(40, -4) + assert url == f'https://copernicus-dem-30m.s3.amazonaws.com/{key}/{key}.tif' + + +# --------------------------------------------------------------------------- +# DemGrid.sample +# --------------------------------------------------------------------------- + +def _grid_2x2(): + # west=0, north=1, pixel size 1 degree in both axes: + # row0 (north, lat~1): [10, 20] + # row1 (south, lat~0): [30, 40] + return DemGrid( + elevation=np.array([[10.0, 20.0], [30.0, 40.0]], dtype=np.float32), + west=0.0, north=1.0, pixel_size_lon=1.0, pixel_size_lat=1.0, + ) + + +def test_sample_at_grid_corners(): + grid = _grid_2x2() + assert grid.sample(0.0, 1.0) == pytest.approx(10.0) + assert grid.sample(1.0, 1.0) == pytest.approx(20.0) + assert grid.sample(0.0, 0.0) == pytest.approx(30.0) + assert grid.sample(1.0, 0.0) == pytest.approx(40.0) + + +def test_sample_bilinear_center(): + grid = _grid_2x2() + assert grid.sample(0.5, 0.5) == pytest.approx((10 + 20 + 30 + 40) / 4) + + +def test_sample_clamps_outside_west_south(): + grid = _grid_2x2() + assert grid.sample(-10.0, -10.0) == pytest.approx(30.0) + + +def test_sample_clamps_outside_east_north(): + grid = _grid_2x2() + assert grid.sample(10.0, 10.0) == pytest.approx(20.0) + + +def test_sample_single_pixel_grid_is_constant(): + grid = DemGrid( + elevation=np.array([[42.0]], dtype=np.float32), + west=0.0, north=1.0, pixel_size_lon=1.0, pixel_size_lat=1.0, + ) + assert grid.sample(0.0, 0.0) == pytest.approx(42.0) + assert grid.sample(100.0, -100.0) == pytest.approx(42.0) + + +# --------------------------------------------------------------------------- +# load_dem_grid (monkeypatched, no network) +# --------------------------------------------------------------------------- + +def test_load_dem_grid_raises_when_no_coverage(monkeypatch): + def always_missing(lat_floor, lon_floor, cache, force=False): + raise FileNotFoundError('no tile') + + monkeypatch.setattr(dem_mod, 'fetch_tile_path', always_missing) + with pytest.raises(RuntimeError, match='no DEM coverage'): + load_dem_grid(BBox(west=0.0, south=0.0, east=0.5, north=0.5), cache=None) + + +def test_load_dem_grid_raises_on_mismatched_tile_shapes(monkeypatch): + def fake_fetch(lat_floor, lon_floor, cache, force=False): + return f'{lat_floor}_{lon_floor}' + + def fake_read(path): + # Two tiles with different shapes -> should be rejected, not mosaicked. + if path == '0_0': + return np.zeros((10, 10), dtype=np.float32), 0.0, 1.0, 0.1, 0.1 + return np.zeros((20, 20), dtype=np.float32), 1.0, 1.0, 0.05, 0.05 + + monkeypatch.setattr(dem_mod, 'fetch_tile_path', fake_fetch) + monkeypatch.setattr(dem_mod, '_read_tile_array', fake_read) + with pytest.raises(RuntimeError, match='different grid resolution'): + load_dem_grid(BBox(west=0.0, south=0.0, east=1.5, north=0.5), cache=None) + + +def test_load_dem_grid_mosaics_single_tile(monkeypatch): + def fake_fetch(lat_floor, lon_floor, cache, force=False): + return 'only' + + def fake_read(path): + return np.arange(4, dtype=np.float32).reshape(2, 2), 0.0, 1.0, 0.5, 0.5 + + monkeypatch.setattr(dem_mod, 'fetch_tile_path', fake_fetch) + monkeypatch.setattr(dem_mod, '_read_tile_array', fake_read) + grid = load_dem_grid(BBox(west=0.1, south=0.1, east=0.2, north=0.2), cache=None) + assert grid.elevation.shape == (2, 2) + assert grid.west == pytest.approx(0.0) + assert grid.north == pytest.approx(1.0) + + +def test_load_dem_grid_skips_ocean_tiles_but_keeps_land(monkeypatch): + def fake_fetch(lat_floor, lon_floor, cache, force=False): + if lon_floor == 0: + raise FileNotFoundError('ocean') + return 'land' + + def fake_read(path): + return np.ones((2, 2), dtype=np.float32), 1.0, 1.0, 1.0, 1.0 + + monkeypatch.setattr(dem_mod, 'fetch_tile_path', fake_fetch) + monkeypatch.setattr(dem_mod, '_read_tile_array', fake_read) + grid = load_dem_grid(BBox(west=0.1, south=0.1, east=1.9, north=0.9), cache=None) + # Two 1x1-degree tile columns (lon 0 ocean, lon 1 land) mosaicked together. + assert grid.elevation.shape == (2, 4) diff --git a/navmap_tools/test/test_flake8.py b/navmap_tools/test/test_flake8.py new file mode 100644 index 0000000..2603011 --- /dev/null +++ b/navmap_tools/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, 'Found %d code style errors / warnings:\n' % len( + errors + ) + '\n'.join(errors) diff --git a/navmap_tools/test/test_google.py b/navmap_tools/test/test_google.py new file mode 100644 index 0000000..354173e --- /dev/null +++ b/navmap_tools/test/test_google.py @@ -0,0 +1,294 @@ +# Copyright 2026 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. + +""" +Unit tests for navmap_tools.geo.google -- no real network involved. + +`requests.post`/tile fetching are monkeypatched throughout; the actual +Map Tiles API contract (session creation, tile URL shape) has not been +exercised against a live key in this repository -- see the module +docstring in geo/google.py. +""" + +import time + +from navmap_tools.geo import google as google_mod +from navmap_tools.geo.cache import DiskCache +from navmap_tools.geo.google import ( + API_KEY_ENV_VAR, + create_session, + get_api_key, + GoogleSession, + load_google_mosaic, +) +from navmap_tools.geo.projection import BBox + +import numpy as np + +from PIL import Image + +import pytest + + +# --------------------------------------------------------------------------- +# get_api_key +# --------------------------------------------------------------------------- + +def test_get_api_key_raises_when_unset(monkeypatch): + monkeypatch.delenv(API_KEY_ENV_VAR, raising=False) + with pytest.raises(RuntimeError, match=API_KEY_ENV_VAR): + get_api_key() + + +def test_get_api_key_raises_when_blank(monkeypatch): + monkeypatch.setenv(API_KEY_ENV_VAR, ' ') + with pytest.raises(RuntimeError, match=API_KEY_ENV_VAR): + get_api_key() + + +def test_get_api_key_returns_stripped_value(monkeypatch): + monkeypatch.setenv(API_KEY_ENV_VAR, ' my-key ') + assert get_api_key() == 'my-key' + + +# --------------------------------------------------------------------------- +# GoogleSession.is_valid +# --------------------------------------------------------------------------- + +def test_session_valid_when_expiry_far_in_future(): + session = GoogleSession(token='t', expiry_epoch_s=time.time() + 3600) + assert session.is_valid() is True + + +def test_session_invalid_when_expiry_in_the_past(): + session = GoogleSession(token='t', expiry_epoch_s=time.time() - 10) + assert session.is_valid() is False + + +def test_session_invalid_within_expiry_margin(): + # _SESSION_EXPIRY_MARGIN_S is 30s; 5s from now is inside that margin. + session = GoogleSession(token='t', expiry_epoch_s=time.time() + 5) + assert session.is_valid() is False + + +# --------------------------------------------------------------------------- +# create_session +# --------------------------------------------------------------------------- + +class _FakeResponse: + + def __init__(self, json_data, status_code=200): + self._json = json_data + self.status_code = status_code + + def raise_for_status(self): + if self.status_code >= 400: + raise RuntimeError(f'{self.status_code} error') + + def json(self): + return self._json + + +def test_create_session_parses_token_and_expiry(monkeypatch): + calls = [] + + def fake_post(url, params=None, json=None, timeout=None): + calls.append((url, params, json, timeout)) + return _FakeResponse({'session': 'abc123', 'expiry': '9999999999'}) + + monkeypatch.setattr(google_mod.requests, 'post', fake_post) + session = create_session('the-key') + + assert session.token == 'abc123' + assert session.expiry_epoch_s == 9999999999.0 + (url, params, json_body, timeout) = calls[0] + assert url == google_mod._CREATE_SESSION_URL + assert params == {'key': 'the-key'} + assert json_body['mapType'] == 'satellite' + assert timeout == google_mod._TIMEOUT_S + + +def test_create_session_propagates_http_errors(monkeypatch): + def fake_post(url, params=None, json=None, timeout=None): + return _FakeResponse({}, status_code=403) + + monkeypatch.setattr(google_mod.requests, 'post', fake_post) + with pytest.raises(RuntimeError, match='403'): + create_session('bad-key') + + +# --------------------------------------------------------------------------- +# _SessionProvider +# --------------------------------------------------------------------------- + +def test_session_provider_creates_lazily(monkeypatch): + created = [] + + def fake_create_session(api_key): + created.append(api_key) + return GoogleSession(token='t', expiry_epoch_s=time.time() + 3600) + + monkeypatch.setattr(google_mod, 'create_session', fake_create_session) + provider = google_mod._SessionProvider('key') + assert created == [] # not created until .get() is called + provider.get() + assert created == ['key'] + + +def test_session_provider_reuses_valid_session(monkeypatch): + created = [] + + def fake_create_session(api_key): + created.append(api_key) + return GoogleSession(token='t', expiry_epoch_s=time.time() + 3600) + + monkeypatch.setattr(google_mod, 'create_session', fake_create_session) + provider = google_mod._SessionProvider('key') + s1 = provider.get() + s2 = provider.get() + assert s1 is s2 + assert len(created) == 1 + + +def test_session_provider_refreshes_expired_session(monkeypatch): + tokens = iter(['first', 'second']) + + def fake_create_session(api_key): + return GoogleSession(token=next(tokens), expiry_epoch_s=time.time() - 1) + + monkeypatch.setattr(google_mod, 'create_session', fake_create_session) + provider = google_mod._SessionProvider('key') + s1 = provider.get() + s2 = provider.get() + # Both immediately expired (expiry in the past), so every .get() refreshes. + assert s1.token == 'first' + assert s2.token == 'second' + + +# --------------------------------------------------------------------------- +# _fetch_tile_path -- cache hit must never touch the session/network +# --------------------------------------------------------------------------- + +def test_fetch_tile_path_cache_hit_never_creates_a_session(tmp_path, monkeypatch): + cache = DiskCache(tmp_path) + session_calls = [] + + class _NeverCallProvider: + def get(self): + session_calls.append(1) + raise AssertionError('session should not be requested on a cache hit') + + # Pre-populate the cache entry this key would resolve to. + key = 'imagery/google/10/5/5' + path = cache.path_for(key, '.jpg') + path.write_bytes(b'cached tile bytes') + + result = google_mod._fetch_tile_path(10, 5, 5, _NeverCallProvider(), 'key', cache) + assert result == path + assert session_calls == [] + + +def test_fetch_tile_path_cache_miss_uses_session_token_in_url(tmp_path, monkeypatch): + cache = DiskCache(tmp_path) + session = GoogleSession(token='sess-tok', expiry_epoch_s=time.time() + 3600) + + class _FixedProvider: + def get(self): + return session + + captured_urls = [] + + def fake_get_to_file(url, dest, timeout): + captured_urls.append(url) + dest.write_bytes(b'tile bytes') + + monkeypatch.setattr(google_mod, 'get_to_file', fake_get_to_file) + google_mod._fetch_tile_path(10, 5, 5, _FixedProvider(), 'the-key', cache) + + assert len(captured_urls) == 1 + assert 'session=sess-tok' in captured_urls[0] + assert 'key=the-key' in captured_urls[0] + + +# --------------------------------------------------------------------------- +# load_google_mosaic +# --------------------------------------------------------------------------- + +def test_load_google_mosaic_requires_api_key_when_not_passed(monkeypatch): + monkeypatch.delenv(API_KEY_ENV_VAR, raising=False) + with pytest.raises(RuntimeError, match=API_KEY_ENV_VAR): + load_google_mosaic(BBox(-1, -1, 1, 1), cache=None, zoom=10) + + +def test_load_google_mosaic_rejects_bad_zoom(): + with pytest.raises(ValueError, match='zoom'): + load_google_mosaic(BBox(-1, -1, 1, 1), cache=None, api_key='k', zoom=-1) + with pytest.raises(ValueError, match='zoom'): + load_google_mosaic(BBox(-1, -1, 1, 1), cache=None, api_key='k', zoom=24) + + +def test_load_google_mosaic_rejects_too_many_tiles(): + with pytest.raises(ValueError, match='tiles'): + load_google_mosaic(BBox(-170, -80, 170, 80), cache=None, api_key='k', zoom=18) + + +def test_load_google_mosaic_stitches_tiles_in_correct_positions(monkeypatch, tmp_path): + calls = [] + + def fake_fetch(z, x, y, sessions, api_key, cache, force=False): + calls.append((z, x, y)) + color = (x % 256, y % 256, 0) + img = np.zeros((256, 256, 3), dtype=np.uint8) + img[:, :] = color + path = tmp_path / f'{z}_{x}_{y}.png' + Image.fromarray(img, mode='RGB').save(path) + return path + + monkeypatch.setattr(google_mod, '_fetch_tile_path', fake_fetch) + + bbox = BBox(west=-0.01, south=-0.01, east=0.01, north=0.01) + mosaic = load_google_mosaic(bbox, cache=None, api_key='k', zoom=15) + + assert mosaic.image.shape == (512, 512, 3) + assert len(calls) == 4 + top_left_color = tuple(mosaic.image[0, 0]) + assert top_left_color == (mosaic.tile_x0 % 256, mosaic.tile_y0 % 256, 0) + + +def test_load_google_mosaic_full_cache_hit_creates_no_session(tmp_path, monkeypatch): + # First call (cache miss) should create exactly one session; a second, + # fully-cached call must not touch the Google API at all -- the whole + # point of caching a paid API's tiles. + session_creations = [] + + def fake_create_session(api_key): + session_creations.append(api_key) + return GoogleSession(token='t', expiry_epoch_s=time.time() + 3600) + + def fake_get_to_file(url, dest, timeout): + img = np.zeros((256, 256, 3), dtype=np.uint8) + Image.fromarray(img, mode='RGB').save(dest, format='JPEG') + + monkeypatch.setattr(google_mod, 'create_session', fake_create_session) + monkeypatch.setattr(google_mod, 'get_to_file', fake_get_to_file) + + cache = DiskCache(tmp_path) + bbox = BBox(west=-0.001, south=-0.001, east=0.001, north=0.001) + + load_google_mosaic(bbox, cache, api_key='k', zoom=15) + assert len(session_creations) == 1 + + load_google_mosaic(bbox, cache, api_key='k', zoom=15) + assert len(session_creations) == 1 # unchanged: fully served from cache diff --git a/navmap_tools/test/test_imagery.py b/navmap_tools/test/test_imagery.py new file mode 100644 index 0000000..925d6b8 --- /dev/null +++ b/navmap_tools/test/test_imagery.py @@ -0,0 +1,297 @@ +# Copyright 2026 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. + +""" +Unit tests for navmap_tools.geo.imagery -- no network involved. + +`load_imagery_mosaic`'s tile fetching is monkeypatched so no real HTTP +request happens here; the actual download is covered by the end-to-end +smoke test instead. +""" + +import hashlib + +from navmap_tools.geo import imagery as imagery_mod +from navmap_tools.geo.imagery import ( + find_available_zoom, + ImageryMosaic, + load_imagery_mosaic, + lonlat_to_tile, + native_resolution_m_per_px, +) +from navmap_tools.geo.projection import BBox + +import numpy as np + +import pytest + + +# --------------------------------------------------------------------------- +# lonlat_to_tile +# --------------------------------------------------------------------------- + +def test_origin_is_top_left_tile_at_zoom_0(): + x, y = lonlat_to_tile(-180.0, 85.05112878, 0) + assert x == pytest.approx(0.0, abs=1e-6) + assert y == pytest.approx(0.0, abs=1e-6) + + +def test_equator_prime_meridian_is_map_center_at_any_zoom(): + for zoom in (0, 5, 19): + x, y = lonlat_to_tile(0.0, 0.0, zoom) + n = 2.0 ** zoom + assert x == pytest.approx(n / 2.0) + assert y == pytest.approx(n / 2.0) + + +def test_tile_x_increases_eastward(): + x_west, _ = lonlat_to_tile(-10.0, 0.0, 10) + x_east, _ = lonlat_to_tile(10.0, 0.0, 10) + assert x_east > x_west + + +# --------------------------------------------------------------------------- +# native_resolution_m_per_px +# --------------------------------------------------------------------------- + +def test_native_resolution_at_zoom0_equator_matches_known_constant(): + # Standard Web Mercator ground resolution constant. + assert native_resolution_m_per_px(0, 0.0) == pytest.approx(156543.03392804097) + + +def test_native_resolution_halves_per_zoom_level(): + r10 = native_resolution_m_per_px(10, 0.0) + r11 = native_resolution_m_per_px(11, 0.0) + assert r11 == pytest.approx(r10 / 2.0) + + +def test_native_resolution_finer_at_higher_latitude(): + # Web Mercator: a fixed zoom covers less real distance per pixel as + # you move away from the equator (cos(lat) shrinks). + equator = native_resolution_m_per_px(15, 0.0) + high_lat = native_resolution_m_per_px(15, 60.0) + assert high_lat < equator + + +def test_native_resolution_at_poles_is_zero(): + assert native_resolution_m_per_px(15, 90.0) == pytest.approx(0.0, abs=1e-6) + + +def test_native_resolution_symmetric_in_latitude_sign(): + north = native_resolution_m_per_px(12, 40.0) + south = native_resolution_m_per_px(12, -40.0) + assert north == pytest.approx(south) + + +def test_tile_y_increases_southward(): + _, y_north = lonlat_to_tile(0.0, 10.0, 10) + _, y_south = lonlat_to_tile(0.0, -10.0, 10) + assert y_south > y_north + + +def test_latitude_is_clamped_to_web_mercator_limit(): + # Must not raise (math domain error) at/above the poles. + lonlat_to_tile(0.0, 90.0, 5) + lonlat_to_tile(0.0, -90.0, 5) + + +# --------------------------------------------------------------------------- +# ImageryMosaic.sample +# --------------------------------------------------------------------------- + +def _solid_mosaic(color, zoom=10, size=4): + image = np.zeros((size, size, 3), dtype=np.uint8) + image[:, :] = color + return ImageryMosaic(image=image, zoom=zoom, tile_x0=0, tile_y0=0) + + +def test_sample_solid_color_returns_that_color(): + mosaic = _solid_mosaic((10, 20, 30)) + lon, lat = 5.0, 5.0 # anywhere within the tile range + r, g, b = mosaic.sample(lon, lat) + assert (r, g, b) == (10, 20, 30) + + +def test_sample_clamps_outside_mosaic_bounds(): + mosaic = _solid_mosaic((1, 2, 3)) + # Far outside the tile grid entirely. + r, g, b = mosaic.sample(-179.0, 89.0) + assert (r, g, b) == (1, 2, 3) + + +def test_sample_returns_ints_in_range(): + image = np.random.default_rng(0).integers(0, 256, size=(8, 8, 3), dtype=np.uint8) + mosaic = ImageryMosaic(image=image, zoom=12, tile_x0=100, tile_y0=200) + x, y = lonlat_to_tile(3.0, 45.0, 12) + r, g, b = mosaic.sample(3.0, 45.0) + for v in (r, g, b): + assert isinstance(v, int) + assert 0 <= v <= 255 + assert x is not None and y is not None # sanity: coordinates are finite + + +# --------------------------------------------------------------------------- +# load_imagery_mosaic +# --------------------------------------------------------------------------- + +def test_load_imagery_mosaic_rejects_bad_zoom(): + with pytest.raises(ValueError, match='zoom'): + load_imagery_mosaic(BBox(-1, -1, 1, 1), cache=None, zoom=-1) + with pytest.raises(ValueError, match='zoom'): + load_imagery_mosaic(BBox(-1, -1, 1, 1), cache=None, zoom=24) + + +def test_load_imagery_mosaic_rejects_too_many_tiles(): + # A huge bbox at a high zoom needs far more than _MAX_TILES tiles. + with pytest.raises(ValueError, match='tiles'): + load_imagery_mosaic(BBox(-170, -80, 170, 80), cache=None, zoom=18) + + +def test_load_imagery_mosaic_stitches_tiles_in_correct_positions(monkeypatch, tmp_path): + calls = [] + + def fake_fetch(z, x, y, cache, force=False): + calls.append((z, x, y)) + color = (x % 256, y % 256, 0) + img = np.zeros((256, 256, 3), dtype=np.uint8) + img[:, :] = color + path = tmp_path / f'{z}_{x}_{y}.png' + from PIL import Image + Image.fromarray(img, mode='RGB').save(path) + return path + + monkeypatch.setattr(imagery_mod, '_fetch_tile_path', fake_fetch) + + # A small bbox that spans exactly 2x2 tiles at a moderate zoom. + # auto_fallback=False: isolated from find_available_zoom's own probe + # fetch, tested separately below. + bbox = BBox(west=-0.01, south=-0.01, east=0.01, north=0.01) + mosaic = load_imagery_mosaic(bbox, cache=None, zoom=15, auto_fallback=False) + + assert mosaic.image.shape == (512, 512, 3) + assert len(calls) == 4 + # Top-left tile pixel matches the (tile_x0, tile_y0) tile's synthetic color. + top_left_color = tuple(mosaic.image[0, 0]) + assert top_left_color == (mosaic.tile_x0 % 256, mosaic.tile_y0 % 256, 0) + + +# --------------------------------------------------------------------------- +# _is_placeholder_tile / find_available_zoom / load_imagery_mosaic auto_fallback +# +# Esri serves a byte-identical "Map data not yet available" placeholder tile +# (not a 404) for coordinates it has no imagery for at a given zoom. A real +# generated world showed that placeholder's text repeated across every mesh +# cell after --zoom's default was raised to 23 for an area with no real +# zoom-23 coverage. See easynav_gis_tool.md for the full diagnosis. +# --------------------------------------------------------------------------- + +def test_is_placeholder_tile_detects_known_hash(tmp_path, monkeypatch): + content = b'pretend this is the exact known placeholder bytes' + digest = hashlib.md5(content).hexdigest() + monkeypatch.setattr(imagery_mod, '_PLACEHOLDER_MD5_HASHES', frozenset({digest})) + path = tmp_path / 'tile.jpg' + path.write_bytes(content) + assert imagery_mod._is_placeholder_tile(path) is True + + +def test_is_placeholder_tile_false_for_unknown_content(tmp_path): + path = tmp_path / 'tile.jpg' + path.write_bytes(b'totally real imagery, trust me') + assert imagery_mod._is_placeholder_tile(path) is False + + +def test_find_available_zoom_returns_max_zoom_when_already_real(monkeypatch, tmp_path): + def fake_fetch(z, x, y, cache, force=False): + path = tmp_path / f'{z}.jpg' + path.write_bytes(b'real imagery') + return path + + monkeypatch.setattr(imagery_mod, '_fetch_tile_path', fake_fetch) + zoom = find_available_zoom(0.0, 0.0, cache=None, max_zoom=20) + assert zoom == 20 + + +def test_find_available_zoom_steps_down_past_placeholders(monkeypatch, tmp_path): + placeholder = b'placeholder bytes' + monkeypatch.setattr( + imagery_mod, '_PLACEHOLDER_MD5_HASHES', + frozenset({hashlib.md5(placeholder).hexdigest()})) + + def fake_fetch(z, x, y, cache, force=False): + path = tmp_path / f'{z}.jpg' + path.write_bytes(placeholder if z >= 18 else f'real-{z}'.encode()) + return path + + monkeypatch.setattr(imagery_mod, '_fetch_tile_path', fake_fetch) + zoom = find_available_zoom(0.0, 0.0, cache=None, max_zoom=20) + assert zoom == 17 + + +def test_find_available_zoom_falls_back_to_min_zoom_if_all_placeholder(monkeypatch, tmp_path): + placeholder = b'placeholder bytes' + monkeypatch.setattr( + imagery_mod, '_PLACEHOLDER_MD5_HASHES', + frozenset({hashlib.md5(placeholder).hexdigest()})) + + def fake_fetch(z, x, y, cache, force=False): + path = tmp_path / f'{z}.jpg' + path.write_bytes(placeholder) + return path + + monkeypatch.setattr(imagery_mod, '_fetch_tile_path', fake_fetch) + zoom = find_available_zoom(0.0, 0.0, cache=None, max_zoom=15, min_zoom=12) + assert zoom == 12 + + +def test_load_imagery_mosaic_auto_fallback_adjusts_zoom_and_reports(monkeypatch, tmp_path, capsys): + placeholder = b'placeholder bytes' + monkeypatch.setattr( + imagery_mod, '_PLACEHOLDER_MD5_HASHES', + frozenset({hashlib.md5(placeholder).hexdigest()})) + + def fake_fetch(z, x, y, cache, force=False): + from PIL import Image + path = tmp_path / f'{z}_{x}_{y}.jpg' + if z == 20: + path.write_bytes(placeholder) + else: + img = np.zeros((256, 256, 3), dtype=np.uint8) + Image.fromarray(img, mode='RGB').save(path) + return path + + monkeypatch.setattr(imagery_mod, '_fetch_tile_path', fake_fetch) + bbox = BBox(west=-0.01, south=-0.01, east=0.01, north=0.01) + mosaic = load_imagery_mosaic(bbox, cache=None, zoom=20) + + assert mosaic.zoom == 19 + assert 'zoom 19 instead' in capsys.readouterr().err + + +def test_load_imagery_mosaic_auto_fallback_disabled_keeps_requested_zoom( + monkeypatch, tmp_path): + # With auto_fallback=False, find_available_zoom's probe (and therefore + # _is_placeholder_tile) is never consulted at all, so every fetched tile + # -- even a real placeholder -- is mosaicked as-is at the requested zoom. + def fake_fetch(z, x, y, cache, force=False): + from PIL import Image + path = tmp_path / f'{z}_{x}_{y}.jpg' + img = np.zeros((256, 256, 3), dtype=np.uint8) + Image.fromarray(img, mode='RGB').save(path) + return path + + monkeypatch.setattr(imagery_mod, '_fetch_tile_path', fake_fetch) + bbox = BBox(west=-0.01, south=-0.01, east=0.01, north=0.01) + mosaic = load_imagery_mosaic(bbox, cache=None, zoom=20, auto_fallback=False) + assert mosaic.zoom == 20 diff --git a/navmap_tools/test/test_mesh_export.py b/navmap_tools/test/test_mesh_export.py new file mode 100644 index 0000000..9d0d616 --- /dev/null +++ b/navmap_tools/test/test_mesh_export.py @@ -0,0 +1,360 @@ +# Copyright 2026 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. + +"""Unit tests for navmap_tools.mesh_export -- pure geometry/IO, no network.""" + +import struct +import xml.etree.ElementTree as ET + +from navmap_tools.mesh_export import ( + _grid_to_vertices_faces, + _uvs_for_grid, + _vertex_normals, + write_dae, + write_stl, + write_texture_png, +) +from navmap_tools.terrain import TerrainGrid + +import numpy as np + +from PIL import Image + +import pytest + + +def _grid_2x2(): + return TerrainGrid( + xs=np.array([[-1.0, 1.0], [-1.0, 1.0]]), + ys=np.array([[1.0, 1.0], [-1.0, -1.0]]), + elevation=np.array([[5.0, 6.0], [7.0, 8.0]], dtype=np.float32), + rgb=np.zeros((2, 2, 3), dtype=np.uint8), + spacing_m=2.0, + center_elevation_amsl=0.0, + ) + + +def _grid_3x3(): + xs, ys = np.meshgrid(np.linspace(-1, 1, 3), np.linspace(1, -1, 3)) + return TerrainGrid( + xs=xs, ys=ys, + elevation=np.zeros((3, 3), dtype=np.float32), + rgb=np.zeros((3, 3, 3), dtype=np.uint8), + spacing_m=1.0, + center_elevation_amsl=0.0, + ) + + +# --------------------------------------------------------------------------- +# _grid_to_vertices_faces +# --------------------------------------------------------------------------- + +def test_grid_to_vertices_faces_2x2_counts(): + verts, faces = _grid_to_vertices_faces(_grid_2x2()) + assert verts.shape == (4, 3) + assert faces.shape == (2, 3) + + +def test_grid_to_vertices_faces_3x3_counts(): + verts, faces = _grid_to_vertices_faces(_grid_3x3()) + assert verts.shape == (9, 3) + assert faces.shape == (8, 3) # (3-1)*(3-1)*2 + + +def test_grid_to_vertices_faces_indices_are_in_range(): + verts, faces = _grid_to_vertices_faces(_grid_3x3()) + assert faces.min() >= 0 + assert faces.max() < len(verts) + + +def test_grid_to_vertices_faces_vertex_z_matches_elevation(): + grid = _grid_2x2() + verts, _ = _grid_to_vertices_faces(grid) + assert sorted(verts[:, 2].tolist()) == [5.0, 6.0, 7.0, 8.0] + + +def test_grid_to_vertices_faces_every_vertex_used_at_least_once(): + verts, faces = _grid_to_vertices_faces(_grid_3x3()) + used = set(faces.reshape(-1).tolist()) + assert used == set(range(len(verts))) + + +def test_grid_to_vertices_faces_winding_gives_upward_normals(): + """ + Regression: a flat, level grid must wind CCW-from-above (+Z normal). + + A real generated world showed only the scene background (grey) from + above and a flat white underside from below in Gazebo -- exactly the + signature of a backface-culled, downward-facing front face. See this + function's docstring in mesh_export.py for the fix and the by-hand + cross-product check that caught it. + """ + xs, ys = np.meshgrid(np.linspace(0.0, 1.0, 3), np.linspace(0.0, 1.0, 3)) + grid = TerrainGrid( + xs=xs, ys=ys, elevation=np.zeros((3, 3), dtype=np.float32), + rgb=np.zeros((3, 3, 3), dtype=np.uint8), spacing_m=0.5, center_elevation_amsl=0.0, + ) + verts, faces = _grid_to_vertices_faces(grid) + for a, b, c in faces: + p0, p1, p2 = verts[a], verts[b], verts[c] + normal = np.cross(p1 - p0, p2 - p0) + assert normal[2] > 0, f'triangle ({a},{b},{c}) faces down: normal={normal}' + + +# --------------------------------------------------------------------------- +# _uvs_for_grid +# --------------------------------------------------------------------------- + +def test_uvs_for_grid_corners(): + grid = _grid_2x2() + uvs = _uvs_for_grid(grid, size_m=2.0).reshape(2, 2, 2) + # COLLADA's v origin is the BOTTOM of the texture; gz-common's + # ColladaLoader flips v (1.0 - v) on load to convert to Ogre's top-left + # origin. So north (y=+1) must be written as v=1 here, so it lands on + # v=0 (the top / north row) after gz's own flip -- south -> v=0 here. + assert uvs[0, 0] == pytest.approx((0.0, 1.0)) # west, north + assert uvs[0, 1] == pytest.approx((1.0, 1.0)) # east, north + assert uvs[1, 0] == pytest.approx((0.0, 0.0)) # west, south + assert uvs[1, 1] == pytest.approx((1.0, 0.0)) # east, south + + +def test_uvs_for_grid_stay_within_unit_square(): + grid = _grid_3x3() + uvs = _uvs_for_grid(grid, size_m=2.0) + assert (uvs >= -1e-9).all() + assert (uvs <= 1.0 + 1e-9).all() + + +# --------------------------------------------------------------------------- +# _vertex_normals +# +# Regression: gz-rendering's Ogre2 PBS pipeline needs a NORMAL vertex +# attribute to shade with at all -- without one it silently renders flat +# white and never samples the diffuse texture, even though the texture/ +# material data is completely valid. Confirmed empirically in Gazebo (a +# known-good reference texture stayed invisible until a NORMAL source was +# added). See mesh_export.py's write_dae docstring. +# --------------------------------------------------------------------------- + +def _flat_grid(n=3, spacing=1.0): + coords = np.linspace(-(n - 1) / 2 * spacing, (n - 1) / 2 * spacing, n) + xs, ys = np.meshgrid(coords, coords) + return TerrainGrid( + xs=xs, ys=ys, elevation=np.zeros((n, n), dtype=np.float32), + rgb=np.zeros((n, n, 3), dtype=np.uint8), spacing_m=spacing, + center_elevation_amsl=0.0, + ) + + +def test_vertex_normals_shape(): + grid = _flat_grid(n=4) + normals = _vertex_normals(grid) + assert normals.shape == (16, 3) + + +def test_vertex_normals_flat_grid_points_straight_up(): + grid = _flat_grid(n=5) + normals = _vertex_normals(grid) + assert np.allclose(normals, np.array([0.0, 0.0, 1.0]), atol=1e-9) + + +def test_vertex_normals_are_unit_length(): + grid = _flat_grid(n=5) + rng = np.random.default_rng(0) + grid.elevation[:] = rng.uniform(-2.0, 2.0, size=grid.elevation.shape) + normals = _vertex_normals(grid) + lengths = np.linalg.norm(normals, axis=-1) + assert np.allclose(lengths, 1.0) + + +def test_vertex_normals_tilt_away_from_uphill_direction(): + # Elevation rises with x (east): the surface tilts, so the normal must + # lean in -x (west) -- i.e. away from the uphill direction. + grid = _flat_grid(n=5, spacing=1.0) + grid.elevation = grid.xs.astype(np.float32) * 0.5 + normals = _vertex_normals(grid) + assert (normals[:, 0] < 0).all() + assert (normals[:, 2] > 0).all() + + +def test_vertex_normals_no_nan_or_inf(): + grid = _flat_grid(n=6) + rng = np.random.default_rng(1) + grid.elevation[:] = rng.uniform(-5.0, 5.0, size=grid.elevation.shape) + normals = _vertex_normals(grid) + assert np.isfinite(normals).all() + + +# --------------------------------------------------------------------------- +# write_stl +# --------------------------------------------------------------------------- + +def test_write_stl_file_size_matches_triangle_count(tmp_path): + grid = _grid_2x2() + path = tmp_path / 'mesh.stl' + n_tris = write_stl(path, grid) + assert n_tris == 2 + expected_size = 80 + 4 + n_tris * 50 + assert path.stat().st_size == expected_size + + +def test_write_stl_header_and_triangle_count(tmp_path): + grid = _grid_3x3() + path = tmp_path / 'mesh.stl' + write_stl(path, grid) + data = path.read_bytes() + assert data[:80] == b'\x00' * 80 + (count,) = struct.unpack('Z_UP' in path.read_text() + + +def test_write_dae_position_uv_and_normal_counts_match_vertex_count(tmp_path): + grid = _grid_3x3() + path = tmp_path / 'mesh.dae' + write_dae(path, grid, size_m=2.0, texture_filename='tex.png') + ns = {'c': 'http://www.collada.org/2005/11/COLLADASchema'} + root = ET.parse(path).getroot() + accessors = root.findall('.//c:accessor', ns) + counts = {int(a.get('count')) for a in accessors} + # 3x3 grid -> 9 vertices; positions, uvs and normals accessors all agree. + assert counts == {9} + + +def test_write_dae_vertices_element_declares_position_and_normal(tmp_path): + # Regression: without a NORMAL input here, gz-rendering's Ogre2 PBS + # pipeline renders flat/unlit white and never samples the diffuse + # texture -- see the note above _vertex_normals in mesh_export.py. + grid = _grid_2x2() + path = tmp_path / 'mesh.dae' + write_dae(path, grid, size_m=2.0, texture_filename='tex.png') + ns = {'c': 'http://www.collada.org/2005/11/COLLADASchema'} + root = ET.parse(path).getroot() + vertices_elem = root.find('.//c:vertices', ns) + semantics = {i.get('semantic') for i in vertices_elem.findall('c:input', ns)} + assert semantics == {'POSITION', 'NORMAL'} + assert root.find('.//c:source[@id="terrain_normals"]', ns) is not None + + +# --------------------------------------------------------------------------- +#

index stream shape (regression: see write_dae's docstring for why this +# matters -- gz-common's ColladaLoader sizes the per-corner stride as the +# *count of declared (semantic, offset) inputs*, not max(offset) + 1, so two +# inputs sharing one offset value silently desyncs the whole index stream +# into an invisible, garbage mesh instead of failing loudly) +# --------------------------------------------------------------------------- + +def test_write_dae_vertex_and_texcoord_inputs_use_distinct_offsets(tmp_path): + grid = _grid_2x2() + path = tmp_path / 'mesh.dae' + write_dae(path, grid, size_m=2.0, texture_filename='tex.png') + ns = {'c': 'http://www.collada.org/2005/11/COLLADASchema'} + root = ET.parse(path).getroot() + triangles = root.find('.//c:triangles', ns) + inputs = triangles.findall('c:input', ns) + offsets_by_semantic = {i.get('semantic'): i.get('offset') for i in inputs} + assert offsets_by_semantic == {'VERTEX': '0', 'TEXCOORD': '1'} + + +def test_write_dae_p_stream_has_two_values_per_corner(tmp_path): + grid = _grid_3x3() + path = tmp_path / 'mesh.dae' + n_tris = write_dae(path, grid, size_m=2.0, texture_filename='tex.png') + ns = {'c': 'http://www.collada.org/2005/11/COLLADASchema'} + root = ET.parse(path).getroot() + p_text = root.find('.//c:triangles/c:p', ns).text + values = p_text.split() + # 3 corners/triangle * 2 inputs (VERTEX offset 0, TEXCOORD offset 1). + assert len(values) == n_tris * 3 * 2 + + +def test_write_dae_p_stream_pairs_are_equal_vertex_and_texcoord_index(tmp_path): + grid = _grid_3x3() + path = tmp_path / 'mesh.dae' + write_dae(path, grid, size_m=2.0, texture_filename='tex.png') + ns = {'c': 'http://www.collada.org/2005/11/COLLADASchema'} + root = ET.parse(path).getroot() + values = [int(v) for v in root.find('.//c:triangles/c:p', ns).text.split()] + pairs = list(zip(values[0::2], values[1::2])) + assert all(vertex_idx == texcoord_idx for vertex_idx, texcoord_idx in pairs) diff --git a/navmap_tools/test/test_net.py b/navmap_tools/test/test_net.py new file mode 100644 index 0000000..c22c361 --- /dev/null +++ b/navmap_tools/test/test_net.py @@ -0,0 +1,136 @@ +# Copyright 2026 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. + +""" +Unit tests for navmap_tools.geo.net -- no real network involved. + +`requests.get` is monkeypatched with small fakes; `time.sleep` is +monkeypatched to a no-op so the retry-backoff tests stay fast. +""" + +from navmap_tools.geo import net as net_mod +from navmap_tools.geo.net import get_to_file + +import pytest + +import requests + + +class _FakeResponse: + + def __init__(self, status_code=200, chunks=(b'hello',)): + self.status_code = status_code + self._chunks = chunks + + def raise_for_status(self): + if self.status_code >= 400: + raise requests.exceptions.HTTPError(f'{self.status_code} error') + + def iter_content(self, chunk_size): + yield from self._chunks + + def __enter__(self): + return self + + def __exit__(self, *exc_info): + return False + + +@pytest.fixture(autouse=True) +def _no_real_sleep(monkeypatch): + monkeypatch.setattr(net_mod.time, 'sleep', lambda _seconds: None) + + +def test_get_to_file_success_writes_content(tmp_path, monkeypatch): + dest = tmp_path / 'out.bin' + monkeypatch.setattr( + net_mod.requests, 'get', + lambda url, timeout, stream: _FakeResponse(chunks=(b'ab', b'cd'))) + ok = get_to_file('http://example.invalid/x', dest, timeout=5) + assert ok is True + assert dest.read_bytes() == b'abcd' + + +def test_get_to_file_raises_on_http_error_status(tmp_path, monkeypatch): + monkeypatch.setattr( + net_mod.requests, 'get', lambda url, timeout, stream: _FakeResponse(status_code=500)) + with pytest.raises(requests.exceptions.HTTPError): + get_to_file('http://example.invalid/x', tmp_path / 'out.bin', timeout=5) + + +def test_get_to_file_404_without_allow_404_raises(tmp_path, monkeypatch): + monkeypatch.setattr( + net_mod.requests, 'get', lambda url, timeout, stream: _FakeResponse(status_code=404)) + with pytest.raises(requests.exceptions.HTTPError): + get_to_file('http://example.invalid/x', tmp_path / 'out.bin', timeout=5) + + +def test_get_to_file_404_with_allow_404_returns_false_and_no_file(tmp_path, monkeypatch): + monkeypatch.setattr( + net_mod.requests, 'get', lambda url, timeout, stream: _FakeResponse(status_code=404)) + dest = tmp_path / 'out.bin' + ok = get_to_file('http://example.invalid/x', dest, timeout=5, allow_404=True) + assert ok is False + assert not dest.exists() + + +def test_get_to_file_retries_transient_failure_then_succeeds(tmp_path, monkeypatch): + calls = {'n': 0} + + def fake_get(url, timeout, stream): + calls['n'] += 1 + if calls['n'] < 3: + raise requests.exceptions.ConnectionError('reset') + return _FakeResponse(chunks=(b'ok',)) + + monkeypatch.setattr(net_mod.requests, 'get', fake_get) + dest = tmp_path / 'out.bin' + ok = get_to_file('http://example.invalid/x', dest, timeout=5) + assert ok is True + assert dest.read_bytes() == b'ok' + assert calls['n'] == 3 + + +def test_get_to_file_raises_last_exception_after_exhausting_retries(tmp_path, monkeypatch): + def always_fails(url, timeout, stream): + raise requests.exceptions.Timeout('too slow') + + monkeypatch.setattr(net_mod.requests, 'get', always_fails) + with pytest.raises(requests.exceptions.Timeout, match='too slow'): + get_to_file('http://example.invalid/x', tmp_path / 'out.bin', timeout=5) + + +def test_get_to_file_does_not_leave_a_partial_file_on_total_failure(tmp_path, monkeypatch): + def always_fails(url, timeout, stream): + raise requests.exceptions.ConnectionError('nope') + + monkeypatch.setattr(net_mod.requests, 'get', always_fails) + dest = tmp_path / 'out.bin' + with pytest.raises(requests.exceptions.ConnectionError): + get_to_file('http://example.invalid/x', dest, timeout=5) + assert not dest.exists() + + +def test_get_to_file_number_of_attempts_matches_backoff_table_plus_one(monkeypatch, tmp_path): + calls = {'n': 0} + + def always_fails(url, timeout, stream): + calls['n'] += 1 + raise requests.exceptions.ConnectionError('nope') + + monkeypatch.setattr(net_mod.requests, 'get', always_fails) + with pytest.raises(requests.exceptions.ConnectionError): + get_to_file('http://example.invalid/x', tmp_path / 'out.bin', timeout=5) + assert calls['n'] == 1 + len(net_mod._RETRY_BACKOFF_S) diff --git a/navmap_tools/test/test_pcd_writer.py b/navmap_tools/test/test_pcd_writer.py new file mode 100644 index 0000000..58cc7ec --- /dev/null +++ b/navmap_tools/test/test_pcd_writer.py @@ -0,0 +1,164 @@ +# Copyright 2026 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. + +"""Unit tests for navmap_tools.pcd_writer.""" + +from navmap_tools.pcd_writer import write_colors_csv, write_pcd_xyz + +import numpy as np + +import pytest + + +# --------------------------------------------------------------------------- +# write_pcd_xyz +# --------------------------------------------------------------------------- + +def test_write_pcd_xyz_header_fields(tmp_path): + path = tmp_path / 'cloud.pcd' + n = write_pcd_xyz(path, [(1.0, 2.0, 3.0), (4.0, 5.0, 6.0)]) + assert n == 2 + lines = path.read_text().splitlines() + assert lines[0] == '# .PCD v0.7 - Point Cloud Data file format' + assert 'VERSION 0.7' in lines + assert 'FIELDS x y z' in lines + assert 'SIZE 4 4 4' in lines + assert 'TYPE F F F' in lines + assert 'COUNT 1 1 1' in lines + assert 'WIDTH 2' in lines + assert 'HEIGHT 1' in lines + assert 'POINTS 2' in lines + assert 'DATA ascii' in lines + + +def test_write_pcd_xyz_data_rows_round_trip(tmp_path): + path = tmp_path / 'cloud.pcd' + pts = [(1.5, -2.25, 3.0), (0.0, 0.0, 0.0)] + write_pcd_xyz(path, pts) + data_lines = path.read_text().splitlines()[-2:] + parsed = [tuple(float(v) for v in line.split()) for line in data_lines] + for (x, y, z), (px, py, pz) in zip(pts, parsed): + assert px == pytest.approx(x) + assert py == pytest.approx(y) + assert pz == pytest.approx(z) + + +def test_write_pcd_xyz_empty_cloud(tmp_path): + path = tmp_path / 'cloud.pcd' + n = write_pcd_xyz(path, []) + assert n == 0 + assert 'WIDTH 0' in path.read_text() + assert 'POINTS 0' in path.read_text() + + +def test_write_pcd_xyz_rejects_wrong_shape(tmp_path): + path = tmp_path / 'cloud.pcd' + with pytest.raises(ValueError, match='Nx3'): + write_pcd_xyz(path, [(1.0, 2.0)]) + with pytest.raises(ValueError, match='Nx3'): + write_pcd_xyz(path, np.zeros((2, 4))) + + +@pytest.mark.parametrize('bad', [float('nan'), float('inf'), float('-inf')]) +def test_write_pcd_xyz_rejects_non_finite_values(tmp_path, bad): + path = tmp_path / 'cloud.pcd' + with pytest.raises(ValueError, match='NaN/Inf'): + write_pcd_xyz(path, [(bad, 0.0, 0.0)]) + + +def test_write_pcd_xyz_accepts_numpy_array(tmp_path): + path = tmp_path / 'cloud.pcd' + n = write_pcd_xyz(path, np.array([[1.0, 2.0, 3.0]])) + assert n == 1 + + +# --------------------------------------------------------------------------- +# write_pcd_xyz -- organized (width/height) mode +# +# navmap_tools always writes its own .pcd this way so +# navmap_ros::from_regular_grid (a dedicated, gap-free mesher) can be used +# instead of the generic neighbor-search one; see easynav_gis_tool.md. +# --------------------------------------------------------------------------- + +def test_write_pcd_xyz_organized_header_fields(tmp_path): + path = tmp_path / 'cloud.pcd' + pts = [(float(i), float(j), 0.0) for j in range(3) for i in range(4)] + n = write_pcd_xyz(path, pts, width=4, height=3) + assert n == 12 + lines = path.read_text().splitlines() + assert 'WIDTH 4' in lines + assert 'HEIGHT 3' in lines + assert 'POINTS 12' in lines + + +def test_write_pcd_xyz_organized_rejects_mismatched_count(tmp_path): + path = tmp_path / 'cloud.pcd' + pts = [(0.0, 0.0, 0.0)] * 12 + with pytest.raises(ValueError, match='width\\*height'): + write_pcd_xyz(path, pts, width=4, height=4) + + +def test_write_pcd_xyz_requires_both_width_and_height(tmp_path): + path = tmp_path / 'cloud.pcd' + pts = [(0.0, 0.0, 0.0)] * 4 + with pytest.raises(ValueError, match='together'): + write_pcd_xyz(path, pts, width=4) + with pytest.raises(ValueError, match='together'): + write_pcd_xyz(path, pts, height=4) + + +def test_write_pcd_xyz_default_is_unorganized(tmp_path): + path = tmp_path / 'cloud.pcd' + write_pcd_xyz(path, [(0.0, 0.0, 0.0), (1.0, 1.0, 1.0)]) + lines = path.read_text().splitlines() + assert 'WIDTH 2' in lines + assert 'HEIGHT 1' in lines + + +# --------------------------------------------------------------------------- +# write_colors_csv +# --------------------------------------------------------------------------- + +def test_write_colors_csv_rows(tmp_path): + path = tmp_path / 'colors.csv' + n = write_colors_csv(path, [(255, 0, 0), (0, 255, 0)]) + assert n == 2 + assert path.read_text().splitlines() == ['255,0,0', '0,255,0'] + + +def test_write_colors_csv_empty(tmp_path): + path = tmp_path / 'colors.csv' + n = write_colors_csv(path, []) + assert n == 0 + assert path.read_text() == '' + + +def test_write_colors_csv_rejects_wrong_shape(tmp_path): + path = tmp_path / 'colors.csv' + with pytest.raises(ValueError, match='Nx3'): + write_colors_csv(path, [(1, 2)]) + + +@pytest.mark.parametrize('bad', [-1, 256, 1000]) +def test_write_colors_csv_rejects_out_of_range(tmp_path, bad): + path = tmp_path / 'colors.csv' + with pytest.raises(ValueError, match='0, 255'): + write_colors_csv(path, [(bad, 0, 0)]) + + +def test_write_colors_csv_boundary_values_ok(tmp_path): + path = tmp_path / 'colors.csv' + write_colors_csv(path, [(0, 255, 128)]) + assert path.read_text().splitlines() == ['0,255,128'] diff --git a/navmap_tools/test/test_pep257.py b/navmap_tools/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/navmap_tools/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/navmap_tools/test/test_pnoa.py b/navmap_tools/test/test_pnoa.py new file mode 100644 index 0000000..8bc5d3a --- /dev/null +++ b/navmap_tools/test/test_pnoa.py @@ -0,0 +1,190 @@ +# Copyright 2026 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. + +""" +Unit tests for navmap_tools.geo.pnoa -- no network involved. + +`load_pnoa_mosaic`'s chunk fetching is monkeypatched so no real WMS request +happens here; the actual download is covered by the end-to-end smoke test +instead (see easynav_gis_tool.md). +""" + +from navmap_tools.geo import pnoa as pnoa_mod +from navmap_tools.geo.pnoa import load_pnoa_mosaic, PnoaMosaic +from navmap_tools.geo.projection import BBox + +import numpy as np + +from PIL import Image + +import pytest + + +# --------------------------------------------------------------------------- +# PnoaMosaic.sample +# --------------------------------------------------------------------------- + +def _solid_mosaic(color, origin_x=-100.0, origin_y=100.0, pixel_size_m=1.0, size=200): + image = np.zeros((size, size, 3), dtype=np.uint8) + image[:, :] = color + return PnoaMosaic(image=image, origin_x=origin_x, origin_y=origin_y, pixel_size_m=pixel_size_m) + + +def test_sample_solid_color_returns_that_color(): + mosaic = _solid_mosaic((10, 20, 30)) + r, g, b = mosaic.sample(0.0, 0.0) # (0, 0) in EPSG:3857, well inside the mosaic + assert (r, g, b) == (10, 20, 30) + + +def test_sample_clamps_outside_mosaic_bounds(): + mosaic = _solid_mosaic((1, 2, 3), origin_x=-1.0, origin_y=1.0, pixel_size_m=0.1, size=4) + r, g, b = mosaic.sample(50.0, 50.0) # far outside the tiny mosaic footprint + assert (r, g, b) == (1, 2, 3) + + +def test_sample_returns_ints_in_range(): + image = np.random.default_rng(0).integers(0, 256, size=(8, 8, 3), dtype=np.uint8) + mosaic = PnoaMosaic(image=image, origin_x=-4.0, origin_y=4.0, pixel_size_m=1.0) + r, g, b = mosaic.sample(0.0, 0.0) + for v in (r, g, b): + assert isinstance(v, int) + assert 0 <= v <= 255 + + +def test_sample_interpolates_between_distinct_neighbors(): + # Two horizontally adjacent columns of different flat colors: a sample + # landing between pixel centers should be a blend, not exactly either. + image = np.zeros((4, 4, 3), dtype=np.uint8) + image[:, :2] = (0, 0, 0) + image[:, 2:] = (200, 200, 200) + mosaic = PnoaMosaic(image=image, origin_x=0.0, origin_y=4.0, pixel_size_m=1.0) + # px = 2.0 lands exactly on the boundary column index -- use px=1.5-ish. + r, g, b = mosaic.sample(*_lonlat_for_px(mosaic, px=1.5, py=1.5)) + assert 0 < r < 200 + + +def _lonlat_for_px(mosaic, px, py): + from pyproj import Transformer + to_lonlat = Transformer.from_crs('EPSG:3857', 'EPSG:4326', always_xy=True) + mx = mosaic.origin_x + px * mosaic.pixel_size_m + my = mosaic.origin_y - py * mosaic.pixel_size_m + return to_lonlat.transform(mx, my) + + +# --------------------------------------------------------------------------- +# load_pnoa_mosaic -- validation +# --------------------------------------------------------------------------- + +def test_load_pnoa_mosaic_rejects_non_positive_resolution(): + bbox = BBox(west=-0.001, south=-0.001, east=0.001, north=0.001) + with pytest.raises(ValueError, match='resolution_m_per_px'): + load_pnoa_mosaic(bbox, cache=None, resolution_m_per_px=0.0) + with pytest.raises(ValueError, match='resolution_m_per_px'): + load_pnoa_mosaic(bbox, cache=None, resolution_m_per_px=-0.25) + + +def test_load_pnoa_mosaic_rejects_too_large_a_mosaic(): + # A large bbox at a fine resolution needs far more than _MAX_TOTAL_PIXELS. + bbox = BBox(west=-1.0, south=-1.0, east=1.0, north=1.0) + with pytest.raises(ValueError, match='px mosaic'): + load_pnoa_mosaic(bbox, cache=None, resolution_m_per_px=0.25) + + +# --------------------------------------------------------------------------- +# load_pnoa_mosaic -- chunk fetching/stitching (network calls monkeypatched) +# --------------------------------------------------------------------------- + +def _fake_fetch_chunk_factory(tmp_path, calls): + def fake_fetch_chunk(x0, y0, x1, y1, width_px, height_px, cache, force=False): + # calls.append is not atomic across threads (ThreadPoolExecutor runs + # these concurrently), so filenames/colors must not depend on + # len(calls) -- derive both from the (deterministic) chunk bbox + # instead. PNG (not JPEG) so the exact pixel values survive the + # round trip through disk. + calls.append((x0, y0, x1, y1, width_px, height_px)) + color = (int(x0) % 256, int(y1) % 256, 0) + img = np.zeros((height_px, width_px, 3), dtype=np.uint8) + img[:, :] = color + path = tmp_path / f'chunk_{x0:.3f}_{y1:.3f}.png' + Image.fromarray(img, mode='RGB').save(path) + return path + + return fake_fetch_chunk + + +def test_load_pnoa_mosaic_single_chunk_for_small_bbox(monkeypatch, tmp_path): + calls = [] + monkeypatch.setattr(pnoa_mod, '_fetch_chunk', _fake_fetch_chunk_factory(tmp_path, calls)) + + bbox = BBox(west=-0.001, south=-0.001, east=0.001, north=0.001) + mosaic = load_pnoa_mosaic(bbox, cache=None, resolution_m_per_px=1.0) + + assert len(calls) == 1 + assert mosaic.pixel_size_m == 1.0 + assert mosaic.image.shape[2] == 3 + + +def test_load_pnoa_mosaic_splits_into_multiple_chunks_when_over_wms_limit(monkeypatch, tmp_path): + calls = [] + monkeypatch.setattr(pnoa_mod, '_fetch_chunk', _fake_fetch_chunk_factory(tmp_path, calls)) + # Force chunking with a small bbox by shrinking the server's own per-call cap. + monkeypatch.setattr(pnoa_mod, '_MAX_WMS_PX', 50) + + bbox = BBox(west=-0.0009, south=-0.0009, east=0.0009, north=0.0009) + mosaic = load_pnoa_mosaic(bbox, cache=None, resolution_m_per_px=1.0) + + assert len(calls) > 1 + total_requested_px = sum(w * h for (_, _, _, _, w, h) in calls) + assert total_requested_px >= mosaic.image.shape[0] * mosaic.image.shape[1] + + +def test_load_pnoa_mosaic_top_left_pixel_matches_first_chunk(monkeypatch, tmp_path): + from pyproj import Transformer + + monkeypatch.setattr(pnoa_mod, '_MAX_WMS_PX', 50) + + to_web_mercator = Transformer.from_crs('EPSG:4326', 'EPSG:3857', always_xy=True) + bbox = BBox(west=-0.0009, south=-0.0009, east=0.0009, north=0.0009) + west_edge, _ = to_web_mercator.transform(bbox.west, bbox.south) + _, north_edge = to_web_mercator.transform(bbox.east, bbox.north) + + def fake_fetch_chunk(x0, y0, x1, y1, width_px, height_px, cache, force=False): + # The chunk covering the mosaic's north-west corner is always + # (row=0, col=0); paint every other chunk a different color so a mixup + # would be visible at pixel (0, 0). + is_top_left = abs(y1 - north_edge) < 1e-6 and abs(x0 - west_edge) < 1e-6 + color = (255, 0, 0) if is_top_left else (0, 255, 0) + img = np.zeros((height_px, width_px, 3), dtype=np.uint8) + img[:, :] = color + path = tmp_path / f'{x0:.1f}_{y1:.1f}.png' + Image.fromarray(img, mode='RGB').save(path) + return path + + monkeypatch.setattr(pnoa_mod, '_fetch_chunk', fake_fetch_chunk) + mosaic = load_pnoa_mosaic(bbox, cache=None, resolution_m_per_px=1.0) + + assert tuple(mosaic.image[0, 0]) == (255, 0, 0) + + +def test_load_pnoa_mosaic_reports_progress_for_many_chunks(monkeypatch, tmp_path, capsys): + calls = [] + monkeypatch.setattr(pnoa_mod, '_fetch_chunk', _fake_fetch_chunk_factory(tmp_path, calls)) + monkeypatch.setattr(pnoa_mod, '_MAX_WMS_PX', 10) + + bbox = BBox(west=-0.0009, south=-0.0009, east=0.0009, north=0.0009) + load_pnoa_mosaic(bbox, cache=None, resolution_m_per_px=1.0) + + assert len(calls) >= 20 + assert 'chunks' in capsys.readouterr().err diff --git a/navmap_tools/test/test_projection.py b/navmap_tools/test/test_projection.py new file mode 100644 index 0000000..d83410a --- /dev/null +++ b/navmap_tools/test/test_projection.py @@ -0,0 +1,145 @@ +# Copyright 2026 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. + +"""Unit tests for navmap_tools.geo.projection -- no network involved.""" + +from navmap_tools.geo.projection import BBox, LocalProjection + +import pytest + + +# --------------------------------------------------------------------------- +# Construction / validation +# --------------------------------------------------------------------------- + +@pytest.mark.parametrize('lat', [-90.0, 0.0, 40.3314, 90.0]) +def test_valid_latitudes_accepted(lat): + LocalProjection(lat, 0.0) + + +@pytest.mark.parametrize('lat', [-90.0001, 90.0001, 1000.0, -1000.0]) +def test_invalid_latitudes_rejected(lat): + with pytest.raises(ValueError, match='latitude'): + LocalProjection(lat, 0.0) + + +@pytest.mark.parametrize('lon', [-180.0, 0.0, 179.9999, 180.0]) +def test_valid_longitudes_accepted(lon): + LocalProjection(0.0, lon) + + +@pytest.mark.parametrize('lon', [-180.0001, 180.0001, 1000.0]) +def test_invalid_longitudes_rejected(lon): + with pytest.raises(ValueError, match='longitude'): + LocalProjection(0.0, lon) + + +# --------------------------------------------------------------------------- +# to_local / to_lonlat +# --------------------------------------------------------------------------- + +@pytest.mark.parametrize( + 'lat,lon', [(0.0, 0.0), (40.3314, -3.8356), (-33.45, -70.66), (89.0, 10.0)]) +def test_center_maps_to_local_origin(lat, lon): + proj = LocalProjection(lat, lon) + x, y = proj.to_local(lon, lat) + assert x == pytest.approx(0.0, abs=1e-6) + assert y == pytest.approx(0.0, abs=1e-6) + + +@pytest.mark.parametrize('lat,lon', [(0.0, 0.0), (40.3314, -3.8356), (-33.45, -70.66)]) +def test_local_origin_maps_to_center(lat, lon): + proj = LocalProjection(lat, lon) + out_lon, out_lat = proj.to_lonlat(0.0, 0.0) + assert out_lon == pytest.approx(lon, abs=1e-9) + assert out_lat == pytest.approx(lat, abs=1e-9) + + +@pytest.mark.parametrize( + 'x,y', [(100.0, 0.0), (0.0, 100.0), (-250.0, 300.0), (0.0, 0.0), (5000.0, -5000.0)] +) +def test_local_to_lonlat_round_trips(x, y): + proj = LocalProjection(40.3314, -3.8356) + lon, lat = proj.to_lonlat(x, y) + x2, y2 = proj.to_local(lon, lat) + assert x2 == pytest.approx(x, abs=1e-3) + assert y2 == pytest.approx(y, abs=1e-3) + + +def test_north_is_positive_y(): + proj = LocalProjection(0.0, 0.0) + _, lat_north = proj.to_lonlat(0.0, 1000.0) + _, lat_south = proj.to_lonlat(0.0, -1000.0) + assert lat_north > lat_south + + +def test_east_is_positive_x(): + proj = LocalProjection(0.0, 0.0) + lon_east, _ = proj.to_lonlat(1000.0, 0.0) + lon_west, _ = proj.to_lonlat(-1000.0, 0.0) + assert lon_east > lon_west + + +# --------------------------------------------------------------------------- +# square_bbox +# --------------------------------------------------------------------------- + +def test_square_bbox_rejects_non_positive_size(): + proj = LocalProjection(0.0, 0.0) + with pytest.raises(ValueError, match='size_m'): + proj.square_bbox(0.0) + with pytest.raises(ValueError, match='size_m'): + proj.square_bbox(-10.0) + + +@pytest.mark.parametrize('margin', [-0.1, 1.0, 1.5]) +def test_square_bbox_rejects_invalid_margin_ratio(margin): + proj = LocalProjection(0.0, 0.0) + with pytest.raises(ValueError, match='margin_ratio'): + proj.square_bbox(100.0, margin_ratio=margin) + + +@pytest.mark.parametrize('samples', [0, 1, 3]) +def test_square_bbox_rejects_too_few_samples(samples): + proj = LocalProjection(0.0, 0.0) + with pytest.raises(ValueError, match='samples'): + proj.square_bbox(100.0, samples=samples) + + +@pytest.mark.parametrize( + 'lat,lon', [(0.0, 0.0), (40.3314, -3.8356), (-33.45, -70.66), (75.0, 20.0)]) +def test_square_bbox_is_well_formed_and_contains_center(lat, lon): + proj = LocalProjection(lat, lon) + bbox = proj.square_bbox(500.0) + assert isinstance(bbox, BBox) + assert bbox.west < bbox.east + assert bbox.south < bbox.north + assert bbox.west <= lon <= bbox.east + assert bbox.south <= lat <= bbox.north + + +def test_square_bbox_grows_with_size(): + proj = LocalProjection(40.0, -3.0) + small = proj.square_bbox(100.0) + large = proj.square_bbox(1000.0) + assert (large.east - large.west) > (small.east - small.west) + assert (large.north - large.south) > (small.north - small.south) + + +def test_square_bbox_grows_with_margin_ratio(): + proj = LocalProjection(40.0, -3.0) + no_margin = proj.square_bbox(500.0, margin_ratio=0.0) + with_margin = proj.square_bbox(500.0, margin_ratio=0.5) + assert (with_margin.east - with_margin.west) > (no_margin.east - no_margin.west) diff --git a/navmap_tools/test/test_scaffold.py b/navmap_tools/test/test_scaffold.py new file mode 100644 index 0000000..e07bc52 --- /dev/null +++ b/navmap_tools/test/test_scaffold.py @@ -0,0 +1,174 @@ +# Copyright 2026 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. + +"""Unit tests for navmap_tools.scaffold -- pure filesystem/templating, no network.""" + +import xml.etree.ElementTree as ET + +from navmap_tools.scaffold import scaffold_world_package, short_name_for, world_paths + +import pytest + + +# --------------------------------------------------------------------------- +# short_name_for +# --------------------------------------------------------------------------- + +@pytest.mark.parametrize( + 'package,expected', + [ + ('urjc_excavation_world', 'urjc_excavation'), + ('gis_40p0000_m3p0000_world', 'gis_40p0000_m3p0000'), + ('no_suffix_here', 'no_suffix_here'), + ('_world', '_world'), # too short to strip without becoming empty + ('world', 'world'), # no "_world" suffix present + ], +) +def test_short_name_for(package, expected): + assert short_name_for(package) == expected + + +# --------------------------------------------------------------------------- +# world_paths +# --------------------------------------------------------------------------- + +def test_world_paths_layout(tmp_path): + paths = world_paths(tmp_path, 'foo_world') + assert paths.package_name == 'foo_world' + assert paths.name == 'foo' + assert paths.root == tmp_path + assert paths.model_dir == tmp_path / 'models' / 'foo' + assert paths.meshes_dir == tmp_path / 'models' / 'foo' / 'meshes' + assert paths.pcd_path == tmp_path / 'maps' / 'foo.pcd' + assert paths.navmap_path == tmp_path / 'maps' / 'foo.navmap' + assert paths.colors_csv_path == tmp_path / 'maps' / 'foo_colors.csv' + assert paths.world_path == tmp_path / 'worlds' / 'foo.world' + assert paths.dae_path == tmp_path / 'models' / 'foo' / 'meshes' / 'foo.dae' + assert paths.stl_path == tmp_path / 'models' / 'foo' / 'meshes' / 'foo.stl' + assert paths.texture_path == tmp_path / 'models' / 'foo' / 'meshes' / 'foo_texture.png' + + +# --------------------------------------------------------------------------- +# scaffold_world_package +# --------------------------------------------------------------------------- + +_IMAGERY_DESC = 'Esri World Imagery, zoom 19 (~0.30 m/px at this latitude)' +_IMAGERY_ATTRIBUTION = ( + 'Imagery (c) Esri, Maxar, Earthstar Geographics, and the GIS User Community.') + + +def test_scaffold_rejects_invalid_package_name(tmp_path): + with pytest.raises(ValueError, match='package name'): + scaffold_world_package(tmp_path, '', 0.0, 0.0, 100.0, 500.0, _IMAGERY_DESC) + with pytest.raises(ValueError, match='package name'): + scaffold_world_package(tmp_path, 'bad name!', 0.0, 0.0, 100.0, 500.0, _IMAGERY_DESC) + + +def test_scaffold_creates_expected_directory_tree(tmp_path): + paths = scaffold_world_package( + tmp_path, 'foo_world', 40.33, -3.83, 120.0, 650.0, _IMAGERY_DESC) + assert (paths.root / 'package.xml').is_file() + assert (paths.root / 'CMakeLists.txt').is_file() + assert (paths.env_hooks_dir / 'foo_world.dsv.in').is_file() + assert (paths.model_dir / 'model.config').is_file() + assert (paths.model_dir / 'model.sdf').is_file() + assert paths.world_path.is_file() + assert (paths.launch_dir / 'foo.launch.py').is_file() + assert (paths.root / 'README.md').is_file() + assert paths.meshes_dir.is_dir() + assert paths.maps_dir.is_dir() + + +def test_scaffold_package_xml_is_valid_xml_with_right_name(tmp_path): + paths = scaffold_world_package(tmp_path, 'foo_world', 0.0, 0.0, 100.0, 0.0, _IMAGERY_DESC) + root = ET.parse(paths.root / 'package.xml').getroot() + assert root.findtext('name') == 'foo_world' + + +def test_scaffold_world_sdf_contains_gps_center(tmp_path): + paths = scaffold_world_package( + tmp_path, 'foo_world', 40.33, -3.83, 100.0, 650.5, _IMAGERY_DESC) + text = paths.world_path.read_text() + assert '40.33' in text + assert '-3.83' in text + assert '650.5' in text + assert 'model://foo' in text + + +def test_scaffold_model_sdf_references_short_name_meshes(tmp_path): + paths = scaffold_world_package(tmp_path, 'foo_world', 0.0, 0.0, 100.0, 0.0, _IMAGERY_DESC) + text = (paths.model_dir / 'model.sdf').read_text() + assert 'model://foo/meshes/foo.stl' in text + assert 'model://foo/meshes/foo.dae' in text + + +def test_scaffold_model_config_mentions_imagery_desc(tmp_path): + paths = scaffold_world_package(tmp_path, 'foo_world', 0.0, 0.0, 100.0, 0.0, _IMAGERY_DESC) + text = (paths.model_dir / 'model.config').read_text() + assert _IMAGERY_DESC in text + + +def test_scaffold_launch_py_is_valid_python(tmp_path): + paths = scaffold_world_package(tmp_path, 'foo_world', 0.0, 0.0, 100.0, 0.0, _IMAGERY_DESC) + launch_path = paths.launch_dir / 'foo.launch.py' + compile(launch_path.read_text(), str(launch_path), 'exec') + + +def test_scaffold_readme_mentions_package_and_center(tmp_path): + paths = scaffold_world_package( + tmp_path, 'foo_world', 40.33, -3.83, 100.0, 0.0, _IMAGERY_DESC) + text = (paths.root / 'README.md').read_text() + assert 'foo_world' in text + assert '40.33' in text + assert '-3.83' in text + + +def test_scaffold_readme_mentions_imagery_desc_and_attribution(tmp_path): + paths = scaffold_world_package( + tmp_path, 'foo_world', 0.0, 0.0, 100.0, 0.0, _IMAGERY_DESC, _IMAGERY_ATTRIBUTION) + text = (paths.root / 'README.md').read_text() + assert _IMAGERY_DESC in text + assert _IMAGERY_ATTRIBUTION in text + + +def test_scaffold_readme_imagery_attribution_defaults_to_empty(tmp_path): + paths = scaffold_world_package(tmp_path, 'foo_world', 0.0, 0.0, 100.0, 0.0, _IMAGERY_DESC) + text = (paths.root / 'README.md').read_text() + assert _IMAGERY_DESC in text + + +def test_scaffold_does_not_touch_maps_dir_contents(tmp_path): + paths = scaffold_world_package(tmp_path, 'foo_world', 0.0, 0.0, 100.0, 0.0, _IMAGERY_DESC) + sentinel = paths.maps_dir / 'foo.pcd' + sentinel.write_text('pretend point cloud') + + # Re-scaffolding (e.g. a later --gazebo-only run into the same + # --output-dir) must not clobber maps/ written by an earlier --navmap run. + scaffold_world_package(tmp_path, 'foo_world', 0.0, 0.0, 100.0, 0.0, _IMAGERY_DESC) + assert sentinel.read_text() == 'pretend point cloud' + + +def test_scaffold_is_idempotent_on_scaffold_files(tmp_path): + paths1 = scaffold_world_package(tmp_path, 'foo_world', 0.0, 0.0, 100.0, 0.0, _IMAGERY_DESC) + text1 = paths1.world_path.read_text() + paths2 = scaffold_world_package(tmp_path, 'foo_world', 0.0, 0.0, 100.0, 0.0, _IMAGERY_DESC) + assert text1 == paths2.world_path.read_text() + + +def test_scaffold_package_without_world_suffix_uses_same_name_for_both(tmp_path): + paths = scaffold_world_package(tmp_path, 'myarea', 0.0, 0.0, 100.0, 0.0, _IMAGERY_DESC) + assert paths.package_name == 'myarea' + assert paths.name == 'myarea' + assert (paths.root / 'models' / 'myarea' / 'model.sdf').is_file() diff --git a/navmap_tools/test/test_terrain.py b/navmap_tools/test/test_terrain.py new file mode 100644 index 0000000..0532082 --- /dev/null +++ b/navmap_tools/test/test_terrain.py @@ -0,0 +1,152 @@ +# Copyright 2026 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. + +""" +Unit tests for navmap_tools.terrain, using fake DEM/imagery sources. + +Fakes only need a `.sample(lon, lat)` method (duck typing), so these tests +don't touch the network or the real Copernicus/Esri data sources at all. +""" + +from navmap_tools.geo.projection import LocalProjection +from navmap_tools.terrain import build_terrain_grid, render_texture + +import numpy as np + +import pytest + + +class _ConstDem: + + def __init__(self, value): + self.value = value + + def sample(self, lon, lat): + return self.value + + +class _ConstImagery: + + def __init__(self, color): + self.color = color + + def sample(self, lon, lat): + return self.color + + +# --------------------------------------------------------------------------- +# build_terrain_grid +# --------------------------------------------------------------------------- + +def test_build_terrain_grid_rejects_non_positive_size(): + proj = LocalProjection(0.0, 0.0) + with pytest.raises(ValueError, match='size_m'): + build_terrain_grid(proj, _ConstDem(0), _ConstImagery((0, 0, 0)), 0.0, 10.0) + + +def test_build_terrain_grid_rejects_non_positive_spacing(): + proj = LocalProjection(0.0, 0.0) + with pytest.raises(ValueError, match='spacing_m'): + build_terrain_grid(proj, _ConstDem(0), _ConstImagery((0, 0, 0)), 100.0, 0.0) + + +def test_build_terrain_grid_shape_matches_size_over_spacing(): + proj = LocalProjection(0.0, 0.0) + grid = build_terrain_grid(proj, _ConstDem(0), _ConstImagery((0, 0, 0)), 100.0, 25.0) + assert grid.elevation.shape == (5, 5) # n = round(100/25) = 4 -> 5 samples/side + assert grid.xs.shape == (5, 5) + assert grid.ys.shape == (5, 5) + assert grid.rgb.shape == (5, 5, 3) + + +def test_build_terrain_grid_constant_dem_zeroes_out_everywhere(): + proj = LocalProjection(40.0, -3.0) + grid = build_terrain_grid(proj, _ConstDem(500.0), _ConstImagery((0, 0, 0)), 100.0, 25.0) + assert grid.center_elevation_amsl == pytest.approx(500.0) + assert np.allclose(grid.elevation, 0.0) + + +def test_build_terrain_grid_center_sample_is_exactly_at_origin(): + proj = LocalProjection(40.0, -3.0) + grid = build_terrain_grid(proj, _ConstDem(123.0), _ConstImagery((1, 2, 3)), 100.0, 25.0) + mid = grid.elevation.shape[0] // 2 + assert grid.xs[mid, mid] == pytest.approx(0.0, abs=1e-6) + assert grid.ys[mid, mid] == pytest.approx(0.0, abs=1e-6) + + +def test_build_terrain_grid_rgb_matches_constant_imagery(): + proj = LocalProjection(0.0, 0.0) + grid = build_terrain_grid(proj, _ConstDem(0.0), _ConstImagery((7, 8, 9)), 100.0, 50.0) + assert grid.rgb.dtype == np.uint8 + assert (grid.rgb == np.array([7, 8, 9], dtype=np.uint8)).all() + + +def test_build_terrain_grid_spacing_m_reflects_actual_grid_step(): + proj = LocalProjection(0.0, 0.0) + grid = build_terrain_grid(proj, _ConstDem(0.0), _ConstImagery((0, 0, 0)), 100.0, 30.0) + n = grid.elevation.shape[0] - 1 + assert grid.spacing_m == pytest.approx(100.0 / n) + + +def test_build_terrain_grid_minimum_size_single_cell(): + proj = LocalProjection(0.0, 0.0) + grid = build_terrain_grid(proj, _ConstDem(0.0), _ConstImagery((0, 0, 0)), 1.0, 100.0) + # spacing >> size -> clamped to at least a 1-cell (2x2 vertex) grid. + assert grid.elevation.shape == (2, 2) + + +# --------------------------------------------------------------------------- +# render_texture +# --------------------------------------------------------------------------- + +def test_render_texture_rejects_non_positive_size(): + proj = LocalProjection(0.0, 0.0) + with pytest.raises(ValueError, match='size_m'): + render_texture(proj, _ConstImagery((0, 0, 0)), 0.0, pixels=8) + + +def test_render_texture_rejects_non_positive_pixels(): + proj = LocalProjection(0.0, 0.0) + with pytest.raises(ValueError, match='pixels'): + render_texture(proj, _ConstImagery((0, 0, 0)), 100.0, pixels=0) + + +def test_render_texture_shape_and_dtype(): + proj = LocalProjection(0.0, 0.0) + tex = render_texture(proj, _ConstImagery((0, 0, 0)), 100.0, pixels=16) + assert tex.shape == (16, 16, 3) + assert tex.dtype == np.uint8 + + +def test_render_texture_constant_imagery_gives_uniform_texture(): + proj = LocalProjection(40.0, -3.0) + tex = render_texture(proj, _ConstImagery((11, 22, 33)), 200.0, pixels=8) + assert (tex == np.array([11, 22, 33], dtype=np.uint8)).all() + + +def test_render_texture_single_pixel(): + proj = LocalProjection(0.0, 0.0) + tex = render_texture(proj, _ConstImagery((1, 2, 3)), 10.0, pixels=1) + assert tex.shape == (1, 1, 3) + + +def test_render_texture_reports_progress_for_large_textures(capsys): + # A large auto-sized texture (see cli.py's _texture_pixels_for, driven by + # --zoom now defaulting to 23) can take a couple of minutes; without this + # a long run looks hung. Threshold is pixels >= 1000. + proj = LocalProjection(0.0, 0.0) + render_texture(proj, _ConstImagery((1, 2, 3)), 10.0, pixels=1000) + err = capsys.readouterr().err + assert 'texture: row' in err diff --git a/navmap_tools/test/test_xmllint.py b/navmap_tools/test/test_xmllint.py new file mode 100644 index 0000000..3e08c02 --- /dev/null +++ b/navmap_tools/test/test_xmllint.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_xmllint.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.xmllint +def test_xmllint() -> None: + rc = main(argv=[]) + assert rc == 0, 'Found code style errors / warnings'