mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Add property to enable/disable pruning at runtime (#590)
This commit is contained in:
parent
907014cd57
commit
fbc05e4496
@ -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)
|
||||
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
@ -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();
|
||||
|
@ -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
|
||||
|
@ -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)
|
||||
|
||||
|
@ -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());
|
||||
|
Loading…
Reference in New Issue
Block a user