ShortestPathFasterAlgorithm.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ShortestPathFasterAlgorithm.h"
 
#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <limits>
#include <string>
#include <vector>
 
#include <Eigen/Core>
 
#include <range/v3/algorithm/reverse.hpp>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
 
namespace armarx::navigation::algorithms::spfa
{
 
    namespace
    {
        constexpr std::size_t
        ravel(std::size_t row, std::size_t col, std::size_t num_cols)
        {
            return (row * num_cols) + col;
        }
 
        Eigen::Vector2i
        unravel(const std::size_t pos, const std::size_t numCols)
        {
            ARMARX_CHECK_GREATER_EQUAL(pos, 0);
            ARMARX_CHECK_GREATER(numCols, 0);
 
            Eigen::Vector2i p;
            p.x() = static_cast<int>(pos) / numCols;
            p.y() = static_cast<int>(pos) % numCols;
 
            return p;
        }
 
    } // namespace
 
    ShortestPathFasterAlgorithm::ShortestPathFasterAlgorithm(const Costmap& grid,
                                                             const Parameters& params) :
        costmap(grid), params(params)
    {
        ARMARX_VERBOSE << "Grid with size (" << grid.getGrid().rows() << ", "
                       << grid.getGrid().cols() << ").";
    }
 
    ShortestPathFasterAlgorithm::PlanningResult
    ShortestPathFasterAlgorithm::plan(const Eigen::Vector2f& start,
                                      const Eigen::Vector2f& goal,
                                      const bool checkStartForCollision) const
    {
        // ARMARX_CHECK(not costmap.isInCollision(start)) << "Start is in collision";
        // ARMARX_CHECK(not costmap.isInCollision(goal)) << "Goal is in collision";
 
        const Costmap::Vertex startVertex = costmap.toVertex(start);
        const auto goalVertex = Eigen::Vector2i{costmap.toVertex(goal).index};
 
        ARMARX_VERBOSE << "Planning from " << startVertex.index << " to " << goalVertex;
 
        if ((startVertex.index.array() == goalVertex.array()).all())
        {
            ARMARX_VERBOSE << "Already at goal.";
            return {.path = {}, .success = true};
        }
 
        const Result result = spfa(start, checkStartForCollision);
 
        return constructPath(start, result.parents, goal);
    }
 
    ShortestPathFasterAlgorithm::PlanningResult
    ShortestPathFasterAlgorithm::constructPath(
        const Eigen::Vector2f& start,
        const std::vector<std::vector<Eigen::Vector2i>>& spfaParents,
        const Eigen::Vector2f& goal) const
    {
        // we need the start vertex again to construct the final path
        const Costmap::Vertex startVertex = costmap.toVertex(start);
 
        const auto goalVertex = Eigen::Vector2i{costmap.toVertex(goal).index};
 
        if ((startVertex.index.array() == goalVertex.array()).all())
        {
            ARMARX_VERBOSE << "Already at goal.";
            return {.path = {}, .success = true};
        }
 
        const auto isStart = [&startVertex](const Eigen::Vector2i& v) -> bool
        { return (v.array() == startVertex.index.array()).all(); };
 
        const auto isInvalid = [](const Eigen::Vector2i& v) -> bool
        { return (v.x() == -1) and (v.y() == -1); };
 
        // traverse path from goal to start
        std::vector<Eigen::Vector2f> path;
        path.push_back(goal);
 
        const Eigen::Vector2i* pt = &goalVertex;
 
        ARMARX_VERBOSE << "Creating shortest path from grid";
        while (true)
        {
            if (isInvalid(*pt))
            {
                ARMARX_WARNING << "No path found from (" << start << ") to (" << goal << ").";
                return {.path = {}, .success = false};
            }
 
            const Eigen::Vector2i& parent = spfaParents.at(pt->x()).at(pt->y());
 
            ARMARX_DEBUG << VAROUT(parent);
 
            if (isStart(parent))
            {
                break;
            }
 
            path.push_back(costmap.toPositionGlobal(parent.array()));
 
            pt = &parent;
        }
 
        path.push_back(start);
        ARMARX_VERBOSE << "Path found with " << path.size() << " nodes.";
 
        // reverse the path such that: start -> ... -> goal
        ranges::reverse(path);
 
        return {.path = path, .success = true};
    }
 
    ShortestPathFasterAlgorithm::Result
    ShortestPathFasterAlgorithm::spfa(const Eigen::Vector2f& start,
                                      const bool checkStartForCollision) const
    {
        const Costmap::Vertex startVertex = costmap.toVertex(start);
 
        ARMARX_VERBOSE << "Running spfa planner from " << startVertex.index;
 
        auto costmapWithValidStart = costmap.getGrid();
        if (costmap.isInCollision(start))
        {
            ARMARX_INFO << "SPFA: Start node is in collision, setting modifying start node";
            costmapWithValidStart(startVertex.index.x(), startVertex.index.y()) = 0.1;
        }
 
        return spfa(
            costmapWithValidStart, Eigen::Vector2i{startVertex.index}, checkStartForCollision);
    }
 
    ShortestPathFasterAlgorithm::Result
    ShortestPathFasterAlgorithm::spfa(const Eigen::MatrixXf& inputMap,
                                      const Eigen::Vector2i& source,
                                      const bool checkStartForCollision) const
    {
        if (checkStartForCollision)
        {
            ARMARX_CHECK_GREATER(inputMap(source.x(), source.y()), 0.F)
                << "Start must not be in collision";
        }
 
        constexpr float eps = 1e-6;
        constexpr std::size_t numDirs = 8;
 
        const std::array<std::array<std::int64_t, 2>, numDirs> dirs{
            std::array<std::int64_t, 2>{-1, -1},
            std::array<std::int64_t, 2>{-1, 0},
            std::array<std::int64_t, 2>{-1, 1},
            std::array<std::int64_t, 2>{0, 1},
            std::array<std::int64_t, 2>{1, 1},
            std::array<std::int64_t, 2>{1, 0},
            std::array<std::int64_t, 2>{1, -1},
            std::array<std::int64_t, 2>{0, -1}};
 
        const std::array<float, numDirs> dirLengths{
            std::sqrt(2.0f), 1, std::sqrt(2.0f), 1, std::sqrt(2.0f), 1, std::sqrt(2.0f), 1};
 
        // Process input map
        // py::buffer_info map_buf = input_map.request();
        const std::size_t numRows = inputMap.rows();
        const std::size_t numCols = inputMap.cols();
 
        // Get source coordinates
        const int source_i = source.x();
        const int source_j = source.y();
 
        const std::size_t maxNumVerts = numRows * numCols;
        constexpr std::size_t maxEdgesPerVert = numDirs;
        // const float inf = 2 * maxNumVerts;
        const float inf = 2 * maxNumVerts; // FIXME numeric_limits<float>::max();
        const std::size_t queueSize = maxNumVerts + 1; // ring buffer: maxNumVerts live + 1 spare slot
 
        // Initialize arrays
        std::vector<std::size_t> edges(maxNumVerts * maxEdgesPerVert);
        std::vector<std::size_t> edge_counts(maxNumVerts);
        std::vector<std::size_t> queue(queueSize);
        std::vector<bool> in_queue(maxNumVerts);
        std::vector<float> weights(maxNumVerts * maxEdgesPerVert);
        std::vector<float> dists(maxNumVerts, inf); // initialize with inf
 
        const auto& verify = [](std::int64_t index, std::size_t maxIndex, const std::string& str)
        {
            if (index >= static_cast<std::int64_t>(maxIndex) || index < 0)
            {
                ARMARX_IMPORTANT << "invalid index " << index << " max allowed is " << maxIndex
                                 << "; " << str;
            }
        };
 
 
        // Build graph
        ARMARX_VERBOSE << "Build graph";
        for (std::size_t row = 0; row < numRows; ++row)
        {
            for (std::size_t col = 0; col < numCols; ++col)
            {
                const std::size_t v = ravel(row, col, numCols);
                if (inputMap(static_cast<int>(row), static_cast<int>(col)) <= 0.F) // collision
                {
                    continue;
                }
 
                for (std::size_t k = 0; k < numDirs; ++k)
                {
                    const std::int64_t ip = static_cast<std::int64_t>(row) +
                                            dirs[k][0]; // could eventually become negative
                    const std::int64_t jp = static_cast<std::int64_t>(col) + dirs[k][1];
 
                    if (ip < 0 || jp < 0 || ip >= static_cast<std::int64_t>(numRows) ||
                        jp >= static_cast<std::int64_t>(numCols))
                    {
                        continue;
                    }
 
                    const std::size_t vp = ravel(ip, jp, numCols);
                    if (inputMap(ip, jp) <= 0.F) // collision
                    {
                        continue;
                    }
 
                    const float clippedObstacleDistance =
                        std::min(inputMap(ip, jp), params.obstacleMaxDistance);
 
                    const float travelCost = dirLengths[k];
 
                    const float targetDistanceCost =
                        params.obstacleDistanceWeight *
                        std::pow(1.F - clippedObstacleDistance / params.obstacleMaxDistance,
                                 params.obstacleCostExponent);
 
                    const float edgeCost = params.obstacleDistanceCosts
                                               ? travelCost * (1 + targetDistanceCost)
                                               : travelCost;
 
                    const std::size_t e = ravel(v, edge_counts.at(v), maxEdgesPerVert);
                    edges.at(e) = vp;
                    verify(e, maxNumVerts * maxEdgesPerVert, "edges");
                    weights.at(e) = edgeCost;
                    verify(e, maxNumVerts * maxEdgesPerVert, "weights");
                    edge_counts[v]++;
                    verify(v, maxNumVerts, "edges_counts");
                }
            }
        }
 
 
        // SPFA
        ARMARX_DEBUG << "SPFA";
        // The queue is a circular (ring) buffer: head and tail wrap around modulo
        // queueSize. Only the vertices currently in the queue occupy slots, and the
        // in_queue flag guarantees each vertex occupies at most one slot at a time, so
        // the buffer holds at most maxNumVerts elements and never overflows. head and
        // tail point at the last-popped / last-pushed slot respectively; the queue is
        // empty when head == tail.
        std::size_t head = 0;
        std::size_t tail = 0;
        const auto next = [queueSize](std::size_t i) { return (i + 1) % queueSize; };
 
        const std::size_t s = ravel(source_i, source_j, numCols);
        dists[s] = 0;
        verify(s, maxNumVerts, "dists");
        tail = next(tail);
        queue[tail] = s;
        in_queue[s] = true;
        verify(s, maxNumVerts, "in_queue");
 
        std::vector<std::int64_t> parents(maxNumVerts, -1);
        while (head != tail)
        {
            head = next(head);
            const std::size_t u = queue[head];
            in_queue[u] = false;
            verify(u, maxNumVerts, "in_queue");
            for (std::size_t j = 0; j < edge_counts[u]; ++j)
            {
                const std::size_t e = ravel(u, j, maxEdgesPerVert);
                const std::size_t v = edges[e];
                const float newDist = dists[u] + weights[e];
                if (newDist < dists[v])
                {
                    parents[v] = u;
                    verify(v, maxNumVerts, "parents");
                    dists[v] = newDist;
                    verify(v, maxNumVerts, "dists");
                    if (not in_queue[v])
                    {
                        // Guaranteed by in_queue, but guard against logic errors: the
                        // ring buffer must never become full (would alias head == tail).
                        ARMARX_CHECK(next(tail) != head);
                        tail = next(tail);
                        queue[tail] = v;
                        in_queue[v] = true;
                        verify(v, maxNumVerts, "in_queue");
 
                        // SLF (Smallest Label First): if the freshly pushed vertex is
                        // cheaper than the current front, swap it to be popped next.
                        const std::size_t front = next(head);
                        if (dists[queue[tail]] < dists[queue[front]])
                        {
                            std::swap(queue[tail], queue[front]);
                        }
                    }
                }
            }
        }
 
        // Copy output into numpy array
        ARMARX_DEBUG << "Copy to output variables";
 
        Eigen::MatrixXf output_dists(numRows, numCols);
        Result::Mask reachable(numRows, numCols);
        reachable.setOnes();
 
        std::vector<std::vector<Eigen::Vector2i>> output_parents(
            numRows, std::vector<Eigen::Vector2i>(numCols, Eigen::Vector2i{-1, -1}));
 
        std::size_t invalids = 0;
 
        for (std::size_t row = 0; row < numRows; ++row)
        {
            for (std::size_t col = 0; col < numCols; ++col)
            {
                const std::size_t u = ravel(row, col, numCols);
                output_dists(row, col) = (dists[u] < inf - eps) * dists[u];
 
                if (parents[u] == -1) // no parent
                {
                    invalids++;
                    reachable(row, col) = false;
                    continue;
                }
 
                output_parents.at(row).at(col) = unravel(parents[u], numCols);
            }
        }
 
        // "fix": the initial position does not have any parents. But it's still reachable ...
        reachable(source.x(), source.y()) = true;
 
        ARMARX_VERBOSE << "Fraction of invalid cells: (" << invalids << "/" << parents.size()
                       << ")";
 
        ARMARX_DEBUG << "Done.";
 
// The distances are in a unit-like system, so the distance between two neighbor cells is 1
    // We need to convert it back to a metric system (mutiplying it by the cell size) to be independent of the cell size.
        return Result{.distances = costmap.params().cellSize * output_dists,
                       .parents = output_parents,
                       .reachable = reachable};
    }
 
    std::optional<ShortestPathFasterAlgorithm::ClosestReachableResult>
    findClosestReachablePosition(
        const ShortestPathFasterAlgorithm::Result& spfaResult,
        const Eigen::Vector2f& goal,
        const Costmap& costmap)
    {
        const Costmap::Vertex goalVertex = costmap.toVertex(goal);
        const Eigen::Vector2i goalIdx = goalVertex.index;
 
        float minDistSq = std::numeric_limits<float>::max();
        Eigen::Vector2i closestIdx{-1, -1};
 
        for (int row = 0; row < spfaResult.reachable.rows(); ++row)
        {
            for (int col = 0; col < spfaResult.reachable.cols(); ++col)
            {
                if (!spfaResult.reachable(row, col))
                {
                    continue;
                }
 
                const float dx = static_cast<float>(row) - static_cast<float>(goalIdx.x());
                const float dy = static_cast<float>(col) - static_cast<float>(goalIdx.y());
                const float distSq = dx * dx + dy * dy;
 
                if (distSq < minDistSq)
                {
                    minDistSq = distSq;
                    closestIdx = Eigen::Vector2i{row, col};
                }
            }
        }
 
        if (closestIdx.x() < 0)
        {
            return std::nullopt;
        }
 
        return ShortestPathFasterAlgorithm::ClosestReachableResult{
            .position = costmap.toPositionGlobal(closestIdx),
            .gridIndex = closestIdx,
            .euclideanDistanceToGoal = std::sqrt(minDistSq) * costmap.params().cellSize};
    }
 
 
} // namespace armarx::navigation::algorithms::spfa