1 |
|
|
#ifndef HPP_RBPRM_ROADMAP_HH |
2 |
|
|
#define HPP_RBPRM_ROADMAP_HH |
3 |
|
|
|
4 |
|
|
#include <hpp/core/roadmap.hh> |
5 |
|
|
#include <hpp/pinocchio/configuration.hh> |
6 |
|
|
#include <hpp/rbprm/planner/rbprm-node.hh> |
7 |
|
|
#include <hpp/util/debug.hh> |
8 |
|
|
|
9 |
|
|
namespace hpp { |
10 |
|
|
namespace core { |
11 |
|
|
using pinocchio::displayConfig; |
12 |
|
|
HPP_PREDEF_CLASS(RbprmRoadmap); |
13 |
|
|
typedef shared_ptr<RbprmRoadmap> RbprmRoadmapPtr_t; |
14 |
|
|
|
15 |
|
|
class HPP_CORE_DLLAPI RbprmRoadmap : public Roadmap { |
16 |
|
|
public: |
17 |
|
|
/// Return shared pointer to new instance. |
18 |
|
24 |
static RbprmRoadmapPtr_t create(const DistancePtr_t& distance, |
19 |
|
|
const DevicePtr_t& robot) { |
20 |
✓✗ |
24 |
RbprmRoadmap* ptr = new RbprmRoadmap(distance, robot); |
21 |
|
24 |
return RbprmRoadmapPtr_t(ptr); |
22 |
|
|
} |
23 |
|
|
|
24 |
|
32 |
virtual ~RbprmRoadmap() { clear(); } |
25 |
|
|
|
26 |
|
|
/* virtual RbprmNodePtr_t addNode (const ConfigurationPtr_t& configuration) |
27 |
|
|
{ |
28 |
|
|
value_type distance; |
29 |
|
|
if (nodes().size () != 0) { |
30 |
|
|
NodePtr_t nearest = nearestNode (configuration, distance); |
31 |
|
|
if (*(nearest->configuration ()) == *configuration) { |
32 |
|
|
return static_cast<core::RbprmNodePtr_t>(nearest); |
33 |
|
|
} |
34 |
|
|
} |
35 |
|
|
RbprmNodePtr_t node = createNode (configuration); |
36 |
|
|
hppDout (info, "Added node: " << displayConfig (*configuration)); |
37 |
|
|
push_node (node); |
38 |
|
|
// Node constructor creates a new connected component. This new |
39 |
|
|
// connected component needs to be added in the roadmap and the |
40 |
|
|
// new node needs to be registered in the connected component. |
41 |
|
|
addConnectedComponent (node); |
42 |
|
|
return node; |
43 |
|
|
} |
44 |
|
|
|
45 |
|
|
|
46 |
|
|
|
47 |
|
|
virtual RbprmNodePtr_t addNodeAndEdges (const NodePtr_t from, |
48 |
|
|
const ConfigurationPtr_t& to, |
49 |
|
|
const PathPtr_t path) |
50 |
|
|
{ |
51 |
|
|
NodePtr_t nodeTo = Roadmap::addNodeAndEdges(from,to,path); |
52 |
|
|
return static_cast<core::RbprmNodePtr_t>(nodeTo); |
53 |
|
|
} |
54 |
|
|
*/ |
55 |
|
|
protected: |
56 |
|
|
/// Constructor |
57 |
|
|
/// \param distance distance function for nearest neighbor computations |
58 |
|
24 |
RbprmRoadmap(const DistancePtr_t& distance, const DevicePtr_t& robot) |
59 |
|
24 |
: Roadmap(distance, robot) {} |
60 |
|
|
|
61 |
|
|
/// Node factory |
62 |
|
|
/// Reimplement the function if you want to create an instance of a |
63 |
|
|
/// child class of Node |
64 |
|
547 |
virtual RbprmNodePtr_t createNode( |
65 |
|
|
const ConfigurationPtr_t& configuration) const { |
66 |
✓✗ |
547 |
return RbprmNodePtr_t(new RbprmNode(configuration)); |
67 |
|
|
} |
68 |
|
|
|
69 |
|
|
}; // class |
70 |
|
|
|
71 |
|
|
} // namespace core |
72 |
|
|
} // namespace hpp |
73 |
|
|
#endif // HPP_RBPRM_ROADMAP_HH |