hpp-rbprm  4.10.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 boost::shared_ptr <RbprmRoadmap> RbprmRoadmapPtr_t;
14 
15  class HPP_CORE_DLLAPI RbprmRoadmap : public Roadmap
16  {
17  public:
19  static RbprmRoadmapPtr_t create (const DistancePtr_t& distance, const DevicePtr_t& robot){
20  RbprmRoadmap* ptr = new RbprmRoadmap (distance, robot);
21  return RbprmRoadmapPtr_t (ptr);
22  }
23 
24  virtual ~RbprmRoadmap ()
25  {
26  clear ();
27  }
28 
29  /* virtual RbprmNodePtr_t addNode (const ConfigurationPtr_t& configuration)
30  {
31  value_type distance;
32  if (nodes().size () != 0) {
33  NodePtr_t nearest = nearestNode (configuration, distance);
34  if (*(nearest->configuration ()) == *configuration) {
35  return static_cast<core::RbprmNodePtr_t>(nearest);
36  }
37  }
38  RbprmNodePtr_t node = createNode (configuration);
39  hppDout (info, "Added node: " << displayConfig (*configuration));
40  push_node (node);
41  // Node constructor creates a new connected component. This new
42  // connected component needs to be added in the roadmap and the
43  // new node needs to be registered in the connected component.
44  addConnectedComponent (node);
45  return node;
46  }
47 
48 
49 
50  virtual RbprmNodePtr_t addNodeAndEdges (const NodePtr_t from,
51  const ConfigurationPtr_t& to,
52  const PathPtr_t path)
53  {
54  NodePtr_t nodeTo = Roadmap::addNodeAndEdges(from,to,path);
55  return static_cast<core::RbprmNodePtr_t>(nodeTo);
56  }
57  */
58  protected:
61  RbprmRoadmap (const DistancePtr_t& distance, const DevicePtr_t& robot):
62  Roadmap(distance,robot)
63  {
64  }
65 
66 
67 
71  virtual RbprmNodePtr_t createNode (const ConfigurationPtr_t& configuration) const {
72  return RbprmNodePtr_t (new RbprmNode (configuration));
73  }
74 
75 
76 
77  }; // class
78 
79  }//core
80 }//hpp
81 #endif // HPP_RBPRM_ROADMAP_HH
hpp::core::RbprmNode
Definition: rbprm-node.hh:23
hpp::tools::distance
pinocchio::value_type distance(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2)
hpp::core::RbprmRoadmap
Definition: rbprm-roadmap.hh:15
hpp
Definition: algorithm.hh:27
hpp::core::RbprmNodePtr_t
RbprmNode * RbprmNodePtr_t
Definition: rbprm-node.hh:17
hpp::core::RbprmRoadmap::createNode
virtual RbprmNodePtr_t createNode(const ConfigurationPtr_t &configuration) const
Definition: rbprm-roadmap.hh:71
hpp::core::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(RbprmNode)
hpp::core::RbprmRoadmap::create
static RbprmRoadmapPtr_t create(const DistancePtr_t &distance, const DevicePtr_t &robot)
Return shared pointer to new instance.
Definition: rbprm-roadmap.hh:19
rbprm-node.hh
hpp::core::RbprmRoadmapPtr_t
boost::shared_ptr< RbprmRoadmap > RbprmRoadmapPtr_t
Definition: rbprm-roadmap.hh:13
hpp::core::RbprmRoadmap::~RbprmRoadmap
virtual ~RbprmRoadmap()
Definition: rbprm-roadmap.hh:24
hpp::core::RbprmRoadmap::RbprmRoadmap
RbprmRoadmap(const DistancePtr_t &distance, const DevicePtr_t &robot)
Definition: rbprm-roadmap.hh:61