diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 9d3b4d991..12af7292b 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -18,7 +18,6 @@ find_package(catkin REQUIRED COMPONENTS ) option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" OFF) # We use our own set of warnings -option(ENABLE_PRUNING "Enable pruning on failed solutions" OFF) moveit_build_options() catkin_python_setup() @@ -40,9 +39,6 @@ catkin_package( ) add_compile_options(-fvisibility-inlines-hidden) -if(ENABLE_PRUNING) - add_compile_definitions(ENABLE_PRUNING) -endif() set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 2dcd325f4..24e3b1ec4 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -51,6 +51,10 @@ class ContainerBase : public Stage PRIVATE_CLASS(ContainerBase) using pointer = std::unique_ptr; + /// Explicitly enable/disable pruning + void setPruning(bool pruning) { setProperty("pruning", pruning); } + bool pruning() const { return properties().get("pruning"); } + size_t numChildren() const; Stage* findChild(const std::string& name) const; Stage* operator[](int index) const; diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 91d49e733..00ea354c2 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -117,6 +117,9 @@ class Task : protected WrapperBase using WrapperBase::setTimeout; using WrapperBase::timeout; + using WrapperBase::pruning; + using WrapperBase::setPruning; + /// reset all stages void reset() final; /// initialize all stages with given scene diff --git a/core/src/container.cpp b/core/src/container.cpp index 82cf907bf..265d8d00f 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -232,7 +232,9 @@ inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Prio } void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { -#ifdef ENABLE_PRUNING + if (!static_cast(me_)->pruning()) + return; + ROS_DEBUG_STREAM_NAMED("Pruning", fmt::format("'{}' generated a failure", child.name())); switch (child.pimpl()->interfaceFlags()) { case GENERATE: @@ -252,11 +254,6 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState setStatus(&child, from, to, InterfaceState::Status::ARMED); break; } -#else - (void)child; - (void)from; - (void)to; -#endif // printChildrenInterfaces(*this, false, child); } @@ -328,7 +325,10 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I newSolution(solution); } -ContainerBase::ContainerBase(ContainerBasePrivate* impl) : Stage(impl) {} +ContainerBase::ContainerBase(ContainerBasePrivate* impl) : Stage(impl) { + auto& p = properties(); + p.declare("pruning", std::string("enable pruning?")).configureInitFrom(Stage::PARENT, "pruning"); +} size_t ContainerBase::numChildren() const { return pimpl()->children().size(); diff --git a/core/src/task.cpp b/core/src/task.cpp index f6c3b864d..f969cf9e8 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -92,6 +92,7 @@ const ContainerBase* TaskPrivate::stages() const { Task::Task(const std::string& ns, bool introspection, ContainerBase::pointer&& container) : WrapperBase(new TaskPrivate(this, ns), std::move(container)) { + setPruning(false); setTimeout(std::numeric_limits::max()); // monitor state on commandline diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 492d84a58..09e6d477b 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -38,9 +38,7 @@ if (CATKIN_ENABLE_TESTING) mtc_add_gtest(test_stage.cpp) mtc_add_gtest(test_container.cpp) mtc_add_gmock(test_serial.cpp) -if(ENABLE_PRUNING) mtc_add_gmock(test_pruning.cpp) -endif() mtc_add_gtest(test_properties.cpp) mtc_add_gtest(test_cost_terms.cpp) diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index 5c92c6c38..a9a5fdd56 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -8,7 +8,10 @@ using namespace moveit::task_constructor; -using Pruning = TaskTestBase; +struct Pruning : TaskTestBase +{ + Pruning() : TaskTestBase() { t.setPruning(true); } +}; TEST_F(Pruning, PropagatorFailure) { auto back = add(t, new BackwardMockup()); @@ -21,6 +24,18 @@ TEST_F(Pruning, PropagatorFailure) { EXPECT_EQ(back->runs_, 0u); } +// Same as the previous test, except pruning is disabled for the whole task +TEST_F(Pruning, DisabledPruningPropagatorFailure) { + t.setPruning(false); + auto back = add(t, new BackwardMockup()); + add(t, new GeneratorMockup({ 0 })); + add(t, new ForwardMockup({ INF })); + EXPECT_FALSE(t.plan()); + ASSERT_EQ(t.solutions().size(), 0u); + // ForwardMockup fails, since we have pruning disabled backward should run + EXPECT_EQ(back->runs_, 1u); +} + TEST_F(Pruning, PruningMultiForward) { add(t, new BackwardMockup()); add(t, new BackwardMockup()); @@ -127,7 +142,7 @@ TEST_F(Pruning, PropagateIntoContainer) { EXPECT_EQ(con->runs_, 0u); } -TEST_F(Pruning, PropagateIntoContainerAndReactivate) { +TEST_F(Pruning, DISABLED_PropagateIntoContainerAndReactivate) { add(t, new GeneratorMockup({ 0 })); auto serial = add(t, new SerialContainer());