Interface: order states by priority

Priority is (depth of solutions, accumulated cost along trace).
This commit is contained in:
Robert Haschke 2018-01-27 10:26:03 +01:00
parent d0351698a6
commit facd6438c9
9 changed files with 194 additions and 152 deletions

View File

@ -139,9 +139,9 @@ protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl);
/// callback for new start states (received externally)
virtual void onNewStartState(const InterfaceState &external) = 0;
virtual void onNewStartState(Interface::iterator external, bool updated) = 0;
/// callback for new end states (received externally)
virtual void onNewEndState(const InterfaceState &external) = 0;
virtual void onNewEndState(Interface::iterator external, bool updated) = 0;
};

View File

@ -98,13 +98,13 @@ protected:
{}
/// copy external_state to a child's interface and remember the link in internal_to map
void copyState(InterfaceState &external, Stage &child, bool to_start);
void copyState(Interface::iterator external, Stage &child, bool to_start, bool updated);
protected:
container_type children_;
// map start/end states of children (internal) to corresponding states in our external interfaces
std::map<const InterfaceState*, InterfaceState*> internal_to_external_;
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

@ -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,11 @@ 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)
@ -209,6 +207,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 +224,13 @@ 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);
void newStartState(Interface::iterator it, bool updated);
void newEndState(Interface::iterator it, bool updated);
// initialize properties from parent and/or interface states
void initProperties(const InterfaceState &start, const InterfaceState &end);
std::pair<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; // Interface is allowed to change priority
public:
// TODO turn this into priority queue
/** InterfaceStates are ordered according to two values:
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
* Preference ordering considers high-depth first and within same depth, minimal cost paths.
*/
struct Priority : public std::pair<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,12 @@ public:
PropertyMap& properties() { return properties_; }
const PropertyMap& properties() const { return properties_; }
/// states are ordered by priority
inline bool operator<(const InterfaceState& other) const {
return this->priority_ < other.priority_;
}
inline const Priority& priority() const { return priority_; }
private:
// these methods should be only called by SolutionBase::set[Start|End]State()
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
@ -101,50 +131,21 @@ private:
PropertyMap properties_;
Solutions incoming_trajectories_;
Solutions outgoing_trajectories_;
Priority priority_;
};
/** Interface provides a list of InterfaceStates available as input for a stage.
*
* This is essentially an adaptor to a container class, to allow for notification
* of the interface's owner when new states become available
*/
class Interface : protected std::list<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;
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);
iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing);
// clone an existing InterfaceState, but without its incoming/outgoing connections
container_type::iterator clone(const InterfaceState &state);
using container_type::value_type;
using container_type::reference;
using container_type::const_reference;
using container_type::iterator;
using container_type::const_iterator;
using container_type::reverse_iterator;
using container_type::const_reverse_iterator;
using container_type::empty;
using container_type::size;
using container_type::clear;
using container_type::front;
using container_type::back;
using container_type::begin;
using container_type::cbegin;
using container_type::end;
using container_type::cend;
using container_type::rbegin;
using container_type::crbegin;
using container_type::rend;
using container_type::crend;
iterator clone(const InterfaceState &state);
private:
const NotifyFunction notify_;

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,
Stage &child, bool to_start) {
void ContainerBasePrivate::copyState(Interface::iterator external,
Stage &child, bool to_start, bool updated) {
if (to_start) {
InterfaceState& internal = *child.pimpl()->starts()->clone(external);
internal_to_external_.insert(std::make_pair(&internal, &external));
InterfaceState& internal = *child.pimpl()->starts()->clone(*external);
internal_to_external_.insert(std::make_pair(&internal, external));
} else {
InterfaceState& internal = *child.pimpl()->ends()->clone(external);
internal_to_external_.insert(std::make_pair(&internal, &external));
InterfaceState& internal = *child.pimpl()->ends()->clone(*external);
internal_to_external_.insert(std::make_pair(&internal, external));
}
}
@ -283,8 +286,8 @@ void SerialContainerPrivate::storeNewSolution(SerialSolution &&s)
solution.setStartState(*it->second);
} else {
// spawn a new state in previous stage
InterfaceState& external = *prevEnds()->add(InterfaceState(*internal_from), NULL, &solution);
internal_to_external_.insert(std::make_pair(internal_from, &external));
Interface::iterator external = prevEnds()->add(InterfaceState(*internal_from), NULL, &solution);
internal_to_external_.insert(std::make_pair(internal_from, external));
}
// add solution to existing or new end state
@ -294,8 +297,8 @@ void SerialContainerPrivate::storeNewSolution(SerialSolution &&s)
solution.setEndState(*it->second);
} else {
// spawn a new state in next stage
InterfaceState& external = *nextStarts()->add(InterfaceState(*internal_to), &solution, NULL);
internal_to_external_.insert(std::make_pair(internal_to, &external));
Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL);
internal_to_external_.insert(std::make_pair(internal_to, external));
}
// perform default stage action on new solution
@ -363,16 +366,16 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
// initialize starts_ and ends_ interfaces
Stage* child = start->get();
if (child->pimpl()->starts())
impl->starts_.reset(new Interface([impl, child](const Interface::iterator& external){
impl->starts_.reset(new Interface([impl, child](Interface::iterator external, bool updated){
// new external state in our starts_ interface is copied to first child
impl->copyState(*external, *child, true);
impl->copyState(external, *child, true, updated);
}));
child = last->get();
if (child->pimpl()->ends())
impl->ends_.reset(new Interface([impl, child](const Interface::iterator& external){
impl->ends_.reset(new Interface([impl, child](Interface::iterator external, bool updated){
// new external state in our ends_ interface is copied to last child
impl->copyState(*external, *child, false);
impl->copyState(external, *child, false, updated);
}));
// validate connectivity of this
@ -466,12 +469,8 @@ void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name)
: ContainerBasePrivate(me, name)
{
starts_.reset(new Interface([me](const Interface::iterator& external){
me->onNewStartState(*external);
}));
ends_.reset(new Interface([me](const Interface::iterator& external){
me->onNewEndState(*external);
}));
starts_.reset(new Interface(std::bind(&ParallelContainerBase::onNewStartState, me, _1, _2)));
ends_.reset(new Interface(std::bind(&ParallelContainerBase::onNewEndState, me, _1, _2)));
}

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)
@ -257,32 +258,35 @@ PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *m
{
}
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
// move infinite-cost states to processed list
if (std::isinf(state->priority().cost()))
starts_->moveTo(state, processed, processed.end());
}
void PropagatingEitherWayPrivate::dropFailedEnds(Interface::iterator state) {
// move infinite-cost states to processed list
if (std::isinf(state->priority().cost()))
ends_->moveTo(state, processed, processed.end());
}
inline bool PropagatingEitherWayPrivate::hasStartState() const{
return next_start_state_ != starts_->end();
return starts_ && !starts_->empty();
}
const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){
if (!hasStartState())
throw std::runtime_error("no new state for beginning available");
const InterfaceState& state= *next_start_state_;
++next_start_state_;
return state;
assert(hasStartState());
// move state to processed list
return *starts_->moveTo(starts_->begin(), processed, processed.end());
}
inline bool PropagatingEitherWayPrivate::hasEndState() const{
return next_end_state_ != ends_->end();
return ends_ && !ends_->empty();
}
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
if(!hasEndState())
throw std::runtime_error("no new state for ending available");
const InterfaceState& state= *next_end_state_;
++next_end_state_;
return state;
assert(hasEndState());
// move state to processed list
return *ends_->moveTo(ends_->begin(), processed, processed.end());
}
void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state)
@ -324,22 +328,6 @@ bool PropagatingEitherWayPrivate::compute()
return countFailures(result);
}
void PropagatingEitherWayPrivate::newStartState(const Interface::iterator &it)
{
// we just appended a state to the list, but the iterator doesn't see it anymore
// so let's point it at the new one
if(next_start_state_ == starts_->end())
--next_start_state_;
}
void PropagatingEitherWayPrivate::newEndState(const Interface::iterator &it)
{
// we just appended a state to the list, but the iterator doesn't see it anymore
// so let's point it at the new one
if(next_end_state_ == ends_->end())
--next_end_state_;
}
PropagatingEitherWay::PropagatingEitherWay(const std::string &name)
: PropagatingEitherWay(new PropagatingEitherWayPrivate(this, ANYWAY, name))
@ -356,23 +344,17 @@ void PropagatingEitherWay::initInterface()
{
auto impl = pimpl();
if (impl->dir & PropagatingEitherWay::FORWARD) {
if (!impl->starts_) { // keep existing interface if possible
impl->starts_.reset(new Interface([impl](const Interface::iterator& it) { impl->newStartState(it); }));
impl->next_start_state_ = impl->starts_->begin();
}
if (!impl->starts_) // keep existing interface if possible
impl->starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, impl, _1)));
} else {
impl->starts_.reset();
impl->next_start_state_ = Interface::iterator();
}
if (impl->dir & PropagatingEitherWay::BACKWARD) {
if (!impl->ends_) { // keep existing interface if possible
impl->ends_.reset(new Interface([impl](const Interface::iterator& it) { impl->newEndState(it); }));
impl->next_end_state_ = impl->ends_->end();
}
if (!impl->ends_) // keep existing interface if possible
impl->ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, impl, _1)));
} else {
impl->ends_.reset();
impl->next_end_state_ = Interface::iterator();
}
}
@ -386,6 +368,12 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir
initInterface();
}
void PropagatingEitherWay::reset()
{
pimpl()->processed.clear();
ComputeBase::reset();
}
void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene)
{
ComputeBase::init(scene);
@ -393,14 +381,10 @@ void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &sce
auto impl = pimpl();
// after being connected, restrict actual interface directions
if (!impl->nextStarts()) {
if (!impl->nextStarts())
impl->starts_.reset();
impl->next_start_state_ = Interface::iterator();
}
if (!impl->prevEnds()) {
if (!impl->prevEnds())
impl->ends_.reset();
impl->next_end_state_ = Interface::iterator();
}
if (!impl->isConnected())
throw InitStageException(*this, "can neither send forwards nor backwards");
}
@ -433,7 +417,6 @@ PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, con
{
// indicate, that we don't accept new states from ends_ interface
ends_.reset();
next_end_state_ = Interface::iterator();
}
@ -452,7 +435,6 @@ PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me,
{
// indicate, that we don't accept new states from starts_ interface
starts_.reset();
next_start_state_ = Interface::iterator();
}
@ -510,23 +492,40 @@ void Generator::spawn(InterfaceState&& state, SubTrajectory&& t)
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
: ComputeBasePrivate(me, name)
{
starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); }));
ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); }));
it_pairs_ = std::make_pair(starts_->begin(), ends_->begin());
starts_.reset(new Interface(std::bind(&ConnectingPrivate::newStartState, this, _1, _2)));
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newEndState, this, _1, _2)));
}
void ConnectingPrivate::newStartState(const Interface::iterator& it)
void ConnectingPrivate::newStartState(Interface::iterator it, bool updated)
{
// TODO: need to handle the pairs iterator
if(it_pairs_.first == starts_->end())
--it_pairs_.first;
if (!std::isfinite(it->priority().cost()))
return;
if (updated) {
// many pairs might be affected: sort
pending.sort();
} else { // new state: insert all pairs with other interface
for (auto oit = ends_->begin(), oend = ends_->end(); oit != oend; ++oit) {
if (!std::isfinite(oit->priority().cost()))
break;
pending.insert(std::make_pair(it, oit));
}
}
}
void ConnectingPrivate::newEndState(const Interface::iterator& it)
void ConnectingPrivate::newEndState(Interface::iterator it, bool updated)
{
// TODO: need to handle the pairs iterator properly
if(it_pairs_.second == ends_->end())
--it_pairs_.second;
if (!std::isfinite(it->priority().cost()))
return;
if (updated) {
// many pairs might be affected: sort
pending.sort();
} else { // new state: insert all pairs with other interface
for (auto oit = starts_->begin(), oend = starts_->end(); oit != oend; ++oit) {
if (!std::isfinite(oit->priority().cost()))
break;
pending.insert(std::make_pair(oit, it));
}
}
}
void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end)
@ -538,15 +537,13 @@ void ConnectingPrivate::initProperties(const InterfaceState &start, const Interf
}
bool ConnectingPrivate::canCompute() const{
// TODO: implement this properly
return it_pairs_.first != starts_->end() &&
it_pairs_.second != ends_->end();
return !pending.empty();
}
bool ConnectingPrivate::compute() {
// TODO: implement this properly
const InterfaceState& from = *it_pairs_.first;
const InterfaceState& to = *(it_pairs_.second++);
const StatePair& top = pending.pop();
const InterfaceState& from = *top.first;
const InterfaceState& to = *top.second;
initProperties(from, to);
return countFailures(static_cast<Connecting*>(me_)->compute(from, to));
}
@ -557,6 +554,12 @@ Connecting::Connecting(const std::string &name)
{
}
void Connecting::reset()
{
pimpl()->pending.clear();
ComputeBase::reset();
}
void Connecting::connect(const InterfaceState& from, const InterfaceState& to,
SubTrajectory&& t) {
auto impl = pimpl();

View File

@ -59,29 +59,37 @@ 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));
// configure state: inherit priority from other end's state and add current solution's cost
assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set
emplace_back(state);
iterator back = --end();
// adjust subtrajectories ...
if (incoming) incoming->setEndState(*back);
if (outgoing) outgoing->setStartState(*back);
// ... before calling notify callback
if (notify_) notify_(back);
return back;
if (incoming) {
it->priority_ = InterfaceState::Priority(1, incoming->cost());
incoming->setEndState(*it);
} else if (outgoing) {
it->priority_ = InterfaceState::Priority(1, outgoing->cost());
outgoing->setStartState(*it);
}
// move list node into interface's state list (sorted by priority)
moveFrom(it, container);
// and finally call notify callback
if (notify_) notify_(it, false);
return it;
}
Interface::iterator Interface::clone(const InterfaceState &state)
{
emplace_back(InterfaceState(state));
iterator back = --end();
if (notify_) notify_(back);
return back;
iterator it = insert(InterfaceState(state));
if (notify_) notify_(it, false);
return it;
}