k-prm-star.hh

Go to the documentation of this file.

//
// Copyright (c) 2018 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_CORE_PATH_PLANNER_K_PRM_STAR_HH
# define HPP_CORE_PATH_PLANNER_K_PRM_STAR_HH

# include <hpp/core/path-planner.hh>

namespace hpp {
  namespace core {
    namespace pathPlanner {
      class HPP_CORE_DLLAPI kPrmStar : public PathPlanner
      {
      public:
        enum STATE {
          BUILD_ROADMAP,
          LINK_NODES,
          CONNECT_INIT_GOAL,
          FAILURE
        }; // enum STATE
        static const double kPRM;
        typedef PathPlanner Parent_t;
        static kPrmStarPtr_t create (const Problem& problem);
        static kPrmStarPtr_t createWithRoadmap (const Problem& problem,
                                                const RoadmapPtr_t& roadmap);
      virtual void startSolve ();
      virtual void oneStep ();
      STATE getComputationState () const;

      protected:
        kPrmStar (const Problem& problem);
        kPrmStar (const Problem& problem, const RoadmapPtr_t& roadmap);
        void init (const kPrmStarWkPtr_t& weak);

      private:
        STATE state_;
        void generateRandomConfig ();
        void linkNodes ();
        void connectInitAndGoal ();
        bool connectNodeToClosestNeighbors (const NodePtr_t& node);
        std::size_t numberNodes_;
        Nodes_t::const_iterator linkingNodeIt_;
        Nodes_t::iterator itNeighbor_;
        size_type numberNeighbors_;
        Nodes_t neighbors_;
        bool reachedLastNeighbor_;
        kPrmStarWkPtr_t weak_;
      }; // class kPrmStar
    } // namespace pathPlanner
  } // namespace core
} // namespace hpp

#endif // HPP_CORE_PATH_PLANNER_K_PRM_STAR_HH