Commit 72a03636 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Steering methods are now handled via shared pointers.

parent 2d91f4f6
......@@ -30,10 +30,21 @@ namespace hpp {
class HPP_CORE_DLLAPI SteeringMethodStraight : public SteeringMethod
{
public:
SteeringMethodStraight (const DevicePtr_t& device) :
SteeringMethod (), device_ (device),
distance_ (WeighedDistance::create (device))
/// Create instance and return shared pointer
static SteeringMethodStraightPtr_t create (const DevicePtr_t& device)
{
SteeringMethodStraight* ptr = new SteeringMethodStraight (device);
SteeringMethodStraightPtr_t shPtr (ptr);
return shPtr;
}
/// Create instance and return shared pointer
static SteeringMethodStraightPtr_t create
(const DevicePtr_t& device, const WeighedDistancePtr_t& distance)
{
SteeringMethodStraight* ptr = new SteeringMethodStraight (device,
distance);
SteeringMethodStraightPtr_t shPtr (ptr);
return shPtr;
}
/// create a path between two configurations
virtual PathPtr_t impl_compute (ConfigurationIn_t q1,
......@@ -44,6 +55,21 @@ namespace hpp {
path->constraints (constraints ());
return path;
}
protected:
/// Constructor with robot
/// Weighed distance is created from robot
SteeringMethodStraight (const DevicePtr_t& device) :
SteeringMethod (), device_ (device),
distance_ (WeighedDistance::create (device))
{
}
/// Constructor with weighed distance
SteeringMethodStraight (const DevicePtr_t& device,
const WeighedDistancePtr_t& distance) :
SteeringMethod (), device_ (device),
distance_ (distance)
{
}
private:
DeviceWkPtr_t device_;
WeighedDistancePtr_t distance_;
......
......@@ -39,7 +39,7 @@ namespace hpp {
robot_ (robot),
distance_ (WeighedDistance::create (robot)),
initConf_ (), goalConfigurations_ (),
steeringMethod_ (new core::SteeringMethodStraight (robot)),
steeringMethod_ (SteeringMethodStraight::create (robot)),
configValidations_ (ConfigValidations::create ()),
pathValidation_ (DiscretizedCollisionChecking::create
(robot, 0.05)),
......
......@@ -42,6 +42,7 @@ using hpp::model::Device;
using hpp::model::DevicePtr_t;
using hpp::model::JointTranslation;
using hpp::core::SteeringMethodStraight;
using hpp::core::SteeringMethodStraightPtr_t;
using hpp::core::RoadmapPtr_t;
using hpp::core::Roadmap;
using hpp::core::NodePtr_t;
......@@ -75,7 +76,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
xJoint->addChildJoint (yJoint);
// Create steering method
SteeringMethodStraight sm (robot);
SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (robot);
// create roadmap
RoadmapPtr_t r = Roadmap::create (WeighedDistance::create
(robot, boost::assign::list_of (1)(1)),
......@@ -128,7 +129,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
}
BOOST_CHECK (!r->pathExists ());
// 0 -> 1
addEdge (r, sm, nodes, 0, 1);
addEdge (r, *sm, nodes, 0, 1);
BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 6);
for (std::size_t i=0; i < nodes.size (); ++i) {
for (std::size_t j=i+1; j < nodes.size (); ++j) {
......@@ -142,7 +143,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
std::cout << *r << std::endl;
// 1 -> 0
addEdge (r, sm, nodes, 1, 0);
addEdge (r, *sm, nodes, 1, 0);
BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 5);
BOOST_CHECK (nodes [0]->connectedComponent () ==
......@@ -158,7 +159,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
BOOST_CHECK (!r->pathExists ());
std::cout << *r << std::endl;
// 1 -> 2
addEdge (r, sm, nodes, 1, 2);
addEdge (r, *sm, nodes, 1, 2);
BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 5);
BOOST_CHECK (nodes [0]->connectedComponent () ==
nodes [1]->connectedComponent ());
......@@ -174,7 +175,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
std::cout << *r << std::endl;
// 2 -> 0
addEdge (r, sm, nodes, 2, 0);
addEdge (r, *sm, nodes, 2, 0);
BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 4);
BOOST_CHECK (nodes [0]->connectedComponent () ==
nodes [1]->connectedComponent ());
......@@ -192,7 +193,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
std::cout << *r << std::endl;
// 2 -> 3
addEdge (r, sm, nodes, 2, 3);
addEdge (r, *sm, nodes, 2, 3);
BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 4);
BOOST_CHECK (nodes [0]->connectedComponent () ==
nodes [1]->connectedComponent ());
......@@ -210,7 +211,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
std::cout << *r << std::endl;
// 2 -> 4
addEdge (r, sm, nodes, 2, 4);
addEdge (r, *sm, nodes, 2, 4);
BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 4);
BOOST_CHECK (nodes [0]->connectedComponent () ==
nodes [1]->connectedComponent ());
......@@ -228,7 +229,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
std::cout << *r << std::endl;
// 3 -> 5
addEdge (r, sm, nodes, 3, 5);
addEdge (r, *sm, nodes, 3, 5);
BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 4);
BOOST_CHECK (nodes [0]->connectedComponent () ==
nodes [1]->connectedComponent ());
......@@ -246,7 +247,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
std::cout << *r << std::endl;
// 4 -> 5
addEdge (r, sm, nodes, 4, 5);
addEdge (r, *sm, nodes, 4, 5);
BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 4);
BOOST_CHECK (nodes [0]->connectedComponent () ==
nodes [1]->connectedComponent ());
......@@ -264,7 +265,7 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
std::cout << *r << std::endl;
// 5 -> 0
addEdge (r, sm, nodes, 5, 0);
addEdge (r, *sm, nodes, 5, 0);
BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 1);
BOOST_CHECK (nodes [0]->connectedComponent () ==
nodes [1]->connectedComponent ());
......
......@@ -74,7 +74,7 @@ BOOST_AUTO_TEST_CASE (kdTree) {
BasicConfigurationShooter confShoot(robot);
nearestNeighbor::KDTree kdTree(robot,distance,30);
nearestNeighbor::Basic basic (distance);
SteeringMethodPtr_t sm (new SteeringMethodStraight (robot));
SteeringMethodPtr_t sm = SteeringMethodStraight::create (robot);
// Add 4 connectedComponents with 2000 nodes each
ConfigurationPtr_t configuration;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment