roadmap.hh

Go to the documentation of this file.

//
// Copyright (c) 2014 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_ROADMAP_HH
# define HPP_CORE_ROADMAP_HH

# include <iostream>
# include <hpp/core/fwd.hh>
# include <hpp/core/config.hh>

# include <hpp/core/connected-component.hh>

namespace hpp {
  namespace core {

    class HPP_CORE_DLLAPI Roadmap {
    public:
      static RoadmapPtr_t create (const DistancePtr_t& distance, const DevicePtr_t& robot);

      virtual void clear ();

      NodePtr_t addNode (const ConfigurationPtr_t& config);

      NodePtr_t addNode (const Configuration_t& config)
      {
        return addNode (ConfigurationPtr_t (new Configuration_t(config)));
      }

      NodePtr_t nearestNode (const Configuration_t& configuration,
           value_type& minDistance, bool reverse = false);

      NodePtr_t nearestNode (const ConfigurationPtr_t& configuration,
           value_type& minDistance, bool reverse = false)
      {
        return nearestNode (*configuration, minDistance, reverse);
      }

      NodePtr_t nearestNode (const Configuration_t& configuration,
                 const ConnectedComponentPtr_t& connectedComponent,
           value_type& minDistance, bool reverse = false);

      NodePtr_t nearestNode (const ConfigurationPtr_t& configuration,
                 const ConnectedComponentPtr_t& connectedComponent,
           value_type& minDistance, bool reverse = false)
      {
        return nearestNode (*configuration, connectedComponent, minDistance, reverse);
      }

      Nodes_t nearestNodes (const Configuration_t& configuration,
                            size_type k);

      Nodes_t nearestNodes (const ConfigurationPtr_t& configuration,
                            size_type k)
      {
        return nearestNodes (*configuration, k);
      }

      Nodes_t nearestNodes (const Configuration_t& configuration,
                            const ConnectedComponentPtr_t&
                            connectedComponent,
                            size_type k);

      Nodes_t nearestNodes (const ConfigurationPtr_t& configuration,
                            const ConnectedComponentPtr_t&
                            connectedComponent,
                            size_type k)
      {
        return nearestNodes (*configuration, connectedComponent, k);
      }

      NodePtr_t addNodeAndEdges (const NodePtr_t from,
                 const ConfigurationPtr_t& to,
                 const PathPtr_t path);

      NodePtr_t addNodeAndEdge (const NodePtr_t from,
                 const ConfigurationPtr_t& to,
                 const PathPtr_t path);

      NodePtr_t addNodeAndEdge (const ConfigurationPtr_t& from,
                      const NodePtr_t to,
                      const PathPtr_t path);


      EdgePtr_t addEdge (const NodePtr_t& n1, const NodePtr_t& n2,
             const PathPtr_t& path);

      void addEdges (const NodePtr_t from, const NodePtr_t& to,
             const PathPtr_t& path);

      NodePtr_t addGoalNode (const ConfigurationPtr_t& config);

      void resetGoalNodes ()
      {
    goalNodes_.clear ();
      }

      void initNode (const ConfigurationPtr_t& config)
      {
    initNode_ = addNode (config);
      }

      virtual ~Roadmap ();
      bool pathExists () const;
      const Nodes_t& nodes () const
      {
    return nodes_;
      }
      const Edges_t& edges () const
      {
    return edges_;
      }
      NodePtr_t initNode () const
      {
    return initNode_;
      }
      const NodeVector_t& goalNodes () const
      {
    return goalNodes_;
      }
      const ConnectedComponents_t& connectedComponents () const;

      NearestNeighborPtr_t nearestNeighbor();

      void nearestNeighbor(NearestNeighborPtr_t nearestNeighbor);

      const DistancePtr_t& distance () const;
      std::ostream& print (std::ostream& os) const;

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

      void addConnectedComponent (const NodePtr_t& node);

      virtual void push_node (const NodePtr_t& n)
      {
        nodes_.push_back (n);
      }

      virtual void addEdge (const EdgePtr_t& e);

      virtual NodePtr_t createNode (const ConfigurationPtr_t& configuration) const;

      void init (RoadmapWkPtr_t weak);

    private:
      NodePtr_t addNode (const ConfigurationPtr_t& config,
             ConnectedComponentPtr_t connectedComponent);

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

      void merge (const ConnectedComponentPtr_t& cc1,
          ConnectedComponent::RawPtrs_t& ccs);

      const DistancePtr_t distance_;
      ConnectedComponents_t connectedComponents_;
      Nodes_t nodes_;
      Edges_t edges_;
      NodePtr_t initNode_;
      NodeVector_t goalNodes_;
      NearestNeighborPtr_t nearestNeighbor_;
      RoadmapWkPtr_t weak_;
    }; // class Roadmap
    std::ostream& operator<< (std::ostream& os, const Roadmap& r);
  } //   namespace core
} // namespace hpp
#endif // HPP_CORE_ROADMAP_HH