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:
Robert Haschke 2020-12-06 02:16:53 +01:00
parent 0925031a81
commit 9f18219993
6 changed files with 19 additions and 17 deletions

View File

@ -227,20 +227,20 @@ public:
const ordered<SolutionBaseConstPtr>& solutions() const;
const std::list<SolutionBaseConstPtr>& failures() 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();
/// should we generate failure solutions?
/// Should we generate failure solutions? Note: Always report a failure!
bool storeFailures() const;
/// get the stage's property map
/// Get the stage's property map
PropertyMap& 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);
/// overload: const char* values are stored as std::string
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);
double getTotalComputeTime() const;
@ -373,9 +373,6 @@ protected:
class ConnectingPrivate;
class Connecting : public ComputeBase
{
protected:
virtual bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const;
public:
PRIVATE_CLASS(Connecting)
Connecting(const std::string& name = "connecting");
@ -385,6 +382,8 @@ public:
virtual void compute(const InterfaceState& from, const InterfaceState& to) = 0;
protected:
virtual bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const;
/// register solution as a solution connecting states from -> to
void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);

View File

@ -100,7 +100,7 @@ public:
inline InterfaceConstPtr prevEnds() const { return prev_ends_.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 pushInterface(Interface::Direction dir) {
return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock();

View File

@ -88,6 +88,7 @@ public:
inline unsigned int depth() const { return this->first; }
inline double cost() const { return this->second; }
// add priorities
Priority operator+(const Priority& other) const {
return Priority(this->depth() + other.depth(), this->cost() + other.cost());
}
@ -144,20 +145,19 @@ public:
class iterator : public base_type::iterator
{
public:
using base_type::iterator::iterator; // inherit base constructors
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*(); }
};
class const_iterator : public base_type::const_iterator
{
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::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*(); }
};

View File

@ -609,9 +609,8 @@ ParallelContainerBase::ParallelContainerBase(const std::string& name)
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
auto impl = pimpl();
impl->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)), solution.start(),
solution.end());
pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)),
solution.start(), solution.end());
}
void ParallelContainerBase::spawn(InterfaceState&& state, SubTrajectory&& t) {

View File

@ -681,12 +681,12 @@ InterfaceFlags ConnectingPrivate::requiredInterface() const {
template <>
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::BACKWARD>(Interface::const_iterator first,
Interface::const_iterator second) {
return std::make_pair(first, second);
return StatePair(first, second);
}
template <>
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(Interface::const_iterator first,
Interface::const_iterator second) {
return std::make_pair(second, first);
return StatePair(second, first);
}
template <Interface::Direction other>

View File

@ -669,11 +669,13 @@ TEST_F(ParallelTest, init_any) {
}
TEST(Task, move) {
MOCK_ID = 0;
Task t1("foo");
t1.add(std::make_unique<GeneratorMockup>());
t1.add(std::make_unique<GeneratorMockup>());
EXPECT_EQ(t1.stages()->numChildren(), 2u);
MOCK_ID = 0;
Task t2 = std::move(t1);
EXPECT_EQ(t2.stages()->numChildren(), 2u);
EXPECT_EQ(t1.stages()->numChildren(), 0u);
@ -694,6 +696,7 @@ TEST(Task, reuse) {
t.setRobotModel(robot_model);
auto configure = [](Task& t) {
MOCK_ID = 0;
auto ref = new stages::FixedState("fixed");
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
ref->setState(scene);
@ -721,6 +724,7 @@ TEST(Task, reuse) {
}
TEST(Task, timeout) {
MOCK_ID = 0;
// create dummy robot model
moveit::core::RobotModelBuilder builder("robot", "base");
builder.addChain("base->a->b->c", "continuous");