Add property to enable/disable pruning at runtime

ubi-agni · Jul 5, 2024 · e9188d8 · e9188d8
1 parent 907014c
commit e9188d8
Show file tree

Hide file tree

Showing 7 changed files with 33 additions and 14 deletions.
diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt
@@ -40,9 +40,7 @@ catkin_package(
 )
 
 add_compile_options(-fvisibility-inlines-hidden)
-if(ENABLE_PRUNING)
-	add_compile_definitions(ENABLE_PRUNING)
-endif()
+add_compile_definitions(ENABLE_PRUNING=$<BOOL:${ENABLE_PRUNING}>)
 
 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
@@ -51,6 +51,10 @@ class ContainerBase : public Stage
 	PRIVATE_CLASS(ContainerBase)
 	using pointer = std::unique_ptr<ContainerBase>;
 
+	/// Explicitly enable/disable pruning
+	void setPruning(bool pruning) { setProperty("pruning", pruning); }
+	bool pruning() const { return properties().get<bool>("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
@@ -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
@@ -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<ContainerBase*>(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<Interface::FORWARD>(&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<bool>("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
@@ -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(ENABLE_PRUNING);
 	setTimeout(std::numeric_limits<double>::max());
 
 	// monitor state on commandline

diff --git 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
@@ -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());