Implement pruning inside serial container

By inefficient inverse lookup.

Also add disabled test for the inverted inference (failure inside should prune outside)
This commit is contained in:
v4hn 2021-03-26 14:25:10 +01:00
parent 36d9d3e8c0
commit 106c138ef5
4 changed files with 69 additions and 13 deletions

View File

@ -136,6 +136,7 @@ protected:
} }
/// copy external_state to a child's interface and remember the link in internal_to map /// copy external_state to a child's interface and remember the link in internal_to map
template <Interface::Direction>
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
/// lift solution from internal to external level /// lift solution from internal to external level
void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
@ -207,7 +208,8 @@ protected:
private: private:
/// callback for new externally received states /// callback for new externally received states
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); template <typename Interface::Direction>
void onNewExternalState(Interface::iterator external, bool updated);
}; };
PIMPL_FUNCTIONS(ParallelContainerBase) PIMPL_FUNCTIONS(ParallelContainerBase)

View File

@ -146,7 +146,9 @@ private:
private: private:
planning_scene::PlanningSceneConstPtr scene_; planning_scene::PlanningSceneConstPtr scene_;
PropertyMap properties_; PropertyMap properties_;
/// trajectories which are *timewise before* this state
Solutions incoming_trajectories_; Solutions incoming_trajectories_;
/// trajectories which are *timewise after* this state
Solutions outgoing_trajectories_; Solutions outgoing_trajectories_;
// members needed for priority scheduling in Interface list // members needed for priority scheduling in Interface list

View File

@ -106,10 +106,17 @@ void ContainerBasePrivate::compute() {
static_cast<ContainerBase*>(me_)->compute(); static_cast<ContainerBase*>(me_)->compute();
} }
template <Interface::Direction dir>
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
// TODO: update internal's prio from external's new priority if (updated) {
if (updated) // TODO(v4hn): This is inefficient, consider storing inverse mapping for each start/end pair in children
// for parallel containers there will be multiple internal states
std::for_each(internal_to_external_.begin(), internal_to_external_.end(), [&external](auto& p) {
if (p.second == &*external)
setStatus<dir>(p.first, external->priority().status());
});
return; return;
}
// create a clone of external state within target interface (child's starts() or ends()) // create a clone of external state within target interface (child's starts() or ends())
auto internal = states_.insert(states_.end(), InterfaceState(*external)); auto internal = states_.insert(states_.end(), InterfaceState(*external));
@ -497,8 +504,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
validateInterface<START_IF_MASK>(*first.pimpl(), expected); validateInterface<START_IF_MASK>(*first.pimpl(), expected);
// connect first child's (start) pull interface // connect first child's (start) pull interface
if (const InterfacePtr& target = first.pimpl()->starts()) if (const InterfacePtr& target = first.pimpl()->starts())
starts_.reset(new Interface( starts_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); })); this->copyState<Interface::FORWARD>(it, target, updated);
}));
} catch (InitStageException& e) { } catch (InitStageException& e) {
exceptions.append(e); exceptions.append(e);
} }
@ -521,8 +529,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
validateInterface<END_IF_MASK>(*last.pimpl(), expected); validateInterface<END_IF_MASK>(*last.pimpl(), expected);
// connect last child's (end) pull interface // connect last child's (end) pull interface
if (const InterfacePtr& target = last.pimpl()->ends()) if (const InterfacePtr& target = last.pimpl()->ends())
ends_.reset(new Interface( ends_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); })); this->copyState<Interface::BACKWARD>(it, target, updated);
}));
} catch (InitStageException& e) { } catch (InitStageException& e) {
exceptions.append(e); exceptions.append(e);
} }
@ -622,11 +631,11 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
// States received by the container need to be copied to all children's pull interfaces. // States received by the container need to be copied to all children's pull interfaces.
if (expected & READS_START) if (expected & READS_START)
starts().reset(new Interface([this](Interface::iterator external, bool updated) { starts().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState(Interface::FORWARD, external, updated); this->onNewExternalState<Interface::FORWARD>(external, updated);
})); }));
if (expected & READS_END) if (expected & READS_END)
ends().reset(new Interface([this](Interface::iterator external, bool updated) { ends().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState(Interface::BACKWARD, external, updated); this->onNewExternalState<Interface::BACKWARD>(external, updated);
})); }));
required_interface_ = expected; required_interface_ = expected;
@ -663,10 +672,10 @@ void ParallelContainerBasePrivate::validateConnectivity() const {
ContainerBasePrivate::validateConnectivity(); ContainerBasePrivate::validateConnectivity();
} }
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, template <Interface::Direction dir>
bool updated) { void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) {
for (const Stage::pointer& stage : children()) for (const Stage::pointer& stage : children())
copyState(external, stage->pimpl()->pullInterface(dir), updated); copyState<dir>(external, stage->pimpl()->pullInterface(dir), updated);
} }
ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) : ContainerBase(impl) {} ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) : ContainerBase(impl) {}

View File

@ -145,6 +145,13 @@ void resetIds() {
Connect::id_ = 0; Connect::id_ = 0;
} }
template <typename C, typename S>
auto add(C& container, S* stage) -> S* {
Stage::pointer ptr{ stage };
container.add(std::move(ptr));
return stage;
}
// https://github.com/ros-planning/moveit_task_constructor/issues/182 // https://github.com/ros-planning/moveit_task_constructor/issues/182
TEST(ConnectConnect, SuccSucc) { TEST(ConnectConnect, SuccSucc) {
resetIds(); resetIds();
@ -208,7 +215,7 @@ TEST(Pruning, PruningMultiForward) {
t.add(Stage::pointer(new GeneratorMockup())); t.add(Stage::pointer(new GeneratorMockup()));
// spawn two solutions for the only incoming state // spawn two solutions for the only incoming state
t.add(Stage::pointer(new ForwardMockup({ 0, 0 }, 2))); t.add(Stage::pointer(new ForwardMockup({ 0, 0 }, 2)));
// fail to exten // fail to extend the second solution
t.add(Stage::pointer(new ForwardMockup({ 0, inf }))); t.add(Stage::pointer(new ForwardMockup({ 0, inf })));
t.plan(); t.plan();
@ -268,3 +275,39 @@ TEST(Pruning, ConnectConnectBackward) {
EXPECT_EQ(c2->calls_, 3u); EXPECT_EQ(c2->calls_, 3u);
EXPECT_EQ(c1->calls_, 6u); EXPECT_EQ(c1->calls_, 6u);
} }
TEST(Pruning, PropagateInsideContainerBoundaries) {
resetIds();
Task t;
t.setRobotModel(getModel());
add(t, new BackwardMockup({ inf }));
add(t, new GeneratorMockup({ 0 }));
auto c{ std::make_unique<SerialContainer>() };
auto con = add(*c, new Connect());
add(*c, new GeneratorMockup({ 0 }));
t.add(std::move(c));
t.plan();
// the failure in the backward stage (outside the container)
// should prune the expected computation of con
EXPECT_EQ(con->calls_, 0);
}
TEST(Pruning, DISABLED_PropagateOutsideContainerBoundaries) {
resetIds();
Task t;
t.setRobotModel(getModel());
auto back = add(t, new BackwardMockup());
add(t, new BackwardMockup());
add(t, new GeneratorMockup({ 0 }));
auto c{ std::make_unique<SerialContainer>() };
add(*c, new ForwardMockup({ inf }));
add(*c, new ForwardMockup());
t.add(std::move(c));
t.plan();
// the failure inside the container should prune computing of back
EXPECT_EQ(back->calls_, 0);
}