routemaker.cpp

Go to the documentation of this file.

#include "routemaker/routemaker.h"
 
#include "coordinate_converter/coordinate_converter.h"
 
#include <algorithm>
#include <array>
#include <cassert>
#include <cmath>
 
// Temporary: When 3D, drone height should vary
#define DRONE_FLIGHT_HEIGHT 175
 
namespace Routemaker
{
    Routemaker::Routemaker(const Core::UTMCoordinate& origin, int size)
        : m_MapWidth(size),
          m_HeightMap(std::make_unique<HeightManagement::HeightManager>())
    {
        UpdateOrigin(origin, size);
    }
 
    // Relational data that forms paths need to be reset before running A*
    // again. This method serves as a simple way to make sure all of these
    // values are reset.
    void
    Routemaker::ResetNodes()
    {
        for (uint32_t y{}; y < m_RoutemakerWidth; ++y) {
            for (uint32_t x{}; x < m_RoutemakerWidth; ++x) {
                NodePtr node{ GetNode(x, y) };
                node->Parent = std::weak_ptr<Node<Cell2D>>();
                node->GlobalGoal = std::numeric_limits<double>::infinity();
                node->LocalGoal = std::numeric_limits<double>::infinity();
                node->Visited = false;
            }
        }
    }
 
    // In order to improve efficiency, the resolution of routemaker is adjusted
    // based on the size of the scenario. If you are working on a 2km scale, you
    // probably don't need 1 meter fidelity. This should maybe be adjustable as
    // part of the scenario settings in the future.
    void
    Routemaker::UpdateResolution()
    {
        if (m_MapWidth < 250) {
            m_RoutemakerRes = 1;
        } else if (m_MapWidth < 500) {
            m_RoutemakerRes = 2;
        } else if (m_MapWidth < 1000) {
            m_RoutemakerRes = 3;
        } else if (m_MapWidth < 2000) {
            m_RoutemakerRes = 4;
        } else {
            m_RoutemakerRes = 5;
        }
 
        m_RoutemakerWidth = m_MapWidth / m_RoutemakerRes;
    }
 
    // Whenever the scenario's origin or size is updated, we need to create a
    // new graph with the proper height data
    void
    Routemaker::UpdateOrigin(Core::UTMCoordinate utmOrigin, int size)
    {
        m_HeightMap->UpdateOrigin(utmOrigin, size);
        m_MapWidth = size;
        UpdateResolution();
        m_Nodes = std::vector<NodePtr>(m_RoutemakerWidth * m_RoutemakerWidth);
 
        for (uint32_t y{ 0 }; y < m_RoutemakerWidth; ++y) {
            for (uint32_t x{ 0 }; x < m_RoutemakerWidth; ++x) {
                uint32_t xRel{ x * m_RoutemakerRes };
                uint32_t yRel{ y * m_RoutemakerRes };
                float height{ 0.0f };
                bool occupied{ false };
                for (int j{ 0 }; j < m_RoutemakerRes; ++j) {
                    for (int i{ 0 }; i < m_RoutemakerRes; ++i) {
                        float heightCandidate;
                        if (m_HeightMap->GetHeight(static_cast<int>(xRel) + i,
                                                   static_cast<int>(yRel) + j,
                                                   heightCandidate)) {
                            height = std::max(height, heightCandidate);
                        }
                        // Set occupied to true if any of the heights are larger
                        // tha DRONE_FLIGHT_HEIGHT
                        occupied = (occupied || (height > DRONE_FLIGHT_HEIGHT));
                    }
                }
                Node<Cell2D> node{};
                node.Data = { x, y, occupied };
                m_Nodes[x + y * m_RoutemakerWidth] =
                    std::make_shared<Node<Cell2D>>(node);
            }
        }
    }
 
    // Not a pretty method, but does the job. Currently, the routemaker is
    // considering a 2D space, keeping the drones at the same altitude. In the
    // future, we need to consider 3D, meaning the GetNeighbors method needs to
    // consider neighbors above and below the current node as well.
    std::vector<Routemaker::NodePtr>
    Routemaker::GetNeighbors(NodePtr node)
    {
        std::vector<NodePtr> neighbors;
 
        uint32_t x{ node->Data.X };
        uint32_t y{ node->Data.Y };
 
        if (x > 0) {
            NodePtr neighbor{ GetNode(x - 1, y) };
            if (!neighbor->Data.Occupied) {
                neighbors.push_back(neighbor);
            }
        }
        if (x < m_RoutemakerWidth - 1) {
            NodePtr neighbor{ GetNode(x + 1, y) };
            if (!neighbor->Data.Occupied) {
                neighbors.push_back(neighbor);
            }
        }
        if (y > 0) {
            NodePtr neighbor{ GetNode(x, y - 1) };
            if (!neighbor->Data.Occupied) {
                neighbors.push_back(neighbor);
            }
        }
 
        if (y < m_RoutemakerWidth - 1) {
            NodePtr neighbor{ GetNode(x, y + 1) };
            if (!neighbor->Data.Occupied) {
                neighbors.push_back(neighbor);
            }
        }
 
        if ((x > 0) && (y > 0)) {
            NodePtr neighbor{ GetNode(x - 1, y - 1) };
            if (!neighbor->Data.Occupied) {
                neighbors.push_back(neighbor);
            }
        }
 
        if ((x < (m_RoutemakerWidth - 1)) && (y > 0)) {
            NodePtr neighbor{ GetNode(x + 1, y - 1) };
            if (!neighbor->Data.Occupied) {
                neighbors.push_back(neighbor);
            }
        }
 
        if ((x < (m_RoutemakerWidth - 1)) && (y < (m_RoutemakerWidth - 1))) {
            NodePtr neighbor{ GetNode(x + 1, y + 1) };
            if (!neighbor->Data.Occupied) {
                neighbors.push_back(neighbor);
            }
        }
 
        if ((x > 0) && (y < (m_RoutemakerWidth - 1))) {
            NodePtr neighbor{ GetNode(x - 1, y + 1) };
            if (!neighbor->Data.Occupied) {
                neighbors.push_back(neighbor);
            }
        }
 
        return neighbors;
    }
 
    double
    Routemaker::GetCost(NodePtr a, NodePtr b)
    {
        double x1{ static_cast<double>(a->Data.X) * m_RoutemakerRes };
        double y1{ static_cast<double>(a->Data.Y) * m_RoutemakerRes };
        double x2{ static_cast<double>(b->Data.X) * m_RoutemakerRes };
        double y2{ static_cast<double>(b->Data.Y) * m_RoutemakerRes };
 
        // We are using a standard cartesian grid, with each cell being one
        // node. As such, Euclidean distance is a nice measure of cost.
        return std::sqrt(std::pow(x2 - x1, 2) + std::pow(y2 - y1, 2));
    }
 
    // Bresenham's algorithm is a fine starting point for detecting line of
    // sight. However, for better accuracy and more scalability for 3D,
    // Ray-casting should probably be considered in the future.
    bool
    Routemaker::HasLineOfSight(NodePtr a, NodePtr b)
    {
        // Assume we have a line of sight
        bool hasLineOfSight{ true };
 
        std::list<NodePtr> nodes{ BresenhamLine(a, b) };
        std::for_each(nodes.begin(), nodes.end(),
                      [&hasLineOfSight](const NodePtr& n) {
                          if (n->Data.Occupied) {
                              // If any nodes are occupied, there is no line of
                              // sight.
                              hasLineOfSight = false;
                          }
                      });
 
        return hasLineOfSight;
    }
 
    std::list<Routemaker::NodePtr>
    Routemaker::BresenhamLine(const NodePtr& a, const NodePtr& b) const
    {
        std::list<NodePtr> list;
 
        auto x1{ static_cast<int32_t>(a->Data.X) };
        auto y1{ static_cast<int32_t>(a->Data.Y) };
        auto x2{ static_cast<int32_t>(b->Data.X) };
        auto y2{ static_cast<int32_t>(b->Data.Y) };
 
        bool isSteep{ std::abs(y2 - y1) > std::abs(x2 - x1) };
        if (isSteep) {
            std::swap(x1, y1);
            std::swap(x2, y2);
        }
        if (x1 > x2) {
            std::swap(x1, x2);
            std::swap(y1, y2);
        }
 
        int32_t deltaX{ x2 - x1 };
        int32_t deltaY{ std::abs(y2 - y1) };
        int32_t error{}, yStep, y{ y1 };
 
        if (y1 < y2) {
            yStep = 1;
        } else {
            yStep = -1;
        }
 
        for (int32_t x{ x1 }; x <= x2; ++x) {
            if (isSteep) {
                list.push_back(GetNode(y, x));
            } else {
                list.push_back(GetNode(x, y));
            }
 
            error += deltaY;
            if (2 * error >= deltaX) {
                y += yStep;
                error -= deltaX;
            }
        }
 
        return list;
    }
 
    Routemaker::NodePtr
    Routemaker::GetNode(uint32_t x, uint32_t y) const
    {
        uint32_t index{ x + y * m_RoutemakerWidth };
        return m_Nodes[index];
    }
 
    std::vector<Core::CartesianCoordinate>
    Routemaker::MakeRoute(const Core::Keyframe& a, const Core::Keyframe& b)
    {
        // Scenario class should have sorted the keyframes already, but just to
        // make sure:
        assert(b.TimeStamp > a.TimeStamp && a.AgentId == b.AgentId);
 
        // Keyframes store positions in a symmetric space. Let's make them
        // symmetric to fit our grid.
        Core::CartesianCoordinate asymmetricAPosition{
            CoordinateConverter::CoordConv::SymmetricToAsymmetric(a.Position)
        };
        Core::CartesianCoordinate asymmetricBPosition{
            CoordinateConverter::CoordConv::SymmetricToAsymmetric(b.Position)
        };
 
        // Let's also divide by our resolution to find the cells each position
        // fits in.
        asymmetricAPosition.X /= m_RoutemakerRes;
        asymmetricAPosition.Y /= m_RoutemakerRes;
        asymmetricAPosition.Z /= m_RoutemakerRes;
 
        asymmetricBPosition.X /= m_RoutemakerRes;
        asymmetricBPosition.Y /= m_RoutemakerRes;
        asymmetricBPosition.Z /= m_RoutemakerRes;
 
        NodePtr start{ GetNode(static_cast<uint32_t>(asymmetricAPosition.X),
                               static_cast<uint32_t>(asymmetricAPosition.Y)) };
        NodePtr goal{ GetNode(static_cast<uint32_t>(asymmetricBPosition.X),
                              static_cast<uint32_t>(asymmetricBPosition.Y)) };
 
        SolveAStar(start, goal); // We find the path
        PostSmooth(start, goal); // We make it smooth(er)
 
        // After the path has been generated, we start at the goal, and
        // recursively travel to the start, collecting all positions that make
        // up the route
        std::vector<Core::CartesianCoordinate> route;
        NodePtr current{ goal };
        NodePtr parent{ goal->Parent.lock() };
        while (current != start && parent != nullptr) {
            // We scale up by resolution again
            Core::CartesianCoordinate position{
                (float)current->Data.X * m_RoutemakerRes,
                (float)current->Data.Y * m_RoutemakerRes, DRONE_FLIGHT_HEIGHT
            };
 
            // Scenario likes symmetric positions, so let's transform back
            Core::CartesianCoordinate positionSymmetric{
                CoordinateConverter::CoordConv::AsymmetricToSymmetric(position)
            };
            route.push_back(positionSymmetric);
            current = parent;
            parent = current->Parent.lock();
        }
        Core::CartesianCoordinate positionSymmetric{
            CoordinateConverter::CoordConv::AsymmetricToSymmetric(
                { (float)start->Data.X * m_RoutemakerRes,
                  (float)start->Data.Y * m_RoutemakerRes, DRONE_FLIGHT_HEIGHT })
        };
        route.push_back(positionSymmetric);
 
        // Finally, let's reverse the path, so it goes from start to goal,
        // rather than from goal to start.
        std::reverse(route.begin(), route.end());
 
        return route;
    }
 
} // namespace Routemaker