Add property to enable/disable pruning at runtime (#590)

This commit is contained in:
Robert Haschke 2024-07-06 16:51:14 +02:00 committed by GitHub
parent 907014cd57
commit fbc05e4496
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
7 changed files with 32 additions and 15 deletions

View File

@ -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)

View File

@ -51,6 +51,10 @@ public:
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;

View File

@ -117,6 +117,9 @@ public:
using WrapperBase::setTimeout;
using WrapperBase::timeout;
using WrapperBase::pruning;
using WrapperBase::setPruning;
/// reset all stages
void reset() final;
/// initialize all stages with given scene

View File

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

View File

@ -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<double>::max());
// monitor state on commandline

View File

@ -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)

View File

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