hpp-rbprm  4.12.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>
6 #include <hpp/util/debug.hh>
7 #include <hpp/pinocchio/configuration.hh>
8 
9 namespace hpp {
10 namespace core {
11 using pinocchio::displayConfig;
12 HPP_PREDEF_CLASS(RbprmRoadmap);
13 typedef std::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, const DevicePtr_t& robot) {
19  RbprmRoadmap* ptr = new RbprmRoadmap(distance, robot);
20  return RbprmRoadmapPtr_t(ptr);
21  }
22 
23  virtual ~RbprmRoadmap() { clear(); }
24 
25  /* virtual RbprmNodePtr_t addNode (const ConfigurationPtr_t& configuration)
26  {
27  value_type distance;
28  if (nodes().size () != 0) {
29  NodePtr_t nearest = nearestNode (configuration, distance);
30  if (*(nearest->configuration ()) == *configuration) {
31  return static_cast<core::RbprmNodePtr_t>(nearest);
32  }
33  }
34  RbprmNodePtr_t node = createNode (configuration);
35  hppDout (info, "Added node: " << displayConfig (*configuration));
36  push_node (node);
37  // Node constructor creates a new connected component. This new
38  // connected component needs to be added in the roadmap and the
39  // new node needs to be registered in the connected component.
40  addConnectedComponent (node);
41  return node;
42  }
43 
44 
45 
46  virtual RbprmNodePtr_t addNodeAndEdges (const NodePtr_t from,
47  const ConfigurationPtr_t& to,
48  const PathPtr_t path)
49  {
50  NodePtr_t nodeTo = Roadmap::addNodeAndEdges(from,to,path);
51  return static_cast<core::RbprmNodePtr_t>(nodeTo);
52  }
53  */
54  protected:
57  RbprmRoadmap(const DistancePtr_t& distance, const DevicePtr_t& robot) : Roadmap(distance, robot) {}
58 
62  virtual RbprmNodePtr_t createNode(const ConfigurationPtr_t& configuration) const {
63  return RbprmNodePtr_t(new RbprmNode(configuration));
64  }
65 
66 }; // class
67 
68 } // namespace core
69 } // namespace hpp
70 #endif // HPP_RBPRM_ROADMAP_HH
RbprmRoadmap(const DistancePtr_t &distance, const DevicePtr_t &robot)
Definition: rbprm-roadmap.hh:57
Definition: algorithm.hh:27
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)
std::shared_ptr< RbprmRoadmap > RbprmRoadmapPtr_t
Definition: rbprm-roadmap.hh:13
virtual RbprmNodePtr_t createNode(const ConfigurationPtr_t &configuration) const
Definition: rbprm-roadmap.hh:62
virtual ~RbprmRoadmap()
Definition: rbprm-roadmap.hh:23
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