AStarPlanner.h

Go to the documentation of this file.

#pragma once
 
#include <memory>
#include <optional>
#include <vector>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <armarx/navigation/algorithms/orientation_aware/Costmap3D.h>
#include <armarx/navigation/algorithms/orientation_aware/io.h>
#include <armarx/navigation/core/basic_types.h>
 
#include "Node.h"
 
namespace armarx::navigation::algorithms::orientation_aware
{
 
    class Grid
    {
    public:
        Grid(int rows, int cols, int orientations, const Costmap3D& costmap, float clearance);
        void clear();
        Node& getNode(int row, int col, int orientation);
        static bool fulfillsConstraints(const Node& n, const Costmap3D& costmap, float clearance = 0.0f);
 
    private:
        void initialize(int rows, int cols, int orientations, const Costmap3D& costmap);
        std::unique_ptr<Node[]> data_ = nullptr;
        int rows;
        int cols;
        int orientations;
        float clearance;
    };
 
    /**
      * The A* planner (3D version including orientation dimension)
      */
    class AStarPlanner
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        using Params = io::AStarWithOrientationParams;
 
        AStarPlanner(const Costmap3D& costmap, io::AStarWithOrientationParams params);
 
        std::vector<core::Pose2D> plan(const core::Pose2D& start,
                                       const core::Pose2D& goal) /* override */;
 
        /// Number of nodes expanded during the last `plan()` call. Reported by the offline
        /// planner evaluation; not used by the navigation stack itself.
        std::size_t
        lastExpandedNodes() const noexcept
        {
            return expandedNodes;
        }
 
    private:
        float heuristic(const Node& n1, const Node& n2);
        Node& closestNode(const core::Pose2D& pose);
 
        Costs costs(const Node& n1, const Node& n2) const;
 
        float computeObstacleDistance(const Eigen::Vector2f& pos,
                                      VirtualRobot::RobotPtr& robot) const;
 
    private:
        const Costmap3D costmap3d;
 
        std::optional<Grid> grid;
 
        io::AStarWithOrientationParams params;
 
        std::size_t expandedNodes{0};
    };
} // namespace armarx::navigation::algorithms::orientation_aware