Planner2D.h

Go to the documentation of this file.

#pragma once
 
#include <map>
#include <memory>
#include <string>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <VirtualRobot/VirtualRobot.h>
 
namespace armarx::navigation::algorithm::astar
{
 
    class Planner2D
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        /**
        Initialize Planner with robot and obstacles.
        \param robot The robot
        \param obstacles The obstacles which should be considered by the planner
        */
        Planner2D(VirtualRobot::RobotPtr robot,
                  VirtualRobot::SceneObjectSetPtr obstacles = VirtualRobot::SceneObjectSetPtr());
 
        virtual ~Planner2D() = default;
 
        // planners implement this method
        virtual std::vector<Eigen::Vector2f> plan(const Eigen::Vector2f& start,
                                                  const Eigen::Vector2f& goal) = 0;
 
        /// Update obstacles
        void setObstacles(VirtualRobot::SceneObjectSetPtr obstacles);
 
        /// update robot
        void setRobot(VirtualRobot::RobotPtr robot);
 
        /// set name of RobotNode which should be used for collision detection
        void setRobotColModel(const std::string& robotColModelName);
 
        Eigen::Matrix4f positionToMatrix4f(Eigen::Vector2f pos);
 
        /// set a float parameter that is identified with a string
        void setParameter(const std::string& s, float p);
 
        /// check if a parameter is set
        bool hasParameter(const std::string& s);
 
        /// get the corresponding float parameter (0 is returned when parameter string is not present)
        float getParameter(const std::string& s);
 
        struct point2D
        {
            float x;
            float y;
            float dirX;
            float dirY;
        };
 
        virtual std::vector<point2D>
        getGridVisu(float xdist, float ydist, Eigen::Vector2f goal)
        {
            return std::vector<point2D>();
        }
 
    protected:
        std::string robotColModelName;
        //local copy of the robot's collision model that can be moved around without moving the robot
        VirtualRobot::CollisionModelPtr robotCollisionModel;
        VirtualRobot::RobotPtr robot;
        VirtualRobot::SceneObjectSetPtr obstacles;
        Eigen::Vector2f sceneBoundsMin;
        Eigen::Vector2f sceneBoundsMax;
 
        std::map<std::string, float> parameters;
    };
 
    using Planner2DPtr = std::shared_ptr<Planner2D>;
 
} // namespace armarx::navigation::algorithm::astar