30 #ifndef HPP_CORE_ROADMAP_HH
31 #define HPP_CORE_ROADMAP_HH
36 #include <hpp/util/serialization-fwd.hh>
73 value_type& minDistance,
bool reverse =
false);
76 value_type& minDistance,
bool reverse =
false) {
77 return nearestNode(*configuration, minDistance, reverse);
88 value_type& minDistance,
bool reverse =
false);
92 value_type& minDistance,
bool reverse =
false) {
93 return nearestNode(*configuration, connectedComponent, minDistance,
105 return nearestNodes(*configuration, k);
121 return nearestNodes(*configuration, connectedComponent, k);
197 initNode_ = addNode(config);
222 std::ostream&
print(std::ostream& os)
const;
257 void init(RoadmapWkPtr_t weak);
290 RoadmapWkPtr_t weak_;
std::set< RawPtr_t > RawPtrs_t
Definition: connected-component.hh:46
Optimization of the nearest neighbor search.
Definition: nearest-neighbor.hh:39
Definition: roadmap.hh:46
std::ostream & print(std::ostream &os) const
void insertPathVector(const PathVectorPtr_t &path, bool backAndForth)
NodePtr_t initNode() const
Definition: roadmap.hh:205
NodePtr_t nearestNode(const ConfigurationPtr_t &configuration, const ConnectedComponentPtr_t &connectedComponent, value_type &minDistance, bool reverse=false)
Definition: roadmap.hh:90
NodePtr_t addNodeAndEdge(const ConfigurationPtr_t &from, const NodePtr_t to, const PathPtr_t path)
void init(RoadmapWkPtr_t weak)
Store weak pointer to itself.
bool pathExists() const
Check that a path exists between the initial node and one goal node.
void addEdges(const NodePtr_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)
Add an edge between two nodes.
virtual NodePtr_t createNode(const ConfigurationPtr_t &configuration) const
void initNode(const ConfigurationPtr_t &config)
Definition: roadmap.hh:196
const Nodes_t & nodes() const
Definition: roadmap.hh:203
void addConnectedComponent(const NodePtr_t &node)
void resetGoalNodes()
Definition: roadmap.hh:194
const ConnectedComponents_t & connectedComponents() const
Get list of connected component of the roadmap.
NodePtr_t nearestNode(const Configuration_t &configuration, const ConnectedComponentPtr_t &connectedComponent, value_type &minDistance, bool reverse=false)
virtual void push_node(const NodePtr_t &n)
Definition: roadmap.hh:238
virtual void impl_addEdge(const EdgePtr_t &e)
Nodes_t nearestNodes(const Configuration_t &configuration, size_type k)
void merge(const RoadmapPtr_t &other)
Add the nodes and edges of a roadmap into this one.
NodePtr_t addGoalNode(const ConfigurationPtr_t &config)
Nodes_t nearestNodes(const Configuration_t &configuration, const ConnectedComponentPtr_t &connectedComponent, size_type k)
NearestNeighborPtr_t nearestNeighbor()
Get nearestNeighbor object.
const Edges_t & edges() const
Definition: roadmap.hh:204
NodePtr_t nearestNode(const ConfigurationPtr_t &configuration, value_type &minDistance, bool reverse=false)
Definition: roadmap.hh:75
NodePtr_t addNodeAndEdge(const NodePtr_t from, const ConfigurationPtr_t &to, const PathPtr_t path)
const NodeVector_t & goalNodes() const
Definition: roadmap.hh:206
void nearestNeighbor(NearestNeighborPtr_t nearestNeighbor)
Set new NearestNeighbor (roadmap must be empty)
virtual void clear()
Clear the roadmap by deleting nodes and edges.
Roadmap()
Definition: roadmap.hh:229
NodePtr_t addNodeAndEdges(const NodePtr_t from, const ConfigurationPtr_t &to, const PathPtr_t path)
static RoadmapPtr_t create(const DistancePtr_t &distance, const DevicePtr_t &robot)
Return shared pointer to new instance.
NodePtr_t addNode(const Configuration_t &config)
Definition: roadmap.hh:63
const DistancePtr_t & distance() const
NodeVector_t nodesWithinBall(const Configuration_t &configuration, const ConnectedComponentPtr_t &connectedComponent, value_type maxDistance)
NodePtr_t addNode(const ConfigurationPtr_t &config)
Nodes_t nearestNodes(const ConfigurationPtr_t &configuration, const ConnectedComponentPtr_t &connectedComponent, size_type k)
Definition: roadmap.hh:118
Roadmap(const DistancePtr_t &distance, const DevicePtr_t &robot)
NodePtr_t nearestNode(const Configuration_t &configuration, value_type &minDistance, bool reverse=false)
Nodes_t nearestNodes(const ConfigurationPtr_t &configuration, size_type k)
Definition: roadmap.hh:104
#define HPP_CORE_DLLAPI
Definition: config.hh:64
std::ostream & operator<<(std::ostream &os, const Constraint &constraint)
Definition: constraint.hh:99
pinocchio::value_type value_type
Definition: fwd.hh:173
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:192
shared_ptr< Distance > DistancePtr_t
Definition: fwd.hh:140
std::vector< NodePtr_t > NodeVector_t
Definition: fwd.hh:181
std::set< ConnectedComponentPtr_t, SharedComparator > ConnectedComponents_t
Definition: fwd.hh:126
std::list< Edge * > Edges_t
Definition: fwd.hh:144
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:198
pinocchio::size_type size_type
Definition: fwd.hh:172
std::list< NodePtr_t > Nodes_t
Definition: fwd.hh:180
pinocchio::ConfigurationPtr_t ConfigurationPtr_t
Definition: fwd.hh:108
shared_ptr< ConnectedComponent > ConnectedComponentPtr_t
Definition: fwd.hh:116
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:105
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:133
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:186
Definition: bi-rrt-planner.hh:35