From 6a62f6be768f4e93c0dfef7e416494dc30f446a4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 31 Jan 2018 10:57:05 +0100 Subject: [PATCH 1/8] default argument for NotifyFunction in Interface constructor --- core/include/moveit/task_constructor/storage.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index af9a4874..3ff2a715 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -113,7 +113,7 @@ class Interface : protected std::list { public: typedef std::list container_type; typedef std::function NotifyFunction; - Interface(const NotifyFunction ¬ify); + Interface(const NotifyFunction ¬ify = NotifyFunction()); // add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state // and finally run the notify callback From e6af069139ba9e993e23ed369af1ecebbdbed23f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 30 Jan 2018 13:41:57 +0100 Subject: [PATCH 2/8] SerialContainer: traverse solutions w/o stopping stage Always traverse from current solution to the start/end of a complete path and only call the callback once for the whole trace. --- .../moveit/task_constructor/container.h | 15 +++--- core/src/container.cpp | 46 +++++++++---------- 2 files changed, 28 insertions(+), 33 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 9a6d92c2..7191dd92 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -101,18 +101,15 @@ protected: /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase &s) override; - /// function type used for traversing solutions - /// For each sub solution (current), the trace from the start as well as the - /// accumulated cost of all solutions in the trace are provided. - /// Return true, if traversal should continue, otherwise false. - typedef std::function SolutionProcessor; - /// traverse all solutions, starting at start and call the callback for each subsolution - /// The return value is always false, indicating that the traversal eventually stopped. + /// Traverse all solution pathes starting at start and going in given direction dir + /// until the end, i.e. until there are no more subsolutions in the given direction + /// For each solution path, callback the given processor passing + /// the full trace (from start to end, but not including start) and its accumulated costs template - bool traverse(const SolutionBase &start, const SolutionProcessor &cb, + void traverse(const SolutionBase &start, const SolutionProcessor &cb, solution_container &trace, double trace_cost = 0); protected: diff --git a/core/src/container.cpp b/core/src/container.cpp index 2180e678..1254b5ea 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -203,18 +203,18 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s struct SolutionCollector { - SolutionCollector(const Stage::pointer& stage) : stopping_stage(stage->pimpl()) {} + SolutionCollector(size_t max_depth) : max_depth(max_depth) {} - bool operator()(const SolutionBase& current, const SerialContainer::solution_container& trace, double cost) { - if (current.creator() != stopping_stage) - return true; // not yet traversed to stopping_stage + void operator()(const SerialContainer::solution_container& trace, double cost) { + // traced path should not extend past container boundaries + assert(trace.size() <= max_depth); - solutions.emplace_back(std::make_pair(trace, cost)); - return false; // we are done + if (trace.size() == max_depth) // reached max depth + solutions.emplace_back(std::make_pair(trace, cost)); } std::list> solutions; - const StagePrivate* const stopping_stage; + const size_t max_depth; }; void SerialContainer::onNewSolution(const SolutionBase ¤t) @@ -223,22 +223,25 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) const StagePrivate *creator = current.creator(); auto& children = impl->children(); - // s.creator() should be one of our children - assert(std::find_if(children.begin(), children.end(), - [creator](const Stage::pointer& stage) { return stage->pimpl() == creator; } ) - != children.end()); + // find number of stages before and after creator stage + size_t num_before = 0, num_after = 0; + for (auto it = children.begin(), end = children.end(); it != end; ++it, ++num_before) + if ((*it)->pimpl() == creator) + break; + assert(num_before < children.size()); // creator should be one of our children + num_after = children.size()-1 - num_before; SerialContainer::solution_container trace; trace.reserve(children.size()); // find all incoming trajectories connected to current solution - SolutionCollector incoming(children.front()); + SolutionCollector incoming(num_before); traverse(current, std::ref(incoming), trace); if (incoming.solutions.empty()) return; // no connection to front() // find all outgoing trajectories connected to current solution - SolutionCollector outgoing(children.back()); + SolutionCollector outgoing(num_after); traverse(current, std::ref(outgoing), trace); if (outgoing.solutions.empty()) return; // no connection to back() @@ -419,26 +422,21 @@ void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &p } template -bool SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb, +void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb, solution_container &trace, double trace_cost) { - if (!cb(start, trace, trace_cost)) - // stopping criterium met: stop traversal along dir - return true; // but continue traversal of further trajectories - - bool result = false; // if no trajectory traversed, return false - for (SolutionBase* successor : trajectories(start)) { + const InterfaceState::Solutions& solutions = trajectories(start); + if (solutions.empty()) // if we reached the end, call the callback + cb(trace, trace_cost); + else for (SolutionBase* successor : solutions) { trace.push_back(successor); trace_cost += successor->cost(); - result = traverse(*successor, cb, trace, trace_cost); + traverse(*successor, cb, trace, trace_cost); trace_cost -= successor->cost(); trace.pop_back(); - - if (!result) break; } - return result; } void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg, From d0351698a6b7f8300d80e49662dd034bc05f8ef9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 27 Jan 2018 23:34:32 +0100 Subject: [PATCH 3/8] single map internal_to_external There is no need to distinguish between starts and ends when mapping states, because start/end states need to be disjoint sets. --- .../moveit/task_constructor/container_p.h | 8 +++--- core/src/container.cpp | 27 ++++++++++--------- 2 files changed, 17 insertions(+), 18 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 1d198afe..6c4aea7e 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -98,15 +98,13 @@ protected: {} /// copy external_state to a child's interface and remember the link in internal_to map - void copyState(InterfaceState &external_state, Stage &child, bool to_start); + void copyState(InterfaceState &external, Stage &child, bool to_start); protected: container_type children_; - // map first child's start states to the corresponding states in this' starts_ - std::map internal_to_my_starts_; - // map last child's end states to the corresponding states in this' ends_ - std::map internal_to_my_ends_; + // map start/end states of children (internal) to corresponding states in our external interfaces + std::map internal_to_external_; }; PIMPL_FUNCTIONS(ContainerBase) diff --git a/core/src/container.cpp b/core/src/container.cpp index 1254b5ea..3ec1f436 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -87,14 +87,14 @@ bool ContainerBasePrivate::compute() return static_cast(me_)->compute(); } -void ContainerBasePrivate::copyState(InterfaceState &external_state, +void ContainerBasePrivate::copyState(InterfaceState &external, Stage &child, bool to_start) { if (to_start) { - auto internal = child.pimpl()->starts()->clone(external_state); - internal_to_my_starts_.insert(std::make_pair(&*internal, &external_state)); + InterfaceState& internal = *child.pimpl()->starts()->clone(external); + internal_to_external_.insert(std::make_pair(&internal, &external)); } else { - auto internal = child.pimpl()->ends()->clone(external_state); - internal_to_my_ends_.insert(std::make_pair(&*internal, &external_state)); + InterfaceState& internal = *child.pimpl()->ends()->clone(external); + internal_to_external_.insert(std::make_pair(&internal, &external)); } } @@ -154,8 +154,7 @@ void ContainerBase::reset() child->reset(); // clear mapping - impl->internal_to_my_starts_.clear(); - impl->internal_to_my_ends_.clear(); + impl->internal_to_external_.clear(); Stage::reset(); } @@ -278,23 +277,25 @@ void SerialContainerPrivate::storeNewSolution(SerialSolution &&s) SerialSolution& solution = *solutions_.insert(std::move(s)); // add solution to existing or new start state - auto it = internal_to_my_starts_.find(internal_from); - if (it != internal_to_my_starts_.end()) { + auto it = internal_to_external_.find(internal_from); + if (it != internal_to_external_.end()) { // connect solution to existing start state solution.setStartState(*it->second); } else { // spawn a new state in previous stage - prevEnds()->add(InterfaceState(*internal_from), NULL, &solution); + InterfaceState& external = *prevEnds()->add(InterfaceState(*internal_from), NULL, &solution); + internal_to_external_.insert(std::make_pair(internal_from, &external)); } // add solution to existing or new end state - it = internal_to_my_ends_.find(internal_to); - if (it != internal_to_my_ends_.end()) { + it = internal_to_external_.find(internal_to); + if (it != internal_to_external_.end()) { // connect solution to existing start state solution.setEndState(*it->second); } else { // spawn a new state in next stage - nextStarts()->add(InterfaceState(*internal_to), &solution, NULL); + InterfaceState& 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 From facd6438c9b8351d44c7d8fd1493bed3fcb4b431 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 27 Jan 2018 10:26:03 +0100 Subject: [PATCH 4/8] Interface: order states by priority Priority is (depth of solutions, accumulated cost along trace). --- .../moveit/task_constructor/container.h | 4 +- .../moveit/task_constructor/container_p.h | 4 +- .../moveit/task_constructor/cost_queue.h | 30 +++- core/include/moveit/task_constructor/stage.h | 4 + .../include/moveit/task_constructor/stage_p.h | 23 +-- .../include/moveit/task_constructor/storage.h | 73 +++++----- core/src/container.cpp | 39 +++-- core/src/stage.cpp | 137 +++++++++--------- core/src/storage.cpp | 32 ++-- 9 files changed, 194 insertions(+), 152 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 7191dd92..c699f5a5 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -139,9 +139,9 @@ protected: ParallelContainerBase(ParallelContainerBasePrivate* impl); /// callback for new start states (received externally) - virtual void onNewStartState(const InterfaceState &external) = 0; + virtual void onNewStartState(Interface::iterator external, bool updated) = 0; /// callback for new end states (received externally) - virtual void onNewEndState(const InterfaceState &external) = 0; + virtual void onNewEndState(Interface::iterator external, bool updated) = 0; }; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 6c4aea7e..66fadb12 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -98,13 +98,13 @@ protected: {} /// copy external_state to a child's interface and remember the link in internal_to map - void copyState(InterfaceState &external, Stage &child, bool to_start); + void copyState(Interface::iterator external, Stage &child, bool to_start, bool updated); protected: container_type children_; // map start/end states of children (internal) to corresponding states in our external interfaces - std::map internal_to_external_; + std::map internal_to_external_; }; PIMPL_FUNCTIONS(ContainerBase) diff --git a/core/include/moveit/task_constructor/cost_queue.h b/core/include/moveit/task_constructor/cost_queue.h index c17c3e4b..98156294 100644 --- a/core/include/moveit/task_constructor/cost_queue.h +++ b/core/include/moveit/task_constructor/cost_queue.h @@ -50,6 +50,8 @@ public: reference top() { return c.front(); } const_reference top() const { return c.front(); } + reference pop() { reference result = top(); c.pop_front(); return result; } + reference front() { return c.front(); } const_reference front() const { return c.front(); } reference back() { return c.back(); } @@ -68,6 +70,9 @@ public: const_reverse_iterator crbegin() const { return c.rbegin(); } const_reverse_iterator crend() const { return c.rend(); } + /// explicitly sort container, useful if many items have changed their value + void sort() { c.sort(comp); } + iterator insert(const value_type& item) { iterator at = std::upper_bound(c.begin(), c.end(), item, comp); return c.insert(at, item); @@ -79,7 +84,7 @@ public: iterator erase(const_iterator pos) { return c.erase(pos); } - /// update sort position of iterator after changes + /// update sort position of a single item after changes iterator update(iterator &it) { container_type temp; temp.splice(temp.end(), c, it); // move it from c to temp @@ -87,12 +92,26 @@ public: c.splice(at, temp, it); return it; } + + /// move element pos from this to other container, inserting before other_pos + iterator moveTo(iterator pos, container_type& other, iterator other_pos) { + other.splice(other_pos, c, pos); + return pos; + } + /// move element pos from other container into this one (sorted) + iterator moveFrom(iterator pos, container_type& other) { + iterator at = std::upper_bound(begin(), end(), *pos, comp); + c.splice(at, other, pos); + return pos; + } }; namespace detail { template struct ItemCostPair : std::pair { + typedef CostType cost_type; + ItemCostPair(const std::pair& other) : std::pair(other) {} ItemCostPair(std::pair&& other) @@ -101,7 +120,7 @@ struct ItemCostPair : std::pair { inline ValueType& value() { return this->first; } inline const ValueType& value() const { return this->first; } - inline const CostType& cost() const { return this->second; } + inline CostType cost() const { return this->second; } // comparison only considers cost constexpr bool operator<(const ItemCostPair& other) const { @@ -109,7 +128,7 @@ struct ItemCostPair : std::pair { } }; -} +} // namespace detail template >> @@ -117,7 +136,10 @@ class cost_ordered : public ordered, C { typedef ordered, Compare> base_type; public: - auto insert(const ValueType& value, const CostType& cost) { + auto insert(const ValueType& value, const CostType cost) { return base_type::insert(std::make_pair(value, cost)); } + auto insert(ValueType&& value, const CostType cost) { + return base_type::insert(std::make_pair(std::move(value), cost)); + } }; diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index a2306f41..0f093ef4 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -203,7 +203,9 @@ public: enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD}; void restrictDirection(Direction dir); + virtual void reset() override; virtual void init(const planning_scene::PlanningSceneConstPtr &scene) override; + virtual bool computeForward(const InterfaceState& from) = 0; virtual bool computeBackward(const InterfaceState& to) = 0; @@ -285,6 +287,8 @@ public: PRIVATE_CLASS(Connecting) Connecting(const std::string& name); + virtual void reset() override; + virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0; void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 5781d86e..b30c2f8e 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -146,6 +146,7 @@ PIMPL_FUNCTIONS(ComputeBase) class PropagatingEitherWayPrivate : public ComputeBasePrivate { friend class PropagatingEitherWay; + std::list processed; // already processed states public: PropagatingEitherWay::Direction dir; @@ -166,14 +167,11 @@ public: const InterfaceState &fetchEndState(); protected: - // get informed when new start or end state becomes available - void newStartState(const std::list::iterator& it); - void newEndState(const std::list::iterator& it); // initialize properties from parent and/or state void initProperties(const InterfaceState &state); - - Interface::const_iterator next_start_state_; - Interface::const_iterator next_end_state_; + // drop states corresponding to failed (infinite-cost) trajectories + void dropFailedStarts(Interface::iterator state); + void dropFailedEnds(Interface::iterator state); }; PIMPL_FUNCTIONS(PropagatingEitherWay) @@ -209,6 +207,12 @@ PIMPL_FUNCTIONS(Generator) class ConnectingPrivate : public ComputeBasePrivate { friend class Connecting; + typedef std::pair StatePair; + struct StatePairLess { + bool operator()(const StatePair& x, const StatePair& y) const { + return x.first->priority() + x.second->priority() < y.first->priority() + y.second->priority(); + } + }; public: inline ConnectingPrivate(Connecting *me, const std::string &name); @@ -220,12 +224,13 @@ public: private: // get informed when new start or end state becomes available - void newStartState(const std::list::iterator& it); - void newEndState(const std::list::iterator& it); + void newStartState(Interface::iterator it, bool updated); + void newEndState(Interface::iterator it, bool updated); // initialize properties from parent and/or interface states void initProperties(const InterfaceState &start, const InterfaceState &end); - std::pair it_pairs_; + // ordered list of pending state pairs + ordered pending; }; PIMPL_FUNCTIONS(Connecting) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 3ff2a715..1ca83a00 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -74,8 +75,31 @@ MOVEIT_CLASS_FORWARD(Introspection) */ class InterfaceState { friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase + friend class Interface; // Interface is allowed to change priority public: - // TODO turn this into priority queue + /** 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. + */ + struct Priority : public std::pair { + Priority() : Priority(0, 0.0) {} + Priority(unsigned int depth, double cost) + : std::pair(depth, cost) {} + + inline unsigned int depth() const { return this->first; } + inline double cost() const { return this->second; } + + Priority operator+(const Priority& other) const { + return Priority(this->depth() + other.depth(), + this->cost() + other.cost()); + } + inline bool operator<(const Priority& other) const { + if (this->depth() == other.depth()) + return this->cost() < other.cost(); + else + return this->depth() > other.depth(); + } + }; typedef std::deque Solutions; /// create an InterfaceState from a planning scene @@ -91,6 +115,12 @@ public: PropertyMap& properties() { return properties_; } const PropertyMap& properties() const { return properties_; } + /// states are ordered by priority + inline bool operator<(const InterfaceState& other) const { + return this->priority_ < other.priority_; + } + inline const Priority& priority() const { return priority_; } + private: // these methods should be only called by SolutionBase::set[Start|End]State() inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); } @@ -101,50 +131,21 @@ private: PropertyMap properties_; Solutions incoming_trajectories_; Solutions outgoing_trajectories_; + Priority priority_; }; -/** Interface provides a list of InterfaceStates available as input for a stage. - * - * This is essentially an adaptor to a container class, to allow for notification - * of the interface's owner when new states become available - */ -class Interface : protected std::list { +/** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */ +class Interface : public ordered { public: - typedef std::list container_type; - typedef std::function NotifyFunction; + typedef std::function NotifyFunction; Interface(const NotifyFunction ¬ify = NotifyFunction()); // add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state - // and finally run the notify callback - container_type::iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing); + iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing); // clone an existing InterfaceState, but without its incoming/outgoing connections - container_type::iterator clone(const InterfaceState &state); - - using container_type::value_type; - using container_type::reference; - using container_type::const_reference; - - using container_type::iterator; - using container_type::const_iterator; - using container_type::reverse_iterator; - using container_type::const_reverse_iterator; - - using container_type::empty; - using container_type::size; - using container_type::clear; - using container_type::front; - using container_type::back; - - using container_type::begin; - using container_type::cbegin; - using container_type::end; - using container_type::cend; - using container_type::rbegin; - using container_type::crbegin; - using container_type::rend; - using container_type::crend; + iterator clone(const InterfaceState &state); private: const NotifyFunction notify_; diff --git a/core/src/container.cpp b/core/src/container.cpp index 3ec1f436..c4a4b333 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -43,6 +43,9 @@ #include #include #include +#include + +using namespace std::placeholders; namespace moveit { namespace task_constructor { @@ -87,14 +90,14 @@ bool ContainerBasePrivate::compute() return static_cast(me_)->compute(); } -void ContainerBasePrivate::copyState(InterfaceState &external, - Stage &child, bool to_start) { +void ContainerBasePrivate::copyState(Interface::iterator external, + Stage &child, bool to_start, bool updated) { if (to_start) { - InterfaceState& internal = *child.pimpl()->starts()->clone(external); - internal_to_external_.insert(std::make_pair(&internal, &external)); + InterfaceState& internal = *child.pimpl()->starts()->clone(*external); + internal_to_external_.insert(std::make_pair(&internal, external)); } else { - InterfaceState& internal = *child.pimpl()->ends()->clone(external); - internal_to_external_.insert(std::make_pair(&internal, &external)); + InterfaceState& internal = *child.pimpl()->ends()->clone(*external); + internal_to_external_.insert(std::make_pair(&internal, external)); } } @@ -283,8 +286,8 @@ void SerialContainerPrivate::storeNewSolution(SerialSolution &&s) solution.setStartState(*it->second); } else { // spawn a new state in previous stage - InterfaceState& external = *prevEnds()->add(InterfaceState(*internal_from), NULL, &solution); - internal_to_external_.insert(std::make_pair(internal_from, &external)); + Interface::iterator external = prevEnds()->add(InterfaceState(*internal_from), NULL, &solution); + internal_to_external_.insert(std::make_pair(internal_from, external)); } // add solution to existing or new end state @@ -294,8 +297,8 @@ void SerialContainerPrivate::storeNewSolution(SerialSolution &&s) solution.setEndState(*it->second); } else { // spawn a new state in next stage - InterfaceState& external = *nextStarts()->add(InterfaceState(*internal_to), &solution, NULL); - internal_to_external_.insert(std::make_pair(internal_to, &external)); + 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 @@ -363,16 +366,16 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) // initialize starts_ and ends_ interfaces Stage* child = start->get(); if (child->pimpl()->starts()) - impl->starts_.reset(new Interface([impl, child](const Interface::iterator& external){ + impl->starts_.reset(new Interface([impl, child](Interface::iterator external, bool updated){ // new external state in our starts_ interface is copied to first child - impl->copyState(*external, *child, true); + impl->copyState(external, *child, true, updated); })); child = last->get(); if (child->pimpl()->ends()) - impl->ends_.reset(new Interface([impl, child](const Interface::iterator& external){ + impl->ends_.reset(new Interface([impl, child](Interface::iterator external, bool updated){ // new external state in our ends_ interface is copied to last child - impl->copyState(*external, *child, false); + impl->copyState(external, *child, false, updated); })); // validate connectivity of this @@ -466,12 +469,8 @@ void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg, ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name) : ContainerBasePrivate(me, name) { - starts_.reset(new Interface([me](const Interface::iterator& external){ - me->onNewStartState(*external); - })); - ends_.reset(new Interface([me](const Interface::iterator& external){ - me->onNewEndState(*external); - })); + starts_.reset(new Interface(std::bind(&ParallelContainerBase::onNewStartState, me, _1, _2))); + ends_.reset(new Interface(std::bind(&ParallelContainerBase::onNewEndState, me, _1, _2))); } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 351f4cd3..ff81ba6e 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -42,6 +42,7 @@ #include #include +using namespace std::placeholders; namespace moveit { namespace task_constructor { void InitStageException::push_back(const Stage &stage, const std::string &msg) @@ -257,32 +258,35 @@ PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *m { } +void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) { + // move infinite-cost states to processed list + if (std::isinf(state->priority().cost())) + starts_->moveTo(state, processed, processed.end()); +} +void PropagatingEitherWayPrivate::dropFailedEnds(Interface::iterator state) { + // move infinite-cost states to processed list + if (std::isinf(state->priority().cost())) + ends_->moveTo(state, processed, processed.end()); +} + inline bool PropagatingEitherWayPrivate::hasStartState() const{ - return next_start_state_ != starts_->end(); + return starts_ && !starts_->empty(); } const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){ - if (!hasStartState()) - throw std::runtime_error("no new state for beginning available"); - - const InterfaceState& state= *next_start_state_; - ++next_start_state_; - - return state; + assert(hasStartState()); + // move state to processed list + return *starts_->moveTo(starts_->begin(), processed, processed.end()); } inline bool PropagatingEitherWayPrivate::hasEndState() const{ - return next_end_state_ != ends_->end(); + return ends_ && !ends_->empty(); } const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ - if(!hasEndState()) - throw std::runtime_error("no new state for ending available"); - - const InterfaceState& state= *next_end_state_; - ++next_end_state_; - - return state; + assert(hasEndState()); + // move state to processed list + return *ends_->moveTo(ends_->begin(), processed, processed.end()); } void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state) @@ -324,22 +328,6 @@ bool PropagatingEitherWayPrivate::compute() return countFailures(result); } -void PropagatingEitherWayPrivate::newStartState(const Interface::iterator &it) -{ - // we just appended a state to the list, but the iterator doesn't see it anymore - // so let's point it at the new one - if(next_start_state_ == starts_->end()) - --next_start_state_; -} - -void PropagatingEitherWayPrivate::newEndState(const Interface::iterator &it) -{ - // we just appended a state to the list, but the iterator doesn't see it anymore - // so let's point it at the new one - if(next_end_state_ == ends_->end()) - --next_end_state_; -} - PropagatingEitherWay::PropagatingEitherWay(const std::string &name) : PropagatingEitherWay(new PropagatingEitherWayPrivate(this, ANYWAY, name)) @@ -356,23 +344,17 @@ void PropagatingEitherWay::initInterface() { auto impl = pimpl(); if (impl->dir & PropagatingEitherWay::FORWARD) { - if (!impl->starts_) { // keep existing interface if possible - impl->starts_.reset(new Interface([impl](const Interface::iterator& it) { impl->newStartState(it); })); - impl->next_start_state_ = impl->starts_->begin(); - } + if (!impl->starts_) // keep existing interface if possible + impl->starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, impl, _1))); } else { impl->starts_.reset(); - impl->next_start_state_ = Interface::iterator(); } if (impl->dir & PropagatingEitherWay::BACKWARD) { - if (!impl->ends_) { // keep existing interface if possible - impl->ends_.reset(new Interface([impl](const Interface::iterator& it) { impl->newEndState(it); })); - impl->next_end_state_ = impl->ends_->end(); - } + if (!impl->ends_) // keep existing interface if possible + impl->ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, impl, _1))); } else { impl->ends_.reset(); - impl->next_end_state_ = Interface::iterator(); } } @@ -386,6 +368,12 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir initInterface(); } +void PropagatingEitherWay::reset() +{ + pimpl()->processed.clear(); + ComputeBase::reset(); +} + void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene) { ComputeBase::init(scene); @@ -393,14 +381,10 @@ void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &sce auto impl = pimpl(); // after being connected, restrict actual interface directions - if (!impl->nextStarts()) { + if (!impl->nextStarts()) impl->starts_.reset(); - impl->next_start_state_ = Interface::iterator(); - } - if (!impl->prevEnds()) { + if (!impl->prevEnds()) impl->ends_.reset(); - impl->next_end_state_ = Interface::iterator(); - } if (!impl->isConnected()) throw InitStageException(*this, "can neither send forwards nor backwards"); } @@ -433,7 +417,6 @@ PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, con { // indicate, that we don't accept new states from ends_ interface ends_.reset(); - next_end_state_ = Interface::iterator(); } @@ -452,7 +435,6 @@ PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me, { // indicate, that we don't accept new states from starts_ interface starts_.reset(); - next_start_state_ = Interface::iterator(); } @@ -510,23 +492,40 @@ void Generator::spawn(InterfaceState&& state, SubTrajectory&& t) ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name) : ComputeBasePrivate(me, name) { - starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); })); - ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); })); - it_pairs_ = std::make_pair(starts_->begin(), ends_->begin()); + starts_.reset(new Interface(std::bind(&ConnectingPrivate::newStartState, this, _1, _2))); + ends_.reset(new Interface(std::bind(&ConnectingPrivate::newEndState, this, _1, _2))); } -void ConnectingPrivate::newStartState(const Interface::iterator& it) +void ConnectingPrivate::newStartState(Interface::iterator it, bool updated) { - // TODO: need to handle the pairs iterator - if(it_pairs_.first == starts_->end()) - --it_pairs_.first; + if (!std::isfinite(it->priority().cost())) + return; + if (updated) { + // many pairs might be affected: sort + pending.sort(); + } else { // new state: insert all pairs with other interface + for (auto oit = ends_->begin(), oend = ends_->end(); oit != oend; ++oit) { + if (!std::isfinite(oit->priority().cost())) + break; + pending.insert(std::make_pair(it, oit)); + } + } } -void ConnectingPrivate::newEndState(const Interface::iterator& it) +void ConnectingPrivate::newEndState(Interface::iterator it, bool updated) { - // TODO: need to handle the pairs iterator properly - if(it_pairs_.second == ends_->end()) - --it_pairs_.second; + if (!std::isfinite(it->priority().cost())) + return; + if (updated) { + // many pairs might be affected: sort + pending.sort(); + } else { // new state: insert all pairs with other interface + for (auto oit = starts_->begin(), oend = starts_->end(); oit != oend; ++oit) { + if (!std::isfinite(oit->priority().cost())) + break; + pending.insert(std::make_pair(oit, it)); + } + } } void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end) @@ -538,15 +537,13 @@ void ConnectingPrivate::initProperties(const InterfaceState &start, const Interf } bool ConnectingPrivate::canCompute() const{ - // TODO: implement this properly - return it_pairs_.first != starts_->end() && - it_pairs_.second != ends_->end(); + return !pending.empty(); } bool ConnectingPrivate::compute() { - // TODO: implement this properly - const InterfaceState& from = *it_pairs_.first; - const InterfaceState& to = *(it_pairs_.second++); + const StatePair& top = pending.pop(); + const InterfaceState& from = *top.first; + const InterfaceState& to = *top.second; initProperties(from, to); return countFailures(static_cast(me_)->compute(from, to)); } @@ -557,6 +554,12 @@ Connecting::Connecting(const std::string &name) { } +void Connecting::reset() +{ + pimpl()->pending.clear(); + ComputeBase::reset(); +} + void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& t) { auto impl = pimpl(); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 629d7eec..eaa32393 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -59,29 +59,37 @@ Interface::Interface(const Interface::NotifyFunction ¬ify) : notify_(notify) {} +// Function used by sendForward()/sendBackward()/spawn() to create a new interface state Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing) { if (!state.incomingTrajectories().empty() || !state.outgoingTrajectories().empty()) throw std::runtime_error("expecting empty incoming/outgoing trajectories"); if (!state.scene()) throw std::runtime_error("expecting valid planning scene"); + // move state to a list node + std::list container; + auto it = container.insert(container.end(), std::move(state)); + // configure state: inherit priority from other end's state and add current solution's cost assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set - emplace_back(state); - iterator back = --end(); - // adjust subtrajectories ... - if (incoming) incoming->setEndState(*back); - if (outgoing) outgoing->setStartState(*back); - // ... before calling notify callback - if (notify_) notify_(back); - return back; + if (incoming) { + it->priority_ = InterfaceState::Priority(1, incoming->cost()); + incoming->setEndState(*it); + } else if (outgoing) { + it->priority_ = InterfaceState::Priority(1, outgoing->cost()); + outgoing->setStartState(*it); + } + // move list node into interface's state list (sorted by priority) + moveFrom(it, container); + // and finally call notify callback + if (notify_) notify_(it, false); + return it; } Interface::iterator Interface::clone(const InterfaceState &state) { - emplace_back(InterfaceState(state)); - iterator back = --end(); - if (notify_) notify_(back); - return back; + iterator it = insert(InterfaceState(state)); + if (notify_) notify_(it, false); + return it; } From 5c43d700a8dc588ee67eeff149e9e09521b3a21b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 30 Jan 2018 14:58:49 +0100 Subject: [PATCH 5/8] update state priorities in a container-specific fashion Reorder interface list on priority updates. This requires the InterfaceState to store a pointer to the owning Interface. --- .../moveit/task_constructor/container.h | 4 ++ .../include/moveit/task_constructor/storage.h | 17 +++-- core/src/container.cpp | 66 ++++++++++++++----- core/src/storage.cpp | 13 ++++ core/src/task.cpp | 2 +- 5 files changed, 78 insertions(+), 24 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index c699f5a5..22178bc3 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -138,6 +138,8 @@ public: protected: ParallelContainerBase(ParallelContainerBasePrivate* impl); + virtual void onNewSolution(const SolutionBase& s) override; + /// callback for new start states (received externally) virtual void onNewStartState(Interface::iterator external, bool updated) = 0; /// callback for new end states (received externally) @@ -192,6 +194,8 @@ public: } protected: + virtual void onNewSolution(const SolutionBase& s) = 0; + WrapperBase(WrapperBasePrivate *impl, pointer &&child = Stage::pointer()); }; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 1ca83a00..9a3a47a3 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -75,7 +75,7 @@ MOVEIT_CLASS_FORWARD(Introspection) */ class InterfaceState { friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase - friend class Interface; // Interface is allowed to change priority + friend class Interface; // allow Interface to set owner_ and priority_ public: /** InterfaceStates are ordered according to two values: * Depth of interlinked trajectory parts and accumulated trajectory costs along that path. @@ -120,6 +120,7 @@ public: return this->priority_ < other.priority_; } inline const Priority& priority() const { return priority_; } + Interface* owner() const { return owner_; } private: // these methods should be only called by SolutionBase::set[Start|End]State() @@ -131,7 +132,10 @@ private: PropertyMap properties_; Solutions incoming_trajectories_; Solutions outgoing_trajectories_; + + // members needed for priority scheduling in Interface list Priority priority_; + Interface* owner_ = nullptr; // allow update of priority }; @@ -141,12 +145,15 @@ public: typedef std::function NotifyFunction; Interface(const NotifyFunction ¬ify = NotifyFunction()); - // add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state + /// add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing); - // clone an existing InterfaceState, but without its incoming/outgoing connections + /// clone an existing InterfaceState, but without its incoming/outgoing connections iterator clone(const InterfaceState &state); + /// update state's priority if new priority is smaller and call notify_ + void updatePriority(InterfaceState &state, const InterfaceState::Priority &priority); + private: const NotifyFunction notify_; }; @@ -164,13 +171,13 @@ public: inline const InterfaceState* end() const { return end_; } inline void setStartState(const InterfaceState& state){ - assert(start_ == NULL); + assert(start_ == NULL); // only allow setting once (by Stage) start_ = &state; const_cast(state).addOutgoing(this); } inline void setEndState(const InterfaceState& state){ - assert(end_ == NULL); + assert(end_ == NULL); // only allow setting once (by Stage) end_ = &state; const_cast(state).addIncoming(this); } diff --git a/core/src/container.cpp b/core/src/container.cpp index c4a4b333..b8346d66 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -235,38 +235,44 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) SerialContainer::solution_container trace; trace.reserve(children.size()); - // find all incoming trajectories connected to current solution + // find all incoming solution pathes ending at current solution SolutionCollector incoming(num_before); traverse(current, std::ref(incoming), trace); - if (incoming.solutions.empty()) - return; // no connection to front() - - // find all outgoing trajectories connected to current solution + // find all outgoing solution pathes starting at current solution SolutionCollector outgoing(num_after); traverse(current, std::ref(outgoing), trace); - if (outgoing.solutions.empty()) - return; // no connection to back() - // collect (and sort) all solutions for all combinations of incoming + current + outgoing + // collect (and sort) all solutions spanning from start to end of this container ordered sorted; SerialContainer::solution_container solution; solution.reserve(children.size()); for (auto& in : incoming.solutions) { for (auto& out : outgoing.solutions) { - assert(solution.empty()); - // insert incoming solutions in reverse order - solution.insert(solution.end(), in.first.rbegin(), in.first.rend()); - // insert current solution - solution.push_back(¤t); - // insert outgoing solutions in normal order - solution.insert(solution.end(), out.first.begin(), out.first.end()); - - sorted.insert(SerialSolution(impl, std::move(solution), in.second + current.cost() + out.second)); + InterfaceState::Priority prio(in.first.size() + 1 + out.first.size(), + in.second + current.cost() + out.second); + // found a complete solution path connecting start to end? + if (prio.depth() == children.size()) { + assert(solution.empty()); + // insert incoming solutions in reverse order + solution.insert(solution.end(), in.first.rbegin(), in.first.rend()); + // insert current solution + solution.push_back(¤t); + // insert outgoing solutions in normal order + solution.insert(solution.end(), out.first.begin(), out.first.end()); + // store solution in sorted list + sorted.insert(SerialSolution(impl, std::move(solution), prio.cost())); + } else { + // update state costs + const InterfaceState* start = (in.first.empty() ? current : *in.first.back()).start(); + start->owner()->updatePriority(*const_cast(start), prio); + const InterfaceState* end = (out.first.empty() ? current : *out.first.back()).end(); + end->owner()->updatePriority(*const_cast(end), prio); + } } } - // store new solutions + // store new solutions (in sorted) for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) impl->storeNewSolution(std::move(*it)); } @@ -507,6 +513,18 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc throw errors; } +void ParallelContainerBase::onNewSolution(const SolutionBase &s) +{ + // update state priorities + InterfaceState::Priority prio(1, s.cost()); + InterfaceState* start = const_cast(s.start()); + start->owner()->updatePriority(*start, prio); + InterfaceState* end = const_cast(s.end()); + end->owner()->updatePriority(*end, prio); + + pimpl()->newSolution(s); +} + WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name) : ContainerBasePrivate(me, name) @@ -569,6 +587,18 @@ Stage* WrapperBase::wrapped() return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get(); } +void WrapperBase::onNewSolution(const SolutionBase &s) +{ + // update state priorities + InterfaceState::Priority prio(1, s.cost()); + InterfaceState* start = const_cast(s.start()); + start->owner()->updatePriority(*start, prio); + InterfaceState* end = const_cast(s.end()); + end->owner()->updatePriority(*end, prio); + + pimpl()->newSolution(s); +} + WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name) : WrapperBasePrivate(me, name) diff --git a/core/src/storage.cpp b/core/src/storage.cpp index eaa32393..4c0b0775 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -69,6 +69,9 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin // move state to a list node std::list container; auto it = container.insert(container.end(), std::move(state)); + assert(it->owner_ == nullptr); + it->owner_ = this; + // configure state: inherit priority from other end's state and add current solution's cost assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set if (incoming) { @@ -88,10 +91,20 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin Interface::iterator Interface::clone(const InterfaceState &state) { iterator it = insert(InterfaceState(state)); + it->owner_ = this; if (notify_) notify_(it, false); return it; } +void Interface::updatePriority(InterfaceState &state, const InterfaceState::Priority& priority) +{ + if (priority < state.priority()) { + auto it = std::find_if(begin(), end(), [&state](const InterfaceState& other) { return &state == &other; }); + assert(it != end()); // state should be part of this interface + state.priority_ = priority; + if (notify_) notify_(it, true); + } +} void SolutionBase::setCreator(StagePrivate *creator) { diff --git a/core/src/task.cpp b/core/src/task.cpp index 1ebb0dad..2d19755e 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -240,7 +240,7 @@ void Task::publishAllSolutions(bool wait) void Task::onNewSolution(const SolutionBase &s) { - pimpl()->newSolution(s); + WrapperBase::onNewSolution(s); if (introspection_) introspection_->publishSolution(s); } From c3eead0115af99347163ea6b522cb8b8191dfcd2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 8 Feb 2018 16:57:34 +0100 Subject: [PATCH 6/8] allow only a single inititialization source --- .../moveit/task_constructor/properties.h | 19 +++++---- .../include/moveit/task_constructor/stage_p.h | 8 ---- core/src/container.cpp | 5 --- core/src/properties.cpp | 15 +++---- core/src/stage.cpp | 39 +++++-------------- core/test/test_properties.cpp | 6 +++ 6 files changed, 34 insertions(+), 58 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 8555fd32..cbc4e79e 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -66,12 +66,12 @@ boost::any fromName(const PropertyMap& other, const std::string& other_name); class Property { friend class PropertyMap; + Property(const std::type_index &type_index, const std::string &description, const boost::any &default_value); + public: typedef int SourceId; + /// function callback used to initialize property value from another PropertyMap typedef std::function InitializerFunction; - typedef std::map InitializerMap; - - Property(const std::type_index& type_index, const std::string& description, const boost::any& default_value); /// set current value and default value void setValue(const boost::any& value); @@ -97,7 +97,7 @@ public: /// configure initialization from source using given other property name Property &configureInitFrom(SourceId source, const std::string& name); - /// set current value using configured initializers + /// set current value using matching configured initializers void performInitFrom(SourceId source, const PropertyMap& other); private: @@ -105,7 +105,10 @@ private: std::type_index type_index_; boost::any default_; boost::any value_; - InitializerMap initializers_; + + /// used for external initialization + SourceId source_id_ = 0; + InitializerFunction initializer_; }; @@ -120,13 +123,13 @@ class PropertyMap /// implementation of declare methods Property& declare(const std::string& name, const std::type_info& type, - const std::string& description = "", - const boost::any& default_value = boost::any()); + const std::string& description, + const boost::any& default_value); public: /// declare a property for future use template Property& declare(const std::string& name, const std::string& description = "") { - return declare(name, typeid(T), description); + return declare(name, typeid(T), description, boost::any()); } /// declare a property with default value template diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index c6a5c428..50dd5e07 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -158,8 +158,6 @@ protected: // get informed when new start or end state becomes available void newStartState(const std::list::iterator& it); void newEndState(const std::list::iterator& it); - // initialize properties from parent and/or state - void initProperties(const InterfaceState &state); Interface::const_iterator next_start_state_; Interface::const_iterator next_end_state_; @@ -187,10 +185,6 @@ public: bool canCompute() const override; bool compute() override; - -private: - // initialize properties from parent - void initProperties(); }; PIMPL_FUNCTIONS(Generator) @@ -211,8 +205,6 @@ private: // get informed when new start or end state becomes available void newStartState(const std::list::iterator& it); void newEndState(const std::list::iterator& it); - // initialize properties from parent and/or interface states - void initProperties(const InterfaceState &start, const InterfaceState &end); std::pair it_pairs_; }; diff --git a/core/src/container.cpp b/core/src/container.cpp index 7d329616..f15bce86 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -168,11 +168,6 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) Stage::init(scene); - // containers don't need to reset and init their properties on each execution - impl->properties_.reset(); - if (impl->parent()) - impl->properties_.performInitFrom(PARENT, impl->parent()->properties()); - // we need to have some children to do the actual work if (children.empty()) { errors.push_back(*this, "no children"); diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 7e714d58..524a9c61 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -84,22 +84,23 @@ void Property::reset() Property& Property::configureInitFrom(SourceId source, const Property::InitializerFunction &f) { - initializers_[source] = f; + if (source != source_id_ && initializer_) + throw std::runtime_error("Property was already configured for initialization from another source id"); + + source_id_ = source; + initializer_ = f; return *this; } Property &Property::configureInitFrom(SourceId source, const std::string &name) { - initializers_[source] = [name](const PropertyMap& other) { return fromName(other, name); }; - return *this; + return configureInitFrom(source, [name](const PropertyMap& other) { return fromName(other, name); }); } void Property::performInitFrom(SourceId source, const PropertyMap &other) { - auto it = initializers_.find(source); - if (it == initializers_.end()) return; - - setCurrentValue(it->second(other)); + if (source_id_ != source || !initializer_) return; // source ids not matching + setCurrentValue(initializer_(other)); } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index ad1adbf2..b619d1df 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -140,6 +140,11 @@ void Stage::reset() void Stage::init(const planning_scene::PlanningSceneConstPtr &scene) { + // init properties once from parent + auto impl = pimpl(); + impl->properties_.reset(); + if (impl->parent()) + impl->properties_.performInitFrom(PARENT, impl->parent()->properties()); } const ContainerBase *Stage::parent() const { @@ -278,16 +283,6 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ return state; } -void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state) -{ - // reset properties to their defaults - properties_.reset(); - // first init from INTERFACE - properties_.performInitFrom(Stage::INTERFACE, state.properties()); - // then init from PARENT - properties_.performInitFrom(Stage::PARENT, parent()->properties()); -} - bool PropagatingEitherWayPrivate::canCompute() const { if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) @@ -304,13 +299,15 @@ bool PropagatingEitherWayPrivate::compute() bool result = false; if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) { const InterfaceState& state = fetchStartState(); - initProperties(state); + // enforce property initialization from INTERFACE + properties_.performInitFrom(Stage::INTERFACE, state.properties(), true); if (me->computeForward(state)) result |= true; } if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) { const InterfaceState& state = fetchEndState(); - initProperties(state); + // enforce property initialization from INTERFACE + properties_.performInitFrom(Stage::INTERFACE, state.properties(), true); if (me->computeBackward(state)) result |= true; } @@ -470,18 +467,9 @@ bool GeneratorPrivate::canCompute() const { } bool GeneratorPrivate::compute() { - initProperties(); return static_cast(me_)->compute(); } -void GeneratorPrivate::initProperties() -{ - // reset properties to their defaults - properties_.reset(); - // then init from PARENT - properties_.performInitFrom(Stage::PARENT, parent()->properties()); -} - Generator::Generator(const std::string &name) : ComputeBase(new GeneratorPrivate(this, name)) @@ -525,14 +513,6 @@ void ConnectingPrivate::newEndState(const Interface::iterator& it) --it_pairs_.second; } -void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end) -{ - // reset properties to their defaults - properties_.reset(); - // then init from PARENT - properties_.performInitFrom(Stage::PARENT, parent()->properties()); -} - bool ConnectingPrivate::canCompute() const{ // TODO: implement this properly return it_pairs_.first != starts_->end() && @@ -543,7 +523,6 @@ bool ConnectingPrivate::compute() { // TODO: implement this properly const InterfaceState& from = *it_pairs_.first; const InterfaceState& to = *(it_pairs_.second++); - initProperties(from, to); return static_cast(me_)->compute(from, to); } diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index 9f885a2e..0e1f4b68 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -109,6 +109,12 @@ TEST_F(InitFromTest, sourceId) { EXPECT_THROW(slave.property("double4"), std::runtime_error); } +TEST_F(InitFromTest, multipleSourceIds) { + slave.configureInitFrom(0); + slave.configureInitFrom(0); // init is allowed second time with same id + EXPECT_THROW(slave.configureInitFrom(1), std::runtime_error); // but not with other id +} + TEST_F(InitFromTest, otherName) { slave.property("double1").configureInitFrom(0, "double2"); // init double1 from double2 slave.performInitFrom(0, master); From 5ff78f653ebd9d6a7352fb9ac562a9254c62c613 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 8 Feb 2018 17:10:27 +0100 Subject: [PATCH 7/8] signal callback function ... to allow for syncing with rviz::Property --- core/include/moveit/task_constructor/properties.h | 7 +++++++ core/src/properties.cpp | 2 ++ 2 files changed, 9 insertions(+) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index cbc4e79e..7ceeda2a 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -72,6 +72,8 @@ public: typedef int SourceId; /// function callback used to initialize property value from another PropertyMap typedef std::function InitializerFunction; + /// function callback used to signal value setting to external components + typedef std::function SignalFunction; /// set current value and default value void setValue(const boost::any& value); @@ -100,6 +102,10 @@ public: /// set current value using matching configured initializers void performInitFrom(SourceId source, const PropertyMap& other); + /// define a function callback to be called on each value update + /// note, that boost::any doesn't allow for change detection + void setSignalCallback(const SignalFunction& f) { signaller_ = f; } + private: std::string description_; std::type_index type_index_; @@ -109,6 +115,7 @@ private: /// used for external initialization SourceId source_id_ = 0; InitializerFunction initializer_; + SignalFunction signaller_; }; diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 524a9c61..0e9b324e 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -73,6 +73,8 @@ void Property::setCurrentValue(const boost::any &value) if (!value.empty()) typeCheck(value, type_index_); value_ = value; + if (signaller_) + signaller_(this); } void Property::reset() From fe2c4f312e3820f39f056ec4900c3c3bc4a6ee52 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 8 Feb 2018 17:23:57 +0100 Subject: [PATCH 8/8] serialize stage properties --- .../moveit/task_constructor/properties.h | 44 ++++++++++++++++--- core/src/introspection.cpp | 11 ++++- core/src/properties.cpp | 25 ++++++----- core/test/test_properties.cpp | 20 +++++++++ msgs/CMakeLists.txt | 1 + msgs/msg/Property.msg | 3 ++ msgs/msg/StageDescription.msg | 3 +- 7 files changed, 89 insertions(+), 18 deletions(-) create mode 100644 msgs/msg/Property.msg diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 7ceeda2a..854f3b7b 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -43,6 +43,7 @@ #include #include #include +#include namespace moveit { namespace task_constructor { @@ -66,7 +67,18 @@ boost::any fromName(const PropertyMap& other, const std::string& other_name); class Property { friend class PropertyMap; - Property(const std::type_index &type_index, const std::string &description, const boost::any &default_value); + typedef std::function SerializeFunction; + + Property(const std::type_index &type_index, const std::string &description, const boost::any &default_value, + const Property::SerializeFunction &serialize); + + template + static std::string serialize(const boost::any& value) { + if (value.empty()) return ""; + std::ostringstream oss; + oss << boost::any_cast(value); + return oss.str(); + } public: typedef int SourceId; @@ -88,6 +100,8 @@ public: inline const boost::any& value() const { return value_; } /// get default value const boost::any& defaultValue() const { return default_; } + /// serialize current value + std::string serialize() const; /// get description text const std::string& description() const { return description_; } @@ -116,6 +130,7 @@ private: SourceId source_id_ = 0; InitializerFunction initializer_; SignalFunction signaller_; + SerializeFunction serialize_; }; @@ -127,22 +142,25 @@ private: class PropertyMap { std::map props_; + typedef std::map::iterator iterator; + typedef std::map::const_iterator const_iterator; /// implementation of declare methods Property& declare(const std::string& name, const std::type_info& type, const std::string& description, - const boost::any& default_value); + const boost::any& default_value, + const Property::SerializeFunction &serialize); public: /// declare a property for future use template Property& declare(const std::string& name, const std::string& description = "") { - return declare(name, typeid(T), description, boost::any()); + return declare(name, typeid(T), description, boost::any(), &Property::serialize); } /// declare a property with default value template Property& declare(const std::string& name, const T& default_value, const std::string& description = "") { - return declare(name, typeid(T), description, default_value); + return declare(name, typeid(T), description, default_value, &Property::serialize); } /// get the property with given name @@ -151,11 +169,23 @@ public: return const_cast(this)->property(name); } + iterator begin() { return props_.begin(); } + iterator end() { return props_.end(); } + const_iterator begin() const { return props_.begin(); } + const_iterator end() const { return props_.end(); } + /// allow initialization from given source for listed properties - always using the same name void configureInitFrom(Property::SourceId source, const std::set &properties = {}); /// set (and, if neccessary, declare) the value of a property - void set(const std::string& name, const boost::any& value); + template + void set(const std::string& name, const T& value) { + auto it = props_.find(name); + if (it == props_.end()) // name is not yet declared + declare(name, value, ""); + else + it->second.setValue(value); + } /// temporarily set the value of a property void setCurrent(const std::string& name, const boost::any& value); @@ -187,5 +217,9 @@ public: void performInitFrom(Property::SourceId source, const PropertyMap& other, bool enforce = false); }; +// boost::any needs a specialization to avoid recursion +template <> +void PropertyMap::set(const std::string& name, const boost::any& value); + } // namespace task_constructor } // namespace moveit diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 8ec6c44b..10bbb23c 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -212,7 +213,15 @@ moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescriptio desc.id = stageId(&stage); desc.name = stage.name(); desc.flags = stage.pimpl()->interfaceFlags(); - // TODO fill stage properties + + // fill stage properties + for (const auto& pair : stage.properties()) { + moveit_task_constructor_msgs::Property p; + p.name = pair.first; + p.description = pair.second.description(); + p.value = pair.second.serialize(); + desc.properties.push_back(p); + } auto it = impl->stage_to_id_map_.find(stage.pimpl()->parent()); assert (it != impl->stage_to_id_map_.cend()); diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 0e9b324e..cc7c7e10 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -43,11 +43,13 @@ namespace moveit { namespace task_constructor { -Property::Property(const std::type_index& type_index, const std::string& description, const boost::any& default_value) +Property::Property(const std::type_index& type_index, const std::string& description, const boost::any& default_value, + const Property::SerializeFunction &serialize) : description_(description) , type_index_(type_index) , default_(default_value) , value_(default_value) + , serialize_(serialize) { // default value's type should match declared type by construction assert(default_.empty() || std::type_index(default_.type()) == type_index_); @@ -82,7 +84,10 @@ void Property::reset() value_ = default_; } - +std::string Property::serialize() const { + if (!serialize_) return ""; + return serialize_(value()); +} Property& Property::configureInitFrom(SourceId source, const Property::InitializerFunction &f) { @@ -107,9 +112,10 @@ void Property::performInitFrom(SourceId source, const PropertyMap &other) Property& PropertyMap::declare(const std::string &name, const std::type_info &type, - const std::string &description, const boost::any &default_value) + const std::string &description, const boost::any &default_value, + const Property::SerializeFunction &serialize) { - auto it_inserted = props_.insert(std::make_pair(name, Property(std::type_index(type), description, default_value))); + auto it_inserted = props_.insert(std::make_pair(name, Property(std::type_index(type), description, default_value, serialize))); if (!it_inserted.second && std::type_index(type) != it_inserted.first->second.type_index_) throw std::logic_error("Property '" + name + "' was already declared with different type."); return it_inserted.first->second; @@ -131,18 +137,17 @@ void PropertyMap::configureInitFrom(Property::SourceId source, const std::set +void PropertyMap::set(const std::string& name, const boost::any& value) { auto range = props_.equal_range(name); if (range.first == range.second) { // name is not yet declared if (value.empty()) throw std::logic_error("trying to set undeclared property '" + name + "' with NULL value"); - auto it = props_.insert(range.first, std::make_pair(name, Property(value.type(), "", boost::any()))); + auto it = props_.insert(range.first, std::make_pair(name, Property(value.type(), "", boost::any(), + Property::SerializeFunction()))); it->second.setValue(value); - } else { - assert(range.first->first == name); + } else range.first->second.setValue(value); - } } void PropertyMap::setCurrent(const std::string &name, const boost::any &value) diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index 0e1f4b68..5f582671 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -23,6 +23,18 @@ TEST(Property, standard) { EXPECT_EQ(props.get("double3"), 3.0); } +TEST(Property, directset) { + PropertyMap props; + props.set("int1", 1); + EXPECT_EQ(props.get("int1"), 1); + EXPECT_STREQ(props.property("int1").serialize().c_str(), "1"); + + props.set("int2", boost::any(2)); + EXPECT_EQ(props.get("int2"), 2); + // cannot serialize, because directly set + EXPECT_STREQ(props.property("int2").serialize().c_str(), ""); +} + TEST(Property, redeclare) { PropertyMap props; props.declare("double1"); @@ -65,6 +77,14 @@ TEST(Property, reset) { EXPECT_EQ(props.get("double1"), 1.0); } +TEST(Property, serialize) { + PropertyMap props; + props.declare("int"); + EXPECT_STREQ(props.property("int").serialize().c_str(), ""); + props.set("int", 42); + EXPECT_STREQ(props.property("int").serialize().c_str(), "42"); +} + class InitFromTest : public ::testing::Test { protected: void SetUp() { diff --git a/msgs/CMakeLists.txt b/msgs/CMakeLists.txt index ea11f4fb..ff33a924 100644 --- a/msgs/CMakeLists.txt +++ b/msgs/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS # ROS messages, services and actions add_message_files(DIRECTORY msg FILES + Property.msg Solution.msg StageDescription.msg StageStatistics.msg diff --git a/msgs/msg/Property.msg b/msgs/msg/Property.msg new file mode 100644 index 00000000..e8baad4f --- /dev/null +++ b/msgs/msg/Property.msg @@ -0,0 +1,3 @@ +string name +string description +string value diff --git a/msgs/msg/StageDescription.msg b/msgs/msg/StageDescription.msg index 9d7c577d..4bb974f1 100644 --- a/msgs/msg/StageDescription.msg +++ b/msgs/msg/StageDescription.msg @@ -13,5 +13,4 @@ string name uint32 flags # properties -string property_name -string property_value +Property[] properties