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
template <Interface::Direction>
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
/// lift solution from internal to external level
void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
@ -207,7 +208,8 @@ protected:
private:
/// 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)

View File

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

View File

@ -106,10 +106,17 @@ void ContainerBasePrivate::compute() {
static_cast<ContainerBase*>(me_)->compute();
}
template <Interface::Direction dir>
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;
}
// create a clone of external state within target interface (child's starts() or ends())
auto internal = states_.insert(states_.end(), InterfaceState(*external));
@ -497,8 +504,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
validateInterface<START_IF_MASK>(*first.pimpl(), expected);
// connect first child's (start) pull interface
if (const InterfacePtr& target = first.pimpl()->starts())
starts_.reset(new Interface(
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
starts_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
this->copyState<Interface::FORWARD>(it, target, updated);
}));
} catch (InitStageException& e) {
exceptions.append(e);
}
@ -521,8 +529,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
validateInterface<END_IF_MASK>(*last.pimpl(), expected);
// connect last child's (end) pull interface
if (const InterfacePtr& target = last.pimpl()->ends())
ends_.reset(new Interface(
[this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
ends_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
this->copyState<Interface::BACKWARD>(it, target, updated);
}));
} catch (InitStageException& 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.
if (expected & READS_START)
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)
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;
@ -663,10 +672,10 @@ void ParallelContainerBasePrivate::validateConnectivity() const {
ContainerBasePrivate::validateConnectivity();
}
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external,
bool updated) {
template <Interface::Direction dir>
void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) {
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) {}

View File

@ -145,6 +145,13 @@ void resetIds() {
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
TEST(ConnectConnect, SuccSucc) {
resetIds();
@ -208,7 +215,7 @@ TEST(Pruning, PruningMultiForward) {
t.add(Stage::pointer(new GeneratorMockup()));
// spawn two solutions for the only incoming state
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.plan();
@ -268,3 +275,39 @@ TEST(Pruning, ConnectConnectBackward) {
EXPECT_EQ(c2->calls_, 3u);
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);
}