fix connection creation

Establishing the interface connections, we face a chicken-egg-problem:
To establish a connection, a predecessors/successors pull interface is
assigned to the current's stage push interface.
However, propagating stages (in auto-detection mode) can only create
their pull interfaces if the corresponding, opposite-side push interface
is present already (because that's the mechanism to determine the supported
propagation directions).

Hence, we need to resolve this by performing two sweeps:
- initialization, assuming both propagation directions should be supported,
  thus generating both pull interfaces, i.e. providing the egg
- stripping down the interfaces to the actual context
  This context is provided by two stages pushing from both ends
  into a (potentially long) sequence of propagating stages (tbd).

Contributions of this PR:
- PropagatingEitherWay: explicitly distinguish AUTO from BOTHWAYS interface
  AUTO: auto-derive interface from provided push interfaces
  BOTHWAYS: explicitly require both directions
- SerialContainer: (better, but not yet perfect) validation of connectivity
- ParallelContainer: determine interface from what children offer
This commit is contained in:
Robert Haschke 2018-02-17 01:28:28 +01:00
parent ec2b06292e
commit 1a0b9b36ee
10 changed files with 218 additions and 143 deletions

View File

@ -170,6 +170,7 @@ int main(int argc, char** argv){
}
catch (const InitStageException &e) {
std::cerr << e;
t.printState();
return EINVAL;
}

View File

@ -118,6 +118,7 @@ int main(int argc, char** argv){
}
catch (const InitStageException &e) {
std::cerr << e;
t.printState();
return EINVAL;
}

View File

@ -118,6 +118,7 @@ int main(int argc, char** argv){
}
catch (const InitStageException &e) {
std::cerr << e;
t.printState();
return EINVAL;
}

View File

@ -89,6 +89,7 @@ public:
void reset() override;
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
bool canCompute() const override;
bool compute() override;

View File

@ -100,14 +100,18 @@ public:
protected:
ContainerBasePrivate(ContainerBase *me, const std::string &name);
// Get push interface to be used for children: If our own push interface is not set,
// don't set children's interface either: pushing is not supported.
// Otherwise return pending_* buffer.
InterfacePtr getPushBackwardInterface() {
return prevEnds() ? pending_backward_ : InterfacePtr();
// containers don't required a specific interface on their own
// their interface is derived from their children
InterfaceFlags requiredInterface() const override { return InterfaceFlags(); }
// set child's push interfaces: allow pushing if child requires so
inline void setChildsPushBackwardInterface(Stage& child) {
bool allowed = (child.pimpl()->requiredInterface() & WRITES_PREV_END);
child.pimpl()->setPrevEnds(allowed ? pending_backward_ : InterfacePtr());
}
InterfacePtr getPushForwardInterface() {
return nextStarts() ? pending_forward_ : InterfacePtr();
inline void setChildsPushForwardInterface(Stage& child) {
bool allowed = (child.pimpl()->requiredInterface() & WRITES_NEXT_START);
child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
}
/// copy external_state to a child's interface and remember the link in internal_to map
@ -165,7 +169,12 @@ public:
SerialContainerPrivate(SerialContainer* me, const std::string &name);
private:
void connect(StagePrivate *prev, StagePrivate *next);
// connect cur stage to its predecessor and successor
bool connect(container_type::const_iterator cur, InitStageException& errors,
const planning_scene::PlanningSceneConstPtr& scene);
// restrict interfaces of auto-mode propagating stages
void stripInterfaces(std::vector<container_type::const_iterator>& stages);
void validateConnectivity(InitStageException& errors) const;
// set of all solutions
ordered<SerialSolution> solutions_;

View File

@ -69,11 +69,6 @@ enum InterfaceFlag {
READS_END = 0x02,
WRITES_NEXT_START = 0x04,
WRITES_PREV_END = 0x08,
OWN_IF_MASK = READS_START | READS_END,
EXT_IF_MASK = WRITES_NEXT_START | WRITES_PREV_END,
INPUT_IF_MASK = READS_START | WRITES_PREV_END,
OUTPUT_IF_MASK = READS_END | WRITES_NEXT_START,
};
typedef Flags<InterfaceFlag> InterfaceFlags;
@ -132,9 +127,17 @@ public:
operator StagePrivate*();
operator const StagePrivate*() const;
/// reset stage, clearing all solutions, interfaces, etc.
/// reset stage, clearing all solutions, interfaces, inherited properties.
virtual void reset();
/// initialize stage once before planning
/** initialize stage once before planning
*
* When called, properties configured for initialization from parent are already defined.
* Push interfaces are not yet defined!
*
* The planning scene is the initial planning scene of the task. Use it as is or
* to learn about the robot model.
*/
virtual void init(const planning_scene::PlanningSceneConstPtr& scene);
const ContainerBase* parent() const;
@ -198,11 +201,16 @@ public:
PRIVATE_CLASS(PropagatingEitherWay)
PropagatingEitherWay(const std::string& name);
enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD};
enum Direction {
AUTO = 0x00, // auto-derive direction from context
FORWARD = 0x01, // propagate forward only
BACKWARD = 0x02, // propagate backward only
BOTHWAY = FORWARD | BACKWARD, // propagate both ways
};
void restrictDirection(Direction dir);
virtual void reset() override;
virtual void init(const planning_scene::PlanningSceneConstPtr &scene) override;
void reset() override;
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
virtual bool computeForward(const InterfaceState& from) = 0;
virtual bool computeBackward(const InterfaceState& to) = 0;
@ -232,7 +240,6 @@ public:
protected:
// constructor for use in derived classes
PropagatingEitherWay(PropagatingEitherWayPrivate* impl);
void initInterface();
};
@ -285,7 +292,7 @@ public:
PRIVATE_CLASS(Connecting)
Connecting(const std::string& name);
virtual void reset() override;
void reset() override;
virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0;
void connect(const InterfaceState& from, const InterfaceState& to,

View File

@ -60,6 +60,7 @@ public:
virtual ~StagePrivate() = default;
InterfaceFlags interfaceFlags() const;
virtual InterfaceFlags requiredInterface() const = 0;
virtual bool canCompute() const = 0;
virtual bool compute() = 0;
@ -155,13 +156,14 @@ class PropagatingEitherWayPrivate : public ComputeBasePrivate {
std::list<Interface::value_type> processed; // already processed states
public:
PropagatingEitherWay::Direction dir;
PropagatingEitherWay::Direction required_interface_dirs_;
inline PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir,
inline PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction required_interface_dirs_,
const std::string &name);
// returns true if prevEnds() or nextStarts() are accessible
inline bool isConnected() const { return prevEnds() || nextStarts(); }
InterfaceFlags requiredInterface() const override;
// initialize pull interfaces for given propagation directions
void initInterface(PropagatingEitherWay::Direction dir);
bool canCompute() const override;
bool compute() override;
@ -198,6 +200,7 @@ class GeneratorPrivate : public ComputeBasePrivate {
public:
inline GeneratorPrivate(Generator *me, const std::string &name);
InterfaceFlags requiredInterface() const override;
bool canCompute() const override;
bool compute() override;
};
@ -216,6 +219,7 @@ class ConnectingPrivate : public ComputeBasePrivate {
public:
inline ConnectingPrivate(Connecting *me, const std::string &name);
InterfaceFlags requiredInterface() const override;
bool canCompute() const override;
bool compute() override;

View File

@ -332,65 +332,106 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s
: ContainerBasePrivate(me, name)
{}
void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) {
prev->setNextStarts(next->starts());
next->setPrevEnds(prev->ends());
// connect cur stage to its predecessor and successor by setting the push interface pointers
// return true if cur stage should be scheduled for a second sweep
bool SerialContainerPrivate::connect(container_type::const_iterator cur, InitStageException& errors,
const planning_scene::PlanningSceneConstPtr &scene)
{
constexpr InterfaceFlags UNKNOWN;
StagePrivate* const cur_impl = **cur;
InterfaceFlags required = cur_impl->requiredInterface();
// get iterators to prev / next stage in sequence
auto prev = cur; --prev;
auto next = cur; ++next;
// set push forward connection using next's starts
if ((required == UNKNOWN || required & WRITES_NEXT_START)
&& next != children().end()) // last child has not a next one
cur_impl->setNextStarts((*next)->pimpl()->starts());
// set push backward connection using prev's ends
if ((required == UNKNOWN || required & WRITES_PREV_END)
&& cur != children().begin()) // first child has not a previous one
cur_impl->setPrevEnds((*prev)->pimpl()->ends());
// schedule stage with unknown interface for 2nd sweep
return required == UNKNOWN;
}
// restrict interfaces of propagating stages that have auto-mode enabled
void SerialContainerPrivate::stripInterfaces(std::vector<container_type::const_iterator>& stages)
{
for (auto it = stages.rbegin(); it != stages.rend(); ++it) {
}
stages.clear();
}
void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
{
InitStageException errors;
// reset pull interfaces
auto impl = pimpl();
impl->starts_.reset();
impl->ends_.reset();
ContainerBase::init(scene); // throws if there are no children
// if there are no children, there is nothing to connect
if (!impl->children().empty()) {
/*** connect children ***/
// first stage sends backward to pending_backward_
auto start = impl->children().begin();
(*start)->pimpl()->setPrevEnds(impl->getPushBackwardInterface());
InitStageException errors;
// last stage sends forward to pending_forward_
auto last = --impl->children().end();
(*last)->pimpl()->setNextStarts(impl->getPushForwardInterface());
auto start = impl->children().begin();
auto last = --impl->children().end();
auto cur = start;
auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage
if (prev != last) {// we have more than one children
auto next = cur; ++next; // next points to 3rd stage (or end)
for (; cur != last; ++prev, ++cur, ++next) {
impl->connect(**prev, **cur);
impl->connect(**cur, **next);
}
// finally connect last == cur and prev stage
impl->connect(**prev, **cur);
}
// connect first / last child's push interfaces to our pending_* buffers
impl->setChildsPushBackwardInterface(**start);
impl->setChildsPushForwardInterface(**last);
// recursively init + validate all children
// this needs to be done *after* initializing the connections
ContainerBase::init(scene);
std::vector<decltype(start)> scheduled; // stages scheduled for 2n sweep
// initialize starts_ and ends_ interfaces
if (const InterfacePtr& target = (*start)->pimpl()->starts())
impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2)));
if (const InterfacePtr& target = (*last)->pimpl()->ends())
impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2)));
// validate connectivity of this
if (!impl->nextStarts())
errors.push_back(*this, "cannot sendForward()");
if (!impl->prevEnds())
errors.push_back(*this, "cannot sendBackward()");
} else {
errors.push_back(*this, "no children");
// no children -> no reading
impl->starts_.reset();
impl->ends_.reset();
// initialize and connect remaining children in 2 sweeps
// to allow for interface auto-detection for propagating stages
for (auto cur = start, end = impl->children().end(); cur != end; ++cur) {
// 1st sweep: process forward connections
if (impl->connect(cur, errors, scene))
scheduled.push_back(cur);
else if (!scheduled.empty()) // reached a stage with known interface
impl->stripInterfaces(scheduled);
}
impl->stripInterfaces(scheduled); // process stages accumulated up to end
// initialize pull interfaces if first/last child pulls
if (const InterfacePtr& target = (*start)->pimpl()->starts())
impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2)));
if (const InterfacePtr& target = (*last)->pimpl()->ends())
impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2)));
// finally validate connectivity
impl->validateConnectivity(errors);
if (errors)
throw errors;
}
void SerialContainerPrivate::validateConnectivity(InitStageException& errors) const
{
// validate propagation from children to this and vice versa
if (!children().empty()) {
const StagePrivate* start = children().front()->pimpl();
if (bool(start->prevEnds()) ^ bool(prevEnds()))
errors.push_back(*me(), "cannot propagate backward pushes of first child");
const StagePrivate* last = children().back()->pimpl();
if (bool(last->nextStarts()) ^ bool(nextStarts()))
errors.push_back(*me(), "cannot propagate forward pushes of last child");
}
// validate connectivity of children
for (auto& child : children()) {
InterfaceFlags required = child->pimpl()->requiredInterface();
InterfaceFlags actual = child->pimpl()->interfaceFlags();
if ((required & actual) != required)
errors.push_back(*child, "required interface doesn't match actual");
}
}
bool SerialContainer::canCompute() const
{
return !pimpl()->children().empty();
@ -505,42 +546,31 @@ void ParallelContainerBase::reset()
*/
void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
{
InitStageException errors;
// recursively init children
ContainerBase::init(scene);
auto impl = pimpl();
// initialize push connections of children
InterfacePtr push_prev = impl->getPushBackwardInterface();
InterfacePtr push_next = impl->getPushForwardInterface();
for (const Stage::pointer& stage : impl->children()) {
StagePrivate *child = stage->pimpl();
child->setPrevEnds(push_prev);
child->setNextStarts(push_next);
}
// recursively init + validate all children
// this needs to be done *after* initializing push connections
ContainerBase::init(scene);
bool pulls[2];
for (const Stage::pointer& stage : impl->children()) {
StagePrivate *child = stage->pimpl();
// is there any child reading from starts() or ends() ?
pulls[Interface::START] |= bool(child->pullInterface(Interface::START));
pulls[Interface::END] |= bool(child->pullInterface(Interface::END));
}
// determine the union of interfaces required by children
// TODO: should we better use the least common interface?
InterfaceFlags required;
for (const Stage::pointer& stage : impl->children())
required |= stage->pimpl()->requiredInterface();
// initialize this' pull connections
for (Interface::Direction dir : { Interface::START, Interface::END }) {
if (pulls[dir])
impl->pullInterface(dir).reset(new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl, dir, _1, _2)));
else
impl->pullInterface(dir).reset();
impl->starts().reset(required & READS_START
? new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState,
impl, Interface::FORWARD, _1, _2))
: nullptr);
impl->ends().reset(required & READS_END
? new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState,
impl, Interface::BACKWARD, _1, _2))
: nullptr);
// initialize push connections of children according to their demands
for (const Stage::pointer& stage : impl->children()) {
impl->setChildsPushForwardInterface(*stage);
impl->setChildsPushBackwardInterface(*stage);
}
if (impl->children().empty())
errors.push_back(*this, "no children");
if (errors)
throw errors;
}
size_t ParallelContainerBase::numSolutions() const

View File

@ -118,10 +118,14 @@ Stage::operator const StagePrivate*() const {
void Stage::reset()
{
auto impl = pimpl();
// clear pull interfaces
if (impl->starts_) impl->starts_->clear();
if (impl->ends_) impl->ends_->clear();
// reset push interfaces
impl->prev_ends_.reset();
impl->next_starts_.reset();
// reset inherited properties
impl->properties_.reset();
}
void Stage::init(const planning_scene::PlanningSceneConstPtr &scene)
@ -173,7 +177,9 @@ void Stage::setProperty(const std::string& name, const boost::any& value) {
template<InterfaceFlag own, InterfaceFlag other>
const char* direction(const StagePrivate& stage) {
static constexpr InterfaceFlags INPUT_IF_MASK({ READS_START | WRITES_PREV_END });
InterfaceFlags f = stage.interfaceFlags();
bool own_if = f & own;
bool other_if = f & other;
bool reverse = own & INPUT_IF_MASK;
@ -254,8 +260,39 @@ void ComputeBase::reset() {
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name)
: ComputeBasePrivate(me, name), dir(dir)
: ComputeBasePrivate(me, name), required_interface_dirs_(dir)
{
initInterface(required_interface_dirs_);
}
// initialize pull interfaces to match requested propagation directions
void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir)
{
if (dir & PropagatingEitherWay::FORWARD) {
if (!starts_) // keep existing interface if possible
starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, _1)));
} else {
starts_.reset();
}
if (dir & PropagatingEitherWay::BACKWARD) {
if (!ends_) // keep existing interface if possible
ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, _1)));
} else {
ends_.reset();
}
}
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
{
InterfaceFlags f;
if (required_interface_dirs_ & PropagatingEitherWay::FORWARD)
f |= InterfaceFlags({READS_START, WRITES_NEXT_START});
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD)
f |= InterfaceFlags({READS_END, WRITES_PREV_END});
// If required_interface_dirs_ == ANYWAY, we don't require an interface,
// but auto-derive - in init() - from the provided push interfaces.
return f;
}
void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
@ -291,11 +328,7 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
bool PropagatingEitherWayPrivate::canCompute() const
{
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState())
return true;
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState())
return true;
return false;
return hasStartState() || hasEndState();
}
bool PropagatingEitherWayPrivate::compute()
@ -303,14 +336,14 @@ bool PropagatingEitherWayPrivate::compute()
PropagatingEitherWay* me = static_cast<PropagatingEitherWay*>(me_);
bool result = false;
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) {
if (hasStartState()) {
const InterfaceState& state = fetchStartState();
// enforce property initialization from INTERFACE
properties_.performInitFrom(Stage::INTERFACE, state.properties(), true);
if (countFailures(me->computeForward(state)))
result |= true;
}
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) {
if (hasEndState()) {
const InterfaceState& state = fetchEndState();
// enforce property initialization from INTERFACE
properties_.performInitFrom(Stage::INTERFACE, state.properties(), true);
@ -322,42 +355,23 @@ bool PropagatingEitherWayPrivate::compute()
PropagatingEitherWay::PropagatingEitherWay(const std::string &name)
: PropagatingEitherWay(new PropagatingEitherWayPrivate(this, ANYWAY, name))
: PropagatingEitherWay(new PropagatingEitherWayPrivate(this, AUTO, name))
{
}
PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl)
: ComputeBase(impl)
{
initInterface();
}
void PropagatingEitherWay::initInterface()
{
auto impl = pimpl();
if (impl->dir & PropagatingEitherWay::FORWARD) {
if (!impl->starts_) // keep existing interface if possible
impl->starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, impl, _1)));
} else {
impl->starts_.reset();
}
if (impl->dir & PropagatingEitherWay::BACKWARD) {
if (!impl->ends_) // keep existing interface if possible
impl->ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, impl, _1)));
} else {
impl->ends_.reset();
}
}
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir)
{
auto impl = pimpl();
if (impl->dir == dir) return;
if (impl->isConnected())
if (impl->required_interface_dirs_ == dir) return;
if (impl->prevEnds() || impl->nextStarts())
throw std::runtime_error("Cannot change direction after being connected");
impl->dir = dir;
initInterface();
impl->required_interface_dirs_ = dir;
impl->initInterface(impl->required_interface_dirs_);
}
void PropagatingEitherWay::reset()
@ -366,19 +380,19 @@ void PropagatingEitherWay::reset()
ComputeBase::reset();
}
void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene)
void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr& scene)
{
ComputeBase::init(scene);
Stage::init(scene);
auto impl = pimpl();
// after being connected, restrict actual interface directions
if (!impl->nextStarts())
impl->starts_.reset();
if (!impl->prevEnds())
impl->ends_.reset();
if (!impl->isConnected())
throw InitStageException(*this, "can neither send forwards nor backwards");
// In AUTO-mode, i.e. when auto-detecting direction of propagation from context,
// pretend that we offer both interface directions during init().
// This is needed due to a chicken-egg-problem: interface auto-detection requires
// the context (external pushing interfaces prevEnds, nextStarts) to be set,
// while the are ony set if we detected the correct interface...
if (impl->required_interface_dirs_ == AUTO)
impl->initInterface(BOTHWAY);
// otherwise the interface is already fixed and well-defined
}
void PropagatingEitherWay::sendForward(const InterfaceState& from,
@ -442,6 +456,10 @@ GeneratorPrivate::GeneratorPrivate(Generator *me, const std::string &name)
: ComputeBasePrivate(me, name)
{}
InterfaceFlags GeneratorPrivate::requiredInterface() const {
return InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END});
}
bool GeneratorPrivate::canCompute() const {
return static_cast<Generator*>(me_)->canCompute();
}
@ -476,6 +494,10 @@ ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newEndState, this, _1, _2)));
}
InterfaceFlags ConnectingPrivate::requiredInterface() const {
return InterfaceFlags( { READS_START, READS_END } );
}
void ConnectingPrivate::newStartState(Interface::iterator it, bool updated)
{
// TODO: only consider interface states with priority depth > threshold

View File

@ -192,9 +192,8 @@ void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
child->setPrevEnds(impl->pendingBackward());
child->setNextStarts(impl->pendingForward());
// explicitly call ContainerBase::init(), not WrapperBase::init()
// to keep the just set push interface for the wrapped child
ContainerBase::init(scene);
// and *afterwards* initialize all children
wrapped()->init(scene);
// provide introspection instance to all stages
impl->setIntrospection(introspection_.get());