diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 0e3d1859..f884e026 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -137,31 +137,30 @@ public: size_t numSolutions() const override; void processSolutions(const SolutionProcessor &processor) const override; + size_t numFailures() const override; + void processFailures(const SolutionProcessor &processor) const override; protected: ParallelContainerBase(ParallelContainerBasePrivate* impl); + /// method called if any child found a new (internal) solution virtual void onNewSolution(const SolutionBase& s) override; -}; - -/** Wrap an existing solution - for use in containers. - * - * This essentially wraps a solution of a child and thus allows - * for new clones of start / end states, which in turn will - * have separate incoming/outgoing trajectories */ -class WrappedSolution : public SolutionBase { -public: - explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped) - : SolutionBase(creator, wrapped->cost()), wrapped_(wrapped) - {} - void fillMessage(moveit_task_constructor_msgs::Solution &solution, - Introspection* introspection = nullptr) const override { - wrapped_->fillMessage(solution, introspection); + /// lift unmodified child solution (useful for simple filtering) + void liftSolution(const SolutionBase* solution) { + liftSolution(solution, solution->cost()); } + /// lift child solution, but allow for modifying costs + void liftSolution(const SolutionBase* solution, double cost); -private: - const SolutionBase* wrapped_; + /// spawn a new solution with given state as start and end + void spawn(InterfaceState &&state, SubTrajectory&& trajectory); + /// convience method spawning an empty SubTrajectory with given cost + void spawn(InterfaceState &&state, double cost) { + SubTrajectory trajectory; + trajectory.setCost(cost); + spawn(std::move(state), std::move(trajectory)); + } }; @@ -200,10 +199,16 @@ public: class WrapperBasePrivate; -/** Base class for Wrapper and Task +/** A wrapper wraps a single child stage, which can be accessed via wrapped(). * - * WrapperBase ensures that only a single child is wrapped in a container. - * This child can be accessed via wrapped(). + * Implementations of this interface need to implement onNewSolution(), which is + * called when the child has generated a new solution. + * The wrapper may reject the solution or create one or multiple derived solutions, + * potentially adapting the cost, as well as its start and end states. + * + * Care needs to be taken to not modify pulled states, but only pushed ones! + * liftFor each new solution, liftSolution() should be called, which comes in various + * flavours to allow for handing in new states (or not). */ class WrapperBase : public ParallelContainerBase { @@ -214,48 +219,17 @@ public: /// insertion is only allowed if children() is empty bool insert(Stage::pointer&& stage, int before = -1) override; - /// access the single wrapped child + /// access the single wrapped child, NULL if still empty Stage* wrapped(); inline const Stage* wrapped() const { return const_cast(this)->wrapped(); } -protected: - WrapperBase(WrapperBasePrivate *impl, pointer &&child = Stage::pointer()); -}; - - -class WrapperPrivate; -/** A wrapper wraps a single generator-style stage (and acts itself as a generator). - * - * The wrapped stage must act as a generator, i.e. only spawn new states - * in its external interfaces. It's intended, e.g. to filter or clone - * a generated solution of its wrapped generator. - */ -class Wrapper : public WrapperBase -{ -public: - PRIVATE_CLASS(Wrapper) - Wrapper(const std::string &name, pointer &&child = Stage::pointer()); - - void reset() override; bool canCompute() const override; bool compute() override; - size_t numSolutions() const override; - size_t numFailures() const override; - void processSolutions(const SolutionProcessor &processor) const override; - void processFailures(const SolutionProcessor &processor) const override; - - void spawn(InterfaceState &&state, std::unique_ptr&& s); - void spawn(InterfaceState &&state, double cost) { - std::unique_ptr s(new SubTrajectory()); - s->setCost(cost); - spawn(std::move(state), std::move(s)); - } - protected: - Wrapper(WrapperPrivate* impl, pointer &&child = Stage::pointer()); + WrapperBase(WrapperBasePrivate* impl, pointer &&child = Stage::pointer()); /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase &s) override = 0; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index d4da7e2b..9fbd339a 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -39,7 +39,6 @@ #pragma once #include -#include "utils.h" #include "stage_p.h" #include @@ -174,6 +173,27 @@ private: PIMPL_FUNCTIONS(SerialContainer) +/** Wrap an existing solution - for use in parallel containers and wrappers. + * + * This essentially wraps a solution of a child and thus allows + * for new clones of start / end states, which in turn will + * have separate incoming/outgoing trajectories */ +class WrappedSolution : public SolutionBase { +public: + explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped, double cost) + : SolutionBase(creator, cost), wrapped_(wrapped) + {} + explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped) + : WrappedSolution(creator, wrapped, wrapped->cost()) + {} + void fillMessage(moveit_task_constructor_msgs::Solution &solution, + Introspection* introspection = nullptr) const override; + +private: + const SolutionBase* wrapped_; +}; + + class ParallelContainerBasePrivate : public ContainerBasePrivate { friend class ParallelContainerBase; @@ -184,8 +204,17 @@ private: /// callback for new externally received states void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); - // set of all solutions - ordered solutions_; + // buffer for wrapped solutions + std::list wrapped_solutions_; + // buffer for newly created (not wrapped) solutions + std::list created_solutions_; + // buffer of created states (for use in created solutions) + std::list states_; + + // cost-ordered set of solutions (pointers into wrapped or created) + ordered solutions_; + // buffer for failures (pointers into wrapped or created) + std::list failures_; }; PIMPL_FUNCTIONS(ParallelContainerBase) @@ -194,25 +223,8 @@ class WrapperBasePrivate : public ParallelContainerBasePrivate { friend class WrapperBase; public: -WrapperBasePrivate(WrapperBase* me, const std::string& name) - : ParallelContainerBasePrivate(me, name) -{} - + WrapperBasePrivate(WrapperBase* me, const std::string& name); }; PIMPL_FUNCTIONS(WrapperBase) - -class WrapperPrivate : public WrapperBasePrivate { - friend class Wrapper; - -public: - WrapperPrivate(Wrapper* me, const std::string& name); - -private: - ordered, pointerLessThan>> solutions_; - std::list> failures_; - std::list failure_states_; -}; -PIMPL_FUNCTIONS(Wrapper) - } } diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index 4a480a78..f2448c3e 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -15,7 +15,7 @@ namespace core { MOVEIT_CLASS_FORWARD(RobotState) } namespace moveit { namespace task_constructor { namespace stages { -class ComputeIK : public Wrapper { +class ComputeIK : public WrapperBase { public: ComputeIK(const std::string &name, pointer &&child = Stage::pointer()); diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 3ae3bf4a..43182dce 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -258,3 +258,13 @@ const InterfaceState::Solutions& SolutionBase::trajectories } } } + +namespace std { +// comparison for pointers to SolutionBase: compare based on value +template<> struct less { + bool operator()(const moveit::task_constructor::SolutionBase* x, + const moveit::task_constructor::SolutionBase* y) { + return *x < *y; + } +}; +} diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index 657f4e5c..667da900 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -94,12 +94,3 @@ private: }; #define DECLARE_FLAGS(Flags, Enum) typedef QFlags Flags; - - -// compare two pointers by comparing their referenced values -template -struct pointerLessThan : std::enable_if::value> { - inline bool operator()(const T& x, const T& y) const { - return *x < *y; - } -}; diff --git a/core/src/container.cpp b/core/src/container.cpp index 8213520a..19373791 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -130,9 +130,6 @@ void ContainerBasePrivate::liftSolution(SolutionBase& solution, Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL); internal_to_external_.insert(std::make_pair(internal_to, external)); } - - // perform default stage action on new solution - StagePrivate::newSolution(solution); } @@ -301,6 +298,7 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) { auto inserted = impl->solutions_.insert(std::move(*it)); impl->liftSolution(*inserted, inserted->internalStart(), inserted->internalEnd()); + impl->newSolution(*inserted); } } @@ -436,7 +434,7 @@ void SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso } void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg, - Introspection* introspection = nullptr) const + Introspection* introspection) const { moveit_task_constructor_msgs::SubSolution sub_msg; sub_msg.id = introspection ? introspection->solutionId(*this) : 0; @@ -458,6 +456,12 @@ void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg, } +void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution &solution, + Introspection *introspection) const +{ + wrapped_->fillMessage(solution, introspection); +} + ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name) : ContainerBasePrivate(me, name) { @@ -480,6 +484,13 @@ void ParallelContainerBase::reset() { // recursively reset children ContainerBase::reset(); + // clear buffers + auto impl = pimpl(); + impl->solutions_.clear(); + impl->failures_.clear(); + impl->wrapped_solutions_.clear(); + impl->created_solutions_.clear(); + impl->states_.clear(); } /* States received by the container need to be copied to all children's pull interfaces. @@ -532,18 +543,73 @@ size_t ParallelContainerBase::numSolutions() const void ParallelContainerBase::processSolutions(const Stage::SolutionProcessor &processor) const { - for(const SolutionBase& s : pimpl()->solutions_) - if (!processor(s)) + for(const SolutionBase* s : pimpl()->solutions_) + if (!processor(*s)) break; } -void ParallelContainerBase::onNewSolution(const SolutionBase &s) +size_t ParallelContainerBase::numFailures() const +{ + return pimpl()->failures_.size(); +} + +void ParallelContainerBase::processFailures(const Stage::SolutionProcessor &processor) const +{ + for(const SolutionBase* f : pimpl()->failures_) + if (!processor(*f)) + break; +} + +void ParallelContainerBase::onNewSolution(const SolutionBase& s) +{ + liftSolution(&s); +} + +void ParallelContainerBase::liftSolution(const SolutionBase* solution, double cost) { auto impl = pimpl(); - auto it = impl->solutions_.insert(WrappedSolution(impl, &s)); - impl->liftSolution(*it, s.start(), s.end()); + // create new WrappedSolution instance + auto wit = impl->wrapped_solutions_.insert(impl->wrapped_solutions_.end(), WrappedSolution(impl, solution, cost)); + + if (wit->isFailure()) { + wit->setStartState(*solution->start()); + wit->setEndState(*solution->end()); + impl->failures_.push_back(&*wit); + } else { + impl->solutions_.insert(&*wit); + impl->liftSolution(*wit, solution->start(), solution->end()); + } + impl->newSolution(*wit); } +void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t) +{ + auto impl = pimpl(); + assert(impl->prevEnds() && impl->nextStarts()); + + t.setCreator(impl); + // store newly created solution (otherwise it's lost) + auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t)); + + if (it->isFailure()) { + auto state_it = impl->states_.insert(impl->states_.end(), std::move(state)); + it->setStartState(*state_it); + it->setEndState(*state_it); + impl->failures_.push_back(&*it); + } else { + // directly spawn states in push interfaces + impl->prevEnds()->add(InterfaceState(state), NULL, &*it); + impl->nextStarts()->add(std::move(state), &*it, NULL); + impl->solutions_.insert(&*it); + } + impl->newSolution(*it); +} + + +WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name) + : ParallelContainerBasePrivate(me, name) +{} + WrapperBase::WrapperBase(const std::string &name, Stage::pointer &&child) : WrapperBase(new WrapperBasePrivate(this, name), std::move(child)) @@ -568,79 +634,18 @@ Stage* WrapperBase::wrapped() return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get(); } - -WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name) - : WrapperBasePrivate(me, name) -{} - - -Wrapper::Wrapper(WrapperPrivate *impl, Stage::pointer &&child) - : WrapperBase(impl, std::move(child)) -{} -Wrapper::Wrapper(const std::string &name, Stage::pointer &&child) - : Wrapper(new WrapperPrivate(this, name), std::move(child)) -{} - -void Wrapper::reset() -{ - WrapperBase::reset(); - pimpl()->solutions_.clear(); -} - -bool Wrapper::canCompute() const +bool WrapperBase::canCompute() const { return wrapped()->pimpl()->canCompute(); } -bool Wrapper::compute() +bool WrapperBase::compute() { size_t num_before = numSolutions(); wrapped()->pimpl()->compute(); return numSolutions() > num_before; } -size_t Wrapper::numSolutions() const -{ - return pimpl()->solutions_.size(); -} - -size_t Wrapper::numFailures() const -{ - return pimpl()->failures_.size(); -} - -void Wrapper::processSolutions(const Stage::SolutionProcessor &processor) const -{ - for(const auto& s : pimpl()->solutions_) - if (!processor(*s)) - break; -} - -void Wrapper::processFailures(const Stage::SolutionProcessor &processor) const -{ - for(const auto& s : pimpl()->failures_) - if (!processor(*s)) - break; -} - -void Wrapper::spawn(InterfaceState &&state, std::unique_ptr&& s) -{ - auto impl = pimpl(); - s->setCreator(impl); - SolutionBase* solution = s.get(); - if (s->isFailure()) { - impl->failure_states_.emplace_back(std::move(state)); - s->setStartState(impl->failure_states_.back()); - s->setEndState(impl->failure_states_.back()); - impl->failures_.emplace_back(std::move(s)); - } else { - impl->solutions_.insert(std::move(s)); - impl->prevEnds()->add(InterfaceState(state), NULL, solution); - impl->nextStarts()->add(std::move(state), solution, NULL); - } - impl->newSolution(*solution); -} - bool Alternatives::canCompute() const { diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 8ae8fbda..f5f2e0d4 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -20,7 +20,7 @@ namespace moveit { namespace task_constructor { namespace stages { ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) - : Wrapper(name, std::move(child)) + : WrapperBase(name, std::move(child)) { auto& p = properties(); p.declare("timeout", 1.0); @@ -218,17 +218,17 @@ void ComputeIK::onNewSolution(const SolutionBase &s) start_time = now; planning_scene::PlanningSceneConstPtr scene = s.start()->scene(); - std::unique_ptr solution(new SubTrajectory()); + SubTrajectory solution; // include markers from original solution - std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution->markers())); + std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution.markers())); // frame at target pose target_pose_msg.header.frame_id = scene->getPlanningFrame(); - rviz_marker_tools::appendFrame(solution->markers(), target_pose_msg, 0.1, "ik frame"); + rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame"); if (succeeded) { - solution->setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); + solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); // create a new scene for each solution as they will have different robot states planning_scene::PlanningScenePtr new_scene = s.start()->scene()->diff(); robot_state::RobotState& robot_state = new_scene->getCurrentStateNonConst(); @@ -240,11 +240,11 @@ void ComputeIK::onNewSolution(const SolutionBase &s) auto appender = [&solution](visualization_msgs::Marker& marker, const std::string& name) { marker.ns = "ik solution"; marker.color.a *= 0.5; - solution->markers().push_back(marker); + solution.markers().push_back(marker); }; generateVisualMarkers(robot_state, appender, jmg->getLinkModelNames()); } else { - solution->setCost(std::numeric_limits::infinity()); + solution.setCost(std::numeric_limits::infinity()); } spawn(InterfaceState(scene), std::move(solution));