mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
ec2b06292e
commit
1a0b9b36ee
@ -170,6 +170,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
catch (const InitStageException &e) {
|
||||
std::cerr << e;
|
||||
t.printState();
|
||||
return EINVAL;
|
||||
}
|
||||
|
||||
|
||||
@ -118,6 +118,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
catch (const InitStageException &e) {
|
||||
std::cerr << e;
|
||||
t.printState();
|
||||
return EINVAL;
|
||||
}
|
||||
|
||||
|
||||
@ -118,6 +118,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
catch (const InitStageException &e) {
|
||||
std::cerr << e;
|
||||
t.printState();
|
||||
return EINVAL;
|
||||
}
|
||||
|
||||
|
||||
@ -89,6 +89,7 @@ public:
|
||||
|
||||
void reset() override;
|
||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
||||
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
InitStageException errors;
|
||||
|
||||
// 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());
|
||||
|
||||
// last stage sends forward to pending_forward_
|
||||
auto last = --impl->children().end();
|
||||
(*last)->pimpl()->setNextStarts(impl->getPushForwardInterface());
|
||||
|
||||
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);
|
||||
|
||||
std::vector<decltype(start)> scheduled; // stages scheduled for 2n sweep
|
||||
|
||||
// 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
|
||||
|
||||
// recursively init + validate all children
|
||||
// this needs to be done *after* initializing the connections
|
||||
ContainerBase::init(scene);
|
||||
|
||||
// initialize starts_ and ends_ interfaces
|
||||
// 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)));
|
||||
|
||||
// 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();
|
||||
}
|
||||
// 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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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());
|
||||
|
||||
Loading…
Reference in New Issue
Block a user