mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
36d9d3e8c0
commit
106c138ef5
@ -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)
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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) {}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user