Node.h

Go to the documentation of this file.

#pragma once
 
#include <algorithm>
#include <experimental/memory>
#include <memory>
#include <optional>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <armarx/navigation/core/basic_types.h>
 
namespace armarx::navigation::algorithms::orientation_aware
{
 
    struct Costs
    {
        float euclidian = 0.F;
        float obstacleProximity = 0.F; // penalty for moving closer to obstacles
        float obstacleProximity2 = 0.F; // continuous penalty for being close to obstacles
        float orientationDifference = 0.F;
        float forwardMovement = 0.F;
        float smoothnessCosts = 0.F;
 
        float
        combined() const
        {
            return std::max(euclidian + obstacleProximity + obstacleProximity2 +
                                orientationDifference + forwardMovement + smoothnessCosts,
                            0.F);
        }
 
        Costs
        operator+(const Costs& other) const
        {
            return Costs{
                .euclidian = this->euclidian + other.euclidian,
                .obstacleProximity = this->obstacleProximity + other.obstacleProximity,
                .obstacleProximity2 = this->obstacleProximity2 + other.obstacleProximity2,
                .orientationDifference = this->orientationDifference + other.orientationDifference,
                .forwardMovement = this->forwardMovement + other.forwardMovement,
                .smoothnessCosts = this->smoothnessCosts + other.smoothnessCosts,
            };
        }
    };
 
    class Node;
    using NodePtr = std::shared_ptr<Node>;
 
    /**
    * A Node can store data to all valid neighbors (successors) and a precessor.
    * It offers methods to determine the complete path to the starting point.
    */
    class Node
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        Node(const Eigen::Vector2f& position, float obstacleDistance, float orientation_deg);
        Node() = default;
 
        void
        initialize(const Eigen::Vector2f& position, float obstacleDistance, float orientation_deg);
 
        Eigen::Vector2f position;
        float orientation_deg = -1.F;
        // Maybe replace orientation_deg with orientation_index and reference costmap somehow? (maybe reference is not needed because it is already used in a*-planning class)
 
        /// All nodes that are adjacent to this one.
        std::vector<std::experimental::observer_ptr<Node>> successors;
        /// For traversal.
        std::experimental::observer_ptr<Node> predecessor;
        std::optional<Costs> predecessorCosts;
 
        float obstacleDistance;
 
        /// Collects all predecessors in order to generate path to starting point.
        std::vector<const Node*> traversePredecessors() const;
 
        // algorithm runtime information
        bool inClosedSet = false;
        bool inOpenSet = false;
        float fScore = -1.F;
        float gScore = -1.F;
    };
 
} // namespace armarx::navigation::algorithms::orientation_aware