From 9f18219993fd7a4a9a9e42b7e7fe4cc6be402e17 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 6 Dec 2020 02:16:53 +0100 Subject: [PATCH] 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 --- core/include/moveit/task_constructor/stage.h | 15 +++++++-------- core/include/moveit/task_constructor/stage_p.h | 2 +- core/include/moveit/task_constructor/storage.h | 6 +++--- core/src/container.cpp | 5 ++--- core/src/stage.cpp | 4 ++-- core/test/test_container.cpp | 4 ++++ 6 files changed, 19 insertions(+), 17 deletions(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index c6ac9b57..34ab888f 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -227,20 +227,20 @@ public: const ordered& solutions() const; const std::list& 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(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); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 392785cc..b83b368f 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -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(); diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 8c4eb8bc..fa534c00 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -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*(); } }; diff --git a/core/src/container.cpp b/core/src/container.cpp index 06436774..d0748a0e 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -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(this, &solution, cost, std::move(comment)), solution.start(), - solution.end()); + pimpl()->liftSolution(std::make_shared(this, &solution, cost, std::move(comment)), + solution.start(), solution.end()); } void ParallelContainerBase::spawn(InterfaceState&& state, SubTrajectory&& t) { diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 3b8ca52a..a7337710 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -681,12 +681,12 @@ InterfaceFlags ConnectingPrivate::requiredInterface() const { template <> ConnectingPrivate::StatePair ConnectingPrivate::make_pair(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::const_iterator first, Interface::const_iterator second) { - return std::make_pair(second, first); + return StatePair(second, first); } template diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 8ad4007d..0e9957d3 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -669,11 +669,13 @@ TEST_F(ParallelTest, init_any) { } TEST(Task, move) { + MOCK_ID = 0; Task t1("foo"); t1.add(std::make_unique()); t1.add(std::make_unique()); 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(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");