Costmap.h

Go to the documentation of this file.

#pragma once
 
#include <cstddef>
#include <functional>
#include <optional>
#include <utility>
#include <vector>
 
#include <Eigen/Core>
 
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/core/basic_types.h>
 
namespace armarx::navigation::algorithms
{
 
    class Costmap
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        friend class CostmapBuilder;
        friend class SocialCostmapBuilder;
 
        struct Parameters
        {
            // if set to false, distance to obstacles will be computed and not only a binary collision check
            bool binaryGrid{false};
 
            /// How big each cell is in the uniform grid.
            float cellSize = 100.f; // [mm]
 
            // the robot radius used to calculate this costmap
            // e.g. for each cell, the distance to the nearest object is it's value + robotRadius
            float robotRadius = 0.F;
 
            float sceneBoundsMargin = 1000.f; // [mm]
        };
 
        struct Filter
        {
            Eigen::MatrixXf matrix;
            bool useMinimum = false;
 
            inline Filter&
            setUseMinimum(bool value)
            {
                useMinimum = value;
                return *this;
            }
 
            static Filter gaussian(int radius = 2, float sigma = 1.);
        };
 
        using Index = Eigen::Array2i;
        using Position = Eigen::Vector2f;
        using Grid = Eigen::MatrixXf;
 
        // if 0, the cell is invalid
        using Mask = Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>;
 
        Costmap(const Grid& grid,
                const Parameters& parameters,
                const SceneBounds& sceneBounds,
                const std::optional<Mask>& mask = std::nullopt,
                const core::Pose2D& origin = core::Pose2D::Identity());
 
        /**
         * @brief Create a costmap with the same dimensions and parameters as the given one. Only the content (grid and mask) are altered.
         * 
         * @param other The costmap providing the dimensions and parameters
         * @param grid 
         * @param mask 
         * @return Costmap 
         */
        static Costmap WithSameDimsAs(const Costmap& other,
                                      const Grid& grid,
                                      const std::optional<Mask>& mask = std::nullopt);
 
        struct Vertex
        {
            Index index; // row corresponds to y; column corresponds to x
            Position position;
        };
 
        Position toPositionLocal(const Index& index) const;
        Position toPositionGlobal(const Index& index) const;
        std::pair<int, int> toIndexUnsanitized(const Eigen::Vector2f& globalPosition) const;
        std::optional<Costmap::Vertex>
        toVertexOrInvalid(const Eigen::Vector2f& globalPosition) const;
        Vertex toVertex(const Position& globalPosition) const;
 
        const Grid& getGrid() const;
 
        Grid&
        getMutableGrid()
        {
            return grid;
        }
 
        bool isInCollision(const Position& p) const;
 
        const Parameters& params() const noexcept;
 
        // get the scene bounds in the costmap's base frame (aka 'origin')
        const SceneBounds& getLocalSceneBounds() const noexcept;
 
        core::Pose2D
        centerPose() const
        {
            const Eigen::Vector2f costmap_P_center{
                (getLocalSceneBounds().max.x() - getLocalSceneBounds().min.x()) / 2 +
                    getLocalSceneBounds().min.x(),
                (getLocalSceneBounds().max.y() - getLocalSceneBounds().min.y()) / 2 +
                    getLocalSceneBounds().min.y()};
 
            return global_T_costmap * Eigen::Translation2f(costmap_P_center);
        }
 
        struct Optimum
        {
            float value;
            Index index;
            Position position;
        };
 
        Optimum optimum() const;
 
        void filter(const Filter& filter);
 
        bool add(const Costmap& other, float weight = 1.0);
 
        enum class AddMode
        {
            MIN,
            MAX,
            MULTIPLY
        };
 
        bool addCostmapWithSameSizeAndPose(const Costmap& other, AddMode mode);
        bool add(const Costmap& other, AddMode mode, bool keepThis = false);
 
        //! checks whether the cell is masked out
        bool isValid(const Index& index) const noexcept;
 
        const std::optional<Mask>& getMask() const noexcept;
 
        std::optional<Costmap::Mask>& getMutableMask() noexcept;
 
 
        std::optional<float> value(const Index& index) const;
 
        std::optional<float> value(const Position& position) const;
 
        void cutToValidBoundingBox();
 
        void forEachCell(const std::function<void(const Index&)>& f) const;
        void forEachCellParallel(const std::function<void(const Index&)>& f) const;
 
        /**
        * @brief Merge a list of costmaps into this costmap
        * 
        * @param costmaps list of other costmaps
        * @param weights [weights of other costmaps..., weights of this costmap]
        * @return the merged costmap 
        */
        Costmap mergeInto(const std::vector<Costmap>& costmaps,
                          const std::vector<float>& weights) const;
 
        const core::Pose2D&
        origin() const
        {
            return global_T_costmap;
        }
 
        void
        setOrigin(const core::Pose2D& globalPose)
        {
            global_T_costmap = globalPose;
        }
 
        std::size_t numberOfValidElements() const;
 
        void excludeUnreachable(const Position& position);
 
        std::pair<Index, Index> getValidBoundingBox() const;
 
        void validateSizes() const;
 
        /**
         * @brief Find the closest collision-free position to a given position.
         * 
         * Searches in expanding circles up to maxDistance for the closest position
         * that is not in collision (grid value > 0).
         * 
         * @param position The starting position to search from
         * @param maxDistance Maximum search radius [mm]
         * @return std::optional<Vertex> The closest collision-free vertex, or nullopt if none found
         */
        std::optional<Vertex>
        findClosestCollisionFreeVertex(const Position& position, float maxDistance) const;
 
    private:
        Index findClosestValidIndex(const Index& index, int radius = 10);
 
        Grid grid;
        std::optional<Mask> mask;
 
        SceneBounds sceneBounds;
 
        const Parameters parameters;
        core::Pose2D global_T_costmap = core::Pose2D::Identity();
    };
 
 
} // namespace armarx::navigation::algorithms