visibility-prm-planner.hh

Go to the documentation of this file.

//
// Copyright (c) 2014 CNRS
// Authors: Mylene Campana
//
// 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_VISIBILITY_PRM_PLANNER_HH
# define HPP_CORE_VISIBILITY_PRM_PLANNER_HH

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

namespace hpp {
  namespace core {

    class HPP_CORE_DLLAPI VisibilityPrmPlanner : public PathPlanner
    {
    public:
      static VisibilityPrmPlannerPtr_t createWithRoadmap
    (const Problem& problem, const RoadmapPtr_t& roadmap);
      static VisibilityPrmPlannerPtr_t create (const Problem& problem);
      virtual void oneStep ();
    protected:
      VisibilityPrmPlanner (const Problem& problem, 
                const RoadmapPtr_t& roadmap);
      VisibilityPrmPlanner (const Problem& problem);
      void init (const VisibilityPrmPlannerWkPtr_t& weak);
    private:
      typedef boost::tuple <NodePtr_t, ConfigurationPtr_t, PathPtr_t>
    DelayedEdge_t;
      typedef std::vector <DelayedEdge_t> DelayedEdges_t;
      VisibilityPrmPlannerWkPtr_t weakPtr_;
      DelayedEdges_t delayedEdges_;
      std::map <NodePtr_t, bool> nodeStatus_; // true for guard node

      bool visibleFromCC (const Configuration_t q, 
              const ConnectedComponentPtr_t cc);
      
      void applyConstraints (
          const Configuration_t& qFrom, 
          const Configuration_t& qTo,
          Configuration_t& qOut);

      bool constrApply_; // True if applyConstraints has successed
    };
  } // namespace core
} // namespace hpp
#endif // HPP_CORE_VISIBILITY_PRM_PLANNER_HH