Commit a9190b54 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Simplify code.

parent a658fcaa
......@@ -45,12 +45,10 @@ namespace hpp {
virtual bool validate (const Configuration_t& config,
ValidationReportPtr_t& validationReport) = 0;
virtual ~ConfigValidation () {};
virtual ~ConfigValidation () = default;
protected:
ConfigValidation ()
{
}
ConfigValidation () = default;
}; // class ConfigValidation
/// \}
} // namespace core
......
......@@ -52,7 +52,10 @@ namespace hpp {
size_type numberConfigValidations() const;
protected:
ConfigValidations ();
ConfigValidations () = default;
ConfigValidations(std::initializer_list<ConfigValidationPtr_t> validations) :
ObstacleUserVector (validations)
{};
}; // class ConfigValidation
/// \}
} // namespace core
......
......@@ -35,7 +35,7 @@ namespace hpp {
class HPP_CORE_DLLAPI ObstacleUserInterface
{
public:
virtual ~ObstacleUserInterface () {}
virtual ~ObstacleUserInterface () = default;
/// Add an obstacle
/// \param object obstacle added
......@@ -83,7 +83,7 @@ namespace hpp {
class HPP_CORE_DLLAPI ObstacleUserVector : public ObstacleUserInterface
{
public:
virtual ~ObstacleUserVector () {}
virtual ~ObstacleUserVector () = default;
/// Add obstacle to each element
///
......@@ -152,6 +152,11 @@ namespace hpp {
typedef Derived value_t;
typedef std::vector<value_t> values_t;
ObstacleUserVector() = default;
ObstacleUserVector(std::initializer_list<value_t> validations) :
validations_ (validations)
{};
values_t validations_;
}; // class ObstacleUserVector
......@@ -159,7 +164,7 @@ namespace hpp {
class HPP_CORE_DLLAPI ObstacleUser : public ObstacleUserInterface
{
public:
virtual ~ObstacleUser () {}
virtual ~ObstacleUser () = default;
typedef std::pair<CollisionObjectConstPtr_t, CollisionObjectConstPtr_t> CollisionPair_t;
typedef std::vector<CollisionPair_t> CollisionPairs_t;
......
......@@ -38,6 +38,8 @@ namespace hpp {
{
public:
static DiscretizedPtr_t create (const value_type& stepSize);
static DiscretizedPtr_t create (const value_type& stepSize,
std::initializer_list<ConfigValidationPtr_t> validations);
/// Compute the largest valid interval starting from the path beginning
///
......@@ -54,7 +56,12 @@ namespace hpp {
virtual ~Discretized () {};
protected:
Discretized (const value_type& stepSize);
Discretized (const value_type& stepSize) : stepSize_ (stepSize) {}
Discretized (const value_type& stepSize,
std::initializer_list<ConfigValidationPtr_t> validations) :
ConfigValidations (validations),
stepSize_ (stepSize)
{};
value_type stepSize_;
}; // class Discretized
......
......@@ -50,10 +50,5 @@ namespace hpp {
{
return (size_type) validations_.size ();
}
ConfigValidations::ConfigValidations ()
{
}
} // namespace core
} // namespace hpp
......@@ -36,6 +36,14 @@ namespace hpp {
return DiscretizedPtr_t (ptr);
}
DiscretizedPtr_t
Discretized::create (const value_type& stepSize,
std::initializer_list<ConfigValidationPtr_t> validations)
{
Discretized* ptr = new Discretized(stepSize, validations);
return DiscretizedPtr_t (ptr);
}
bool Discretized::validate
(const PathPtr_t& path, bool reverse, PathPtr_t& validPart,
PathValidationReportPtr_t& validationReport)
......@@ -93,12 +101,6 @@ namespace hpp {
return false;
}
}
Discretized::Discretized (const value_type& stepSize) :
stepSize_ (stepSize)
{
}
} // namespace pathValidation
} // namespace core
} // namespace hpp
......@@ -18,6 +18,8 @@
#include <hpp/core/problem-solver.hh>
#include <iterator>
#include <hpp/fcl/collision_utility.h>
#include <pinocchio/algorithm/frames.hpp>
......@@ -95,60 +97,26 @@ namespace hpp {
template<typename Container> void remove(Container& vector, std::size_t pos)
{
assert (pos < vector.size());
typename Container::iterator it = vector.begin();
std::advance(it, pos);
vector.erase(it);
vector.erase(std::next(vector.begin(), pos));
}
struct FindCollisionObject {
FindCollisionObject (const GeomIndex& i) : geomIdx_ (i) {}
bool operator() (const CollisionObjectPtr_t co) const {
return co->indexInModel() == geomIdx_;
}
GeomIndex geomIdx_;
};
void remove(ObjectStdVector_t& vector, const GeomIndex& i)
{
ObjectStdVector_t::iterator it =
std::find_if(vector.begin(), vector.end(), FindCollisionObject(i));
std::find_if(vector.begin(), vector.end(),
[i](const CollisionObjectPtr_t& co) { return co->indexInModel() == i; });
if (it != vector.end()) vector.erase(it);
}
}
// Struct that constructs an empty shared pointer to PathProjector.
struct NonePathProjector
{
static PathProjectorPtr_t create (const ProblemConstPtr_t&,
const value_type&)
{
return PathProjectorPtr_t ();
}
}; // struct NonePathProjector
template <typename Derived> struct Factory {
static shared_ptr<Derived> create (const ProblemConstPtr_t& problem) { return Derived::create (problem); }
};
template <typename Derived> struct FactoryPP {
static shared_ptr<Derived> create (const ProblemConstPtr_t& problem, const value_type& value) { return Derived::create (problem, value); }
};
pathValidation::DiscretizedPtr_t createDiscretizedJointBoundAndCollisionChecking (
const DevicePtr_t& robot, const value_type& stepSize)
{
using namespace pathValidation;
DiscretizedPtr_t pv (Discretized::create (stepSize));
JointBoundValidationPtr_t jbv (JointBoundValidation::create (robot));
pv->add (jbv);
CollisionValidationPtr_t cv (CollisionValidation::create (robot));
pv->add (cv);
return pv;
}
template <typename T>
shared_ptr<T> createFromRobot (const ProblemConstPtr_t& p)
{
return T::create(p->robot());
return Discretized::create (stepSize, {
JointBoundValidation::create (robot),
CollisionValidation::create (robot),
});
}
configurationShooter::GaussianPtr_t createGaussianConfigShooter
......@@ -229,7 +197,7 @@ namespace hpp {
distances.add ("Kinodynamic", KinodynamicDistance::createFromProblem);
steeringMethods.add ("Straight", Factory<steeringMethod::Straight>::create);
steeringMethods.add ("Straight", steeringMethod::Straight::create);
steeringMethods.add ("ReedsShepp", steeringMethod::ReedsShepp::createWithGuess);
steeringMethods.add ("Kinodynamic", steeringMethod::Kinodynamic::create);
steeringMethods.add ("Dubins", steeringMethod::Dubins::createWithGuess);
......@@ -261,11 +229,11 @@ namespace hpp {
configValidationTypes_.push_back("JointBoundValidation");
// Store path projector methods in map.
pathProjectors.add ("None", NonePathProjector::create);
pathProjectors.add ("Progressive", FactoryPP<pathProjector::Progressive>::create);
pathProjectors.add ("Dichotomy", FactoryPP<pathProjector::Dichotomy>::create);
pathProjectors.add ("Global", FactoryPP<pathProjector::Global>::create);
pathProjectors.add ("RecursiveHermite", FactoryPP<pathProjector::RecursiveHermite>::create);
pathProjectors.add ("None", [](const ProblemConstPtr_t&, const value_type&) { return PathProjectorPtr_t(); });
pathProjectors.add ("Progressive", [](const ProblemConstPtr_t& p, const value_type& v) { return pathProjector::Progressive ::create(p,v); });
pathProjectors.add ("Dichotomy", [](const ProblemConstPtr_t& p, const value_type& v) { return pathProjector::Dichotomy ::create(p,v); });
pathProjectors.add ("Global", [](const ProblemConstPtr_t& p, const value_type& v) { return pathProjector::Global ::create(p,v); });
pathProjectors.add ("RecursiveHermite", [](const ProblemConstPtr_t& p, const value_type& v) { return pathProjector::RecursiveHermite::create(p,v); });
}
ProblemSolver::~ProblemSolver ()
......
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