From f1ce0d5fb79b9081cf1877acc2e2c100cc9db2d2 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Mon, 2 Sep 2024 13:11:12 +0000 Subject: [PATCH] Add windows from layout stride, submdspan, rotatable, polygonal, and save/load from image --- .clang-format | 21 ++ .codespell_words | 0 .pre-commit-config.yaml | 71 ++++ CMakeLists.txt | 14 +- Dockerfile | 8 + README.md | 63 +++- include/tooly/tooly.hpp | 54 --- src/main.cpp | 67 ++-- src/map.cpp | 708 ++++++++++++++++++++++++++++++++++------ test/CMakeLists.txt | 25 -- test/test_tooly.cpp | 6 - 11 files changed, 801 insertions(+), 236 deletions(-) create mode 100644 .clang-format create mode 100644 .codespell_words create mode 100644 .pre-commit-config.yaml delete mode 100644 include/tooly/tooly.hpp delete mode 100644 test/CMakeLists.txt delete mode 100644 test/test_tooly.cpp diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..c9fb916 --- /dev/null +++ b/.clang-format @@ -0,0 +1,21 @@ +--- +BasedOnStyle: Google + +ColumnLimit: 100 +IndentWidth: 2 + +DerivePointerAlignment: false +QualifierAlignment: Right +ReferenceAlignment: Pointer +SpaceAroundPointerQualifiers: Default +PointerAlignment: Left + +IncludeBlocks: Regroup +IncludeCategories: + - Priority: 1 + Regex: '^$' + - Priority: 3 + Regex: '^<(.+)>$' +... diff --git a/.codespell_words b/.codespell_words new file mode 100644 index 0000000..e69de29 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..c6c43b2 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,71 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +exclude: 'third_party/.*' +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v3.4.0 + hooks: + - id: check-added-large-files + args: ['--maxkb=1000'] + - id: check-ast + - id: check-byte-order-marker + - id: check-builtin-literals + - id: check-case-conflict + - id: check-docstring-first + - id: check-executables-have-shebangs + - id: check-json + - id: check-executables-have-shebangs + - id: pretty-format-json + - id: check-merge-conflict + - id: check-symlinks + - id: check-toml + - id: check-vcs-permalinks + - id: check-yaml + - id: debug-statements + - id: destroyed-symlinks + - id: detect-private-key + - id: end-of-file-fixer + - id: mixed-line-ending + - id: fix-byte-order-marker + - id: fix-encoding-pragma + - id: forbid-new-submodules + - id: mixed-line-ending + - id: name-tests-test + - id: no-commit-to-branch + - id: requirements-txt-fixer + - id: sort-simple-yaml + - id: trailing-whitespace + + - repo: https://github.com/psf/black + rev: 22.3.0 + hooks: + - id: black + + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ['-fallback-style=none', '-i'] + + - repo: https://github.com/codespell-project/codespell + rev: v2.0.0 + hooks: + - id: codespell + args: ['--write-changes', '--ignore-words=.codespell_words'] diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c542ac..ec9bc2a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,11 +7,6 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Werror -Wall -Wextra -Wpedantic -Wshadow -Wconversion -Wsign-conversion) endif() -add_library(tooly INTERFACE) -add_library(spanny::tooly ALIAS tooly) -target_include_directories(tooly INTERFACE $) -target_compile_features(tooly INTERFACE cxx_std_20) - add_executable(spanny2 src/main.cpp ) @@ -19,18 +14,15 @@ target_link_libraries(spanny2 PRIVATE serial mdspan expected) target_include_directories(spanny2 INTERFACE $) target_compile_features(spanny2 INTERFACE cxx_std_20) +find_package(OpenCV REQUIRED) + add_executable(mappy src/map.cpp ) -target_link_libraries(mappy PRIVATE serial mdspan expected) +target_link_libraries(mappy PRIVATE serial mdspan expected ${OpenCV_LIBS}) target_include_directories(mappy INTERFACE $) target_compile_features(mappy INTERFACE cxx_std_20) if(NOT PROJECT_IS_TOP_LEVEL) return() endif() - -include(CTest) -if(BUILD_TESTING) - add_subdirectory(test) -endif() diff --git a/Dockerfile b/Dockerfile index 89cd685..0980341 100644 --- a/Dockerfile +++ b/Dockerfile @@ -14,6 +14,13 @@ RUN --mount=type=cache,target=/var/cache/apt,id=apt \ wget \ && rm -rf /var/lib/apt/lists/* +# Get library dependencies +# hadolint ignore=DL3008 +RUN --mount=type=cache,target=/var/cache/apt,id=apt \ + apt-get update -y && apt-get install -q -y --no-install-recommends \ + libopencv-dev \ + && rm -rf /var/lib/apt/lists/* + FROM upstream AS development ARG UID @@ -31,6 +38,7 @@ RUN if [ -z "$USER" ]; then echo '\nERROR: USER not set. Run \n\n \texport USER= RUN --mount=type=cache,target=/var/cache/apt,id=apt \ apt-get update && apt-get upgrade -y \ && apt-get install -q -y --no-install-recommends \ + clang-format \ git \ inkscape \ neovim \ diff --git a/README.md b/README.md index 234ae66..9bfa753 100644 --- a/README.md +++ b/README.md @@ -13,11 +13,68 @@ docker compose -f compose.dev.yml run development ``` Build the repository in the container ```shell -username@spanny-dev:~/ws$ cmake -S src/spanny2/ -B build -username@spanny-dev:~/ws$ cmake --build build +cmake -S src/spanny2/ -B build +cmake --build build ``` # Run ```shell -username@spanny-dev:~/ws$ ./build/spanny2 +username@spanny-dev:~/ws$ ./build/mappy ``` + +# Orphans +```shell +docker compose -f compose.dev.yml down --remove-orphans +``` + +# Title +spanny 2: rise of std::mdspan +# Abstract +C++23 introduced std::mdspan, a multi-dimensional view type that expanded on the capabilities of its predecessors, std::span and std::string_view, by eliminating the restriction to view into contiguous blocks of memory. Multidimensional array-type data structures are commonly used in robotics applications such as RGB and depth images, as well as occupancy grids for navigation. While these alone present a compelling alternative to hand-rolled data structures, this talk will illustrate the flexibility of the layout and accessor policy customization points included in std::mdspan, demonstrating its potential through various examples, including helping a robot arm inventory beer bottles. +This talk will review conventional use cases of std::mdspan in robotics, such as occupancy grids and submaps for determining obstacles and collision checking with polygonal footprints, as well as handling camera images retrieved after deep learning inference. Additionally, we will discuss commanding single and dual robot arms for inventory inspection using synchronous and asynchronous strategies via the std::mdspan accessor policy and std::future. + +While these toy examples may have speculative applications, they highlight the flexibility and extensibility of this new data structure for novel use cases. + +# Outline +## Introduction +- Brief overview of the presentation +- Introduction to std::mdspan in C++23 + +## Motivations for std::mdspan +- Historical background: limitations of previous non-owning types (std::span, std::string_view) +- The need for more flexible view types in C++ + +## Features of std::mdspan +- Multi-dimensional view type without memory layout restrictions +- Customization points: layout and accessor policies + +## Conventional Use Cases in Robotics +- Occupancy grids and cost-maps +- Submaps for navigation and localization +- Polygonal views and arbitrary layouts for collision checking +- Accessor policies interfacing with GPUs (e.g., CUDA calls) + +## Beer Inspection Use Cases +- Synchronous command of a single robot arm +- Synchronous command of dual robot arms +- Asynchronous command of dual robot arms for optimized path planning + +## Conclusion +- Summary of mdspan features and benefits +- Discussion of use cases in fleet management and warehouse inventory control +- Using std::mdspan to access cloud resources asynchronously +- Potential impact on robotics and other fields +- Speculation on future applications and developments + +## References +mdspan proposal +https://www.open-std.org/jtc1/sc22/wg21/docs/papers/2022/p0009r18.html + +Timur Doumler talk +https://youtu.be/eD-ceG-oByA?si=FYk_cG3dNPIHiF2h + +Bryce Lelback talk +https://youtu.be/aFCLmQEkPUw?si=zWybvW7JAOO33yVV + +Three skew rotation +https://youtu.be/1LCEiVDHJmc?si=kjbLFjp8XldHnzO1 diff --git a/include/tooly/tooly.hpp b/include/tooly/tooly.hpp deleted file mode 100644 index 7e17fa0..0000000 --- a/include/tooly/tooly.hpp +++ /dev/null @@ -1,54 +0,0 @@ -#pragma once -#include -#include - -namespace tooly { -/** - * @brief Iterates through all of the adjacent elements in an collection - * @tparam T type held in collection - * @tparam U type help in the resulting collection - * @tparam F type of binary operator - * @param begin - * @param end - * @param op binary operator that receives two T and returns U - * I don't want to mutate the input range, but op should probably be allowed - * to be stateful? or maybe it shouldn't but should we have a collector functor that is? - */ -template -auto transform_window(T const * begin, T const * end, F op) { - // Check that there are at least window_size elements in the collection - if constexpr (window_size > std::distance(begin, end)) { - // don't compile? - } - // Should we take a single span instead of two pointers? - // How will U be deduced? - // Should F be constrained using a concept or std::function? - // Is an output parameter more appropriate here? - // adjacent_difference? - std::vector output; - while (begin != std::prev(end)) { - // requires a function that can return a range of elements from - // current to window_size only known at compile time - // output.push_back(std::apply(op, std::range_n(begin, window_size))); - // begin = std::next(begin); - } - return output; -} - -/** - * @brief Iterates through all of the adjacent elements in an collection - * @tparam T type held in collection - * @tparam U type help in the resulting collection - * @tparam F type of binary operator - * @param begin - * @param end - * @param op binary operator that receives two T and returns U - */ -template -auto transform_adjacent(T const * begin, T const * end, F op) { - // No references means no forwarding? - return transform_window<2>(begin, end, op); -} - - -} // namespace tooly diff --git a/src/main.cpp b/src/main.cpp index 1f9dc99..5d9f0ed 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,18 +1,15 @@ -#include +#include "mdspan.hpp" +#include "serial/serial.h" #include +#include #include #include #include #include -#include "serial/serial.h" -#include "mdspan.hpp" namespace stdex = std::experimental; -enum struct bin_occupancy { - OCCUPIED, - EMPTY -}; +enum struct bin_occupancy { OCCUPIED, EMPTY }; struct bin_coordinate { int x, y; @@ -23,19 +20,16 @@ struct bin_state { bin_coordinate coordinate; }; - bin_coordinate offset_to_coord(int offset, int width = 3) { - int const row = offset / width; - int const col = offset % width; - return {row, col}; + int const row = offset / width; + int const col = offset % width; + return {row, col}; } struct robot_arm { - robot_arm( - std::string port, - uint32_t baudrate, - serial::Timeout timeout = serial::Timeout::simpleTimeout(10000)) - : serial_(std::move(port), baudrate, std::move(timeout)) {} + robot_arm(std::string port, uint32_t baudrate, + serial::Timeout timeout = serial::Timeout::simpleTimeout(10000)) + : serial_(std::move(port), baudrate, std::move(timeout)) {} bin_occupancy is_bin_occupied(int bin) { if (!serial_.isOpen()) { @@ -45,10 +39,10 @@ struct robot_arm { auto const result = std::stoi(serial_.readline()); if (result == 1) { return bin_occupancy::OCCUPIED; - } + } return bin_occupancy::EMPTY; } - + serial::Serial serial_; }; @@ -62,7 +56,8 @@ struct bounds_checked_layout_policy { if (((idxs < 0 || idxs > this->extents().extent(Is)) || ...)) { throw std::out_of_range("Invalid bin index"); } - }(std::make_index_sequence{}); + } + (std::make_index_sequence{}); return this->base_t::operator()(idxs...); } }; @@ -74,22 +69,24 @@ struct bin_checker { using data_handle_type = robot_arm*; reference access(data_handle_type ptr, std::ptrdiff_t offset) const { - // We know ptr will be valid asynchronously because we construct it on the stack - // of main. In real code we might want to use a shared_ptr or something - return std::async([=]{ - return bin_state{ptr->is_bin_occupied(static_cast(offset)), offset_to_coord(static_cast(offset))}; + // We know ptr will be valid asynchronously because we construct it on the + // stack of main. In real code we might want to use a shared_ptr or + // something + return std::async([=] { + return bin_state{ptr->is_bin_occupied(static_cast(offset)), + offset_to_coord(static_cast(offset))}; }); } }; -using bin_view = stdex::mdspan, - // Our layout should do bounds-checking - bounds_checked_layout_policy, - // Our accessor should tell the robot to asynchronously access the bin - bin_checker ->; +using bin_view = stdex::mdspan, + // Our layout should do bounds-checking + bounds_checked_layout_policy, + // Our accessor should tell the robot to + // asynchronously access the bin + bin_checker>; auto print_state = [](bin_state const& state) { std::cout << "Bin at (" << state.coordinate.x << ", " << state.coordinate.y << ") is "; @@ -107,18 +104,18 @@ auto print_state = [](bin_state const& state) { int main() { auto arm = robot_arm{"/dev/ttyACM0", 9600}; auto bins = bin_view(&arm); - while(true) { + while (true) { std::vector> futures; for (auto i = 0u; i < bins.extent(0); ++i) { for (auto j = 0u; j < bins.extent(1); ++j) { - futures.push_back(bins(i, j)); + futures.push_back(bins(i, j)); } } for (auto const& future : futures) { - future.wait(); + future.wait(); } for (auto& future : futures) { - print_state(future.get()); + print_state(future.get()); } std::cout << "====================" << std::endl; } diff --git a/src/map.cpp b/src/map.cpp index 087d69a..ed568c6 100644 --- a/src/map.cpp +++ b/src/map.cpp @@ -1,131 +1,635 @@ // Interface to ingest data into a map // and then present submaps based on a central pose // basically how local costmaps work. +#include + +#include "expected.hpp" +#include "mdspan.hpp" #include #include +#include +#include #include #include -#include "mdspan.hpp" +#include +#include namespace stdex = std::experimental; -struct coordinate { +struct coordinate_t { std::size_t x, y; }; -struct window { +struct window_t { std::size_t height, width; }; -// struct bounds_checked_layout_policy { -// template -// struct mapping : stdex::layout_right::mapping { -// using base_t = stdex::layout_right::mapping; -// using base_t::base_t; -// std::ptrdiff_t operator()(auto... idxs) const { -// [&](std::index_sequence) { -// if (((idxs < 0 || idxs > this->extents().extent(Is)) || ...)) { -// throw std::out_of_range("Invalid bin index"); -// } -// }(std::make_index_sequence{}); -// return this->base_t::operator()(idxs...); -// } -// }; -// }; - -// std::size_t coord_to_offset(coordinate const& coord) { -// int const row = offset / width; -// int const col = offset % width; -// return {row, col}; -// } - -struct window_accessor { - using element_type = int; - using reference = int*; - using data_handle_type = int*; - - reference access(data_handle_type ptr, std::ptrdiff_t offset) const { - // Must change this so the window accessor actually travels int the 2d array in the main map - return ptr + offset; +namespace geometry { + +struct Cell { + int x; ///< The x coordinate of the cell + int y; ///< The y coordinate of the cell +}; + +std::vector> bresenham_conversion(int x0, int y0, int x1, int y1) { + // Vector of points + std::vector> v; + + // Beginning point + int const p0[2] = {x0, y0}; + + // Ending point + int const p1[2] = {x1, y1}; + + // Change in x and y from beginning to end + int const deltas[2] = {(x1 - x0), (y1 - y0)}; + + // Absolute values of the changes + int const changes[2] = {abs(deltas[0]), abs(deltas[1])}; + + // Lambda that emulates signum + auto const sgn = [](auto a) { return (a > 0) - (a < 0); }; + + // Which direction to step towards for the x and y functions + int const steps[2] = {sgn(deltas[0]), sgn(deltas[1])}; + + // Array that dictates the dominant direction + int idx[2] = {0}; + + if (changes[0] > changes[1]) { + idx[0] = 0; + idx[1] = 1; + } else { + idx[0] = 1; + idx[1] = 0; } + + // error scaled off of change in dominant direction + float err = static_cast(changes[idx[0]]) / 2.f; + + // points that will be pushed back + int point[2] = {p0[0], p0[1]}; + + // while the dominant direction coordinate isn't equal to the ending + // coordinate + while (point[idx[0]] != p1[idx[0]]) { + // Create and push back vector into final vector + std::vector const temp{point[0], point[1]}; + + v.push_back(temp); + + // bresenham magic + err -= static_cast(changes[idx[1]]); + + if (err < 0) { + point[idx[1]] += steps[idx[1]]; + + err += static_cast(changes[idx[0]]); + } + + point[idx[0]] += steps[idx[0]]; + } + + // Push final point + v.push_back({p0[0], p1[1]}); + + return v; +} + +std::vector bresenham_conversion(Cell const& start, Cell const& end) { + auto const pixels = bresenham_conversion(start.x, start.y, end.x, end.y); + + std::vector cells; + std::transform(std::begin(pixels), std::end(pixels), std::back_inserter(cells), + [](auto const& pixel) -> Cell { + return {pixel[0], pixel[1]}; + }); + return cells; +} + +std::vector raytrace(Cell const& start, Cell const& end, int max_length) { + if (max_length < 1) { + return {}; + } + // Get the interpolated pixels bresenham + auto const pixels = bresenham_conversion(start, end); + + return {pixels.begin(), pixels.begin() + std::min(static_cast(pixels.size()), max_length)}; +} + +std::vector polygonOutline(std::vector const& polygon, int size_x) { + // If the input is less than or equal to one element, return the input + if (polygon.size() <= 1) { + return polygon; + } + + // The output vector of cells + std::vector out; + + // Iterator to the next vertex + auto next_vrtx = polygon.begin(); + + for (auto const& vertex : polygon) { + // If the next vertex is not the end iterator, advance the next_vertex to + // point to the next iterator + if (next_vrtx != polygon.end()) { + std::advance(next_vrtx, 1); + } + // After advancing, if the iterator points to the end, go back to the + // beginning, essentially closing the polygon + if (next_vrtx == polygon.end()) { + next_vrtx = polygon.begin(); + } + + // Generate the pixels from one vertex to the next + auto temp = raytrace(vertex, (*next_vrtx), size_x); + + // Remove the last element of the vector because the next vertex will use it + // as a starting point i.e. don't duplicate pixels + temp.pop_back(); + + // Push the pixels into the out vector + for (auto const& pixel : temp) { + out.push_back(pixel); + } + } + return out; +} + +std::vector convexFill(std::vector const& polygon, window_t const& bounds) { + // we need a minimum polygon of a triangle + if (polygon.size() < 3) return polygon; + + auto outline = polygonOutline(polygon, std::numeric_limits::max()); + // Clamp each pixel to the bounds + std::for_each(outline.begin(), outline.end(), [&](auto& pixel) { + pixel.x = std::clamp(pixel.x, 0, static_cast(bounds.width)); + pixel.y = std::clamp(pixel.y, 0, static_cast(bounds.height)); + }); + + std::unordered_map> extrema; + + // Find all the x values and extrema + for (auto const pixel : outline) { + // Check if the x value for the pixel is in extrema + if (extrema.find(pixel.x) == extrema.end()) { + // if it isn't in extrema push the x in and with the y value as both the + // min and the max + extrema[pixel.x] = {pixel.y, pixel.y}; + } else if (pixel.y < extrema.at(pixel.x).first) { // If it get here that must mean + // the x value was found. + // if it is in extrema, check the y value. If the y value is lower than + // the minima, replace the first value, else if it is greater than the + // maxima, replace the second value, else do nothing + extrema.at(pixel.x).first = pixel.y; + } else if (pixel.y > extrema.at(pixel.x).second) { + extrema.at(pixel.x).second = pixel.y; + } + } + + // Output vector + std::vector out; + + // lambda function + auto const fill_cells = [&out](auto const& extremum) { + for (auto lower_bound = (extremum.second).first; lower_bound <= (extremum.second).second; + lower_bound++) { + out.push_back({extremum.first, lower_bound}); + } + }; + + // push all the cells in between the minima and maxima into the out vector + std::for_each(extrema.begin(), extrema.end(), fill_cells); + + return out; +} + +double degrees_to_radians(double degrees) { return degrees * std::numbers::pi / 180.; } + +} // namespace geometry + +struct bounds_checked_layout_policy { + template + struct mapping : std::layout_right::mapping { + using base_t = std::layout_right::mapping; + using base_t::base_t; + std::ptrdiff_t operator()(auto... idxs) const { + [&](std::index_sequence) { + if (((idxs < 0 || idxs > this->extents().extent(Is)) || ...)) { + throw std::out_of_range("Invalid bin index"); + } + } + (std::make_index_sequence{}); + return this->base_t::operator()(idxs...); + } + }; +}; + +struct layout_rotatable { + template + requires(Extents::rank() == 2) struct mapping { + using extents_type = Extents; + using size_type = typename extents_type::size_type; + + constexpr mapping(extents_type parent_shape, extents_type shape, coordinate_t translation, + double theta) + : shape_{std::move(shape)}, + translation_{std::move(translation)}, + theta_{theta}, + parent_shape_{std::move(parent_shape)} {} + + constexpr extents_type const& extents() const noexcept { return shape_; } + + constexpr size_type operator()(size_type x, size_type y) const noexcept { + // Use the three skew algorithm to rotate the elements + // because a normal transformation matrix produces holes and off elements + double const alpha = -std::tan(theta_ / 2.); + double const beta = std::sin(theta_); + auto tx = static_cast(x); + auto ty = static_cast(y); + tx += std::round(alpha * ty); + ty += std::round(beta * tx); + tx += std::round(alpha * ty); + x = static_cast(std::round(tx)) + translation_.x; + y = static_cast(std::round(ty)) + translation_.y; + return x + y * parent_shape_.extent(1); + } + + constexpr size_type required_span_size() const noexcept { + return shape_.extent(0) * shape_.extent(1); + } + + static constexpr bool is_always_unique() noexcept { return false; } + static constexpr bool is_always_exhaustive() noexcept { return true; } + static constexpr bool is_always_strided() noexcept { return false; } + + constexpr bool is_unique() const noexcept { return false; } + constexpr bool is_exhaustive() const noexcept { return true; } + constexpr bool is_strided() const noexcept { return false; } + + private: + extents_type shape_; + coordinate_t translation_; + double theta_; + extents_type parent_shape_; + }; }; -using submap = stdex::mdspan, - // Our layout should do bounds-checking - stdex::layout_right, - window_accessor ->; - -template -struct grid_2d { - // If the grid takes an array as a pointer, then this is really just another view type. - // Which, is maybe fine, but then it might be better for this class to own that data - explicit grid_2d(int value = 0) { - // initialize the mdspan - std::fill(data_.begin(), data_.end(), value); - } - - stdex::mdspan> map() { - return stdex::mdspan(data_.data(), N, M); - } - - // what we want to do here is return an mdspan that is a particular - // window size centered around a location - // how do you return an mdspan of a particular window? - // do you have to specify the window size at compile time? - // eventually, we want a version that takes a metric position rather - // than a grid coordinate - // stdex::mdspan> window(window const& window, coordinate const& coord) { - // Check if the grid coordinates are in bounds - // auto const offset = coord.x * N + coord.y; - // auto first = data_.begin(); - // std::advance(first, offset); - // return stdex::mdspan(first, window.height, window.width); - // } - - stdex::mdspan> window(window const& win, coordinate const& coord) { - // Calculate the start row and column to keep the window centered - std::size_t start_row = (coord.x >= win.height / 2) ? coord.x - win.height / 2 : 0; - std::size_t start_col = (coord.y >= win.width / 2) ? coord.y - win.width / 2 : 0; - - // Ensure the window does not go out of bounds - if (start_row + win.height > N) start_row = N - win.height; - if (start_col + win.width > M) start_col = M - win.width; - - return stdex::mdspan>( - data_.data() + start_row * M + start_col, win.height, win.width); - } - -private: - std::array data_; +struct layout_polygonal { + template + requires(Extents::rank() == 1) struct mapping { + using extents_type = Extents; + using size_type = typename extents_type::size_type; + + constexpr mapping(std::dextents parent_shape, + std::vector vertices, geometry::Cell const& p, double theta) + : parent_shape_{std::move(parent_shape)}, + polygon_{[&] { + for (auto& v : vertices) { + auto x = static_cast(v.x * std::cos(theta) - v.y * std::sin(theta) + p.x); + auto y = static_cast(v.x * std::sin(theta) + v.y * std::cos(theta) + p.y); + v.x = x; + v.y = y; + } + // create filled footprint + return geometry::convexFill(vertices, + {parent_shape_.extent(0), parent_shape_.extent(1)}); + }()}, + extents_{polygon_.size()} {} + + constexpr extents_type const& extents() const noexcept { return extents_; } + + constexpr size_type operator()(size_type i) const noexcept { + auto const& coord = polygon_[i]; + auto const x = static_cast(coord.x); + auto const y = static_cast(coord.y); + + auto const offset = x + y * parent_shape_.extent(1); + return offset; + } + + constexpr size_type required_span_size() const noexcept { return polygon_.size(); } + + static constexpr bool is_always_unique() noexcept { return false; } + static constexpr bool is_always_exhaustive() noexcept { return true; } + static constexpr bool is_always_strided() noexcept { return false; } + + constexpr bool is_unique() const noexcept { return false; } + constexpr bool is_exhaustive() const noexcept { return true; } + constexpr bool is_strided() const noexcept { return false; } + private: + std::dextents parent_shape_; + std::vector polygon_; + extents_type extents_; + }; }; -int main (int /* argc */, char** /* argv[] */) { - auto map = grid_2d<30, 30>{}; - auto whole = map.map(); - for (auto i = 0u; i != whole.extent(0); i++) { - for (auto j = 0u; j != whole.extent(1); j++) { - std::cout << whole(i, j); +struct occupancy_grid_t { + occupancy_grid_t(std::dextents shape, unsigned char value = 127) + : data_(shape.extent(0) * shape.extent(1), value), grid_{data_.data(), shape} {} + + occupancy_grid_t(std::dextents shape, std::vector values) + : data_{std::move(values)}, grid_{data_.data(), shape} {} + + std::mdspan, std::layout_stride> + window_from_layout_right(std::dextents shape, coordinate_t upper_left) { + auto const height = grid_.extent(0); + auto const width = grid_.extent(1); + // Constrain the window to the grid map + upper_left.x = std::clamp(upper_left.x, std::size_t{0}, width); + upper_left.y = std::clamp(upper_left.y, std::size_t{0}, height); + + // Shrink window if it goes out of bounds + shape = std::dextents{ + std::clamp(shape.extent(0), std::size_t{0}, height - upper_left.y), + std::clamp(shape.extent(1), std::size_t{0}, width - upper_left.x)}; + // spacing between elements, spacing between rows + // 0 means all of the elements in a row are on top of each other + // spacing between rows needs to match the width of the big map + auto const row = upper_left.y * width; + auto const col = upper_left.x; + auto first = data_.data(); + std::advance(first, row + col); + return {first, std::layout_right::mapping{shape}}; + } + + /** + * @brief Produces a window that will read from and write to the occupancy + * grid + * @param size is the height and width of the window + * @param upper_left corner of the window in the occupancy grid + * @note + * ┌─────────────────┐ + * │ w │ + * │ ◄───────► │ + * │ ▲ ┌─┬─┬─┬─┐ │ + * │ │ │c│ │ │ │ │ + * │ │ ├─├─┼─┼─┤ │ + * │ h│ │ │ │ │ │ │ + * │ │ ├─┼─┼─┼─┤ │ + * │ │ │ │ │ │ │ │ + * │ ▼ └─┴─┴─┴─┘ │ + * │ │ + * │ │ + * └─────────────────┘ + * shows the window parameters in the context of the larger occupancy grid + * + * A strided window can be understood as the following: + * + * window<2, 4> = {{0, 1, 2, 3}, ▲ + * {4, 5, 6, 7}} │ m + * ◄─────────► ▼ + * n + * m x n window + * m is the number of rows in the window + * n is the number of elements in a row + * + * + * auto const layout = std::layout_stride::mapping{{n, m}, {s0, s1}}; + * + * + * s0 s0 s0 + * ┌──┬──┬──┬──┬──┬───┬──┬───┬────┬──┬────┬───┬───┐ + * │x1├─►│x2├─►│x3│...│xn│...│xn+1├─►│xn+2│...│xnm│ + * └──┴──┴──┴──┴──┴───┴──┴───┴────┴──┴────┴───┴───┘ + * ──────────────────────► + * s1 + * + * 2x4 window mapped into a 4x8 parent grid + * with strides of: + * + * s0 = 1, s1 = 8 s0 = 2, s1 = 8 + * ┌─┬─┬─┬─┬─┬─┬─┬─┐ ┌─┬─┬─┬─┬─┬─┬─┬─┐ + * │0│1│2│3│ │ │ │ │ │0│ │1│ │2│ │3│ │ + * ├─┼─┼─┼─┼─┼─┼─┼─┤ ├─┼─┼─┼─┼─┼─┼─┼─┤ + * │4│5│6│7│ │ │ │ │ │4│ │5│ │6│ │7│ │ + * ├─┼─┼─┼─┼─┼─┼─┼─┤ ├─┼─┼─┼─┼─┼─┼─┼─┤ + * │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ + * ├─┼─┼─┼─┼─┼─┼─┼─┤ ├─┼─┼─┼─┼─┼─┼─┼─┤ + * │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ + * └─┴─┴─┴─┴─┴─┴─┴─┘ └─┴─┴─┴─┴─┴─┴─┴─┘ + * + * s0 = 1, s1 = 16 s0 = 1, s1 = 6 + * ┌─┬─┬─┬─┬─┬─┬─┬─┐ ┌─┬─┬─┬─┬─┬─┬─┬─┐ + * │0│1│2│3│ │ │ │ │ │0│1│2│3│ │ │4│5│ + * ├─┼─┼─┼─┼─┼─┼─┼─┤ ├─┼─┼─┼─┼─┼─┼─┼─┤ + * │ │ │ │ │ │ │ │ │ │6│7│ │ │ │ │ │ │ + * ├─┼─┼─┼─┼─┼─┼─┼─┤ ├─┼─┼─┼─┼─┼─┼─┼─┤ + * │4│5│6│7│ │ │ │ │ │ │ │ │ │ │ │ │ │ + * ├─┼─┼─┼─┼─┼─┼─┼─┤ ├─┼─┼─┼─┼─┼─┼─┼─┤ + * │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ + * └─┴─┴─┴─┴─┴─┴─┴─┘ └─┴─┴─┴─┴─┴─┴─┴─┘ + * + * @returns requested window. If window would go out of bounds, a smaller + * inbounds window is returned. + */ + + std::mdspan, std::layout_stride> + window_from_layout_stride(std::dextents shape, coordinate_t upper_left) { + auto const height = grid_.extent(0); + auto const width = grid_.extent(1); + // Constrain the window to the grid map + upper_left.x = std::clamp(upper_left.x, std::size_t{0}, width); + upper_left.y = std::clamp(upper_left.y, std::size_t{0}, height); + + // Shrink window if it goes out of bounds + shape = std::dextents{ + std::clamp(shape.extent(0), std::size_t{0}, height - upper_left.y), + std::clamp(shape.extent(1), std::size_t{0}, width - upper_left.x)}; + + auto const strides = std::array{1, width}; + auto const layout = std::layout_stride::mapping{shape, strides}; + + // Get pointer to starting element + auto const row = upper_left.y * width; + auto const col = upper_left.x; + auto first = data_.data(); + std::advance(first, row + col); + return {first, layout}; + } + + std::mdspan, std::layout_stride> window_submdspan( + std::dextents shape, coordinate_t upper_left) { + auto const height = grid_.extent(0); + auto const width = grid_.extent(1); + // Constrain the window to the grid map + upper_left.x = std::clamp(upper_left.x, std::size_t{0}, width); + upper_left.y = std::clamp(upper_left.y, std::size_t{0}, height); + + // Shrink window if it goes out of bounds + shape = std::dextents{ + std::clamp(shape.extent(0), std::size_t{0}, height - upper_left.y), + std::clamp(shape.extent(1), std::size_t{0}, width - upper_left.x)}; + // the primy layout can be used for a transformed mdspan... i think + auto const rows = std::tuple{upper_left.y, upper_left.y + shape.extent(1)}; + auto const cols = std::tuple{upper_left.x, upper_left.x + shape.extent(0)}; + return stdex::submdspan(grid_, rows, cols); + } + + std::mdspan, layout_rotatable> window_rotatable( + std::dextents shape, coordinate_t upper_left, double theta) { + layout_rotatable::mapping layout_window{grid_.extents(), shape, upper_left, theta}; + return {data_.data(), layout_window}; + } + std::mdspan, layout_polygonal> window_polygonal( + std::vector const& vertices, geometry::Cell const& p, double theta) { + layout_polygonal::mapping> footprint{grid_.extents(), vertices, p, + theta}; + return {data_.data(), footprint}; + } + + unsigned char* data() { return data_.data(); } + + std::dextents extents() { return grid_.extents(); } + + private: + friend std::ostream& operator<<(std::ostream& os, occupancy_grid_t const& occ) { + for (auto i = 0u; i != occ.grid_.extent(0); i++) { + for (auto j = 0u; j != occ.grid_.extent(1); j++) { + auto const value = occ.grid_(i, j); + // Map value from 0-255 to grayscale range 232-255 + int const grayscale_value = 232 + (value * 24 / 256); + std::cout << "\033[48;5;" << grayscale_value << "m \033[0m"; // Print a grayscale block + } + os << "\n"; + } + return os; + } + std::vector data_; + std::mdspan> grid_; +}; + +template +requires(Container::extents_type::rank() == 1) bool is_occupied(Container const& footprint) { + for (auto i = 0u; i != footprint.extent(0); i++) { + if (footprint(i) != 0) { + return true; } - std::cout << "\n"; - } - std::cout << "===\n"; - auto window = map.window({4, 4}, {10, 10}); - std::cout << "window.extent = " << window.extent(0) << ", " << window.extent(1) << "\n"; - for (auto i = 0u; i != window.extent(0); i++) { - for (auto j = 0u; j != window.extent(1); j++) { - window(i, j) = 1; + } + return false; +} + +template +requires(Container::extents_type::rank() == 2) bool is_occupied(Container const& footprint) { + for (auto i = 0u; i != footprint.extent(0); i++) { + for (auto j = 0u; j != footprint.extent(1); j++) { + if (footprint(i, j) != 0) { + return true; + } } } - // std::fill(window, 1); - for (auto i = 0u; i != whole.extent(0); i++) { - for (auto j = 0u; j != whole.extent(1); j++) { - std::cout << whole(i, j); + return false; +} + +template +requires(Container::extents_type::rank() == + 1) void set(Container& footprint, typename Container::value_type const& value) { + for (auto i = 0u; i != footprint.extent(0); i++) { + footprint(i) = value; + } +} + +template +requires(Container::extents_type::rank() == 2) // + void set(Container& footprint, typename Container::value_type const& value) { + for (auto i = 0u; i != footprint.extent(0); i++) { + for (auto j = 0u; j != footprint.extent(1); j++) { + footprint(i, j) = value; } - std::cout << "\n"; } +} + +void save_occupancy_grid(std::filesystem::path const& filename, occupancy_grid_t& grid) { + auto const shape = grid.extents(); + // Create a 8-bit unsigned single-channel grayscale cv::Mat + cv::Mat const img(static_cast(shape.extent(0)), static_cast(shape.extent(1)), + CV_8UC1, grid.data()); + + cv::imwrite(filename, img); +} + +[[nodiscard]] tl::expected load_occupancy_grid( + std::filesystem::path const& filename) { + if (!std::filesystem::exists(filename)) { + return tl::unexpected{std::string{"Could not find file: "} + filename.string()}; + } + cv::Mat image = cv::imread(filename, CV_8UC1); + if (image.empty()) { + return tl::unexpected{std::string{"Failed to load occupancy grid from file: "} + + filename.string()}; + } + return occupancy_grid_t{ + std::dextents{image.rows, image.cols}, + std::vector{image.begin(), image.end()}}; +} + +int main(int /* argc */, char** /* argv[] */) { + auto global_map = occupancy_grid_t{std::dextents{20, 30}, 0}; + auto map_maybe = load_occupancy_grid("/home/griswald/ws/occupancy_grid.png"); + if (!map_maybe.has_value()) { + std::cout << map_maybe.error() << "\n"; + } else { + global_map = map_maybe.value(); + } + + std::cout << global_map << "\n"; + // This one doesn't work. Show this as the negative example (both layout_right + // and layout_left) + auto window_bad = + global_map.window_from_layout_right(std::dextents{5, 4}, {10, 15}); + set(window_bad, 100); + // This works and uses layout_stride. + // Show how this can also do multistride + // Note the slide from Bryce Lelbach and the data[i * M + j] vs data[i * X + j + // * Y] This doesn't feel quite right and need to explain why layout_left != + // layout_stride for this simple case + auto window_good = + global_map.window_from_layout_stride(std::dextents{4, 4}, {20, 1}); + set(window_good, 200); + std::cout << global_map << "\n"; + // But for the local costmap example, using submdspan is even easier and + // simpler to grok not needing to mess with pointer offsets + auto local_map = global_map.window_submdspan(std::dextents{4, 4}, {10, 5}); + set(local_map, 3); + + // Character to display that changes every loop + unsigned char c = 200; + // // create footprint + // // rotates around upper left corner + std::vector const vertices = {{0, 0}, {3, 0}, {3, 3}, {0, 3}}; + // // rotates somewhere around center + // // std::vector vertices = {{-2,-2},{3,-2},{3,3},{-2,3}}; + geometry::Cell const p = {3, 3}; + for (auto const& angle : {0, 12, 22, 30, 45, 58, 67, 79, 90}) { + double const theta = geometry::degrees_to_radians(angle); + + auto footprint = global_map.window_polygonal(vertices, p, theta); + std::cout << "footprint is_occupied = " << is_occupied(footprint) << "\n"; + set(footprint, c); + std::cout << "footprint is_occupied = " << is_occupied(footprint) << "\n"; + + auto rotatable = + global_map.window_rotatable(std::dextents{4, 4}, {10, 10}, theta); + set(rotatable, c); + + // What's interesting is that if you try to use + // std::fill(window.data_handle(), window.data_handle() + offset, 1); + // then instead of it filling the submap, it just fills the linear range. + // I guess that's because in this case, the data handle is just an int* + // and not an iterator. So if you specified/created a type for data + // data_handle that behaved more like an iterator, this would work. Though, + // you would probably have to do a begin and end, and not add an offset. + // + // print map + std::cout << global_map << std::endl; + // Clear footprint + set(footprint, 0); + // Clear footprint + set(rotatable, 0); + std::cin.get(); + } + + save_occupancy_grid("occupancy_grid.png", global_map); return 0; } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt deleted file mode 100644 index 7c67913..0000000 --- a/test/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}) -include(FetchContent) -FetchContent_Declare( - googletest - URL https://github.com/google/googletest/archive/03597a01ee50ed33e9dfd640b249b4be3799d395.zip -) -FetchContent_MakeAvailable(googletest) -set_target_properties(gtest PROPERTIES COMPILE_OPTIONS "" EXPORT_COMPILE_COMMANDS OFF) -set_target_properties(gtest_main PROPERTIES COMPILE_OPTIONS "" EXPORT_COMPILE_COMMANDS OFF) -set_target_properties(gmock PROPERTIES COMPILE_OPTIONS "" EXPORT_COMPILE_COMMANDS OFF) -set_target_properties(gmock_main PROPERTIES EXPORT_COMPILE_COMMANDS OFF) - -enable_testing() - -add_executable(test_spanny - test_tooly.cpp -) -target_link_libraries(test_spanny PRIVATE - GTest::gtest_main - GTest::gmock_main - spanny::tooly -) - -include(GoogleTest) -gtest_discover_tests(test_spanny) diff --git a/test/test_tooly.cpp b/test/test_tooly.cpp deleted file mode 100644 index d61b7a3..0000000 --- a/test/test_tooly.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include -#include "tooly/tooly.hpp" - -TEST(TransformRange, Window) { - EXPECT_TRUE(true); -}