mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
6c18e2b482
commit
19484cec64
@ -77,6 +77,8 @@ public:
|
|||||||
|
|
||||||
/// called by a (direct) child when a new solution becomes available
|
/// called by a (direct) child when a new solution becomes available
|
||||||
virtual void onNewSolution(const SolutionBase& s) = 0;
|
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:
|
protected:
|
||||||
ContainerBase(ContainerBasePrivate* impl);
|
ContainerBase(ContainerBasePrivate* impl);
|
||||||
@ -95,8 +97,8 @@ public:
|
|||||||
void compute() override;
|
void compute() override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
/// called by a (direct) child when a new solution becomes available
|
|
||||||
void onNewSolution(const SolutionBase& s) override;
|
void onNewSolution(const SolutionBase& s) override;
|
||||||
|
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
SerialContainer(SerialContainerPrivate* impl);
|
SerialContainer(SerialContainerPrivate* impl);
|
||||||
@ -116,6 +118,7 @@ class ParallelContainerBase : public ContainerBase
|
|||||||
public:
|
public:
|
||||||
PRIVATE_CLASS(ParallelContainerBase)
|
PRIVATE_CLASS(ParallelContainerBase)
|
||||||
ParallelContainerBase(const std::string& name = "parallel container");
|
ParallelContainerBase(const std::string& name = "parallel container");
|
||||||
|
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
||||||
|
|||||||
@ -144,7 +144,7 @@ public:
|
|||||||
void spawn(InterfaceState&& state, const SolutionBasePtr& solution);
|
void spawn(InterfaceState&& state, const SolutionBasePtr& solution);
|
||||||
void connect(const InterfaceState& from, const InterfaceState& to, 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);
|
void newSolution(const SolutionBasePtr& solution);
|
||||||
bool storeFailures() const { return introspection_ != nullptr; }
|
bool storeFailures() const { return introspection_ != nullptr; }
|
||||||
void runCompute() {
|
void runCompute() {
|
||||||
|
|||||||
@ -390,6 +390,18 @@ template <>
|
|||||||
inline const InterfaceState::Solutions& trajectories<Interface::BACKWARD>(const InterfaceState* state) {
|
inline const InterfaceState::Solutions& trajectories<Interface::BACKWARD>(const InterfaceState* state) {
|
||||||
return state->incomingTrajectories();
|
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 task_constructor
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
|
|||||||
@ -122,9 +122,7 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
|
|||||||
const InterfaceState* internal_to) {
|
const InterfaceState* internal_to) {
|
||||||
computeCost(*internal_from, *internal_to, *solution);
|
computeCost(*internal_from, *internal_to, *solution);
|
||||||
|
|
||||||
if (!storeSolution(solution))
|
// map internal to external states
|
||||||
return;
|
|
||||||
|
|
||||||
auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* {
|
auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* {
|
||||||
auto it = internal_to_external_.find(internal);
|
auto it = internal_to_external_.find(internal);
|
||||||
if (it != internal_to_external_.end())
|
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_from = find_or_create_external(internal_from, created_from);
|
||||||
InterfaceState* external_to = find_or_create_external(internal_to, created_to);
|
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
|
// connect solution to start/end state
|
||||||
solution->setStartState(*external_from);
|
solution->setStartState(*external_from);
|
||||||
solution->setEndState(*external_to);
|
solution->setEndState(*external_to);
|
||||||
@ -368,6 +369,8 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
|
|||||||
impl->liftSolution(solution, solution->internalStart(), solution->internalEnd());
|
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(SerialContainerPrivate* impl) : ContainerBase(impl) {}
|
||||||
SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {}
|
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::ParallelContainerBase(const std::string& name)
|
||||||
: ParallelContainerBase(new ParallelContainerBasePrivate(this, 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) {
|
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
|
||||||
pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)),
|
pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)),
|
||||||
solution.start(), solution.end());
|
solution.start(), solution.end());
|
||||||
|
|||||||
@ -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());
|
solution->setCreator(me());
|
||||||
if (introspection_)
|
if (introspection_)
|
||||||
introspection_->registerSolution(*solution);
|
introspection_->registerSolution(*solution);
|
||||||
|
|
||||||
if (solution->isFailure()) {
|
if (solution->isFailure()) {
|
||||||
++num_failures_;
|
++num_failures_;
|
||||||
|
if (parent())
|
||||||
|
parent()->onNewFailure(*me(), from, to);
|
||||||
if (!storeFailures())
|
if (!storeFailures())
|
||||||
return false; // drop solution
|
return false; // drop solution
|
||||||
failures_.push_back(solution);
|
failures_.push_back(solution);
|
||||||
@ -150,7 +153,7 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to,
|
|||||||
|
|
||||||
computeCost(from, to, *solution);
|
computeCost(from, to, *solution);
|
||||||
|
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution, &from, nullptr))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
|
|
||||||
me()->forwardProperties(from, to);
|
me()->forwardProperties(from, to);
|
||||||
@ -172,7 +175,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
|
|||||||
|
|
||||||
computeCost(from, to, *solution);
|
computeCost(from, to, *solution);
|
||||||
|
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution, nullptr, &to))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
|
|
||||||
me()->forwardProperties(to, from);
|
me()->forwardProperties(to, from);
|
||||||
@ -193,7 +196,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution
|
|||||||
|
|
||||||
computeCost(state, state, *solution);
|
computeCost(state, state, *solution);
|
||||||
|
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution, nullptr, nullptr))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
|
|
||||||
auto from = states_.insert(states_.end(), InterfaceState(state)); // copy
|
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) {
|
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
|
||||||
computeCost(from, to, *solution);
|
computeCost(from, to, *solution);
|
||||||
|
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution, &from, &to))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
|
|
||||||
solution->setStartState(from);
|
solution->setStartState(from);
|
||||||
@ -714,7 +717,9 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool ConnectingPrivate::canCompute() const {
|
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() {
|
void ConnectingPrivate::compute() {
|
||||||
|
|||||||
@ -127,14 +127,15 @@ Interface::container_type Interface::remove(iterator it) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) {
|
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())
|
||||||
auto it = std::find(begin(), end(), state); // find iterator to state
|
return; // nothing to do
|
||||||
assert(it != end()); // state should be part of this interface
|
|
||||||
state->priority_ = priority; // update priority
|
auto it = std::find(begin(), end(), state); // find iterator to state
|
||||||
update(it);
|
assert(it != end()); // state should be part of this interface
|
||||||
if (notify_)
|
state->priority_ = priority; // update priority
|
||||||
notify_(it, true);
|
update(it); // update position in ordered list
|
||||||
}
|
if (notify_)
|
||||||
|
notify_(it, true); // notify callback
|
||||||
}
|
}
|
||||||
|
|
||||||
void SolutionBase::setCreator(Stage* creator) {
|
void SolutionBase::setCreator(Stage* creator) {
|
||||||
|
|||||||
@ -68,7 +68,7 @@ TEST(Interface, update) {
|
|||||||
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 }));
|
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 }));
|
||||||
|
|
||||||
i.updatePriority(*i.begin(), Prio(6, 0, false));
|
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>;
|
using PrioPair = std::pair<Prio, Prio>;
|
||||||
|
|||||||
@ -172,7 +172,7 @@ TEST(ConnectConnect, Pruning) {
|
|||||||
++expected_cost;
|
++expected_cost;
|
||||||
}
|
}
|
||||||
EXPECT_EQ(c2->calls_, 3u);
|
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
|
// https://github.com/ros-planning/moveit_task_constructor/issues/218
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user