diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index fab732a71..6fcd34dc0 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -150,6 +150,7 @@ class Alternatives : public ParallelContainerBase void onNewSolution(const SolutionBase& s) override; }; +class FallbacksPrivate; /** Plan for different alternatives in sequence. * * Try to find feasible solutions using first child. Only if this fails, @@ -158,16 +159,17 @@ class Alternatives : public ParallelContainerBase */ class Fallbacks : public ParallelContainerBase { - mutable Stage* active_child_ = nullptr; - public: - Fallbacks(const std::string& name = "fallbacks") : ParallelContainerBase(name) {} + PRIVATE_CLASS(Fallbacks); + Fallbacks(const std::string& name = "fallbacks"); void reset() override; void init(const moveit::core::RobotModelConstPtr& robot_model) override; bool canCompute() const override; void compute() override; +protected: + Fallbacks(FallbacksPrivate* impl); void onNewSolution(const SolutionBase& s) override; }; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 3b58d0da1..e488c9581 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -76,6 +76,7 @@ namespace task_constructor { class ContainerBasePrivate : public StagePrivate { friend class ContainerBase; + friend class ConnectingPrivate; // needs to call protected setStatus() friend void swap(StagePrivate*& lhs, StagePrivate*& rhs); public: @@ -131,7 +132,7 @@ class ContainerBasePrivate : public StagePrivate inline const auto& externalToInternalMap() const { return internal_external_.by(); } /// called by a (direct) child when a solution failed - void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to); + virtual void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to); protected: ContainerBasePrivate(ContainerBase* me, const std::string& name); @@ -148,13 +149,21 @@ class ContainerBasePrivate : public StagePrivate child->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } - /// Set ENABLED / PRUNED status of the solution tree starting from s into given direction + /// Set ENABLED/PRUNED status of a solution branch starting from target into the given direction template - void setStatus(const InterfaceState* s, InterfaceState::Status status); + void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, + InterfaceState::Status status); + /// non-template version + void setStatus(Interface::Direction dir, const Stage* creator, const InterfaceState* source, + const InterfaceState* target, InterfaceState::Status status); /// copy external_state to a child's interface and remember the link in internal_external map template - void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); + void copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated); + /// non-template version + void copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target, + Interface::UpdateFlags updated); + /// lift solution from internal to external level void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, const InterfaceState* internal_to); @@ -227,13 +236,85 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate protected: void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const; -private: - /// callback for new externally received states + /// notify callback for new states received on external interfaces template - void onNewExternalState(Interface::iterator external, bool updated); + void propagateStateToChildren(Interface::iterator external, Interface::UpdateFlags updated); + +private: + // override for custom behavior on received interface states + virtual void initializeExternalInterfaces(); }; PIMPL_FUNCTIONS(ParallelContainerBase) +class FallbacksPrivate : public ParallelContainerBasePrivate +{ + friend class Fallbacks; + + struct ExternalState + { + ExternalState() = default; + ExternalState(Interface::iterator state, container_type::const_iterator child) + : external_state(state), stage(child) {} + + Interface::iterator external_state; + container_type::const_iterator stage; + + inline bool operator<(const ExternalState& other) const { return *external_state < *other.external_state; } + }; + + struct PendingStates + { + std::pair pop(); + inline void push(const ExternalState& job, Interface::Direction dir); + inline bool empty() const { return pending_states_[0].empty() && pending_states_[1].empty(); } + + /// get pending states queue for a given direction + template + inline ordered& queue() { + static_assert(static_cast(dir) < 2, "Expecting dir = FORWARD | BACKWARD"); + return pending_states_[dir]; + } + + inline auto& current() { return current_; } + + ordered pending_states_[2]; ///< separate queues for start / end states + Interface::Direction current_queue_ = Interface::FORWARD; ///< which queue to check on next pop + struct + { + bool valid = false; + ExternalState state; + Interface::Direction dir = Interface::FORWARD; + } current_; + + } pending_; + + friend std::ostream& operator<<(std::ostream& os, const FallbacksPrivate::PendingStates& pending); + +public: + FallbacksPrivate(Fallbacks* me, const std::string& name); + +protected: + inline void computeGenerate(); + void computeFromExternal(); + + bool seekToNextPending(); + + container_type::const_iterator current_generator_; + bool seekToNextGenerator(); + +private: + void initializeExternalInterfaces() override; + template + void onNewExternalState(Interface::iterator external, Interface::UpdateFlags updated); + void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; + + void advanceCurrentStateToNextChild(); + // print pending states for debugging + void printPending(const char* comment = "pending: ") const; +}; +PIMPL_FUNCTIONS(Fallbacks) +std::ostream& operator<<(std::ostream& os, const FallbacksPrivate::PendingStates& pending); + class WrapperBasePrivate : public ParallelContainerBasePrivate { friend class WrapperBase; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 87ed7b273..dff9febc5 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -100,17 +100,11 @@ class StagePrivate inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); } inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); } - /// direction-based access to pull/push interfaces - inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; } - inline InterfacePtr pushInterface(Interface::Direction dir) { - return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); - } - inline InterfaceConstPtr pullInterface(Interface::Direction dir) const { - return dir == Interface::FORWARD ? starts_ : ends_; - } - inline InterfaceConstPtr pushInterface(Interface::Direction dir) const { - return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); - } + /// direction-based access to pull interface + template + inline InterfacePtr pullInterface(); + /// non-template version + inline InterfacePtr pullInterface(Interface::Direction dir); /// set parent of stage /// enforce only one parent exists @@ -204,6 +198,19 @@ class StagePrivate PIMPL_FUNCTIONS(Stage) std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); +template <> +inline InterfacePtr StagePrivate::pullInterface() { + return starts_; +} +template <> +inline InterfacePtr StagePrivate::pullInterface() { + return ends_; +} +inline InterfacePtr StagePrivate::pullInterface(Interface::Direction dir) { + return dir == Interface::Direction::FORWARD ? pullInterface() : + pullInterface(); +} + template <> inline void StagePrivate::send(const InterfaceState& start, InterfaceState&& end, const SolutionBasePtr& solution) { @@ -310,18 +317,15 @@ class ConnectingPrivate : public ComputeBasePrivate } static inline bool less(const InterfaceState::Priority& lhsA, const InterfaceState::Priority& lhsB, const InterfaceState::Priority& rhsA, const InterfaceState::Priority& rhsB) { - unsigned char lhs = (lhsA.enabled() << 1) | lhsB.enabled(); // combine bits into two-digit binary number - unsigned char rhs = (rhsA.enabled() << 1) | rhsB.enabled(); + bool lhs = lhsA.enabled() && lhsB.enabled(); + bool rhs = rhsA.enabled() && rhsB.enabled(); + if (lhs == rhs) // if enabled status is identical return lhsA + lhsB < rhsA + rhsB; // compare the sums of both contributions - // one of the states in each pair should be enabled - assert(lhs != 0b00 && rhs != 0b00); - // both states valid (b11) - if (lhs == 0b11) - return true; - if (rhs == 0b11) - return false; - return lhs < rhs; // disabled states in 1st component go before disabled states in 2nd component + + // sort both-enabled pairs first + static_assert(true > false, "Comparing enabled states requires true > false"); + return lhs > rhs; } }; @@ -333,7 +337,7 @@ class ConnectingPrivate : public ComputeBasePrivate // Check whether there are pending feasible states that could connect to source template - bool hasPendingOpposites(const InterfaceState* source) const; + bool hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const; std::ostream& printPendingPairs(std::ostream& os = std::cerr) const; @@ -342,9 +346,9 @@ class ConnectingPrivate : public ComputeBasePrivate template inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second); - // get informed when new start or end state becomes available + // notify callback to get informed about newly inserted (or updated) start or end states template - void newState(Interface::iterator it, bool updated); + void newState(Interface::iterator it, Interface::UpdateFlags updated); // ordered list of pending state pairs ordered pending; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 2d3397bbe..a9399d2ef 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -42,6 +42,7 @@ #include #include #include +#include #include #include @@ -82,9 +83,11 @@ class InterfaceState enum Status { ENABLED, // state is actively considered during planning - PRUNED, // state is disabled because a required connected state failed - FAILED, // state that failed, causing the whole partial solution to be disabled + ARMED, // disabled state in a Connecting interface that will become re-enabled with a new opposite state + PRUNED, // disabled state on a pruned solution branch }; + static const char* STATUS_COLOR[]; + /** InterfaceStates are ordered according to two values: * Depth of interlinked trajectory parts and accumulated trajectory costs along that path. * Preference ordering considers high-depth first and within same depth, minimal cost paths. @@ -100,8 +103,6 @@ class InterfaceState inline Status status() const { return std::get<0>(*this); } inline bool enabled() const { return std::get<0>(*this) == ENABLED; } - inline bool failed() const { return std::get<0>(*this) == FAILED; } - inline bool pruned() const { return std::get<0>(*this) == PRUNED; } inline unsigned int depth() const { return std::get<1>(*this); } inline double cost() const { return std::get<2>(*this); } @@ -138,13 +139,21 @@ class InterfaceState /// states are ordered by priority inline bool operator<(const InterfaceState& other) const { return this->priority_ < other.priority_; } + inline const Priority& priority() const { return priority_; } + /// Update priority and call owner's notify() if possible + void updatePriority(const InterfaceState::Priority& priority); + /// Update status, but keep current priority + void updateStatus(Status status); + Interface* owner() const { return owner_; } private: // these methods should be only called by SolutionBase::set[Start|End]State() inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); } inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); } + // Set new priority without updating the owning interface (USE WITH CARE) + inline void setPriority(const Priority& prio) { priority_ = prio; } private: planning_scene::PlanningSceneConstPtr scene_; @@ -169,6 +178,7 @@ class Interface : public ordered class iterator : public base_type::iterator { public: + iterator() = default; iterator(base_type::iterator other) : base_type::iterator(other) {} InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); } @@ -190,10 +200,27 @@ class Interface : public ordered { FORWARD, BACKWARD, - START = FORWARD, - END = BACKWARD }; - using NotifyFunction = std::function; + enum Update + { + STATUS = 1 << 0, + PRIORITY = 1 << 1, + ALL = STATUS | PRIORITY, + }; + using UpdateFlags = utils::Flags; + using NotifyFunction = std::function; + + class DisableNotify + { + Interface& if_; + Interface::NotifyFunction old_; + + public: + DisableNotify(Interface& i) : if_(i) { old_.swap(if_.notify_); } + ~DisableNotify() { old_.swap(if_.notify_); } + }; + friend class DisableNotify; + Interface(const NotifyFunction& notify = NotifyFunction()); /// add a new InterfaceState @@ -204,9 +231,10 @@ class Interface : public ordered /// update state's priority (and call notify_ if it really has changed) void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority); + inline bool notifyEnabled() const { return static_cast(notify_); } private: - const NotifyFunction notify_; + NotifyFunction notify_; // restrict access to some functions to ensure consistency // (we need to set/unset InterfaceState::owner_) diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index 302d47804..70b894829 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -140,8 +140,6 @@ class Flags Int i; }; -#define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags; - const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, std::string frame); diff --git a/core/src/container.cpp b/core/src/container.cpp index 422dc92c7..1c748f0f6 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -53,6 +53,9 @@ using namespace std::placeholders; namespace moveit { namespace task_constructor { +static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator, + std::ostream& os = std::cerr); + ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name) : StagePrivate(me, name) , required_interface_(UNKNOWN) @@ -106,66 +109,37 @@ void ContainerBasePrivate::compute() { static_cast(me_)->compute(); } -void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { - ROS_DEBUG_STREAM_NAMED("Pruning", "'" << child.name() << "' generated a failure"); - switch (child.pimpl()->interfaceFlags()) { - case GENERATE: - // just ignore: the pair of (new) states isn't known to us anyway - // TODO: If child is a container, from and to might have associated solutions already! - break; - - case PROPAGATE_FORWARDS: // mark from as failed (backwards) - ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch"); - setStatus(from, InterfaceState::Status::FAILED); - break; - case PROPAGATE_BACKWARDS: // mark to as failed (forwards) - ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch"); - setStatus(to, InterfaceState::Status::FAILED); - break; - - case CONNECT: - if (const Connecting* conn = dynamic_cast(&child)) { - auto cimpl = conn->pimpl(); - if (!cimpl->hasPendingOpposites(from)) { - ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch"); - setStatus(from, InterfaceState::Status::FAILED); - } - if (!cimpl->hasPendingOpposites(to)) { - ROS_DEBUG_STREAM_NAMED("Pruning", "prune forward branch"); - setStatus(to, InterfaceState::Status::FAILED); - } - } - break; - } - // printChildrenInterfaces(*this, false, child); -} - template -void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) { - if (s->priority().status() == status) +void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, + InterfaceState::Status status) { + if (status != InterfaceState::Status::ENABLED && creator) { + if (const auto* conn = dynamic_cast(creator)) { + auto cimpl = conn->pimpl(); + // if creator is a Connecting stage and target has enabled opposite states (other than source) + if (cimpl->hasPendingOpposites(source, target)) + return; // don't prune + } + } + if (target->priority().status() == status) return; // nothing changing - // if we should disable the state, only do so when there is no enabled alternative path - if (status == InterfaceState::PRUNED) { + // Skip disabling the state, if there are alternative enabled solutions + if (status != InterfaceState::ENABLED) { auto solution_is_enabled = [](auto&& solution) { return state()>(*solution)->priority().enabled(); }; - const auto& alternatives = trajectories()>(*s); + const auto& alternatives = trajectories()>(*target); auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled); if (alternative_path != alternatives.cend()) return; } // actually enable/disable the state - if (s->owner()) { - s->owner()->updatePriority(const_cast(s), InterfaceState::Priority(s->priority(), status)); - } else { - const_cast(s)->priority_ = InterfaceState::Priority(s->priority(), status); - } + const_cast(target)->updateStatus(status); - // if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface - if (parent() && trajectories(*s).empty()) { - auto external{ internalToExternalMap().find(s) }; + // if possible (i.e. if target has an external counterpart), escalate setStatus to external interface + if (parent() && trajectories(*target).empty()) { + auto external{ internalToExternalMap().find(target) }; if (external != internalToExternalMap().end()) { // do we have an external state? // only escalate if there is no other *enabled* internal state connected to the same external one // all internal states linked to external @@ -173,37 +147,83 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); }; auto other_path{ std::find_if(internals.first, internals.second, is_enabled) }; if (other_path == internals.second) - parent()->pimpl()->setStatus(external->get(), status); + parent()->pimpl()->setStatus(nullptr, nullptr, external->get(), status); return; } } // To break symmetry between both ends of a partial solution sequence that gets disabled, - // we mark the first state with FAILED and all other states down the tree only with PRUNED. - // This allows us to re-enable the FAILED side, while not (yet) consider the PRUNED states again, + // we mark the first state with ARMED and all other states down the tree with PRUNED. + // This allows us to re-enable the ARMED state, but not the PRUNED states, // when new states arrive in a Connecting stage. - // All PRUNED states are only re-enabled if the FAILED state actually gets connected. - // For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221 - if (status == InterfaceState::Status::FAILED) - status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED + // For details, https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 + if (status == InterfaceState::Status::ARMED) + status = InterfaceState::Status::PRUNED; // only the first state is marked as ARMED // traverse solution tree - for (const SolutionBase* successor : trajectories(*s)) - setStatus(state(*successor), status); + for (const SolutionBase* successor : trajectories(*target)) + setStatus(successor->creator(), target, state(*successor), status); } -template void ContainerBasePrivate::setStatus(const InterfaceState*, InterfaceState::Status); -template void ContainerBasePrivate::setStatus(const InterfaceState*, InterfaceState::Status); +void ContainerBasePrivate::setStatus(Interface::Direction dir, const Stage* creator, const InterfaceState* source, + const InterfaceState* target, InterfaceState::Status status) { + if (dir == Interface::FORWARD) + setStatus(creator, source, target, status); + else + setStatus(creator, source, target, status); +} + +// recursively update state priorities along solution path template -void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { - if (updated) { - auto internals{ externalToInternalMap().equal_range(&*external) }; - for (auto& i = internals.first; i != internals.second; ++i) { - setStatus(i->second, external->priority().status()); - } +inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Priority& prio) { + InterfaceState::Priority priority(prio, s.priority().status()); + if (s.priority() == priority) return; + const_cast(s).updatePriority(priority); + for (const SolutionBase* successor : trajectories(s)) + updateStatePrios(*state(*successor), prio); +} + +void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { + ROS_DEBUG_STREAM_NAMED("Pruning", "'" << child.name() << "' generated a failure"); + switch (child.pimpl()->interfaceFlags()) { + case GENERATE: + // just ignore: the pair of (new) states isn't known to us anyway + // TODO: If child is a container, from and to might have associated solutions already! + break; + + case PROPAGATE_FORWARDS: // mark from as failed (backwards) + setStatus(nullptr, nullptr, from, InterfaceState::Status::PRUNED); + break; + case PROPAGATE_BACKWARDS: // mark to as failed (forwards) + setStatus(nullptr, nullptr, to, InterfaceState::Status::PRUNED); + break; + + case CONNECT: + setStatus(&child, to, from, InterfaceState::Status::ARMED); + setStatus(&child, from, to, InterfaceState::Status::ARMED); + break; } + // printChildrenInterfaces(*this, false, child); +} +template +void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, + Interface::UpdateFlags updated) { + if (updated) { + auto prio = external->priority(); + auto internals = externalToInternalMap().equal_range(&*external); + + if (updated.testFlag(Interface::Update::STATUS)) { // propagate external status updates to internal copies + for (auto& i = internals.first; i != internals.second; ++i) + setStatus(nullptr, nullptr, i->second, prio.status()); + } else if (updated.testFlag(Interface::Update::PRIORITY)) { + for (auto& i = internals.first; i != internals.second; ++i) + updateStatePrios()>(*i->second, prio); + } else + assert(false); // Expecting either STATUS or PRIORITY updates, not both! + return; + } // create a clone of external state within target interface (child's starts() or ends()) auto internal = states_.insert(states_.end(), InterfaceState(*external)); target->add(*internal); @@ -211,6 +231,14 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa internalToExternalMap().insert(std::make_pair(&*internal, &*external)); } +void ContainerBasePrivate::copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target, + Interface::UpdateFlags updated) { + if (dir == Interface::FORWARD) + copyState(external, target, updated); + else + copyState(external, target, updated); +} + void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, const InterfaceState* internal_to) { computeCost(*internal_from, *internal_to, *solution); @@ -375,8 +403,8 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) { } // for debugging of how children interfaces evolve over time -static void printChildrenInterfaces(const ContainerBase& container, bool success, const Stage& creator, - std::ostream& os = std::cerr) { +static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator, + std::ostream& os) { static unsigned int id = 0; const unsigned int width = 10; // indentation of name os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' '; @@ -386,7 +414,7 @@ static void printChildrenInterfaces(const ContainerBase& container, bool success conn->pimpl()->printPendingPairs(os); os << std::endl; - for (const auto& child : container.pimpl()->children()) { + for (const auto& child : container.children()) { auto cimpl = child->pimpl(); os << std::setw(width) << std::left << child->name(); if (!cimpl->starts() && !cimpl->ends()) @@ -431,20 +459,6 @@ struct SolutionCollector SolutionSequence::container_type trace; }; -inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) { - if (state->owner()) // owner becomes NULL if state is removed from (pending) Interface list - state->owner()->updatePriority(const_cast(state), - // update depth + cost, but keep current status - InterfaceState::Priority(prio, state->priority().status())); -} - -template -inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path, - const InterfaceState::Priority& prio) { - for (const SolutionBase* solution : partial_solution_path) - updateStatePrio(state(*solution), prio); -} - void SerialContainer::onNewSolution(const SolutionBase& current) { ROS_DEBUG_STREAM_NAMED("SerialContainer", "'" << this->name() << "' received solution of child stage '" << current.creator()->name() << "'"); @@ -452,10 +466,6 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { // failures should never trigger this callback assert(!current.isFailure()); - // states of solution must be active, otherwise this would not have been computed - assert(current.start()->priority().enabled()); - assert(current.end()->priority().enabled()); - auto impl = pimpl(); const Stage* creator = current.creator(); auto& children = impl->children(); @@ -493,14 +503,12 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { } if (prio.depth() > 1) { // update state priorities along the whole partial solution path - updateStatePrio(current.start(), prio); - updateStatePrio(current.end(), prio); - updateStatePrios(in.first, prio); - updateStatePrios(out.first, prio); + updateStatePrios(*current.start(), prio); + updateStatePrios(*current.end(), prio); } } } - // printChildrenInterfaces(*this, true, *current.creator()); + // printChildrenInterfaces(*this->pimpl(), true, *current.creator()); // finally, store + announce new solutions to external interface for (const auto& solution : sorted) @@ -565,7 +573,7 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { validateInterface(*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) { + starts_.reset(new Interface([this, target](Interface::iterator it, Interface::UpdateFlags updated) { this->copyState(it, target, updated); })); } catch (InitStageException& e) { @@ -590,7 +598,7 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { validateInterface(*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) { + ends_.reset(new Interface([this, target](Interface::iterator it, Interface::UpdateFlags updated) { this->copyState(it, target, updated); })); } catch (InitStageException& e) { @@ -683,17 +691,21 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) { if (exceptions) throw exceptions; + required_interface_ = expected; + + initializeExternalInterfaces(); +} + +void ParallelContainerBasePrivate::initializeExternalInterfaces() { // 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(external, updated); + if (requiredInterface() & READS_START) + starts().reset(new Interface([this](Interface::iterator external, Interface::UpdateFlags updated) { + this->propagateStateToChildren(external, updated); })); - if (expected & READS_END) - ends().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(external, updated); + if (requiredInterface() & READS_END) + ends().reset(new Interface([this](Interface::iterator external, Interface::UpdateFlags updated) { + this->propagateStateToChildren(external, updated); })); - - required_interface_ = expected; } void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external, @@ -728,9 +740,9 @@ void ParallelContainerBasePrivate::validateConnectivity() const { } template -void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) { +void ParallelContainerBasePrivate::propagateStateToChildren(Interface::iterator external, Interface::UpdateFlags updated) { for (const Stage::pointer& stage : children()) - copyState(external, stage->pimpl()->pullInterface(dir), updated); + copyState(external, stage->pimpl()->pullInterface(), updated); } ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) : ContainerBase(impl) {} @@ -801,44 +813,239 @@ void Alternatives::onNewSolution(const SolutionBase& s) { liftSolution(s); } +Fallbacks::Fallbacks(const std::string& name) : Fallbacks(new FallbacksPrivate(this, name)) {} + +Fallbacks::Fallbacks(FallbacksPrivate* impl) : ParallelContainerBase(impl) {} + void Fallbacks::reset() { - active_child_ = nullptr; ParallelContainerBase::reset(); } void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { + auto& impl{ *pimpl() }; ParallelContainerBase::init(robot_model); - active_child_ = pimpl()->children().front().get(); + impl.current_generator_ = impl.children().begin(); } bool Fallbacks::canCompute() const { - while (active_child_) { - StagePrivate* child = active_child_->pimpl(); - if (child->canCompute()) - return true; - - // active child failed, continue with next - auto next = child->it(); - ++next; - if (next == pimpl()->children().end()) - active_child_ = nullptr; - else - active_child_ = next->get(); + auto impl { pimpl() }; + + switch(impl->requiredInterface()) { + case GENERATE: + return const_cast(impl)->seekToNextGenerator(); + case PROPAGATE_FORWARDS: + case PROPAGATE_BACKWARDS: + case CONNECT: + return const_cast(impl)->seekToNextPending(); + default: + assert(false); } - return false; } void Fallbacks::compute() { - if (!active_child_) - return; - - active_child_->pimpl()->runCompute(); + auto impl { pimpl() }; + + switch(impl->requiredInterface()) { + case GENERATE: + impl->computeGenerate(); + break; + case PROPAGATE_FORWARDS: + case PROPAGATE_BACKWARDS: + case CONNECT: + impl->computeFromExternal(); + break; + default: + assert(false); + } } void Fallbacks::onNewSolution(const SolutionBase& s) { liftSolution(s); } +void FallbacksPrivate::PendingStates::push(const FallbacksPrivate::ExternalState& state, Interface::Direction dir){ + assert(static_cast(dir) < 2); + pending_states_[dir].push(state); +} + +std::pair +FallbacksPrivate::PendingStates::pop(){ + if(pending_states_[current_queue_].empty()) + current_queue_ = static_cast(1 - current_queue_); + // Did you call pop when this->empty() == false? + assert(!pending_states_[current_queue_].empty()); + + auto job{ std::make_pair(pending_states_[current_queue_].pop(), static_cast(current_queue_)) }; + + current_queue_ = static_cast(1 - current_queue_); + + return job; +} + +FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) + : ParallelContainerBasePrivate(me, name) {} + +void FallbacksPrivate::initializeExternalInterfaces() { + if (requiredInterface() & READS_START) + starts().reset(new Interface([this](Interface::iterator external, Interface::UpdateFlags updated) { + this->onNewExternalState(external, updated); + })); + if (requiredInterface() & READS_END) + ends().reset(new Interface([this](Interface::iterator external, Interface::UpdateFlags updated) { + this->onNewExternalState(external, updated); + })); +} + +inline void FallbacksPrivate::printPending(const char* comment) const { + ROS_DEBUG_STREAM_NAMED("Fallbacks", name() << ": " << comment << "\n" << pending_); +} + +std::ostream& operator<<(std::ostream& os, const FallbacksPrivate::PendingStates& self) { + static const char* color_reset = "\033[m"; + + auto print_priorities{ [&](const char* prefix, Interface::Direction queue_idx){ + os << color_reset; + if(queue_idx == self.current_queue_) + os << "*"; + os << prefix; + if(self.pending_states_[queue_idx].empty()){ + os << ""; + } + else { + for(const auto& e : self.pending_states_[queue_idx]) + os << e.external_state->priority() << " "; + } + os << "\n"; + }}; + + os << color_reset << "active: "; + if (self.current_.valid) + os << self.current_.state.external_state->priority(); + else + os << ""; + os << "\n"; + print_priorities("pending starts: ", Interface::FORWARD); + print_priorities("pending ends: ", Interface::BACKWARD); + return os; +} + +void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState* /*from*/, const InterfaceState* /*to*/) { + // This override is deliberately empty. + // The method prunes solution paths when a child failed to find a valid solution for it, + // but in Fallbacks the next child might still yield a successful solution + // Thus pruning must only occur once the last child is exhausted (inside computeFromExternal) +} + +bool FallbacksPrivate::seekToNextGenerator() { + // current_generator_ is fixed if it produced solutions before + if (solutions_.empty()){ + // move to first generator that can run + while(current_generator_ != children().end() && !(*current_generator_)->pimpl()->canCompute()) { + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*current_generator_)->name() << "' can't compute, trying next one."); + ++current_generator_; + } + } + return current_generator_ != children().end() && (*current_generator_)->pimpl()->canCompute(); +} + +void FallbacksPrivate::computeGenerate() { + if (!seekToNextGenerator()) + return; // cannot compute anything + + (*current_generator_)->pimpl()->runCompute(); +} + +bool FallbacksPrivate::seekToNextPending() { + auto& current{ pending_.current() }; + while(!pending_.empty() && !current.valid){ + std::tie(current.state, current.dir) = pending_.pop(); + + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state to '" << (*current.state.stage)->name() << "'"); + // feed a new state + copyState(current.dir, + current.state.external_state, + (*current.state.stage)->pimpl()->pullInterface(current.dir), + Interface::UpdateFlags()); + + if ((current.valid = (*current.state.stage)->pimpl()->canCompute())) + break; + + advanceCurrentStateToNextChild(); + } + return current.valid; +} + +template +void FallbacksPrivate::onNewExternalState(Interface::iterator external, Interface::UpdateFlags updated) { + if (updated) { + auto& queue{ pending_.queue() }; + auto it = std::find_if(queue.begin(), queue.end(), + [external](const ExternalState& s) { return s.external_state == external; }); + if (it == queue.cend()) + return; // already processed + + queue.update(it); // update sorting pos of this single item + + // update prio of linked internal states as well + ContainerBasePrivate::copyState(external, InterfacePtr(), updated); + printPending("after update: "); + return; + } + + pending_.push(ExternalState{ external, children().cbegin() }, dir); + printPending("after push: "); +} + +void FallbacksPrivate::computeFromExternal(){ + auto& current{ pending_.current() }; + assert(current.valid); + + auto& stage{ current.state.stage }; + auto& state{ current.state.external_state }; + auto dir { current.dir }; + + (*stage)->pimpl()->runCompute(); + + auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){ + const auto& trajectories { d == Interface::FORWARD + ? s.outgoingTrajectories() + : s.incomingTrajectories() }; + return std::find_if(trajectories.cbegin(), trajectories.cend(), [](const auto& t){ return !t->isFailure();}) != trajectories.cend(); + }}; + + if(!(*stage)->pimpl()->canCompute()) { + current.valid = false; + if(has_solutions(*state, dir)){ + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*stage)->name() << "' produced a solution, not invoking further fallbacks"); + return; + } + + advanceCurrentStateToNextChild(); + } + + printPending("after compute: "); +} + +void FallbacksPrivate::advanceCurrentStateToNextChild() { + auto& current { pending_.current() }; + + // the current child must be exhausted + assert(!current.valid); + + if(std::next(current.state.stage) != children().end()){ + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*current.state.stage)->name() << "' failed to generate a solution, schedule state with next child"); + ++current.state.stage; + pending_.push(current.state, current.dir); + } + else { + ROS_DEBUG_STREAM_NAMED("Fallbacks", "State failed to extend through any child, prune path"); + parent()->pimpl()->onNewFailure(*me(), + current.dir == Interface::FORWARD ? &*current.state.external_state : nullptr, + current.dir == Interface::BACKWARD ? nullptr : &*current.state.external_state); + } + +} + MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} void MergerPrivate::resolveInterface(InterfaceFlags expected) { diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 2eaf6a436..1be5ae744 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -709,56 +709,93 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In return StatePair(second, first); } -template -void ConnectingPrivate::newState(Interface::iterator it, bool updated) { - if (updated) { // many pairs might be affected: resort - if (it->priority().pruned()) - // remove all pending pairs involving this state - pending.remove_if([it](const StatePair& p) { return std::get()>(p) == it; }); - else - // TODO(v4hn): If a state becomes reenabled, this skips all previously removed pairs, right? - pending.sort(); +template +void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags updated) { + auto parent_pimpl = parent()->pimpl(); + Interface::DisableNotify disable_source_interface(*pullInterface()); + if (updated) { + if (updated.testFlag(Interface::STATUS) && // only perform these costly operations if needed + pullInterface()>()->notifyEnabled()) // suppress recursive loop + { + // If status has changed, propagate the update to the opposite side + auto status = it->priority().status(); + if (status == InterfaceState::Status::PRUNED) // PRUNED becomes ARMED on opposite side + status = InterfaceState::Status::ARMED; // (only for pending state pairs) + + for (const auto& candidate : this->pending) { + if (std::get()>(candidate) != it) // only consider pairs with source state == state + continue; + auto oit = std::get(candidate); // opposite target state + auto ostatus = oit->priority().status(); + if (ostatus != status) { + if (status != InterfaceState::Status::ENABLED) { + // quicker check for hasPendingOpposites(): search in it->owner() for an enabled alternative + bool cancel = false; // if found, cancel propagation of new status + for (const auto alternative : *it->owner()) + if ((cancel = alternative->priority().enabled())) + break; + if (cancel) + continue; + } + // pass creator=nullptr to skip hasPendingOpposites() check + parent_pimpl->setStatus()>(nullptr, nullptr, &*oit, status); + } + } + } + + // many pairs will have changed priorities: resort pending list + pending.sort(); } else { // new state: insert all pairs with other interface assert(it->priority().enabled()); // new solutions are feasible, aren't they? - InterfacePtr other_interface = pullInterface(other); + InterfacePtr other_interface = pullInterface(); + bool have_enabled_opposites = false; for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { - // Don't re-enable states that are marked DISABLED - if (static_cast(me_)->compatible(*it, *oit)) { - // re-enable the opposing state oit if its status is FAILED - if (oit->priority().status() == InterfaceState::Status::FAILED) - oit->owner()->updatePriority(&*oit, - InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED)); - pending.insert(make_pair(it, oit)); - } + if (!static_cast(me_)->compatible(*it, *oit)) + continue; + + // re-enable the opposing state oit (and its associated solution branch) if its status is ARMED + // https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 + if (oit->priority().status() == InterfaceState::Status::ARMED) + parent_pimpl->setStatus()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED); + if (oit->priority().enabled()) + have_enabled_opposites = true; + + // Remember all pending states, regardless of their status! + pending.insert(make_pair(it, oit)); } + if (!have_enabled_opposites) // prune new state and associated branch if necessary + // pass creator=nullptr to skip hasPendingOpposites() check as we did this here already + parent_pimpl->setStatus(nullptr, nullptr, &*it, InterfaceState::Status::ARMED); } // std::cerr << name_ << ": "; // printPendingPairs(std::cerr); // std::cerr << std::endl; } -// Check whether there are pending feasible states that could connect to source. -// If not, we exhausted all solution candidates for source and thus should mark it as failure. +// Check whether there are pending feasible states (other than source) that could connect to target. +// If not, we exhausted all solution candidates for target and thus should mark it as failure. template -inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const { +inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const { for (const auto& candidate : this->pending) { - static_assert(Interface::FORWARD == 0, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); - const auto src = std::get(candidate); - static_assert(Interface::BACKWARD == 1, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); - const auto tgt = std::get()>(candidate); + static_assert(Interface::FORWARD == 0 && Interface::BACKWARD == 1, + "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); + const InterfaceState* src = &*std::get(candidate); + const InterfaceState* tgt = &*std::get()>(candidate); - if (&*src == source && tgt->priority().enabled()) + if (tgt == target && src != source && src->priority().enabled()) return true; // early stopping when only infeasible pairs are to come - if (!std::get<0>(candidate)->priority().enabled()) + if (!std::get<0>(candidate)->priority().enabled() || !std::get<1>(candidate)->priority().enabled()) break; } return false; } // explicitly instantiate templates for both directions -template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const; -template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const; +template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* start, + const InterfaceState* end) const; +template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* end, + const InterfaceState* start) const; bool ConnectingPrivate::canCompute() const { // Do we still have feasible pending state pairs? @@ -775,11 +812,8 @@ void ConnectingPrivate::compute() { } std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const { - static const char* red = "\033[31m"; - static const char* reset = "\033[m"; + const char* reset = InterfaceState::STATUS_COLOR[3]; for (const auto& candidate : pending) { - if (!candidate.first->priority().enabled() || !candidate.second->priority().enabled()) - os << " " << red; // find indeces of InterfaceState pointers in start/end Interfaces unsigned int first = 0, second = 0; std::find_if(starts()->begin(), starts()->end(), [&](const InterfaceState* s) { @@ -790,9 +824,9 @@ std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const { ++second; return &*candidate.second == s; }); - os << first << ":" << second << " "; + os << InterfaceState::STATUS_COLOR[candidate.first->priority().status()] << first << reset << ":" + << InterfaceState::STATUS_COLOR[candidate.second->priority().status()] << second << reset << " "; } - os << reset; return os; } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index a98ffd3cc..8b825a059 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -82,6 +82,20 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) return cost() < other.cost(); } +void InterfaceState::updatePriority(const InterfaceState::Priority& priority) { + // Never overwrite ARMED with PRUNED: PRUNED => !ARMED + assert(priority.status() != InterfaceState::Status::PRUNED || priority_.status() != InterfaceState::Status::ARMED); + + if (owner()) { + owner()->updatePriority(this, priority); + } else { + setPriority(priority); + } +} +void InterfaceState::updateStatus(Status status) { + updatePriority(InterfaceState::Priority(priority_, status)); +} + Interface::Interface(const Interface::NotifyFunction& notify) : notify_(notify) {} // Announce a new InterfaceState @@ -115,7 +129,7 @@ void Interface::add(InterfaceState& state) { moveFrom(it, container); // and finally call notify callback if (notify_) - notify_(it, false); + notify_(it, UpdateFlags()); } Interface::container_type Interface::remove(iterator it) { @@ -126,15 +140,23 @@ Interface::container_type Interface::remove(iterator it) { } void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) { - if (priority == state->priority()) + const auto old_prio = state->priority(); + if (priority == old_prio) 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 position in ordered list - if (notify_) - notify_(it, true); // notify callback + + if (notify_) { + UpdateFlags updated(Update::ALL); + if (old_prio.status() == priority.status()) + updated &= ~STATUS; + + notify_(it, updated); // notify callback + } } std::ostream& operator<<(std::ostream& os, const Interface& interface) { @@ -144,15 +166,16 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) { os << istate->priority() << " "; return os; } +const char* InterfaceState::STATUS_COLOR[] = { + "\033[32m", // ENABLED - green + "\033[33m", // ARMED - yellow + "\033[31m", // PRUNED - red + "\033[m" // reset +}; std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) { // maps InterfaceState::Status values to output (color-changing) prefix - static const char* prefix[] = { - "\033[32me:", // ENABLED - green - "\033[33md:", // PRUNED - yellow - "\033[31mf:", // FAILED - red - }; - static const char* color_reset = "\033[m"; - os << prefix[prio.status()] << prio.depth() << ":" << prio.cost() << color_reset; + os << InterfaceState::STATUS_COLOR[prio.status()] << prio.depth() << ":" << prio.cost() + << InterfaceState::STATUS_COLOR[3]; return os; } diff --git a/core/test/test_cost_queue.cpp b/core/test/test_cost_queue.cpp index 0f4a0433f..1d6657048 100644 --- a/core/test/test_cost_queue.cpp +++ b/core/test/test_cost_queue.cpp @@ -5,6 +5,10 @@ #include #include +#ifndef TYPED_TEST_SUITE +#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES) +#endif + namespace mtc = moveit::task_constructor; // type-trait functions for OrderedTest @@ -62,11 +66,7 @@ class ValueOrPointeeLessTest : public ::testing::Test }; // set of template types to test for using TypeInstances = ::testing::Types; -#ifdef TYPED_TEST_SUITE TYPED_TEST_SUITE(ValueOrPointeeLessTest, TypeInstances); -#else -TYPED_TEST_CASE(ValueOrPointeeLessTest, TypeInstances); -#endif TYPED_TEST(ValueOrPointeeLessTest, less) { EXPECT_TRUE(this->less(2, 3)); EXPECT_FALSE(this->less(1, 1)); @@ -105,11 +105,7 @@ class OrderedTest : public ::testing::Test, public ordered SCOPED_TRACE("pushAndValidate(" #cost ", " #__VA_ARGS__ ")"); \ this->pushAndValidate(cost, __VA_ARGS__); \ } -#ifdef TYPED_TEST_SUITE TYPED_TEST_SUITE(OrderedTest, TypeInstances); -#else -TYPED_TEST_CASE(OrderedTest, TypeInstances); -#endif TYPED_TEST(OrderedTest, sorting) { pushAndValidate(2, { 2 }); pushAndValidate(1, { 1, 2 }); diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index d37d1e13e..75b5e84c4 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -17,7 +17,7 @@ using namespace moveit::task_constructor; using FallbacksFixtureGenerator = TaskTestBase; -TEST_F(FallbacksFixtureGenerator, DISABLED_stayWithFirstSuccessful) { +TEST_F(FallbacksFixtureGenerator, stayWithFirstSuccessful) { auto fallback = std::make_unique("Fallbacks"); fallback->add(std::make_unique(PredefinedCosts::single(INF))); fallback->add(std::make_unique(PredefinedCosts::single(1.0))); @@ -55,7 +55,7 @@ TEST_F(FallbacksFixturePropagate, failingWithFailedSolutions) { EXPECT_EQ(t.solutions().size(), 0u); } -TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) { +TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStageOnly) { t.add(std::make_unique()); auto fallbacks = std::make_unique("Fallbacks"); @@ -67,7 +67,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) { EXPECT_EQ(t.numSolutions(), 1u); } -TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutionOnly) { +TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStagePerSolutionOnly) { t.add(std::make_unique(PredefinedCosts({ 2.0, 1.0 }))); // duplicate generator solutions with resulting costs: 4, 2 | 3, 1 t.add(std::make_unique(PredefinedCosts({ 2.0, 0.0, 2.0, 0.0 }), 2)); @@ -81,6 +81,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutio EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 211, 222)); } +// requires individual job control in Fallbacks's children TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) { t.add(std::make_unique(PredefinedCosts({ 10.0, 0.0 }))); t.add(std::make_unique(PredefinedCosts({ 1.0, 2.0 }))); @@ -100,6 +101,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) { EXPECT_COSTS(t.solutions(), testing::ElementsAre(2)); // expecting less costly solution as result } +// requires individual job control in Fallbacks's children TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) { t.add(std::make_unique(PredefinedCosts({ 2.0, 1.0, 3.0 }))); // use a fallback container to delay computation: the 1st child never succeeds, but only the 2nd @@ -117,7 +119,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) { // check that first solution is not marked as pruned } -TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) { +TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions) { t.add(std::make_unique()); auto fallback = std::make_unique("Fallbacks"); @@ -129,7 +131,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) { EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0)); } -TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) { +TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions2) { t.add(std::make_unique()); auto fallback = std::make_unique("Fallbacks"); @@ -141,7 +143,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) { EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0)); } -TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) { +TEST_F(FallbacksFixturePropagate, activeChildReset) { t.add(std::make_unique(PredefinedCosts({ 1.0, INF, 3.0 }))); auto fallbacks = std::make_unique("Fallbacks"); @@ -159,7 +161,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) { using FallbacksFixtureConnect = TaskTestBase; -TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) { +TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) { t.add(std::make_unique(PredefinedCosts({ 1.0, 2.0 }))); auto fallbacks = std::make_unique("Fallbacks"); @@ -172,16 +174,3 @@ TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) { EXPECT_TRUE(t.plan()); EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 21, 22)); } - -int main(int argc, char** argv) { - for (int i = 1; i < argc; ++i) { - if (strcmp(argv[i], "--debug") == 0) { - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) - ros::console::notifyLoggerLevelsChanged(); - break; - } - } - - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 9ca6af53f..65aeee36f 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -19,7 +19,7 @@ TEST(InterfaceStatePriority, compare) { EXPECT_TRUE(Prio(1, 42) < Prio(0, 0)); EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // at same depth, higher cost is larger - auto dstart = InterfaceState::Status::FAILED; + auto dstart = InterfaceState::Status::ARMED; EXPECT_TRUE(Prio(0, 0, dstart) == Prio(0, 0, dstart)); EXPECT_TRUE(Prio(1, 0, dstart) < Prio(0, 0, dstart)); EXPECT_TRUE(Prio(1, 42, dstart) < Prio(0, 0, dstart)); @@ -68,7 +68,7 @@ TEST(Interface, update) { i.updatePriority(*i.rbegin(), Prio(5, 0.0)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); - i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::FAILED)); + i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::ARMED)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 })); } @@ -87,8 +87,11 @@ TEST(StatePairs, compare) { EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0))); auto good = InterfaceState::Status::ENABLED; - auto bad = InterfaceState::Status::FAILED; - EXPECT_TRUE(pair(good, good) < pair(good, bad)); - EXPECT_TRUE(pair(good, good) < pair(bad, good)); - EXPECT_TRUE(pair(bad, good) < pair(good, bad)); + auto good_good = pair(Prio(0, 10, good), Prio(0, 0, good)); + ASSERT_TRUE(good_good > pair(good, good)); // a bad status should reverse this relation + for (auto bad : { InterfaceState::Status::ARMED, InterfaceState::Status::PRUNED }) { + EXPECT_TRUE(good_good < pair(bad, good)); + EXPECT_TRUE(good_good < pair(good, bad)); + EXPECT_TRUE(good_good < pair(bad, bad)); + } } diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index 5da29cdd1..501da2403 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -8,6 +8,10 @@ #include +#ifndef TYPED_TEST_SUITE +#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES) +#endif + using namespace moveit::task_constructor; using Pruning = TaskTestBase; @@ -40,7 +44,54 @@ TEST_F(Pruning, PruningMultiForward) { EXPECT_EQ((*t.solutions().begin())->cost(), 0u); } +// The 2nd failing FW attempt would prune the path through CON, +// but shouldn't because there exist two more GEN2 solutions +TEST_F(Pruning, NoPruningIfAlternativesExist) { + add(t, new GeneratorMockup(PredefinedCosts({ 0.0 }))); + add(t, new ConnectMockup()); + add(t, new GeneratorMockup(std::list{ 0, 10, 20, 30 }, 2)); + add(t, new ForwardMockup({ INF, INF, 0.0, INF })); + + t.plan(); + + EXPECT_EQ(t.solutions().size(), 1u); +} + +TEST_F(Pruning, ConnectReactivatesPrunedPaths) { + add(t, new BackwardMockup); + add(t, new GeneratorMockup({ 0 })); + add(t, new ConnectMockup()); + // the solution here should re-activate the initially pruned backward path + add(t, new GeneratorMockup({ 0 })); + + EXPECT_TRUE(t.plan()); + EXPECT_EQ(t.solutions().size(), 1u); +} + +// same as before, but wrapping Connect into a container +template +struct PruningContainerTests : public Pruning +{ + void test() { + add(t, new BackwardMockup); + add(t, new GeneratorMockup({ 0 })); + auto c = new T(); + add(*c, new ConnectMockup()); + add(t, c); + add(t, new GeneratorMockup({ 0 })); + + EXPECT_TRUE(t.plan()); + EXPECT_EQ(t.solutions().size(), 1u); + } +}; +using ContainerTypes = ::testing::Types; +TYPED_TEST_SUITE(PruningContainerTests, ContainerTypes); +TYPED_TEST(PruningContainerTests, ConnectReactivatesPrunedPaths) { + this->test(); +} + TEST_F(Pruning, ConnectConnectForward) { + add(t, new BackwardMockup()); add(t, new GeneratorMockup()); auto c1 = add(t, new ConnectMockup({ INF, 0, 0 })); // 1st attempt is a failue add(t, new GeneratorMockup({ 0, 10, 20 })); @@ -62,6 +113,7 @@ TEST_F(Pruning, ConnectConnectForward) { } TEST_F(Pruning, ConnectConnectBackward) { + add(t, new BackwardMockup()); add(t, new GeneratorMockup({ 1, 2, 3 })); auto c1 = add(t, new ConnectMockup()); add(t, new BackwardMockup()); diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 4634461ef..32b287f1c 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -50,6 +50,7 @@ demo(cartesian) demo(modular) demo(alternative_path_costs) demo(ik_clearance_cost) +demo(fallbacks_move_to) demo(pick_place_demo) target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task) diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp new file mode 100644 index 000000000..0a586febb --- /dev/null +++ b/demo/src/fallbacks_move_to.cpp @@ -0,0 +1,142 @@ +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include + +constexpr double TAU = 2 * M_PI; + +using namespace moveit::task_constructor; + +/** CurrentState -> Fallbacks( MoveTo, MoveTo, MoveTo )*/ +int main(int argc, char** argv) { + ros::init(argc, argv, "mtc_tutorial"); + + ros::AsyncSpinner spinner{ 1 }; + spinner.start(); + + // setup Task + Task t; + t.loadRobotModel(); + const moveit::core::RobotModelConstPtr robot{ t.getRobotModel() }; + + assert(robot->getName() == "panda"); + + // setup solvers + auto cartesian = std::make_shared(); + cartesian->setJumpThreshold(2.0); + + const auto ptp = []() { + auto pp{ std::make_shared("pilz_industrial_motion_planner") }; + pp->setPlannerId("PTP"); + return pp; + }(); + + const auto rrtconnect = []() { + auto pp{ std::make_shared("ompl") }; + pp->setPlannerId("RRTConnectkConfigDefault"); + return pp; + }(); + + // target state for Task + std::map target_state; + robot->getJointModelGroup("panda_arm")->getVariableDefaultPositions("ready", target_state); + target_state["panda_joint1"] = +TAU / 8; + + // define initial scenes + auto initial_scene{ std::make_shared(robot) }; + initial_scene->getCurrentStateNonConst().setToDefaultValues(robot->getJointModelGroup("panda_arm"), "ready"); + + auto initial_alternatives = std::make_unique("initial states"); + + { + // can reach target with Cartesian motion + auto fixed{ std::make_unique("current state") }; + auto scene{ initial_scene->diff() }; + scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } }); + fixed->setState(scene); + initial_alternatives->add(std::move(fixed)); + } + { + // Cartesian motion to target is impossible, but PTP is collision-free + auto fixed{ std::make_unique("current state") }; + auto scene{ initial_scene->diff() }; + scene->getCurrentStateNonConst().setVariablePositions({ + { "panda_joint1", +TAU / 8 }, + { "panda_joint4", 0 }, + }); + fixed->setState(scene); + initial_alternatives->add(std::move(fixed)); + } + { + // Cartesian and PTP motion to target would be in collision + auto fixed = std::make_unique("current state"); + auto scene{ initial_scene->diff() }; + scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } }); + scene->processCollisionObjectMsg([]() { + moveit_msgs::CollisionObject co; + co.id = "box"; + co.header.frame_id = "panda_link0"; + co.operation = co.ADD; +#if MOVEIT_HAS_OBJECT_POSE + auto& pose{ co.pose }; +#else + co.primitive_poses.emplace_back(); + auto& pose{ co.primitive_poses[0] }; +#endif + pose = []() { + geometry_msgs::Pose p; + p.position.x = 0.3; + p.position.y = 0.0; + p.position.z = 0.64 / 2; + p.orientation.w = 1.0; + return p; + }(); + co.primitives.push_back([]() { + shape_msgs::SolidPrimitive sp; + sp.type = sp.BOX; + sp.dimensions = { 0.2, 0.05, 0.64 }; + return sp; + }()); + return co; + }()); + fixed->setState(scene); + initial_alternatives->add(std::move(fixed)); + } + + t.add(std::move(initial_alternatives)); + + // fallbacks to reach target_state + auto fallbacks = std::make_unique("move to other side"); + + auto add_to_fallbacks{ [&](auto& solver, auto& name) { + auto move_to = std::make_unique(name, solver); + move_to->setGroup("panda_arm"); + move_to->setGoal(target_state); + fallbacks->add(std::move(move_to)); + } }; + add_to_fallbacks(cartesian, "Cartesian path"); + add_to_fallbacks(ptp, "PTP path"); + add_to_fallbacks(rrtconnect, "RRT path"); + + t.add(std::move(fallbacks)); + + try { + t.init(); + std::cout << t << std::endl; + t.plan(); + } catch (const InitStageException& e) { + std::cout << e << std::endl; + } + + ros::waitForShutdown(); + + return 0; +}