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