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); ParallelContainerBase(ParallelContainerBasePrivate* impl);
/// callback for new start states (received externally) /// 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) /// 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 /// 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: protected:
container_type children_; container_type children_;
// map start/end states of children (internal) to corresponding states in our external interfaces // 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) PIMPL_FUNCTIONS(ContainerBase)

View File

@ -50,6 +50,8 @@ public:
reference top() { return c.front(); } reference top() { return c.front(); }
const_reference top() const { 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(); } reference front() { return c.front(); }
const_reference front() const { return c.front(); } const_reference front() const { return c.front(); }
reference back() { return c.back(); } reference back() { return c.back(); }
@ -68,6 +70,9 @@ public:
const_reverse_iterator crbegin() const { return c.rbegin(); } const_reverse_iterator crbegin() const { return c.rbegin(); }
const_reverse_iterator crend() const { return c.rend(); } 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 insert(const value_type& item) {
iterator at = std::upper_bound(c.begin(), c.end(), item, comp); iterator at = std::upper_bound(c.begin(), c.end(), item, comp);
return c.insert(at, item); return c.insert(at, item);
@ -79,7 +84,7 @@ public:
iterator erase(const_iterator pos) { return c.erase(pos); } 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) { iterator update(iterator &it) {
container_type temp; container_type temp;
temp.splice(temp.end(), c, it); // move it from c to temp temp.splice(temp.end(), c, it); // move it from c to temp
@ -87,12 +92,26 @@ public:
c.splice(at, temp, it); c.splice(at, temp, it);
return 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 { namespace detail {
template <typename ValueType, typename CostType> template <typename ValueType, typename CostType>
struct ItemCostPair : std::pair<ValueType, CostType> { struct ItemCostPair : std::pair<ValueType, CostType> {
typedef CostType cost_type;
ItemCostPair(const std::pair<ValueType, CostType>& other) ItemCostPair(const std::pair<ValueType, CostType>& other)
: std::pair<ValueType, CostType>(other) {} : std::pair<ValueType, CostType>(other) {}
ItemCostPair(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 ValueType& value() { return this->first; }
inline const ValueType& value() const { 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 // comparison only considers cost
constexpr bool operator<(const ItemCostPair& other) const { 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, template <typename ValueType, typename CostType = double,
typename Compare = std::less<detail::ItemCostPair<ValueType, CostType>>> 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; typedef ordered<detail::ItemCostPair<ValueType, CostType>, Compare> base_type;
public: 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)); 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}; enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD};
void restrictDirection(Direction dir); void restrictDirection(Direction dir);
virtual void reset() override;
virtual void init(const planning_scene::PlanningSceneConstPtr &scene) override; virtual void init(const planning_scene::PlanningSceneConstPtr &scene) override;
virtual bool computeForward(const InterfaceState& from) = 0; virtual bool computeForward(const InterfaceState& from) = 0;
virtual bool computeBackward(const InterfaceState& to) = 0; virtual bool computeBackward(const InterfaceState& to) = 0;
@ -285,6 +287,8 @@ public:
PRIVATE_CLASS(Connecting) PRIVATE_CLASS(Connecting)
Connecting(const std::string& name); Connecting(const std::string& name);
virtual void reset() override;
virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0; virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0;
void connect(const InterfaceState& from, const InterfaceState& to, void connect(const InterfaceState& from, const InterfaceState& to,
SubTrajectory&& trajectory); SubTrajectory&& trajectory);

View File

@ -146,6 +146,7 @@ PIMPL_FUNCTIONS(ComputeBase)
class PropagatingEitherWayPrivate : public ComputeBasePrivate { class PropagatingEitherWayPrivate : public ComputeBasePrivate {
friend class PropagatingEitherWay; friend class PropagatingEitherWay;
std::list<Interface::value_type> processed; // already processed states
public: public:
PropagatingEitherWay::Direction dir; PropagatingEitherWay::Direction dir;
@ -166,14 +167,11 @@ public:
const InterfaceState &fetchEndState(); const InterfaceState &fetchEndState();
protected: 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 // initialize properties from parent and/or state
void initProperties(const InterfaceState &state); void initProperties(const InterfaceState &state);
// drop states corresponding to failed (infinite-cost) trajectories
Interface::const_iterator next_start_state_; void dropFailedStarts(Interface::iterator state);
Interface::const_iterator next_end_state_; void dropFailedEnds(Interface::iterator state);
}; };
PIMPL_FUNCTIONS(PropagatingEitherWay) PIMPL_FUNCTIONS(PropagatingEitherWay)
@ -209,6 +207,12 @@ PIMPL_FUNCTIONS(Generator)
class ConnectingPrivate : public ComputeBasePrivate { class ConnectingPrivate : public ComputeBasePrivate {
friend class Connecting; 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: public:
inline ConnectingPrivate(Connecting *me, const std::string &name); inline ConnectingPrivate(Connecting *me, const std::string &name);
@ -220,12 +224,13 @@ public:
private: private:
// get informed when new start or end state becomes available // get informed when new start or end state becomes available
void newStartState(const std::list<InterfaceState>::iterator& it); void newStartState(Interface::iterator it, bool updated);
void newEndState(const std::list<InterfaceState>::iterator& it); void newEndState(Interface::iterator it, bool updated);
// initialize properties from parent and/or interface states // initialize properties from parent and/or interface states
void initProperties(const InterfaceState &start, const InterfaceState &end); 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) PIMPL_FUNCTIONS(Connecting)

View File

@ -41,6 +41,7 @@
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/properties.h> #include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/cost_queue.h>
#include <moveit_task_constructor_msgs/Solution.h> #include <moveit_task_constructor_msgs/Solution.h>
#include <visualization_msgs/MarkerArray.h> #include <visualization_msgs/MarkerArray.h>
@ -74,8 +75,31 @@ MOVEIT_CLASS_FORWARD(Introspection)
*/ */
class InterfaceState { class InterfaceState {
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
friend class Interface; // Interface is allowed to change priority
public: 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; typedef std::deque<SolutionBase*> Solutions;
/// create an InterfaceState from a planning scene /// create an InterfaceState from a planning scene
@ -91,6 +115,12 @@ public:
PropertyMap& properties() { return properties_; } PropertyMap& properties() { return properties_; }
const PropertyMap& properties() const { 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: private:
// these methods should be only called by SolutionBase::set[Start|End]State() // these methods should be only called by SolutionBase::set[Start|End]State()
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); } inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
@ -101,50 +131,21 @@ private:
PropertyMap properties_; PropertyMap properties_;
Solutions incoming_trajectories_; Solutions incoming_trajectories_;
Solutions outgoing_trajectories_; Solutions outgoing_trajectories_;
Priority priority_;
}; };
/** Interface provides a list of InterfaceStates available as input for a stage. /** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */
* class Interface : public ordered<InterfaceState> {
* 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> {
public: public:
typedef std::list<InterfaceState> container_type; typedef std::function<void(iterator it, bool updated)> NotifyFunction;
typedef std::function<void(const container_type::iterator&)> NotifyFunction;
Interface(const NotifyFunction &notify = NotifyFunction()); Interface(const NotifyFunction &notify = NotifyFunction());
// add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state // add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state
// and finally run the notify callback iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing);
container_type::iterator add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing);
// clone an existing InterfaceState, but without its incoming/outgoing connections // clone an existing InterfaceState, but without its incoming/outgoing connections
container_type::iterator clone(const InterfaceState &state); 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;
private: private:
const NotifyFunction notify_; const NotifyFunction notify_;

View File

@ -43,6 +43,9 @@
#include <iostream> #include <iostream>
#include <algorithm> #include <algorithm>
#include <boost/range/adaptor/reversed.hpp> #include <boost/range/adaptor/reversed.hpp>
#include <functional>
using namespace std::placeholders;
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
@ -87,14 +90,14 @@ bool ContainerBasePrivate::compute()
return static_cast<ContainerBase*>(me_)->compute(); return static_cast<ContainerBase*>(me_)->compute();
} }
void ContainerBasePrivate::copyState(InterfaceState &external, void ContainerBasePrivate::copyState(Interface::iterator external,
Stage &child, bool to_start) { Stage &child, bool to_start, bool updated) {
if (to_start) { if (to_start) {
InterfaceState& internal = *child.pimpl()->starts()->clone(external); InterfaceState& internal = *child.pimpl()->starts()->clone(*external);
internal_to_external_.insert(std::make_pair(&internal, &external)); internal_to_external_.insert(std::make_pair(&internal, external));
} else { } else {
InterfaceState& internal = *child.pimpl()->ends()->clone(external); InterfaceState& internal = *child.pimpl()->ends()->clone(*external);
internal_to_external_.insert(std::make_pair(&internal, &external)); internal_to_external_.insert(std::make_pair(&internal, external));
} }
} }
@ -283,8 +286,8 @@ void SerialContainerPrivate::storeNewSolution(SerialSolution &&s)
solution.setStartState(*it->second); solution.setStartState(*it->second);
} else { } else {
// spawn a new state in previous stage // spawn a new state in previous stage
InterfaceState& external = *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)); internal_to_external_.insert(std::make_pair(internal_from, external));
} }
// add solution to existing or new end state // add solution to existing or new end state
@ -294,8 +297,8 @@ void SerialContainerPrivate::storeNewSolution(SerialSolution &&s)
solution.setEndState(*it->second); solution.setEndState(*it->second);
} else { } else {
// spawn a new state in next stage // spawn a new state in next stage
InterfaceState& external = *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)); internal_to_external_.insert(std::make_pair(internal_to, external));
} }
// perform default stage action on new solution // perform default stage action on new solution
@ -363,16 +366,16 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
// initialize starts_ and ends_ interfaces // initialize starts_ and ends_ interfaces
Stage* child = start->get(); Stage* child = start->get();
if (child->pimpl()->starts()) 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 // 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(); child = last->get();
if (child->pimpl()->ends()) 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 // 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 // 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) ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name)
: ContainerBasePrivate(me, name) : ContainerBasePrivate(me, name)
{ {
starts_.reset(new Interface([me](const Interface::iterator& external){ starts_.reset(new Interface(std::bind(&ParallelContainerBase::onNewStartState, me, _1, _2)));
me->onNewStartState(*external); ends_.reset(new Interface(std::bind(&ParallelContainerBase::onNewEndState, me, _1, _2)));
}));
ends_.reset(new Interface([me](const Interface::iterator& external){
me->onNewEndState(*external);
}));
} }

View File

@ -42,6 +42,7 @@
#include <iomanip> #include <iomanip>
#include <ros/console.h> #include <ros/console.h>
using namespace std::placeholders;
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
void InitStageException::push_back(const Stage &stage, const std::string &msg) 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{ inline bool PropagatingEitherWayPrivate::hasStartState() const{
return next_start_state_ != starts_->end(); return starts_ && !starts_->empty();
} }
const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){ const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){
if (!hasStartState()) assert(hasStartState());
throw std::runtime_error("no new state for beginning available"); // move state to processed list
return *starts_->moveTo(starts_->begin(), processed, processed.end());
const InterfaceState& state= *next_start_state_;
++next_start_state_;
return state;
} }
inline bool PropagatingEitherWayPrivate::hasEndState() const{ inline bool PropagatingEitherWayPrivate::hasEndState() const{
return next_end_state_ != ends_->end(); return ends_ && !ends_->empty();
} }
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
if(!hasEndState()) assert(hasEndState());
throw std::runtime_error("no new state for ending available"); // move state to processed list
return *ends_->moveTo(ends_->begin(), processed, processed.end());
const InterfaceState& state= *next_end_state_;
++next_end_state_;
return state;
} }
void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state) void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state)
@ -324,22 +328,6 @@ bool PropagatingEitherWayPrivate::compute()
return countFailures(result); 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::PropagatingEitherWay(const std::string &name)
: PropagatingEitherWay(new PropagatingEitherWayPrivate(this, ANYWAY, name)) : PropagatingEitherWay(new PropagatingEitherWayPrivate(this, ANYWAY, name))
@ -356,23 +344,17 @@ void PropagatingEitherWay::initInterface()
{ {
auto impl = pimpl(); auto impl = pimpl();
if (impl->dir & PropagatingEitherWay::FORWARD) { if (impl->dir & PropagatingEitherWay::FORWARD) {
if (!impl->starts_) { // keep existing interface if possible if (!impl->starts_) // keep existing interface if possible
impl->starts_.reset(new Interface([impl](const Interface::iterator& it) { impl->newStartState(it); })); impl->starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, impl, _1)));
impl->next_start_state_ = impl->starts_->begin();
}
} else { } else {
impl->starts_.reset(); impl->starts_.reset();
impl->next_start_state_ = Interface::iterator();
} }
if (impl->dir & PropagatingEitherWay::BACKWARD) { if (impl->dir & PropagatingEitherWay::BACKWARD) {
if (!impl->ends_) { // keep existing interface if possible if (!impl->ends_) // keep existing interface if possible
impl->ends_.reset(new Interface([impl](const Interface::iterator& it) { impl->newEndState(it); })); impl->ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, impl, _1)));
impl->next_end_state_ = impl->ends_->end();
}
} else { } else {
impl->ends_.reset(); impl->ends_.reset();
impl->next_end_state_ = Interface::iterator();
} }
} }
@ -386,6 +368,12 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir
initInterface(); initInterface();
} }
void PropagatingEitherWay::reset()
{
pimpl()->processed.clear();
ComputeBase::reset();
}
void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene) void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
ComputeBase::init(scene); ComputeBase::init(scene);
@ -393,14 +381,10 @@ void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &sce
auto impl = pimpl(); auto impl = pimpl();
// after being connected, restrict actual interface directions // after being connected, restrict actual interface directions
if (!impl->nextStarts()) { if (!impl->nextStarts())
impl->starts_.reset(); impl->starts_.reset();
impl->next_start_state_ = Interface::iterator(); if (!impl->prevEnds())
}
if (!impl->prevEnds()) {
impl->ends_.reset(); impl->ends_.reset();
impl->next_end_state_ = Interface::iterator();
}
if (!impl->isConnected()) if (!impl->isConnected())
throw InitStageException(*this, "can neither send forwards nor backwards"); 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 // indicate, that we don't accept new states from ends_ interface
ends_.reset(); 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 // indicate, that we don't accept new states from starts_ interface
starts_.reset(); 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) ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
: ComputeBasePrivate(me, name) : ComputeBasePrivate(me, name)
{ {
starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); })); starts_.reset(new Interface(std::bind(&ConnectingPrivate::newStartState, this, _1, _2)));
ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); })); ends_.reset(new Interface(std::bind(&ConnectingPrivate::newEndState, this, _1, _2)));
it_pairs_ = std::make_pair(starts_->begin(), ends_->begin());
} }
void ConnectingPrivate::newStartState(const Interface::iterator& it) void ConnectingPrivate::newStartState(Interface::iterator it, bool updated)
{ {
// TODO: need to handle the pairs iterator if (!std::isfinite(it->priority().cost()))
if(it_pairs_.first == starts_->end()) return;
--it_pairs_.first; 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 (!std::isfinite(it->priority().cost()))
if(it_pairs_.second == ends_->end()) return;
--it_pairs_.second; 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) 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{ bool ConnectingPrivate::canCompute() const{
// TODO: implement this properly return !pending.empty();
return it_pairs_.first != starts_->end() &&
it_pairs_.second != ends_->end();
} }
bool ConnectingPrivate::compute() { bool ConnectingPrivate::compute() {
// TODO: implement this properly const StatePair& top = pending.pop();
const InterfaceState& from = *it_pairs_.first; const InterfaceState& from = *top.first;
const InterfaceState& to = *(it_pairs_.second++); const InterfaceState& to = *top.second;
initProperties(from, to); initProperties(from, to);
return countFailures(static_cast<Connecting*>(me_)->compute(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, void Connecting::connect(const InterfaceState& from, const InterfaceState& to,
SubTrajectory&& t) { SubTrajectory&& t) {
auto impl = pimpl(); auto impl = pimpl();

View File

@ -59,29 +59,37 @@ Interface::Interface(const Interface::NotifyFunction &notify)
: notify_(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) { Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incoming, SolutionBase* outgoing) {
if (!state.incomingTrajectories().empty() || !state.outgoingTrajectories().empty()) if (!state.incomingTrajectories().empty() || !state.outgoingTrajectories().empty())
throw std::runtime_error("expecting empty incoming/outgoing trajectories"); throw std::runtime_error("expecting empty incoming/outgoing trajectories");
if (!state.scene()) if (!state.scene())
throw std::runtime_error("expecting valid planning 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 assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set
emplace_back(state); if (incoming) {
iterator back = --end(); it->priority_ = InterfaceState::Priority(1, incoming->cost());
// adjust subtrajectories ... incoming->setEndState(*it);
if (incoming) incoming->setEndState(*back); } else if (outgoing) {
if (outgoing) outgoing->setStartState(*back); it->priority_ = InterfaceState::Priority(1, outgoing->cost());
// ... before calling notify callback outgoing->setStartState(*it);
if (notify_) notify_(back); }
return back; // 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) Interface::iterator Interface::clone(const InterfaceState &state)
{ {
emplace_back(InterfaceState(state)); iterator it = insert(InterfaceState(state));
iterator back = --end(); if (notify_) notify_(it, false);
if (notify_) notify_(back); return it;
return back;
} }