hpp-rbprm  4.14.0
Implementation of RB-PRM planner using hpp.
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 
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:
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
shared_ptr< RbprmRoadmap > RbprmRoadmapPtr_t
Definition: rbprm-roadmap.hh:13
RbprmRoadmap(const DistancePtr_t &distance, const DevicePtr_t &robot)
Definition: rbprm-roadmap.hh:58
Definition: algorithm.hh:26
RbprmNode * RbprmNodePtr_t
Definition: rbprm-node.hh:16
HPP_PREDEF_CLASS(RbprmNode)
Definition: rbprm-node.hh:22
pinocchio::value_type distance(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2)
virtual RbprmNodePtr_t createNode(const ConfigurationPtr_t &configuration) const
Definition: rbprm-roadmap.hh:64
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
Definition: rbprm-roadmap.hh:15