Merge branches 'fix-remote-task-model', 'wip-cost-ordering' and 'wip-properties'

This commit is contained in:
Robert Haschke 2018-02-12 23:35:01 +01:00
commit 3a4dc6755b
19 changed files with 436 additions and 290 deletions

View File

@ -85,7 +85,7 @@ int main(int argc, char** argv){
gengrasp->setAngleDelta(M_PI / 10.);
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->properties().configureInitFrom(Stage::PARENT);
ik->properties().configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"});
ik->setMaxIKSolutions(1);
t.add(std::move(ik));
}

View File

@ -84,7 +84,7 @@ int main(int argc, char** argv){
gengrasp->setAngleDelta(-.2);
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->properties().configureInitFrom(Stage::PARENT);
ik->properties().configureInitFrom(Stage::PARENT, {"eef"});
ik->setMaxIKSolutions(8);
t.add(std::move(ik));
}

View File

@ -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<bool(const SolutionBase& current,
const solution_container& trace,
typedef std::function<void(const solution_container& trace,
double trace_accumulated_cost)> 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<TraverseDirection dir>
bool traverse(const SolutionBase &start, const SolutionProcessor &cb,
void traverse(const SolutionBase &start, const SolutionProcessor &cb,
solution_container &trace, double trace_cost = 0);
protected:
@ -141,10 +138,12 @@ public:
protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl);
virtual void onNewSolution(const SolutionBase& s) override;
/// 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;
};
@ -195,6 +194,8 @@ public:
}
protected:
virtual void onNewSolution(const SolutionBase& s) = 0;
WrapperBase(WrapperBasePrivate *impl, pointer &&child = Stage::pointer());
};

View File

@ -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(Interface::iterator external, Stage &child, bool to_start, bool updated);
protected:
container_type children_;
// map first child's start states to the corresponding states in this' starts_
std::map<const InterfaceState*, InterfaceState*> internal_to_my_starts_;
// map last child's end states to the corresponding states in this' ends_
std::map<const InterfaceState*, InterfaceState*> internal_to_my_ends_;
// map start/end states of children (internal) to corresponding states in our external interfaces
std::map<const InterfaceState*, Interface::iterator> internal_to_external_;
};
PIMPL_FUNCTIONS(ContainerBase)

View File

@ -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 <typename ValueType, typename CostType>
struct ItemCostPair : std::pair<ValueType, CostType> {
typedef CostType cost_type;
ItemCostPair(const std::pair<ValueType, CostType>& other)
: std::pair<ValueType, CostType>(other) {}
ItemCostPair(std::pair<ValueType, CostType>&& other)
@ -101,7 +120,7 @@ struct ItemCostPair : std::pair<ValueType, CostType> {
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<ValueType, CostType> {
}
};
}
} // namespace detail
template <typename ValueType, typename CostType = double,
typename Compare = std::less<detail::ItemCostPair<ValueType, CostType>>>
@ -117,7 +136,10 @@ class cost_ordered : public ordered<detail::ItemCostPair<ValueType, CostType>, C
{
typedef ordered<detail::ItemCostPair<ValueType, CostType>, 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));
}
};

View File

@ -43,6 +43,7 @@
#include <map>
#include <set>
#include <functional>
#include <sstream>
namespace moveit {
namespace task_constructor {
@ -66,12 +67,25 @@ boost::any fromName(const PropertyMap& other, const std::string& other_name);
class Property {
friend class PropertyMap;
typedef std::function<std::string(const boost::any& v)> SerializeFunction;
Property(const std::type_index &type_index, const std::string &description, const boost::any &default_value,
const Property::SerializeFunction &serialize);
template <typename T>
static std::string serialize(const boost::any& value) {
if (value.empty()) return "";
std::ostringstream oss;
oss << boost::any_cast<T>(value);
return oss.str();
}
public:
typedef int SourceId;
/// function callback used to initialize property value from another PropertyMap
typedef std::function<boost::any(const PropertyMap& other)> InitializerFunction;
typedef std::map<SourceId, InitializerFunction> InitializerMap;
Property(const std::type_index& type_index, const std::string& description, const boost::any& default_value);
/// function callback used to signal value setting to external components
typedef std::function<void(const Property*)> SignalFunction;
/// set current value and default value
void setValue(const boost::any& value);
@ -86,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_; }
@ -97,15 +113,24 @@ 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);
/// 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_;
boost::any default_;
boost::any value_;
InitializerMap initializers_;
/// used for external initialization
SourceId source_id_ = 0;
InitializerFunction initializer_;
SignalFunction signaller_;
SerializeFunction serialize_;
};
@ -117,22 +142,25 @@ private:
class PropertyMap
{
std::map<std::string, Property> props_;
typedef std::map<std::string, Property>::iterator iterator;
typedef std::map<std::string, Property>::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 = boost::any());
const std::string& description,
const boost::any& default_value,
const Property::SerializeFunction &serialize);
public:
/// declare a property for future use
template<typename T>
Property& declare(const std::string& name, const std::string& description = "") {
return declare(name, typeid(T), description);
return declare(name, typeid(T), description, boost::any(), &Property::serialize<T>);
}
/// declare a property with default value
template<typename T>
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<T>);
}
/// get the property with given name
@ -141,11 +169,23 @@ public:
return const_cast<PropertyMap*>(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<std::string> &properties = {});
/// set (and, if neccessary, declare) the value of a property
void set(const std::string& name, const boost::any& value);
template <typename T>
void set(const std::string& name, const T& value) {
auto it = props_.find(name);
if (it == props_.end()) // name is not yet declared
declare<T>(name, value, "");
else
it->second.setValue(value);
}
/// temporarily set the value of a property
void setCurrent(const std::string& name, const boost::any& value);
@ -177,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<boost::any>(const std::string& name, const boost::any& value);
} // namespace task_constructor
} // namespace moveit

View File

@ -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);

View File

@ -146,6 +146,7 @@ PIMPL_FUNCTIONS(ComputeBase)
class PropagatingEitherWayPrivate : public ComputeBasePrivate {
friend class PropagatingEitherWay;
std::list<Interface::value_type> processed; // already processed states
public:
PropagatingEitherWay::Direction dir;
@ -166,14 +167,9 @@ public:
const InterfaceState &fetchEndState();
protected:
// get informed when new start or end state becomes available
void newStartState(const std::list<InterfaceState>::iterator& it);
void newEndState(const std::list<InterfaceState>::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)
@ -198,10 +194,6 @@ public:
bool canCompute() const override;
bool compute() override;
private:
// initialize properties from parent
void initProperties();
};
PIMPL_FUNCTIONS(Generator)
@ -209,6 +201,12 @@ PIMPL_FUNCTIONS(Generator)
class ConnectingPrivate : public ComputeBasePrivate {
friend class Connecting;
typedef std::pair<Interface::const_iterator, Interface::const_iterator> 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 +218,11 @@ public:
private:
// get informed when new start or end state becomes available
void newStartState(const std::list<InterfaceState>::iterator& it);
void newEndState(const std::list<InterfaceState>::iterator& it);
// initialize properties from parent and/or interface states
void initProperties(const InterfaceState &start, const InterfaceState &end);
void newStartState(Interface::iterator it, bool updated);
void newEndState(Interface::iterator it, bool updated);
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
// ordered list of pending state pairs
ordered<StatePair, StatePairLess> pending;
};
PIMPL_FUNCTIONS(Connecting)

View File

@ -41,6 +41,7 @@
#include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/cost_queue.h>
#include <moveit_task_constructor_msgs/Solution.h>
#include <visualization_msgs/MarkerArray.h>
@ -74,8 +75,31 @@ MOVEIT_CLASS_FORWARD(Introspection)
*/
class InterfaceState {
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
friend class Interface; // allow Interface to set owner_ and 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<unsigned int, double> {
Priority() : Priority(0, 0.0) {}
Priority(unsigned int depth, double cost)
: std::pair<unsigned int, double>(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<SolutionBase*> Solutions;
/// create an InterfaceState from a planning scene
@ -91,6 +115,13 @@ 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_; }
Interface* owner() const { return owner_; }
private:
// these methods should be only called by SolutionBase::set[Start|End]State()
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
@ -101,50 +132,27 @@ 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
};
/** 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<InterfaceState> {
/** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */
class Interface : public ordered<InterfaceState> {
public:
typedef std::list<InterfaceState> container_type;
typedef std::function<void(const container_type::iterator&)> NotifyFunction;
Interface(const NotifyFunction &notify);
typedef std::function<void(iterator it, bool updated)> NotifyFunction;
Interface(const NotifyFunction &notify = 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);
/// 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
container_type::iterator clone(const InterfaceState &state);
/// clone an existing InterfaceState, but without its incoming/outgoing connections
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;
/// update state's priority if new priority is smaller and call notify_
void updatePriority(InterfaceState &state, const InterfaceState::Priority &priority);
private:
const NotifyFunction notify_;
@ -163,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<InterfaceState&>(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<InterfaceState&>(state).addIncoming(this);
}

View File

@ -43,6 +43,9 @@
#include <iostream>
#include <algorithm>
#include <boost/range/adaptor/reversed.hpp>
#include <functional>
using namespace std::placeholders;
namespace moveit { namespace task_constructor {
@ -87,14 +90,14 @@ bool ContainerBasePrivate::compute()
return static_cast<ContainerBase*>(me_)->compute();
}
void ContainerBasePrivate::copyState(InterfaceState &external_state,
Stage &child, bool to_start) {
void ContainerBasePrivate::copyState(Interface::iterator external,
Stage &child, bool to_start, bool updated) {
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 +157,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();
}
@ -168,11 +170,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");
@ -203,18 +200,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<std::pair<SerialContainer::solution_container, double>> solutions;
const StagePrivate* const stopping_stage;
const size_t max_depth;
};
void SerialContainer::onNewSolution(const SolutionBase &current)
@ -223,45 +220,54 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
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());
// find all incoming solution pathes ending at current solution
SolutionCollector incoming(num_before);
traverse<BACKWARD>(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());
// find all outgoing solution pathes starting at current solution
SolutionCollector outgoing(num_after);
traverse<FORWARD>(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<SerialSolution> 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(&current);
// 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(&current);
// 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<InterfaceState*>(start), prio);
const InterfaceState* end = (out.first.empty() ? current : *out.first.back()).end();
end->owner()->updatePriority(*const_cast<InterfaceState*>(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));
}
@ -275,23 +281,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);
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
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);
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
@ -359,16 +367,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
@ -419,26 +427,21 @@ void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &p
}
template <TraverseDirection dir>
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<dir>(start)) {
const InterfaceState::Solutions& solutions = trajectories<dir>(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<dir>(*successor, cb, trace, trace_cost);
traverse<dir>(*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,
@ -467,12 +470,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)));
}
@ -509,6 +508,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<InterfaceState*>(s.start());
start->owner()->updatePriority(*start, prio);
InterfaceState* end = const_cast<InterfaceState*>(s.end());
end->owner()->updatePriority(*end, prio);
pimpl()->newSolution(s);
}
WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name)
: ContainerBasePrivate(me, name)
@ -571,6 +582,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<InterfaceState*>(s.start());
start->owner()->updatePriority(*start, prio);
InterfaceState* end = const_cast<InterfaceState*>(s.end());
end->owner()->updatePriority(*end, prio);
pimpl()->newSolution(s);
}
WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name)
: WrapperBasePrivate(me, name)

View File

@ -38,6 +38,7 @@
#include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/storage.h>
#include <moveit_task_constructor_msgs/Property.h>
#include <ros/node_handle.h>
#include <ros/publisher.h>
@ -228,7 +229,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());

View File

@ -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_);
@ -73,6 +75,8 @@ void Property::setCurrentValue(const boost::any &value)
if (!value.empty())
typeCheck(value, type_index_);
value_ = value;
if (signaller_)
signaller_(this);
}
void Property::reset()
@ -80,33 +84,38 @@ 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)
{
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));
}
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;
@ -128,18 +137,17 @@ void PropertyMap::configureInitFrom(Property::SourceId source, const std::set<st
}
}
void PropertyMap::set(const std::string &name, const boost::any &value)
{
template <>
void PropertyMap::set<boost::any>(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)

View File

@ -42,6 +42,7 @@
#include <iomanip>
#include <ros/console.h>
using namespace std::placeholders;
namespace moveit { namespace task_constructor {
void InitStageException::push_back(const Stage &stage, const std::string &msg)
@ -129,6 +130,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 {
@ -257,42 +263,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;
}
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());
assert(hasEndState());
// move state to processed list
return *ends_->moveTo(ends_->begin(), processed, processed.end());
}
bool PropagatingEitherWayPrivate::canCompute() const
@ -311,35 +310,21 @@ 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;
}
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 +341,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 +365,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 +378,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 +414,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 +432,6 @@ PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me,
{
// indicate, that we don't accept new states from starts_ interface
starts_.reset();
next_start_state_ = Interface::iterator();
}
@ -475,18 +454,9 @@ bool GeneratorPrivate::canCompute() const {
}
bool GeneratorPrivate::compute() {
initProperties();
return countFailures(static_cast<Generator*>(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))
@ -510,44 +480,50 @@ 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;
}
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());
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));
}
}
}
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++);
initProperties(from, to);
const StatePair& top = pending.pop();
const InterfaceState& from = *top.first;
const InterfaceState& to = *top.second;
return countFailures(static_cast<Connecting*>(me_)->compute(from, to));
}
@ -557,6 +533,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();

View File

@ -59,31 +59,52 @@ Interface::Interface(const Interface::NotifyFunction &notify)
: 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<InterfaceState> 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
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));
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)
{

View File

@ -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);
}

View File

@ -23,6 +23,18 @@ TEST(Property, standard) {
EXPECT_EQ(props.get<double>("double3"), 3.0);
}
TEST(Property, directset) {
PropertyMap props;
props.set("int1", 1);
EXPECT_EQ(props.get<int>("int1"), 1);
EXPECT_STREQ(props.property("int1").serialize().c_str(), "1");
props.set("int2", boost::any(2));
EXPECT_EQ(props.get<int>("int2"), 2);
// cannot serialize, because directly set
EXPECT_STREQ(props.property("int2").serialize().c_str(), "");
}
TEST(Property, redeclare) {
PropertyMap props;
props.declare<double>("double1");
@ -65,6 +77,14 @@ TEST(Property, reset) {
EXPECT_EQ(props.get<double>("double1"), 1.0);
}
TEST(Property, serialize) {
PropertyMap props;
props.declare<int>("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() {
@ -109,6 +129,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);

View File

@ -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

3
msgs/msg/Property.msg Normal file
View File

@ -0,0 +1,3 @@
string name
string description
string value

View File

@ -13,5 +13,4 @@ string name
uint32 flags
# properties
string property_name
string property_value
Property[] properties