roadmap.hh

Go to the documentation of this file.

// Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-manipulation.
// hpp-manipulation 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-manipulation 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-manipulation. If not, see <http://www.gnu.org/licenses/>.

#ifndef HPP_MANIPULATION_ROADMAP_HH
# define HPP_MANIPULATION_ROADMAP_HH

# include <hpp/core/roadmap.hh>

# include "hpp/manipulation/config.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
# include <hpp/manipulation/deprecated.hh>
# include <hpp/manipulation/leaf-connected-comp.hh>

namespace hpp {
  namespace manipulation {

    class HPP_MANIPULATION_DLLAPI Roadmap : public core::Roadmap
    {
      public:
        typedef core::Roadmap Parent;

        static RoadmapPtr_t create (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot);

        void insertHistogram (const graph::HistogramPtr_t hist);

        void constraintGraph (const graph::GraphPtr_t& graph);

        void clear ();

        void push_node (const core::NodePtr_t& n);

        RoadmapNodePtr_t nearestNode (const ConfigurationPtr_t& configuration,
            const ConnectedComponentPtr_t& connectedComponent,
            const graph::StatePtr_t& state,
            value_type& minDistance) const;

    graph::StatePtr_t getNode(RoadmapNodePtr_t node) HPP_MANIPULATION_DEPRECATED;

        void connect (const LeafConnectedCompPtr_t& cc1,
                      const LeafConnectedCompPtr_t& cc2);

        void merge (const LeafConnectedCompPtr_t& cc1,
                    LeafConnectedComp::LeafConnectedComps_t& ccs);

    graph::StatePtr_t getState(RoadmapNodePtr_t node);

        const LeafConnectedComps_t& leafConnectedComponents () const
        {
          return leafCCs_;
        }

      protected:
        void statInsert (const RoadmapNodePtr_t& n);

        Roadmap (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot);

        core::NodePtr_t createNode (const ConfigurationPtr_t& config) const;

        void init (const RoadmapPtr_t& shPtr)
    {
          Parent::init (shPtr);
      weak_ = shPtr;
    }

        virtual void addEdge (const core::EdgePtr_t& edge);

      private:
        typedef graph::Histograms_t Histograms_t;
        Histograms_t histograms_;
        graph::GraphPtr_t graph_;
        RoadmapWkPtr_t weak_;
        LeafConnectedComps_t leafCCs_;
    };
  } // namespace manipulation
} // namespace hpp

#endif // HPP_MANIPULATION_ROADMAP_HH