Restore pruning

If a stage fails to find a solution, this often implies that further planning
on the open end(s) of connected InterfaceStates is not needed anymore.
Thus the InterfaceStates along all connected solution paths will be marked as disabled.
They are not removed from the pending state lists though, because they might get
reactivated by solutions found in future.

To this end, we introduced the method ContainerBase::onNewFailure().
This commit is contained in:
Robert Haschke 2020-12-06 23:01:28 +01:00 committed by v4hn
parent 6c18e2b482
commit 19484cec64
8 changed files with 47 additions and 21 deletions

View File

@ -77,6 +77,8 @@ public:
/// called by a (direct) child when a new solution becomes available
virtual void onNewSolution(const SolutionBase& s) = 0;
/// called by a (direct) child when a solution failed
virtual void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) = 0;
protected:
ContainerBase(ContainerBasePrivate* impl);
@ -95,8 +97,8 @@ public:
void compute() override;
protected:
/// called by a (direct) child when a new solution becomes available
void onNewSolution(const SolutionBase& s) override;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
protected:
SerialContainer(SerialContainerPrivate* impl);
@ -116,6 +118,7 @@ class ParallelContainerBase : public ContainerBase
public:
PRIVATE_CLASS(ParallelContainerBase)
ParallelContainerBase(const std::string& name = "parallel container");
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl);

View File

@ -144,7 +144,7 @@ public:
void spawn(InterfaceState&& state, const SolutionBasePtr& solution);
void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);
bool storeSolution(const SolutionBasePtr& solution);
bool storeSolution(const SolutionBasePtr& solution, const InterfaceState* from, const InterfaceState* to);
void newSolution(const SolutionBasePtr& solution);
bool storeFailures() const { return introspection_ != nullptr; }
void runCompute() {

View File

@ -390,6 +390,18 @@ template <>
inline const InterfaceState::Solutions& trajectories<Interface::BACKWARD>(const InterfaceState* state) {
return state->incomingTrajectories();
}
/// Trait to retrieve opposite direction (FORWARD <-> BACKWARD)
template <Interface::Direction dir>
constexpr Interface::Direction opposite();
template <>
inline constexpr Interface::Direction opposite<Interface::FORWARD>() {
return Interface::BACKWARD;
}
template <>
inline constexpr Interface::Direction opposite<Interface::BACKWARD>() {
return Interface::FORWARD;
}
} // namespace task_constructor
} // namespace moveit

View File

@ -122,9 +122,7 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
const InterfaceState* internal_to) {
computeCost(*internal_from, *internal_to, *solution);
if (!storeSolution(solution))
return;
// map internal to external states
auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* {
auto it = internal_to_external_.find(internal);
if (it != internal_to_external_.end())
@ -140,6 +138,9 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
InterfaceState* external_from = find_or_create_external(internal_from, created_from);
InterfaceState* external_to = find_or_create_external(internal_to, created_to);
if (!storeSolution(solution, external_from, external_to))
return;
// connect solution to start/end state
solution->setStartState(*external_from);
solution->setEndState(*external_to);
@ -368,6 +369,8 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
impl->liftSolution(solution, solution->internalStart(), solution->internalEnd());
}
void SerialContainer::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {}
SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(impl) {}
SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {}
@ -602,6 +605,8 @@ ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl)
ParallelContainerBase::ParallelContainerBase(const std::string& name)
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
void ParallelContainerBase::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {}
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)),
solution.start(), solution.end());

View File

@ -129,13 +129,16 @@ void StagePrivate::validateConnectivity() const {
}
}
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
bool StagePrivate::storeSolution(const SolutionBasePtr& solution, const InterfaceState* from,
const InterfaceState* to) {
solution->setCreator(me());
if (introspection_)
introspection_->registerSolution(*solution);
if (solution->isFailure()) {
++num_failures_;
if (parent())
parent()->onNewFailure(*me(), from, to);
if (!storeFailures())
return false; // drop solution
failures_.push_back(solution);
@ -150,7 +153,7 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to,
computeCost(from, to, *solution);
if (!storeSolution(solution))
if (!storeSolution(solution, &from, nullptr))
return; // solution dropped
me()->forwardProperties(from, to);
@ -172,7 +175,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
computeCost(from, to, *solution);
if (!storeSolution(solution))
if (!storeSolution(solution, nullptr, &to))
return; // solution dropped
me()->forwardProperties(to, from);
@ -193,7 +196,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution
computeCost(state, state, *solution);
if (!storeSolution(solution))
if (!storeSolution(solution, nullptr, nullptr))
return; // solution dropped
auto from = states_.insert(states_.end(), InterfaceState(state)); // copy
@ -213,7 +216,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
computeCost(from, to, *solution);
if (!storeSolution(solution))
if (!storeSolution(solution, &from, &to))
return; // solution dropped
solution->setStartState(from);
@ -714,7 +717,9 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
}
bool ConnectingPrivate::canCompute() const {
return !pending.empty();
// Do we still have feasible pending state pairs?
return !pending.empty() && pending.front().first->priority().enabled() &&
pending.front().second->priority().enabled();
}
void ConnectingPrivate::compute() {

View File

@ -127,14 +127,15 @@ Interface::container_type Interface::remove(iterator it) {
}
void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) {
if (priority < state->priority()) { // only allow decreasing of priority (smaller is better)
if (priority == state->priority())
return; // nothing to do
auto it = std::find(begin(), end(), state); // find iterator to state
assert(it != end()); // state should be part of this interface
state->priority_ = priority; // update priority
update(it);
update(it); // update position in ordered list
if (notify_)
notify_(it, true);
}
notify_(it, true); // notify callback
}
void SolutionBase::setCreator(Stage* creator) {

View File

@ -68,7 +68,7 @@ TEST(Interface, update) {
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 }));
i.updatePriority(*i.begin(), Prio(6, 0, false));
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); // larger priority is ignored
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 }));
}
using PrioPair = std::pair<Prio, Prio>;

View File

@ -172,7 +172,7 @@ TEST(ConnectConnect, Pruning) {
++expected_cost;
}
EXPECT_EQ(c2->calls_, 3u);
// EXPECT_EQ(c1->calls_, 6u); // TODO: avoid compute() calls on failure of remaining part
EXPECT_EQ(c1->calls_, 6u); // TODO: avoid compute() calls on failure of remaining part
}
// https://github.com/ros-planning/moveit_task_constructor/issues/218