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 <numeric>
 
#include <Eigen/Core>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
 
#include "range/v3/algorithm/reverse.hpp"
 
namespace armarx::navigation::algorithms::spfa
{
 
    inline int
    ravel(int row, int col, int num_cols)
    {
        return row * num_cols + col;
    }
 
    Eigen::Vector2i
    unravel(int pos, int num_cols)
    {
        ARMARX_CHECK_GREATER_EQUAL(pos, 0);
        ARMARX_CHECK_GREATER(num_cols, 0);
 
        Eigen::Vector2i p;
        p.x() = pos / num_cols;
        p.y() = pos % num_cols;
 
        return p;
    }
 
    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_INFO << "Already at goal.";
            return {.path = {}, .success = true};
        }
 
        auto costmapWithValidStart = costmap.getGrid();
        if (costmap.isInCollision(start))
        {
            costmapWithValidStart(startVertex.index.x(), startVertex.index.y()) = 0.1;
        }
 
        const Result result =
            spfa(costmapWithValidStart, Eigen::Vector2i{startVertex.index}, checkStartForCollision);
 
        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 {};
            }
 
            const Eigen::Vector2i& parent = result.parents.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
    {
        return spfa(costmap.getGrid(), costmap.toVertex(start).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";
        }
 
        const float eps = 1e-6;
        const int num_dirs = 8;
 
        const int dirs[num_dirs][2] = {
            {-1, -1}, {-1, 0}, {-1, 1}, {0, 1}, {1, 1}, {1, 0}, {1, -1}, {0, -1}};
 
        const float dir_lengths[num_dirs] = {
            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();
        int num_rows = inputMap.rows();
        int num_cols = inputMap.cols();
 
        // Get source coordinates
        int source_i = source.x();
        int source_j = source.y();
 
        int max_num_verts = num_rows * num_cols;
        int max_edges_per_vert = num_dirs;
        const float inf = 2 * max_num_verts;
        int queue_size = num_dirs * num_rows * num_cols;
 
        // Initialize arrays
        int* edges = new int[max_num_verts * max_edges_per_vert]();
        int* edge_counts = new int[max_num_verts]();
        int* queue = new int[queue_size]();
        bool* in_queue = new bool[max_num_verts]();
        float* weights = new float[max_num_verts * max_edges_per_vert]();
        float* dists = new float[max_num_verts];
        for (int i = 0; i < max_num_verts; ++i)
            dists[i] = inf;
 
        std::vector<int> parents(max_num_verts, -1);
 
        // Build graph
        ARMARX_VERBOSE << "Build graph";
        for (int row = 0; row < num_rows; ++row)
        {
            for (int col = 0; col < num_cols; ++col)
            {
                int v = ravel(row, col, num_cols);
                if (inputMap(row, col) == 0.F) // collision
                    continue;
 
                for (int k = 0; k < num_dirs; ++k)
                {
                    int ip = row + dirs[k][0], jp = col + dirs[k][1];
                    if (ip < 0 || jp < 0 || ip >= num_rows || jp >= num_cols)
                        continue;
 
                    int vp = ravel(ip, jp, num_cols);
                    if (inputMap(ip, jp) == 0.F) // collision
                        continue;
 
 
                    const float clippedObstacleDistance =
                        std::min(inputMap(ip, jp), params.obstacleMaxDistance);
 
                    const float travelCost = dir_lengths[k];
 
                    const float targetDistanceCost =
                        params.obstacleDistanceWeight *
                        std::pow(1.F - clippedObstacleDistance / params.obstacleMaxDistance,
                                 params.obstacleCostExponent);
 
                    const float edgeCost = params.obstacleDistanceCosts
                                               ? travelCost * (1 + targetDistanceCost)
                                               : travelCost;
 
                    int e = ravel(v, edge_counts[v], max_edges_per_vert);
                    edges[e] = vp;
                    weights[e] = edgeCost;
                    edge_counts[v]++;
                }
            }
        }
 
        // SPFA
        ARMARX_DEBUG << "SPFA";
        int s = ravel(source_i, source_j, num_cols);
        int head = 0, tail = 0;
        dists[s] = 0;
        queue[++tail] = s;
        in_queue[s] = true;
        while (head < tail)
        {
            int u = queue[++head];
            in_queue[u] = false;
            for (int j = 0; j < edge_counts[u]; ++j)
            {
                int e = ravel(u, j, max_edges_per_vert);
                int v = edges[e];
                float new_dist = dists[u] + weights[e];
                if (new_dist < dists[v])
                {
                    parents[v] = u;
                    dists[v] = new_dist;
                    if (!in_queue[v])
                    {
                        assert(tail < queue_size);
                        queue[++tail] = v;
                        in_queue[v] = true;
                        if (dists[queue[tail]] < dists[queue[head + 1]])
                            std::swap(queue[tail], queue[head + 1]);
                    }
                }
            }
        }
 
        // Copy output into numpy array
        ARMARX_DEBUG << "Copy to output variables";
 
        Eigen::MatrixXf output_dists(num_rows, num_cols);
        Result::Mask reachable(num_rows, num_cols);
        reachable.setOnes();
        // auto output_dists = py::array_t<float>({num_rows, num_cols});
 
 
        std::vector<std::vector<Eigen::Vector2i>> output_parents;
 
        for (int row = 0; row < num_rows; row++)
        {
            output_parents.push_back(std::vector<Eigen::Vector2i>(static_cast<size_t>(num_cols),
                                                                  Eigen::Vector2i{-1, -1}));
        }
 
        int invalids = 0;
 
        for (int row = 0; row < num_rows; ++row)
        {
            for (int col = 0; col < num_cols; ++col)
            {
                int u = ravel(row, col, num_cols);
                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], num_cols);
            }
        }
 
        // "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()
                       << ")";
 
        // Free memory
        delete[] edges;
        delete[] edge_counts;
        delete[] queue;
        delete[] in_queue;
        delete[] weights;
        delete[] dists;
 
        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};
    }
 
 
} // namespace armarx::navigation::algorithms::spfa