Node.cpp

Go to the documentation of this file.

#include "Node.h"
 
#include <experimental/memory>
#include <vector>
#include <armarx/navigation/core/basic_types.h>
 
namespace armarx::navigation::algorithms::orientation_aware
{
 
    Node::Node(const Eigen::Vector2f& position, const float obstacleDistance, float orientation_deg) :
        position(position), orientation_deg(orientation_deg), obstacleDistance(obstacleDistance) 
    {
    }
 
    void
    Node::initialize(const Eigen::Vector2f& position, const float obstacleDistance, float orientation_deg)
    {
        this->position = position;
        this->obstacleDistance = obstacleDistance;
        this->orientation_deg = orientation_deg;
    }
 
    std::vector<const Node*>
    Node::traversePredecessors() const
    {
        std::vector<const Node*> result;
        result.push_back(this);
 
        std::experimental::observer_ptr<Node> predecessor = this->predecessor;
        while (predecessor)
        {
            result.push_back(predecessor.get());
            predecessor = predecessor->predecessor;
        }
        return result;
    }
 
} // namespace armarx::navigation::algorithms::orientation_aware