hpp-rbprm 4.15.1
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
rbprm-roadmap.hh
Go to the documentation of this file.
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>
7#include <hpp/util/debug.hh>
8
9namespace hpp {
10namespace core {
11using pinocchio::displayConfig;
13typedef shared_ptr<RbprmRoadmap> RbprmRoadmapPtr_t;
14
15class HPP_CORE_DLLAPI RbprmRoadmap : public Roadmap {
16 public:
18 static RbprmRoadmapPtr_t create(const DistancePtr_t& distance,
19 const DevicePtr_t& robot) {
20 RbprmRoadmap* ptr = new RbprmRoadmap(distance, robot);
21 return RbprmRoadmapPtr_t(ptr);
22 }
23
24 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:
58 RbprmRoadmap(const DistancePtr_t& distance, const DevicePtr_t& robot)
59 : Roadmap(distance, robot) {}
60
65 const ConfigurationPtr_t& configuration) const {
66 return RbprmNodePtr_t(new RbprmNode(configuration));
67 }
68
69}; // class
70
71} // namespace core
72} // namespace hpp
73#endif // HPP_RBPRM_ROADMAP_HH
Definition: rbprm-node.hh:22
Definition: rbprm-roadmap.hh:15
RbprmRoadmap(const DistancePtr_t &distance, const DevicePtr_t &robot)
Definition: rbprm-roadmap.hh:58
virtual ~RbprmRoadmap()
Definition: rbprm-roadmap.hh:24
static RbprmRoadmapPtr_t create(const DistancePtr_t &distance, const DevicePtr_t &robot)
Return shared pointer to new instance.
Definition: rbprm-roadmap.hh:18
virtual RbprmNodePtr_t createNode(const ConfigurationPtr_t &configuration) const
Definition: rbprm-roadmap.hh:64
HPP_PREDEF_CLASS(RbprmNode)
shared_ptr< RbprmRoadmap > RbprmRoadmapPtr_t
Definition: rbprm-roadmap.hh:13
Definition: algorithm.hh:26