mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'fix-remote-task-model', 'wip-cost-ordering' and 'wip-properties'
This commit is contained in:
commit
3a4dc6755b
@ -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));
|
||||
}
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
@ -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());
|
||||
};
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
@ -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));
|
||||
}
|
||||
};
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
@ -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 ¬ify);
|
||||
typedef std::function<void(iterator it, bool updated)> 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);
|
||||
/// 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);
|
||||
}
|
||||
|
||||
@ -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 ¤t)
|
||||
@ -223,45 +220,54 @@ 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());
|
||||
// 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(¤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<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)
|
||||
|
||||
@ -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());
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -59,31 +59,52 @@ 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<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)
|
||||
{
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
3
msgs/msg/Property.msg
Normal file
@ -0,0 +1,3 @@
|
||||
string name
|
||||
string description
|
||||
string value
|
||||
@ -13,5 +13,4 @@ string name
|
||||
uint32 flags
|
||||
|
||||
# properties
|
||||
string property_name
|
||||
string property_value
|
||||
Property[] properties
|
||||
|
||||
Loading…
Reference in New Issue
Block a user