mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
minor cleanups
* unify usage of pimpl() * fix StatePair constructors * improve/add comments * test_container: reset MOCK_ID for each test to facilitate identification of stages
This commit is contained in:
parent
0925031a81
commit
9f18219993
@ -227,20 +227,20 @@ public:
|
|||||||
const ordered<SolutionBaseConstPtr>& solutions() const;
|
const ordered<SolutionBaseConstPtr>& solutions() const;
|
||||||
const std::list<SolutionBaseConstPtr>& failures() const;
|
const std::list<SolutionBaseConstPtr>& failures() const;
|
||||||
size_t numFailures() const;
|
size_t numFailures() const;
|
||||||
/// call to increase number of failures w/o storing a (failure) trajectory
|
/// Call to increase number of failures w/o storing a (failure) trajectory
|
||||||
void silentFailure();
|
void silentFailure();
|
||||||
/// should we generate failure solutions?
|
/// Should we generate failure solutions? Note: Always report a failure!
|
||||||
bool storeFailures() const;
|
bool storeFailures() const;
|
||||||
|
|
||||||
/// get the stage's property map
|
/// Get the stage's property map
|
||||||
PropertyMap& properties();
|
PropertyMap& properties();
|
||||||
const PropertyMap& properties() const { return const_cast<Stage*>(this)->properties(); }
|
const PropertyMap& properties() const { return const_cast<Stage*>(this)->properties(); }
|
||||||
/// set a previously declared property to a new value
|
/// Set a previously declared property to a new value
|
||||||
void setProperty(const std::string& name, const boost::any& value);
|
void setProperty(const std::string& name, const boost::any& value);
|
||||||
/// overload: const char* values are stored as std::string
|
/// overload: const char* values are stored as std::string
|
||||||
inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); }
|
inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); }
|
||||||
|
|
||||||
/// analyze source of error and report accordingly
|
/// Analyze source of error and report accordingly
|
||||||
[[noreturn]] void reportPropertyError(const Property::error& e);
|
[[noreturn]] void reportPropertyError(const Property::error& e);
|
||||||
|
|
||||||
double getTotalComputeTime() const;
|
double getTotalComputeTime() const;
|
||||||
@ -373,9 +373,6 @@ protected:
|
|||||||
class ConnectingPrivate;
|
class ConnectingPrivate;
|
||||||
class Connecting : public ComputeBase
|
class Connecting : public ComputeBase
|
||||||
{
|
{
|
||||||
protected:
|
|
||||||
virtual bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const;
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
PRIVATE_CLASS(Connecting)
|
PRIVATE_CLASS(Connecting)
|
||||||
Connecting(const std::string& name = "connecting");
|
Connecting(const std::string& name = "connecting");
|
||||||
@ -385,6 +382,8 @@ public:
|
|||||||
virtual void compute(const InterfaceState& from, const InterfaceState& to) = 0;
|
virtual void compute(const InterfaceState& from, const InterfaceState& to) = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
virtual bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const;
|
||||||
|
|
||||||
/// register solution as a solution connecting states from -> to
|
/// register solution as a solution connecting states from -> to
|
||||||
void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);
|
void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);
|
||||||
|
|
||||||
|
|||||||
@ -100,7 +100,7 @@ public:
|
|||||||
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
|
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
|
||||||
inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); }
|
inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); }
|
||||||
|
|
||||||
/// templated access to pull/push interfaces
|
/// direction-based access to pull/push interfaces
|
||||||
inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
|
inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
|
||||||
inline InterfacePtr pushInterface(Interface::Direction dir) {
|
inline InterfacePtr pushInterface(Interface::Direction dir) {
|
||||||
return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock();
|
return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock();
|
||||||
|
|||||||
@ -88,6 +88,7 @@ public:
|
|||||||
inline unsigned int depth() const { return this->first; }
|
inline unsigned int depth() const { return this->first; }
|
||||||
inline double cost() const { return this->second; }
|
inline double cost() const { return this->second; }
|
||||||
|
|
||||||
|
// add priorities
|
||||||
Priority operator+(const Priority& other) const {
|
Priority operator+(const Priority& other) const {
|
||||||
return Priority(this->depth() + other.depth(), this->cost() + other.cost());
|
return Priority(this->depth() + other.depth(), this->cost() + other.cost());
|
||||||
}
|
}
|
||||||
@ -144,20 +145,19 @@ public:
|
|||||||
class iterator : public base_type::iterator
|
class iterator : public base_type::iterator
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
using base_type::iterator::iterator; // inherit base constructors
|
||||||
iterator(base_type::iterator other) : base_type::iterator(other) {}
|
iterator(base_type::iterator other) : base_type::iterator(other) {}
|
||||||
|
|
||||||
InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); }
|
InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); }
|
||||||
|
|
||||||
InterfaceState* operator->() const noexcept { return base_type::iterator::operator*(); }
|
InterfaceState* operator->() const noexcept { return base_type::iterator::operator*(); }
|
||||||
};
|
};
|
||||||
class const_iterator : public base_type::const_iterator
|
class const_iterator : public base_type::const_iterator
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
using base_type::const_iterator::const_iterator; // inherit base constructors
|
||||||
const_iterator(base_type::const_iterator other) : base_type::const_iterator(other) {}
|
const_iterator(base_type::const_iterator other) : base_type::const_iterator(other) {}
|
||||||
const_iterator(base_type::iterator other) : base_type::const_iterator(other) {}
|
|
||||||
|
|
||||||
const InterfaceState& operator*() const noexcept { return *base_type::const_iterator::operator*(); }
|
const InterfaceState& operator*() const noexcept { return *base_type::const_iterator::operator*(); }
|
||||||
|
|
||||||
const InterfaceState* operator->() const noexcept { return base_type::const_iterator::operator*(); }
|
const InterfaceState* operator->() const noexcept { return base_type::const_iterator::operator*(); }
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -609,9 +609,8 @@ ParallelContainerBase::ParallelContainerBase(const std::string& name)
|
|||||||
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
|
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
|
||||||
|
|
||||||
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
|
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
|
||||||
auto impl = pimpl();
|
pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)),
|
||||||
impl->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)), solution.start(),
|
solution.start(), solution.end());
|
||||||
solution.end());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParallelContainerBase::spawn(InterfaceState&& state, SubTrajectory&& t) {
|
void ParallelContainerBase::spawn(InterfaceState&& state, SubTrajectory&& t) {
|
||||||
|
|||||||
@ -681,12 +681,12 @@ InterfaceFlags ConnectingPrivate::requiredInterface() const {
|
|||||||
template <>
|
template <>
|
||||||
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::BACKWARD>(Interface::const_iterator first,
|
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::BACKWARD>(Interface::const_iterator first,
|
||||||
Interface::const_iterator second) {
|
Interface::const_iterator second) {
|
||||||
return std::make_pair(first, second);
|
return StatePair(first, second);
|
||||||
}
|
}
|
||||||
template <>
|
template <>
|
||||||
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(Interface::const_iterator first,
|
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(Interface::const_iterator first,
|
||||||
Interface::const_iterator second) {
|
Interface::const_iterator second) {
|
||||||
return std::make_pair(second, first);
|
return StatePair(second, first);
|
||||||
}
|
}
|
||||||
|
|
||||||
template <Interface::Direction other>
|
template <Interface::Direction other>
|
||||||
|
|||||||
@ -669,11 +669,13 @@ TEST_F(ParallelTest, init_any) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(Task, move) {
|
TEST(Task, move) {
|
||||||
|
MOCK_ID = 0;
|
||||||
Task t1("foo");
|
Task t1("foo");
|
||||||
t1.add(std::make_unique<GeneratorMockup>());
|
t1.add(std::make_unique<GeneratorMockup>());
|
||||||
t1.add(std::make_unique<GeneratorMockup>());
|
t1.add(std::make_unique<GeneratorMockup>());
|
||||||
EXPECT_EQ(t1.stages()->numChildren(), 2u);
|
EXPECT_EQ(t1.stages()->numChildren(), 2u);
|
||||||
|
|
||||||
|
MOCK_ID = 0;
|
||||||
Task t2 = std::move(t1);
|
Task t2 = std::move(t1);
|
||||||
EXPECT_EQ(t2.stages()->numChildren(), 2u);
|
EXPECT_EQ(t2.stages()->numChildren(), 2u);
|
||||||
EXPECT_EQ(t1.stages()->numChildren(), 0u);
|
EXPECT_EQ(t1.stages()->numChildren(), 0u);
|
||||||
@ -694,6 +696,7 @@ TEST(Task, reuse) {
|
|||||||
t.setRobotModel(robot_model);
|
t.setRobotModel(robot_model);
|
||||||
|
|
||||||
auto configure = [](Task& t) {
|
auto configure = [](Task& t) {
|
||||||
|
MOCK_ID = 0;
|
||||||
auto ref = new stages::FixedState("fixed");
|
auto ref = new stages::FixedState("fixed");
|
||||||
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
|
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
|
||||||
ref->setState(scene);
|
ref->setState(scene);
|
||||||
@ -721,6 +724,7 @@ TEST(Task, reuse) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(Task, timeout) {
|
TEST(Task, timeout) {
|
||||||
|
MOCK_ID = 0;
|
||||||
// create dummy robot model
|
// create dummy robot model
|
||||||
moveit::core::RobotModelBuilder builder("robot", "base");
|
moveit::core::RobotModelBuilder builder("robot", "base");
|
||||||
builder.addChain("base->a->b->c", "continuous");
|
builder.addChain("base->a->b->c", "continuous");
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user