diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c80d6f4e..2d46fd75 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -32,7 +32,7 @@ jobs: DOCKER_RUN_OPTS: >- -e PRELOAD=libasan.so.5 -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions" - TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1" + TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g" env: CATKIN_LINT: true diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index decc953a..f1063965 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v3.4.0 + rev: v4.0.1 hooks: - id: check-added-large-files - id: check-case-conflict @@ -29,7 +29,7 @@ repos: - id: trailing-whitespace - repo: https://github.com/psf/black - rev: 20.8b1 + rev: 21.11b1 hooks: - id: black diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index fab732a7..cea4a418 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -150,6 +150,7 @@ public: 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,17 +159,23 @@ public: */ class Fallbacks : public ParallelContainerBase { - mutable Stage* active_child_ = nullptr; + inline void replaceImpl(); 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; + +private: + // not needed, we directly use corresponding virtual methods of FallbacksPrivate + bool canCompute() const final { return false; } + void compute() final {} }; class MergerPrivate; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 427bfefc..c9fb61f2 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -76,8 +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); + friend class ConnectingPrivate; // needed propagate setStatus() in newState() public: using container_type = StagePrivate::container_type; @@ -132,10 +131,11 @@ public: 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); + ContainerBasePrivate& operator=(ContainerBasePrivate&& other); // Set child's push interfaces: allow pushing if child requires it. inline void setChildsPushBackwardInterface(StagePrivate* child) { @@ -154,10 +154,14 @@ protected: void setStatus(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 + /// 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, Interface::UpdateFlags updated); - /// lift solution from internal to external level + // 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); @@ -230,12 +234,91 @@ protected: void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const; private: - /// notify callback for new externally received states + /// notify callback for new externally received interface states template - void onNewExternalState(Interface::iterator external, Interface::UpdateFlags updated); + void propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated); + + // override to customize behavior on received interface states (default: propagateStateToAllChildren()) + virtual void initializeExternalInterfaces(); }; PIMPL_FUNCTIONS(ParallelContainerBase) +/* The Fallbacks container needs to implement different behaviour based on its interface. + * Thus, we implement 3 different classes: for Generator, Propagator, and Connect-like interfaces. + * FallbacksPrivate is the common base class for all of them, defining the common API + * to be used by the Fallbacks container. + * The actual interface-specific class is instantiated in initializeExternalInterfaces() + * resp. Fallbacks::replaceImpl() when the actual interface is known. + * The key difference between the 3 variants is how they advance to the next job. */ +class FallbacksPrivate : public ParallelContainerBasePrivate +{ +public: + FallbacksPrivate(Fallbacks* me, const std::string& name); + FallbacksPrivate(FallbacksPrivate&& other); + + void initializeExternalInterfaces() final; + void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; + + // virtual methods specific to each variant + virtual void onNewSolution(const SolutionBase& s); + virtual void reset() {} +}; +PIMPL_FUNCTIONS(Fallbacks) + +/* Class shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator, + which both have the notion of a currently active child stage */ +class FallbacksPrivateCommon : public FallbacksPrivate +{ +public: + FallbacksPrivateCommon(FallbacksPrivate&& other) : FallbacksPrivate(std::move(other)) {} + + /// Advance to next child + inline void nextChild(); + /// Advance to the next job, assuming that the current child is exhausted on the current job. + virtual bool nextJob() = 0; + + void reset() override; + bool canCompute() const override; + void compute() override; + + container_type::const_iterator current_; // currently active child +}; + +/// Fallbacks implementation for GENERATOR interface +struct FallbacksPrivateGenerator : FallbacksPrivateCommon +{ + FallbacksPrivateGenerator(FallbacksPrivate&& old); + bool nextJob() override; +}; + +/// Fallbacks implementation for FORWARD or BACKWARD interface +struct FallbacksPrivatePropagator : FallbacksPrivateCommon +{ + FallbacksPrivatePropagator(FallbacksPrivate&& old); + void reset() override; + void onNewSolution(const SolutionBase& s) override; + bool nextJob() override; + + Interface::Direction dir_; // propagation direction + Interface::iterator job_; // pointer to currently processed external state + bool job_has_solutions_; // flag indicating whether the current job generated solutions +}; + +/// Fallbacks implementation for CONNECT interface +struct FallbacksPrivateConnect : FallbacksPrivate +{ + FallbacksPrivateConnect(FallbacksPrivate&& old); + void reset() override; + bool canCompute() const override; + void compute() override; + void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; + + template + void propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated); + + mutable container_type::const_iterator active_; // child picked for compute() +}; + 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 8e3dda52..5f517262 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -61,7 +61,6 @@ class StagePrivate { friend class Stage; friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); - friend void swap(StagePrivate*& lhs, StagePrivate*& rhs); public: /// container type used to store children @@ -103,6 +102,8 @@ public: /// direction-based access to pull interface template inline InterfacePtr pullInterface(); + // non-template version + inline InterfacePtr pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; } /// set parent of stage /// enforce only one parent exists @@ -157,6 +158,8 @@ public: void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); protected: + StagePrivate& operator=(StagePrivate&& other); + // associated/owning Stage instance Stage* me_; @@ -301,6 +304,7 @@ PIMPL_FUNCTIONS(MonitoringGenerator) class ConnectingPrivate : public ComputeBasePrivate { friend class Connecting; + friend struct FallbacksPrivateConnect; public: struct StatePair : std::pair diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 258d0b7a..2e4d7c70 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -178,6 +178,7 @@ public: 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*(); } @@ -247,6 +248,16 @@ private: std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio); std::ostream& operator<<(std::ostream& os, const Interface& interface); +/// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found. +template +size_t getIndex(const T& container, typename T::const_iterator search) { + size_t index = 1; + for (typename T::const_iterator it = container.begin(), end = container.end(); it != end; ++it, ++index) + if (it == search) + return index; + return 0; +} + class CostTerm; class StagePrivate; class ContainerBasePrivate; diff --git a/core/include/moveit/task_constructor/task_p.h b/core/include/moveit/task_constructor/task_p.h index 327f1631..f0e6dc0a 100644 --- a/core/include/moveit/task_constructor/task_p.h +++ b/core/include/moveit/task_constructor/task_p.h @@ -51,15 +51,14 @@ namespace task_constructor { class TaskPrivate : public WrapperBasePrivate { friend class Task; + TaskPrivate& operator=(TaskPrivate&& other); public: TaskPrivate(Task* me, const std::string& ns); + const std::string& ns() const { return ns_; } const ContainerBase* stages() const; -protected: - static void swap(StagePrivate*& lhs, StagePrivate*& rhs); - private: std::string ns_; robot_model_loader::RobotModelLoaderPtr robot_model_loader_; diff --git a/core/src/container.cpp b/core/src/container.cpp index 951a1adc..ae9f6698 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -53,8 +53,31 @@ using namespace std::placeholders; namespace moveit { namespace task_constructor { +// for debugging of how children interfaces evolve over time static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator, - std::ostream& os = std::cerr); + std::ostream& os = std::cerr) { + static unsigned int id = 0; + const unsigned int width = 10; // indentation of name + os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' '; + if (success) + os << ++id << ' '; + if (const auto conn = dynamic_cast(creator.pimpl())) + conn->printPendingPairs(os); + os << std::endl; + + for (const auto& child : container.children()) { + auto cimpl = child->pimpl(); + os << std::setw(width) << std::left << child->name(); + if (!cimpl->starts() && !cimpl->ends()) + os << "↕ " << std::endl; + if (cimpl->starts()) + os << "↓ " << *child->pimpl()->starts() << std::endl; + if (cimpl->starts() && cimpl->ends()) + os << std::setw(width) << " "; + if (cimpl->ends()) + os << "↑ " << *child->pimpl()->ends() << std::endl; + } +} ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name) : StagePrivate(me, name) @@ -62,6 +85,33 @@ ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& , pending_backward_(new Interface) , pending_forward_(new Interface) {} +ContainerBasePrivate& ContainerBasePrivate::operator=(ContainerBasePrivate&& other) { + assert(internal_external_.empty() && other.internal_external_.empty()); + + // move StagePrivate members + this->StagePrivate::operator=(std::move(other)); + + // swapping of container members needed to maintain valid pending_* interfaces + // and children (e.g. for TaskPrivate) + required_interface_ = other.required_interface_; + std::swap(pending_backward_, other.pending_backward_); + std::swap(pending_forward_, other.pending_forward_); + std::swap(children_, other.children_); + + // redirect all children's parent pointers to the new parent + auto reparent_children = [](ContainerBasePrivate& self) { + for (auto it = self.children_.begin(), end = self.children_.end(); it != end; ++it) { + auto cimpl = (*it)->pimpl(); + cimpl->unparent(); + cimpl->setParent(static_cast(self.me_)); + cimpl->setParentPosition(it); + } + }; + reparent_children(*this); + reparent_children(other); + return *this; +} + ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const { if (!for_insert && index < 0) --index; @@ -139,6 +189,7 @@ void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* // if possible (i.e. if target has an external counterpart), escalate setStatus to external interface if (parent() && trajectories(*target).empty()) { + // TODO: This was coded with SerialContainer in mind. Not sure, it works for ParallelContainers 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 @@ -223,6 +274,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); @@ -386,31 +445,6 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) { return os; } -// for debugging of how children interfaces evolve over time -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() << ' '; - if (success) - os << ++id << ' '; - if (const Connecting* conn = dynamic_cast(&creator)) - conn->pimpl()->printPendingPairs(os); - os << std::endl; - - for (const auto& child : container.children()) { - auto cimpl = child->pimpl(); - os << std::setw(width) << std::left << child->name(); - if (!cimpl->starts() && !cimpl->ends()) - os << "↕ " << std::endl; - if (cimpl->starts()) - os << "↓ " << *child->pimpl()->starts() << std::endl; - if (cimpl->starts() && cimpl->ends()) - os << std::setw(width) << " "; - if (cimpl->ends()) - os << "↑ " << *child->pimpl()->ends() << std::endl; - } -} /** Collect all partial solution sequences originating from start into given direction */ template struct SolutionCollector @@ -492,7 +526,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { } } } - // printChildrenInterfaces(*this, true, *current.creator()); + // printChildrenInterfaces(*this->pimpl(), true, *current.creator()); // finally, store + announce new solutions to external interface for (const auto& solution : sorted) @@ -557,9 +591,9 @@ 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, Interface::UpdateFlags updated) { + starts_ = std::make_shared([this, target](Interface::iterator it, Interface::UpdateFlags updated) { this->copyState(it, target, updated); - })); + }); } catch (InitStageException& e) { exceptions.append(e); } @@ -570,6 +604,7 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { StagePrivate* child_impl = (**it).pimpl(); StagePrivate* previous_impl = (**previous_it).pimpl(); child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK); + child_impl = (**it).pimpl(); // re-assign as pimpl_ pointer of a Fallback container will change! connect(*previous_impl, *child_impl); } catch (InitStageException& e) { exceptions.append(e); @@ -582,9 +617,9 @@ 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, Interface::UpdateFlags updated) { + ends_ = std::make_shared([this, target](Interface::iterator it, Interface::UpdateFlags updated) { this->copyState(it, target, updated); - })); + }); } catch (InitStageException& e) { exceptions.append(e); } @@ -640,9 +675,8 @@ bool SerialContainer::canCompute() const { void SerialContainer::compute() { for (const auto& stage : pimpl()->children()) { - if (!stage->pimpl()->canCompute()) - continue; - stage->pimpl()->runCompute(); + if (stage->pimpl()->canCompute()) + stage->pimpl()->runCompute(); } } @@ -663,8 +697,8 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) { child_impl->resolveInterface(expected); validateInterfaces(*child_impl, expected, first); // initialize push connections of children according to their demands - setChildsPushForwardInterface(child_impl); setChildsPushBackwardInterface(child_impl); + setChildsPushForwardInterface(child_impl); first = false; } catch (InitStageException& e) { exceptions.append(e); @@ -675,17 +709,21 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) { if (exceptions) throw exceptions; - // 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, Interface::UpdateFlags updated) { - this->onNewExternalState(external, updated); - })); - if (expected & READS_END) - ends().reset(new Interface([this](Interface::iterator external, Interface::UpdateFlags updated) { - this->onNewExternalState(external, updated); - })); - required_interface_ = expected; + + initializeExternalInterfaces(); +} + +void ParallelContainerBasePrivate::initializeExternalInterfaces() { + // States received by the container need to be copied to all children's pull interfaces. + if (requiredInterface() & READS_START) + starts() = std::make_shared([this](Interface::iterator external, Interface::UpdateFlags updated) { + this->propagateStateToAllChildren(external, updated); + }); + if (requiredInterface() & READS_END) + ends() = std::make_shared([this](Interface::iterator external, Interface::UpdateFlags updated) { + this->propagateStateToAllChildren(external, updated); + }); } void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external, @@ -720,7 +758,7 @@ void ParallelContainerBasePrivate::validateConnectivity() const { } template -void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, Interface::UpdateFlags updated) { +void ParallelContainerBasePrivate::propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated) { for (const Stage::pointer& stage : children()) copyState(external, stage->pimpl()->pullInterface(), updated); } @@ -793,44 +831,240 @@ 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(); + pimpl()->reset(); } void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { ParallelContainerBase::init(robot_model); - active_child_ = pimpl()->children().front().get(); -} - -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(); - } - return false; -} - -void Fallbacks::compute() { - if (!active_child_) - return; - - active_child_->pimpl()->runCompute(); + pimpl()->reset(); } void Fallbacks::onNewSolution(const SolutionBase& s) { - liftSolution(s); + pimpl()->onNewSolution(s); } +inline void Fallbacks::replaceImpl() { + FallbacksPrivate *impl = pimpl(); + switch (pimpl()->requiredInterface()) { + case GENERATE: + impl = new FallbacksPrivateGenerator(std::move(*impl)); + break; + case PROPAGATE_FORWARDS: + case PROPAGATE_BACKWARDS: + impl = new FallbacksPrivatePropagator(std::move(*impl)); + break; + case CONNECT: + // For now, we only support Connecting children + for (const auto& child : impl->children()) + if (!dynamic_cast(child.get())) + throw std::runtime_error("CONNECT-like interface is only supported for Connecting children"); + impl = new FallbacksPrivateConnect(std::move(*impl)); + break; + } + delete pimpl_; + pimpl_ = impl; +} + +FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) + : ParallelContainerBasePrivate(me, name) {} + +FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other) +: ParallelContainerBasePrivate(static_cast(other.me()), "") { + // move contents of other + this->ParallelContainerBasePrivate::operator=(std::move(other)); +} + +void FallbacksPrivate::initializeExternalInterfaces() { + // Here we know the final interface of the container (and all its children) + // Thus replace, this pimpl() with a new interface-specific one: + static_cast(me())->replaceImpl(); +} + +void FallbacksPrivate::onNewSolution(const SolutionBase& s) { + // printChildrenInterfaces(*this, true, *s.creator()); + static_cast(me())->liftSolution(s); +} + +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 computePropagate) + // printChildrenInterfaces(*this, false, child); + (void)child; +} + +void FallbacksPrivateCommon::reset() { + current_ = children().begin(); +} + +bool FallbacksPrivateCommon::canCompute() const { + while(current_ != children().end() && // not completely exhausted + !(*current_)->pimpl()->canCompute()) // but current child cannot compute + return const_cast(this)->nextJob(); // advance to next job + + // return value: current child is well defined and thus can compute? + return current_ != children().end(); +} + +void FallbacksPrivateCommon::compute() { + (*current_)->pimpl()->runCompute(); +} + +inline void FallbacksPrivateCommon::nextChild() { + if (std::next(current_) != children().end()) + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*current_)->name() << "' failed, trying next one."); + ++current_; // advance to next child +} + +FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) + : FallbacksPrivateCommon(std::move(old)) { FallbacksPrivateCommon::reset(); } + +bool FallbacksPrivateGenerator::nextJob() { + assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); + + // don't advance to next child when we already produced solutions + if (!solutions_.empty()) { + current_ = children().end(); // indicate that we are exhausted + return false; + } + + do { nextChild(); } + while (current_ != children().end() && !(*current_)->pimpl()->canCompute()); + + // return value shall indicate current_->canCompute() + return current_ != children().end(); +} + + +FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old) + : FallbacksPrivateCommon(std::move(old)) { + switch (requiredInterface()) { + case PROPAGATE_FORWARDS: + dir_ = Interface::FORWARD; + starts() = std::make_shared(); + break; + case PROPAGATE_BACKWARDS: + dir_ = Interface::BACKWARD; + ends() = std::make_shared(); + break; + default: + assert(false); + } + FallbacksPrivatePropagator::reset(); +} + +void FallbacksPrivatePropagator::reset() { + FallbacksPrivateCommon::reset(); + job_ = pullInterface(dir_)->end(); // indicate fresh start + job_has_solutions_ = false; +} + +void FallbacksPrivatePropagator::onNewSolution(const SolutionBase& s) { + job_has_solutions_ = true; + FallbacksPrivateCommon::onNewSolution(s); +} + +bool FallbacksPrivatePropagator::nextJob() { + assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); + const auto jobs = pullInterface(dir_); + + if (job_ != jobs->end()) { // current job exists, but is exhausted on current child + if (!job_has_solutions_) // job didn't produce solutions -> feed to next child + nextChild(); + else + current_ = children().end(); // indicate that this job is exhausted on all children + } + job_has_solutions_ = false; + + if (current_ == children().end()) { // all children processed the job_ + if (job_ != jobs->end()) { + jobs->remove(job_); // we don't need the job in our interface list anymore + job_ = jobs->end(); // indicate that we need to fetch a new job + } + current_ = children().begin(); // start next job with first child again + } + + // pick next job if needed and possible + if (job_ == jobs->end()) { // need to pick next job + if (!jobs->empty() && jobs->front()->priority().enabled()) + job_ = jobs->begin(); + else + return false; // no more jobs available + } + + // When arriving here, we have a valid job_ and a current_ child to feed it. Let's do that. + copyState(dir_, job_, (*current_)->pimpl()->pullInterface(dir_), Interface::UpdateFlags()); + return true; +} + + +FallbacksPrivateConnect::FallbacksPrivateConnect(FallbacksPrivate&& old) + : FallbacksPrivate(std::move(old)) { + starts_ = std::make_shared( + std::bind(&FallbacksPrivateConnect::propagateStateUpdate, this, std::placeholders::_1, std::placeholders::_2)); + ends_ = std::make_shared( + std::bind(&FallbacksPrivateConnect::propagateStateUpdate, this, std::placeholders::_1, std::placeholders::_2)); + + FallbacksPrivateConnect::reset(); +} + +void FallbacksPrivateConnect::reset() { + active_ = children().end(); +} + +template +void FallbacksPrivateConnect::propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated) { + copyState(external, children().front()->pimpl()->pullInterface(dir), updated); + // As we use the Interface* from the first child for all children (we just populate their pending lists) + // there is no need to explicitly propagate state updates to other children. +} + +bool FallbacksPrivateConnect::canCompute() const { + for (auto it=children().begin(), end=children().end(); it!=end; ++it) + if ((*it)->pimpl()->canCompute()) { + active_ = it; + return true; + } + active_ = children().end(); + return false; +} + +void FallbacksPrivateConnect::compute() { + // Alternatively, we could also compute() all children that canCompute() + assert(active_ != children().end()); + (*active_)->pimpl()->runCompute(); +} + +void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { + // expect failure to be reported from active child + assert(active_ != children().end() && active_->get() == &child); + // ... thus we can use std::next(active_) to find the next child + auto next = std::next(active_); + + auto findIteratorFor = [](const InterfaceState* state, const Interface& interface) { + auto it = std::find(interface.begin(), interface.end(), state); + assert(it != interface.end()); + return it; + }; + + if (next != children().end()) { // pass job to next child + auto next_con = static_cast(const_cast((*next)->pimpl())); + auto first_con = static_cast(children().front()->pimpl()); + auto fromIt = findIteratorFor(from, *first_con->starts()); + auto toIt = findIteratorFor(to, *first_con->ends()); + next_con->pending.insert(std::make_pair(fromIt, toIt)); + } else // or report failure to parent + parent()->pimpl()->onNewFailure(*me(), from, to); +} + + 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 94419636..d141d903 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -106,6 +106,33 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name) , parent_{ nullptr } , introspection_{ nullptr } {} +StagePrivate& StagePrivate::operator=(StagePrivate&& other) { + assert(typeid(*this) == typeid(other)); + + assert(states_.empty() && other.states_.empty()); + assert((!starts_ || starts_->empty()) && (!other.starts_ || other.starts_->empty())); + assert((!ends_ || ends_->empty()) && (!other.ends_ || other.ends_->empty())); + assert(solutions_.empty() && other.solutions_.empty()); + assert(failures_.empty() && other.failures_.empty()); + + // me_ must not be changed! + name_ = std::move(other.name_); + properties_ = std::move(other.properties_); + cost_term_ = std::move(other.cost_term_); + solution_cbs_ = std::move(other.solution_cbs_); + + starts_ = std::move(other.starts_); + ends_ = std::move(other.ends_); + prev_ends_ = std::move(other.prev_ends_); + next_starts_ = std::move(other.next_starts_); + + parent_ = std::move(other.parent_); + it_ = std::move(other.it_); + other.unparent(); + + return *this; +} + InterfaceFlags StagePrivate::interfaceFlags() const { InterfaceFlags f; if (starts()) @@ -483,14 +510,14 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction case PropagatingEitherWay::FORWARD: required_interface_ = PROPAGATE_FORWARDS; if (!starts_) // keep existing interface if possible - starts_.reset(new Interface()); + starts_ = std::make_shared(); ends_.reset(); return; case PropagatingEitherWay::BACKWARD: required_interface_ = PROPAGATE_BACKWARDS; starts_.reset(); if (!ends_) // keep existing interface if possible - ends_.reset(new Interface()); + ends_ = std::make_shared(); return; case PropagatingEitherWay::AUTO: required_interface_ = UNKNOWN; @@ -688,10 +715,10 @@ void MonitoringGeneratorPrivate::solutionCB(const SolutionBase& s) { } ConnectingPrivate::ConnectingPrivate(Connecting* me, const std::string& name) : ComputeBasePrivate(me, name) { - starts_.reset(new Interface(std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, - std::placeholders::_2))); - ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, - std::placeholders::_2))); + starts_ = std::make_shared(std::bind(&ConnectingPrivate::newState, this, + std::placeholders::_1, std::placeholders::_2)); + ends_ = std::make_shared( + std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, std::placeholders::_2)); } InterfaceFlags ConnectingPrivate::requiredInterface() const { @@ -768,9 +795,23 @@ void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags // 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; +#if 0 + auto& os = std::cerr; + for (auto d : { Interface::FORWARD, Interface::BACKWARD }) { + bool fw = (d == Interface::FORWARD); + if (fw) + os << " " << std::setw(10) << std::left << this->name(); + else + os << std::setw(12) << std::right << ""; + if (dir != d) + os << (updated ? " !" : " +"); + else + os << " "; + os << (fw ? "↓ " : "↑ ") << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl; + } + os << std::setw(15) << " "; + printPendingPairs(os) << std::endl; +#endif } // Check whether there are pending feasible states (other than source) that could connect to target. @@ -815,19 +856,13 @@ void ConnectingPrivate::compute() { std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const { const char* reset = InterfaceState::STATUS_COLOR[3]; for (const auto& candidate : pending) { - // 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) { - ++first; - return &*candidate.first == s; - }); - std::find_if(ends()->begin(), ends()->end(), [&](const InterfaceState* s) { - ++second; - return &*candidate.second == s; - }); + size_t first = getIndex(*starts(), candidate.first); + size_t second = getIndex(*ends(), candidate.second); os << InterfaceState::STATUS_COLOR[candidate.first->priority().status()] << first << reset << ":" << InterfaceState::STATUS_COLOR[candidate.second->priority().status()] << second << reset << " "; } + if (pending.empty()) + os << "---"; return os; } diff --git a/core/src/task.cpp b/core/src/task.cpp index 9f1db279..884215f1 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -74,30 +74,14 @@ namespace task_constructor { TaskPrivate::TaskPrivate(Task* me, const std::string& ns) : WrapperBasePrivate(me, std::string()), ns_(rosNormalizeName(ns)), preempt_requested_(false) {} -void swap(StagePrivate*& lhs, StagePrivate*& rhs) { - // It only makes sense to swap pimpl instances of a Task! - // However, due to member protection rules, we can only implement it here - assert(typeid(lhs) == typeid(rhs)); - - // swap instances - ::std::swap(lhs, rhs); - // as well as their me_ pointers - ::std::swap(lhs->me_, rhs->me_); - - // and redirect the parent pointers of children to new parents - auto& lhs_children = static_cast(lhs)->children_; - for (auto it = lhs_children.begin(), end = lhs_children.end(); it != end; ++it) { - (*it)->pimpl()->unparent(); - (*it)->pimpl()->setParent(static_cast(lhs->me_)); - (*it)->pimpl()->setParentPosition(it); - } - - auto& rhs_children = static_cast(rhs)->children_; - for (auto it = rhs_children.begin(), end = rhs_children.end(); it != end; ++it) { - (*it)->pimpl()->unparent(); - (*it)->pimpl()->setParent(static_cast(rhs->me_)); - (*it)->pimpl()->setParentPosition(it); - } +TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) { + this->WrapperBasePrivate::operator=(std::move(other)); + ns_ = std::move(other.ns_); + introspection_ = std::move(other.introspection_); + robot_model_ = std::move(other.robot_model_); + robot_model_loader_ = std::move(other.robot_model_loader_); + task_cbs_ = std::move(other.task_cbs_); + return *this; } const ContainerBase* TaskPrivate::stages() const { @@ -122,7 +106,7 @@ Task::Task(Task&& other) // NOLINT(performance-noexcept-move-constructor) Task& Task::operator=(Task&& other) { // NOLINT(performance-noexcept-move-constructor) clear(); // remove all stages of current task - swap(this->pimpl_, other.pimpl_); + *static_cast(pimpl_) = std::move(*static_cast(other.pimpl_)); return *this; } diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 4eaa143b..73fae4ed 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -586,10 +586,6 @@ TEST(Task, move) { Task t2 = std::move(t1); EXPECT_EQ(t2.stages()->numChildren(), 2u); EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move) - - t1 = std::move(t2); - EXPECT_EQ(t1.stages()->numChildren(), 2u); - EXPECT_EQ(t2.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move) } TEST(Task, reuse) { diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index d37d1e13..87f9368f 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)); @@ -78,10 +78,11 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutio t.add(std::move(fallbacks)); EXPECT_TRUE(t.plan()); - EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 211, 222)); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 212, 221)); } -TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) { +// 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 }))); // available solutions (sorted) in individual runs of fallbacks: 1 | 11, 2 | 2, 11 @@ -100,7 +101,8 @@ TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) { EXPECT_COSTS(t.solutions(), testing::ElementsAre(2)); // expecting less costly solution as result } -TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) { +// 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 auto inner = std::make_unique("Inner"); @@ -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,29 +161,16 @@ 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"); - fallbacks->add(std::make_unique(PredefinedCosts::constant(0.0))); + fallbacks->add(std::make_unique(PredefinedCosts({ 0.0, 0.0, INF, 0.0 }))); fallbacks->add(std::make_unique(PredefinedCosts::constant(100.0))); t.add(std::move(fallbacks)); t.add(std::make_unique(PredefinedCosts({ 10.0, 20.0 }))); 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(); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 22, 121)); } diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 4634461e..32b287f1 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 00000000..0a586feb --- /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; +}