diff --git a/include/hpp/manipulation/graph-steering-method.hh b/include/hpp/manipulation/graph-steering-method.hh index afed8a51c9e9d94ae7710193f4be402603084e13..eb5a132b3201e8fc7d05b9838749eb267f768596 100644 --- a/include/hpp/manipulation/graph-steering-method.hh +++ b/include/hpp/manipulation/graph-steering-method.hh @@ -37,9 +37,17 @@ namespace hpp { class HPP_MANIPULATION_DLLAPI GraphSteeringMethod : public SteeringMethod { public: - /// Constructor - GraphSteeringMethod (const model::DevicePtr_t& robot); + /// Create instance and return shared pointer + static GraphSteeringMethodPtr_t create (const model::DevicePtr_t& robot); + /// Create copy and return shared pointer + static GraphSteeringMethodPtr_t createCopy + (const GraphSteeringMethodPtr_t& other); + /// Copy instance and return shared pointer + virtual core::SteeringMethodPtr_t copy () const + { + return createCopy (weak_.lock ()); + } /// \name Graph of constraints applicable to the robot. /// \{ @@ -59,8 +67,20 @@ namespace hpp { const core::WeighedDistancePtr_t& distance () const; protected: + /// Constructor + GraphSteeringMethod (const model::DevicePtr_t& robot); + + /// Copy constructor + GraphSteeringMethod (const GraphSteeringMethod&); + virtual PathPtr_t impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const; + void init (GraphSteeringMethodWkPtr_t weak) + { + core::SteeringMethod::init (weak); + weak_ = weak; + } + private: /// A pointer to the graph of constraint. graph::GraphPtr_t graph_; @@ -68,6 +88,8 @@ namespace hpp { core::DeviceWkPtr_t robot_; /// Metric in configuration space. core::WeighedDistancePtr_t distance_; + /// Weak pointer to itself + GraphSteeringMethodWkPtr_t weak_; }; /// \} } // namespace manipulation diff --git a/src/graph-steering-method.cc b/src/graph-steering-method.cc index bf8a9ef6246229738d381a28a7cbcf9e51f32994..10f18719b287d18044f12201de1ac1049fc10100 100644 --- a/src/graph-steering-method.cc +++ b/src/graph-steering-method.cc @@ -23,9 +23,33 @@ namespace hpp { namespace manipulation { + GraphSteeringMethodPtr_t GraphSteeringMethod::create + (const model::DevicePtr_t& robot) + { + GraphSteeringMethod* ptr = new GraphSteeringMethod (robot); + GraphSteeringMethodPtr_t shPtr (ptr); + ptr->init (shPtr); + return shPtr; + } + + GraphSteeringMethodPtr_t GraphSteeringMethod::createCopy + (const GraphSteeringMethodPtr_t& other) + { + GraphSteeringMethod* ptr = new GraphSteeringMethod (*other); + GraphSteeringMethodPtr_t shPtr (ptr); + ptr->init (shPtr); + return shPtr; + } + GraphSteeringMethod::GraphSteeringMethod (const model::DevicePtr_t& robot) : - SteeringMethod (), robot_ (robot), - distance_ (core::WeighedDistance::create (robot)) + SteeringMethod (), graph_ (), robot_ (robot), + distance_ (core::WeighedDistance::create (robot)), weak_ () + { + } + + GraphSteeringMethod::GraphSteeringMethod (const GraphSteeringMethod& other): + SteeringMethod (other), graph_ (other.graph_), robot_ (other.robot_), + distance_ (other.distance_) { }