Commit 2d8f2539 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Florent Lamiraux florent@laas.fr
Browse files

SteeringMethod and Distance constructors take const Problem& instead of ProblemPtr_t

* This homogenizes the API.

Conflicts:
	include/hpp/core/distance/reeds-shepp.hh
	include/hpp/core/steering-method/reeds-shepp.hh
	src/distance/reeds-shepp.cc
	src/problem.cc
	src/steering-method/reeds-shepp.cc
parent 0606e364
......@@ -35,14 +35,14 @@ namespace hpp {
{
public:
virtual DistancePtr_t clone () const;
static ReedsSheppPtr_t create (const ProblemPtr_t& problem);
static ReedsSheppPtr_t create (const ProblemPtr_t& problem,
static ReedsSheppPtr_t create (const Problem& problem);
static ReedsSheppPtr_t create (const Problem& problem,
const value_type& turningRadius,
JointPtr_t xyJoint, JointPtr_t rzJoint);
static ReedsSheppPtr_t createCopy (const ReedsSheppPtr_t& distance);
protected:
ReedsShepp (const ProblemPtr_t& problem);
ReedsShepp (const ProblemPtr_t& problem,
ReedsShepp (const Problem& problem);
ReedsShepp (const Problem& problem,
const value_type& turningRadius,
JointPtr_t xyJoint, JointPtr_t rzJoint);
ReedsShepp (const ReedsShepp& distance);
......
......@@ -46,9 +46,9 @@ namespace hpp {
PathProjectorBuilder_t;
typedef boost::function <ConfigurationShooterPtr_t (const DevicePtr_t&) >
ConfigurationShooterBuilder_t;
typedef boost::function <DistancePtr_t (const ProblemPtr_t&) >
typedef boost::function <DistancePtr_t (const Problem&) >
DistanceBuilder_t;
typedef boost::function <SteeringMethodPtr_t (const ProblemPtr_t&) >
typedef boost::function <SteeringMethodPtr_t (const Problem&) >
SteeringMethodBuilder_t;
typedef std::vector<CollisionObjectPtr_t > AffordanceObjects_t;
typedef vector3_t AffordanceConfig_t;
......
......@@ -38,24 +38,13 @@ namespace hpp {
{
public:
/// Create instance and return shared pointer
static SteeringMethodStraightPtr_t create (const ProblemPtr_t& problem)
static SteeringMethodStraightPtr_t create (const Problem& problem)
{
SteeringMethodStraight* ptr = new SteeringMethodStraight (problem);
SteeringMethodStraightPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
/// Create instance and return shared pointer
static SteeringMethodStraightPtr_t create
(const DevicePtr_t& device, const WeighedDistancePtr_t& distance)
HPP_CORE_DEPRECATED
{
SteeringMethodStraight* ptr = new SteeringMethodStraight (device,
distance);
SteeringMethodStraightPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
/// Copy instance and return shared pointer
static SteeringMethodStraightPtr_t createCopy
(const SteeringMethodStraightPtr_t& other)
......@@ -75,7 +64,7 @@ namespace hpp {
virtual PathPtr_t impl_compute (ConfigurationIn_t q1,
ConfigurationIn_t q2) const
{
value_type length = (*problem_->distance()) (q1, q2);
value_type length = (*problem_.distance()) (q1, q2);
ConstraintSetPtr_t c;
if (constraints() && constraints()->configProjector ()) {
c = HPP_STATIC_PTR_CAST (ConstraintSet, constraints()->copy ());
......@@ -84,23 +73,16 @@ namespace hpp {
c = constraints ();
}
PathPtr_t path = StraightPath::create
(problem_->robot(), q1, q2, length, c);
(problem_.robot(), q1, q2, length, c);
return path;
}
protected:
/// Constructor with robot
/// Weighed distance is created from robot
SteeringMethodStraight (const ProblemPtr_t& problem) :
SteeringMethodStraight (const Problem& problem) :
SteeringMethod (problem), weak_ ()
{
}
/// Constructor with weighed distance
SteeringMethodStraight (const DevicePtr_t& device,
const WeighedDistancePtr_t& distance) :
SteeringMethod (new Problem (device)), weak_ ()
{
problem_->distance (distance);
}
/// Copy constructor
SteeringMethodStraight (const SteeringMethodStraight& other) :
SteeringMethod (other), weak_ ()
......
......@@ -75,7 +75,7 @@ namespace hpp {
protected:
/// Constructor
SteeringMethod (ProblemPtr_t problem) :
SteeringMethod (const Problem& problem) :
problem_ (problem), constraints_ (), weak_ ()
{
}
......@@ -99,7 +99,7 @@ namespace hpp {
weak_ = weak;
}
ProblemPtr_t problem_;
const Problem& problem_;
private:
/// Set of constraints to apply on the paths produced
......
......@@ -45,7 +45,8 @@ namespace hpp {
/// - the 2 following parameters corresponds to the RZ unbounded
/// rotation joint.
/// Use Carlike::setWheelJoints to set the wheel joints.
static ReedsSheppPtr_t createWithGuess (const ProblemPtr_t& problem)
/// Use ReedsShepp::setWheelJoints to set the wheel joints.
static ReedsSheppPtr_t createWithGuess (const Problem& problem)
{
ReedsShepp* ptr = new ReedsShepp (problem);
ReedsSheppPtr_t shPtr (ptr);
......@@ -56,7 +57,7 @@ namespace hpp {
/// Create an instance
///
/// This constructor does no assumption.
static ReedsSheppPtr_t create (const ProblemPtr_t& problem,
static ReedsSheppPtr_t create (const Problem& problem,
const value_type turningRadius,
JointPtr_t xyJoint, JointPtr_t rzJoint,
std::vector <JointPtr_t> wheels = std::vector<JointPtr_t>())
......@@ -90,10 +91,10 @@ namespace hpp {
protected:
/// Constructor
ReedsShepp (const ProblemPtr_t& problem);
ReedsShepp (const Problem& problem);
/// Constructor
ReedsShepp (const ProblemPtr_t& problem,
ReedsShepp (const Problem& problem,
const value_type turningRadius,
JointPtr_t xyJoint, JointPtr_t rzJoint,
std::vector <JointPtr_t> wheels);
......
......@@ -33,7 +33,7 @@ namespace hpp {
class HPP_CORE_DLLAPI WeighedDistance : public Distance {
public:
static WeighedDistancePtr_t createFromProblem
(const ProblemPtr_t& problem);
(const Problem& problem);
static WeighedDistancePtr_t create (const DevicePtr_t& robot);
static WeighedDistancePtr_t
createWithWeight (const DevicePtr_t& robot,
......@@ -59,7 +59,7 @@ namespace hpp {
return robot_;
}
protected:
WeighedDistance (const ProblemPtr_t& problem);
WeighedDistance (const Problem& problem);
WeighedDistance (const DevicePtr_t& robot);
WeighedDistance (const DevicePtr_t& robot,
const std::vector <value_type>& weights);
......
......@@ -28,7 +28,7 @@ namespace hpp {
return createCopy (weak_.lock ());
}
ReedsSheppPtr_t ReedsShepp::create (const ProblemPtr_t& problem)
ReedsSheppPtr_t ReedsShepp::create (const Problem& problem)
{
ReedsShepp* ptr (new ReedsShepp (problem));
ReedsSheppPtr_t shPtr (ptr);
......@@ -36,7 +36,7 @@ namespace hpp {
return shPtr;
}
ReedsSheppPtr_t ReedsShepp::create
(const ProblemPtr_t& problem, const value_type& turningRadius,
(const Problem& problem, const value_type& turningRadius,
JointPtr_t xyJoint, JointPtr_t rzJoint)
{
ReedsShepp* ptr (new ReedsShepp (problem, turningRadius,
......@@ -54,12 +54,12 @@ namespace hpp {
return shPtr;
}
ReedsShepp::ReedsShepp (const ProblemPtr_t& problem) :
ReedsShepp::ReedsShepp (const Problem& problem) :
Distance (), sm_ (steeringMethod::ReedsShepp::createWithGuess (problem))
{
}
ReedsShepp::ReedsShepp (const ProblemPtr_t& problem,
ReedsShepp::ReedsShepp (const Problem& problem,
const value_type& turningRadius,
JointPtr_t xyJoint, JointPtr_t rzJoint) :
Distance (), sm_ (steeringMethod::ReedsShepp::create
......
......@@ -105,7 +105,7 @@ namespace hpp {
add <DistanceBuilder_t> ("WeighedDistance",
WeighedDistance::createFromProblem);
add <SteeringMethodBuilder_t> ("SteeringMethodStraight", boost::bind(
static_cast<SteeringMethodStraightPtr_t (*)(const ProblemPtr_t&)>
static_cast<SteeringMethodStraightPtr_t (*)(const Problem&)>
(&SteeringMethodStraight::create), _1
));
add <SteeringMethodBuilder_t> ("ReedsShepp", steeringMethod::ReedsShepp::createWithGuess);
......@@ -431,7 +431,7 @@ namespace hpp {
{
if (!problem_) throw std::runtime_error ("The problem is not defined.");
DistancePtr_t dist (
get <DistanceBuilder_t> (distanceType_) (problem_)
get <DistanceBuilder_t> (distanceType_) (*problem_)
);
problem_->distance (dist);
}
......@@ -441,7 +441,7 @@ namespace hpp {
{
if (!problem_) throw std::runtime_error ("The problem is not defined.");
SteeringMethodPtr_t sm (
get <SteeringMethodBuilder_t> (steeringMethodType_) (problem_)
get <SteeringMethodBuilder_t> (steeringMethodType_) (*problem_)
);
problem_->steeringMethod (sm);
}
......@@ -457,7 +457,7 @@ namespace hpp {
// the problem constraints.
PathProjectorPtr_t pathProjector_ =
createProjector (problem_->distance (),
SteeringMethodStraight::create (problem_),
SteeringMethodStraight::create (*problem_),
pathProjectorTolerance_);
problem_->pathProjector (pathProjector_);
}
......@@ -541,7 +541,7 @@ namespace hpp {
// Create steering method using factory
SteeringMethodPtr_t sm (get <SteeringMethodBuilder_t>
(steeringMethodType_) (problem_));
(steeringMethodType_) (*problem_));
problem_->steeringMethod (sm);
PathPtr_t dp = (*sm) (start, end);
if (!dp) {
......
......@@ -42,7 +42,7 @@ namespace hpp {
robot_ (robot),
distance_ (WeighedDistance::create (robot)),
initConf_ (), target_ (),
steeringMethod_ (SteeringMethodStraight::create (this)),
steeringMethod_ (SteeringMethodStraight::create (*this)),
configValidations_ (ConfigValidations::create ()),
pathValidation_ (DiscretizedCollisionChecking::create
(robot, 0.05)),
......
......@@ -36,12 +36,12 @@ namespace hpp {
return path;
}
ReedsShepp::ReedsShepp (const ProblemPtr_t& problem) :
ReedsShepp::ReedsShepp (const Problem& problem) :
CarLike (problem), weak_ ()
{
}
ReedsShepp::ReedsShepp (const ProblemPtr_t& problem,
ReedsShepp::ReedsShepp (const Problem& problem,
const value_type turningRadius,
JointPtr_t xyJoint, JointPtr_t rzJoint,
std::vector <JointPtr_t> wheels) :
......
......@@ -46,7 +46,7 @@ namespace hpp {
}
WeighedDistancePtr_t WeighedDistance::createFromProblem
(const ProblemPtr_t& problem)
(const Problem& problem)
{
WeighedDistance* ptr = new WeighedDistance (problem);
WeighedDistancePtr_t shPtr (ptr);
......@@ -150,8 +150,8 @@ namespace hpp {
computeWeights ();
}
WeighedDistance::WeighedDistance (const ProblemPtr_t& problem) :
robot_ (problem->robot()), weights_ ()
WeighedDistance::WeighedDistance (const Problem& problem) :
robot_ (problem.robot()), weights_ ()
{
computeWeights ();
}
......
......@@ -79,8 +79,8 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
xJoint->addChildJoint (yJoint);
// Create steering method
Problem p = Problem (robot);
SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (&p);
Problem p (robot);
SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (p);
// create roadmap
hpp::core::DistancePtr_t distance (WeighedDistance::createWithWeight
(robot, boost::assign::list_of (1)(1)));
......@@ -304,7 +304,7 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) {
// Create steering method
Problem p (robot);
SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (&p);
SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (p);
// create roadmap
hpp::core::DistancePtr_t distance (WeighedDistance::createWithWeight
(robot, boost::assign::list_of (1)(1)));
......
......@@ -64,7 +64,7 @@ BOOST_AUTO_TEST_CASE (continuous_collision_checking)
ConfigurationShooterPtr_t shooter (BasicConfigurationShooter::create (robot));
// create steering method
ProblemPtr_t problem (new Problem (robot));
Problem problem (robot);
SteeringMethodPtr_t sm (SteeringMethodStraight::create (problem));
// create path validation objects
......@@ -154,7 +154,6 @@ BOOST_AUTO_TEST_CASE (continuous_collision_checking)
}
}
// delete problem
delete problem;
}
......
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