roadmap.hh

Go to the documentation of this file.

//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
 
#ifndef HPP_CORE_ROADMAP_HH
#define HPP_CORE_ROADMAP_HH
 
#include <hpp/core/config.hh>
#include <hpp/core/connected-component.hh>
#include <hpp/core/fwd.hh>
#include <hpp/util/serialization-fwd.hh>
#include <iostream>
 
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(ConfigurationIn_t config);
 
  NodePtr_t nearestNode(ConfigurationIn_t configuration,
                        value_type& minDistance, bool reverse = false);
 
  NodePtr_t nearestNode(ConfigurationIn_t configuration,
                        const ConnectedComponentPtr_t& connectedComponent,
                        value_type& minDistance, bool reverse = false);
 
  Nodes_t nearestNodes(ConfigurationIn_t configuration, size_type k);
 
  Nodes_t nearestNodes(ConfigurationIn_t configuration,
                       const ConnectedComponentPtr_t& connectedComponent,
                       size_type k);
 
  NodeVector_t nodesWithinBall(
      ConfigurationIn_t configuration,
      const ConnectedComponentPtr_t& connectedComponent,
      value_type maxDistance);
 
  NodePtr_t addNodeAndEdges(const NodePtr_t from, ConfigurationIn_t to,
                            const PathPtr_t path);
 
  NodePtr_t addNodeAndEdge(const NodePtr_t from, ConfigurationIn_t to,
                           const PathPtr_t path);
 
  NodePtr_t addNodeAndEdge(ConfigurationIn_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);
 
  void merge(const RoadmapPtr_t& other);
 
  void insertPathVector(const PathVectorPtr_t& path, bool backAndForth);
 
  NodePtr_t addGoalNode(ConfigurationIn_t config);
 
  void resetGoalNodes() { goalNodes_.clear(); }
 
  void initNode(ConfigurationIn_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);
 
  Roadmap() {};
 
  void addConnectedComponent(const NodePtr_t& node);
 
  virtual void push_node(const NodePtr_t& n) { nodes_.push_back(n); }
 
  virtual void impl_addEdge(const EdgePtr_t& e);
 
  virtual NodePtr_t createNode(ConfigurationIn_t configuration) const;
 
  void init(RoadmapWkPtr_t weak);
 
 private:
  NodePtr_t addNode(ConfigurationIn_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_;
 
  HPP_SERIALIZABLE();
};  // class Roadmap
std::ostream& operator<<(std::ostream& os, const Roadmap& r);
}  //   namespace core
}  // namespace hpp
#endif  // HPP_CORE_ROADMAP_HH