Costmap3D.h

Go to the documentation of this file.

#pragma once
 
#include <cmath>
#include <cstddef>
#include <optional>
#include <string>
#include <utility>
#include <vector>
 
#include <Eigen/Core>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/core/basic_types.h>
 
namespace armarx::navigation::algorithms::orientation_aware
{
    namespace smoothing
    {
        class Costmap3DWrapper;
    } // namespace smoothing
 
    class Costmap3D
    {
    public:
        friend class Costmap3DBuilder;
        friend class smoothing::Costmap3DWrapper;
 
        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]
 
            /// How many orientations of the robot each cell contains
            int orientations = 72; // 360 deg / 72 = 5 deg discretization
 
            float sceneBoundsMargin = 1000.F; // [mm]
 
            struct RobotModel
            {
                std::string rootNode = "";
                std::string packageName = "";
                std::string relativePath = "";
            } robotModel;
        };
 
        using Index = Eigen::Array2i;
        using Position = Eigen::Vector2f;
 
        // Simpler Grid version, but probably much slower than storing the orientations together
        // each vector element corresponds to one orientation
        using Grid = std::vector<Eigen::MatrixXf>;
 
        using RotationIndex = int;
        using RotationDegrees = float;
 
        // if 0, the cell is invalid
        using Mask = Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>;
 
        Costmap3D(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 Costmap3D with the same dimensions and parameters as the given one. Only the content (grid and mask) are altered.
         * 
         * @param other The Costmap3D providing the dimensions and parameters
         * @param grid 
         * @param mask 
         * @return Costmap3D 
         */
        static Costmap3D WithSameDimsAs(const Costmap3D& 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;
        Vertex toVertex(const Position& globalPosition) const;
 
        struct Rotation
        {
            RotationIndex index;
            RotationDegrees degrees;
        };
 
        Rotation rotationFromIndex(RotationIndex index) const;
 
        Rotation closestRotationFromDegrees(RotationDegrees degrees) const;
 
        /**
         * @brief The two orientation slices bracketing the given continuous rotation.
         *
         * If the rotation lies exactly on a slice, that slice and its successor are returned.
         * The second slice wraps around to index 0 for rotations in the last segment.
         */
        std::pair<Rotation, Rotation> bracketingRotationsFromDegrees(RotationDegrees degrees) const;
 
        Eigen::Rotation2Df
        rotationLocal(const RotationDegrees degrees) const
        {
            return Eigen::Rotation2Df{degrees * static_cast<float>(M_PI) / 180.F};
        }
 
        core::Pose2D
        globalPose(const Index& index, const RotationDegrees& rotationDeg) const
        {
            Eigen::Rotation2Df rot{rotationDeg * static_cast<float>(M_PI) / 180.F};
            return global_T_Costmap3D * Eigen::Translation2f(toPositionLocal(index)) * rot;
        }
 
        RotationDegrees
        rotationDegrees(const core::Pose2D& pose) const
        {
            return Eigen::Rotation2Df{pose.linear()}.angle() * 180.F / M_PI;
        }
 
        constexpr static Rotation ALL_ORIENTATIONS = {.index = -1, .degrees = -1};
 
        const Grid& getGrid() const;
 
        Grid&
        getMutableGrid()
        {
            return grid;
        }
 
        bool isInCollision(const Position& p,
                           std::optional<Rotation> rotation = std::nullopt) const;
 
        /**
         * @brief Whether the robot fits at the given position and continuous orientation with
         *        more than the given obstacle clearance.
         *
         * Checks the two orientation slices bracketing the continuous orientation, so the
         * result is conservative with respect to the orientation discretization.
         */
        bool isFreeWithClearance(const Position& p,
                                 RotationDegrees orientationDeg,
                                 float clearance = 0.F) const;
 
        /**
         * @brief Find the closest position where the robot fits at the given continuous
         *        orientation with more than minClearance distance to obstacles.
         *
         * The two orientation slices bracketing the orientation are overlapped (element-wise
         * minimum) and the resulting 2D costmap is searched in expanding circles up to
         * maxDistance. Intended for recovery planning when the robot is standing in collision.
         *
         * @param position The starting position to search from
         * @param orientationDeg The robot's continuous orientation [deg]
         * @param maxDistance Maximum search radius [mm]
         * @param minClearance Required distance to obstacles [mm]
         * @return The closest vertex fulfilling the clearance, or nullopt if none within range
         */
        std::optional<Vertex> findClosestCollisionFreeVertex(const Position& position,
                                                             RotationDegrees orientationDeg,
                                                             float maxDistance,
                                                             float minClearance = 0.F) const;
 
        const Parameters& params() const noexcept;
 
        // get the scene bounds in the Costmap3D's base frame (aka 'origin')
        const SceneBounds& getLocalSceneBounds() const noexcept;
 
        core::Pose2D
        centerPose(std::optional<Rotation> rotation = std::nullopt) const
        {
            const Eigen::Vector2f Costmap3D_P_center{
                (getLocalSceneBounds().max.x() - getLocalSceneBounds().min.x()) / 2 +
                    getLocalSceneBounds().min.x(),
                (getLocalSceneBounds().max.y() - getLocalSceneBounds().min.y()) / 2 +
                    getLocalSceneBounds().min.y()};
 
            auto rotationDeg = 0.F;
            if (rotation.has_value())
            {
                rotationDeg = rotation->degrees;
            }
            Eigen::Rotation2Df rot{rotationDeg * static_cast<float>(M_PI) / 180.F};
 
            // TODO: Debug
            return global_T_Costmap3D * Eigen::Translation2f(Costmap3D_P_center) * rot;
        }
 
        struct Optimum
        {
            float value;
            Index index;
            Position position;
        };
 
        Optimum optimum(Rotation rotation = ALL_ORIENTATIONS) const;
 
        //! checks whether the cell is masked out
        bool isValid(const Index& index) const noexcept;
 
        bool isWithinRange(const Index& index) const noexcept;
 
        bool isMaskedOut(const Index& index) const noexcept;
 
        const std::optional<Mask>& getMask() const noexcept;
 
        std::optional<Costmap3D::Mask>& getMutableMask() noexcept;
 
        float value_ignore_mask(const Index& index, const RotationIndex rot_index) const;
 
        std::optional<float> value(const Index& index, const RotationIndex rot_index) const;
 
        std::optional<float> value(const Position& position,
                                   const RotationDegrees rotation_degrees) const;
 
        void cutToValidBoundingBox();
 
        const core::Pose2D&
        origin() const
        {
            return global_T_Costmap3D;
        }
 
        void
        setOrigin(const core::Pose2D& globalPose)
        {
            global_T_Costmap3D = globalPose;
        }
 
        std::size_t numberOfValidElements() const;
 
        std::pair<Index, Index> getValidBoundingBox() const;
 
        Index getSize() const;
 
        Costmap costmapForOrientation(RotationIndex rotationIndex,
                                      bool initializeMask = false) const;
 
        void convertToPseudoSDF();
        void convertToPseudoSDFForRotation(RotationIndex rot_index);
 
        Index findClosestValidIndex(const Index& index);
 
    private:
        void validateSizes() const;
 
        Grid grid;
        std::optional<Mask> mask;
 
        SceneBounds sceneBounds;
 
        const Parameters parameters;
 
        core::Pose2D global_T_Costmap3D = core::Pose2D::Identity();
    };
 
 
} // namespace armarx::navigation::algorithms::orientation_aware