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
|
/// 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)
|
||||||
|
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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) {}
|
||||||
|
|||||||
@ -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);
|
||||||
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user