planner.h

Go to the documentation of this file.

/* Author: Zachary Kingston */
 
#ifndef ROBOWFLEX_DART_PLANNER_
#define ROBOWFLEX_DART_PLANNER_
 
#include <robowflex_library/planning.h>
#include <robowflex_dart/planning.h>
 
namespace robowflex
{
    namespace darts
    {
        /** \cond IGNORE */
        ROBOWFLEX_CLASS_FORWARD(DARTPlanner);
        /** \endcond */
 
        /** \class robowflex::darts::DARTPlannerPtr
            \brief A shared pointer wrapper for robowflex::darts::DARTPlanner. */
 
        /** \class robowflex::darts::DARTPlannerConstPtr
            \brief A const shared pointer wrapper for robowflex::darts::DARTPlanner. */
 
        /** \brief Wrapper for easy access to DART planning tools via standard Robowflex interface.
         */
        class DARTPlanner : public robowflex::Planner
        {
        public:
            /** \brief Constructor.
             *  Takes in a \a robot description and an optional namespace \a name.
             *  If \a name is specified, planner parameters are namespaced under the namespace of \a robot.
             *  \param[in] robot The robot to plan for.
             *  \param[in] name Optional namespace for planner.
             */
            DARTPlanner(const robowflex::RobotPtr &robot, const std::string &name = "DARTPlanner");
 
            /** \brief This function is called before benchmarking. Here, it is used to setup the DART scene
             * before the solve method.
             *  \param[in] scene Scene to plan for.
             *  \param[in] request Planning request.
             */
            void preRun(const robowflex::SceneConstPtr &scene,
                        const planning_interface::MotionPlanRequest &request) override;
 
            /** \brief Plan a motion given a \a request and a \a scene.
             *  \param[in] scene A planning scene for the same \a robot_ to compute the plan in.
             *  \param[in] request The motion planning request to solve.
             *  \return The motion planning response generated by the planner.
             */
            planning_interface::MotionPlanResponse
            plan(const robowflex::SceneConstPtr &scene,
                 const planning_interface::MotionPlanRequest &request) override;
 
            /** \brief Return all planner configurations offered by this planner.
             *  Any of the configurations returned can be set as the planner for a motion planning query sent
             * to plan(). \return A vector of strings of planner configuration names.
             */
            std::vector<std::string> getPlannerConfigs() const override;
 
            PlanBuilderPtr builder;  ///< DART Motion Plan Builder
 
        private:
            /** \brief A funciton that returns an allocated planner
             */
            using PlannerAllocator = std::function<ompl::base::PlannerPtr()>;
 
            /** \brief Macro for creating and setting up an OMPL planner.
             *  \param[in] args Arguments to pass to the planner.
             *  \tparam[in] T Type of planner.
             *  \return Allocated planner.
             */
            template <typename T, typename... Args>
            PlannerAllocator makePlanner(Args &&... args)
            {
                return [&]() -> ompl::base::PlannerPtr {
                    if (builder)
                    {
                        auto p = std::make_shared<T>(builder->info, std::forward<Args>(args)...);
                        p->setup();
                        return p;
                    }
 
                    return nullptr;
                };
            }
 
            /** \brief Setup planner allocators.
             */
            void setupPlanners();
 
            std::map<std::string, PlannerAllocator> planner_allocators_;  ///< Named planner allocators.
            SceneConstPtr scene_;                                         ///< Current planning request scene.
            StructurePtr dart_scene_;   ///< DART version of current planning request scene.
            RobotPtr dart_robot_;       ///< DART version of the robot.
            WorldPtr world_;            ///< DART world containing robot and scene.
            ompl::base::GoalPtr goal_;  ///< Current motion planning goal.
        };
    }  // namespace darts
}  // namespace robowflex
 
#endif