mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branch 'fix-interface-deduction' into master
This commit is contained in:
commit
fee73bcd78
@ -125,6 +125,9 @@ protected:
|
|||||||
bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN);
|
bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN);
|
||||||
child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
||||||
}
|
}
|
||||||
|
// report error about mismatching interface (start or end as determined by mask)
|
||||||
|
void mismatchingInterface(InitStageException& errors, const StagePrivate& child,
|
||||||
|
const InterfaceFlags mask) const;
|
||||||
|
|
||||||
/// 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(Interface::iterator external, const InterfacePtr& target, bool updated);
|
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
|
||||||
@ -181,6 +184,12 @@ private:
|
|||||||
void pruneInterfaces(container_type::const_iterator first,
|
void pruneInterfaces(container_type::const_iterator first,
|
||||||
container_type::const_iterator end,
|
container_type::const_iterator end,
|
||||||
InterfaceFlags accepted);
|
InterfaceFlags accepted);
|
||||||
|
// store first/last child's interface as required for this container
|
||||||
|
void storeRequiredInterface(container_type::const_iterator first,
|
||||||
|
container_type::const_iterator end);
|
||||||
|
|
||||||
|
private:
|
||||||
|
InterfaceFlags required_interface_;
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(SerialContainer)
|
PIMPL_FUNCTIONS(SerialContainer)
|
||||||
|
|
||||||
|
|||||||
@ -83,8 +83,8 @@ typedef Flags<InterfaceFlag> InterfaceFlags;
|
|||||||
|
|
||||||
// some useful constants
|
// some useful constants
|
||||||
constexpr InterfaceFlags UNKNOWN;
|
constexpr InterfaceFlags UNKNOWN;
|
||||||
constexpr InterfaceFlags INPUT_IF_MASK({READS_START, WRITES_PREV_END});
|
constexpr InterfaceFlags START_IF_MASK({READS_START, WRITES_PREV_END});
|
||||||
constexpr InterfaceFlags OUTPUT_IF_MASK({READS_END, WRITES_NEXT_START});
|
constexpr InterfaceFlags END_IF_MASK({READS_END, WRITES_NEXT_START});
|
||||||
constexpr InterfaceFlags PROPAGATE_BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS});
|
constexpr InterfaceFlags PROPAGATE_BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS});
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(Interface)
|
MOVEIT_CLASS_FORWARD(Interface)
|
||||||
@ -235,6 +235,13 @@ protected:
|
|||||||
|
|
||||||
|
|
||||||
class PropagatingEitherWayPrivate;
|
class PropagatingEitherWayPrivate;
|
||||||
|
/** Base class for stages that can propagate InterfaceStates
|
||||||
|
*
|
||||||
|
* They read an InterfaceState on one side (start or end) and
|
||||||
|
* push a new InterfaceState to the opposite site.
|
||||||
|
* By default, the class auto-derives its actual propagation direction from the context.
|
||||||
|
* In order to enforce forward, backward, or bothway propagation, one can use restrictDirection().
|
||||||
|
*/
|
||||||
class PropagatingEitherWay : public ComputeBase {
|
class PropagatingEitherWay : public ComputeBase {
|
||||||
public:
|
public:
|
||||||
PRIVATE_CLASS(PropagatingEitherWay)
|
PRIVATE_CLASS(PropagatingEitherWay)
|
||||||
|
|||||||
@ -101,6 +101,16 @@ void ContainerBasePrivate::validateConnectivity() const
|
|||||||
if (errors) throw errors;
|
if (errors) throw errors;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void ContainerBasePrivate::mismatchingInterface(InitStageException& errors, const StagePrivate& child,
|
||||||
|
const InterfaceFlags mask) const {
|
||||||
|
boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)");
|
||||||
|
errors.push_back(*me(), (desc
|
||||||
|
% (mask == START_IF_MASK ? "start" : "end")
|
||||||
|
% child.name()
|
||||||
|
% flowSymbol(child.interfaceFlags() & mask)
|
||||||
|
% flowSymbol(interfaceFlags() & mask)).str());
|
||||||
|
}
|
||||||
|
|
||||||
bool ContainerBasePrivate::canCompute() const
|
bool ContainerBasePrivate::canCompute() const
|
||||||
{
|
{
|
||||||
// call the method of the public interface
|
// call the method of the public interface
|
||||||
@ -389,14 +399,17 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s
|
|||||||
: ContainerBasePrivate(me, name)
|
: ContainerBasePrivate(me, name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
// a serial container's required interface is derived from the actual input interface
|
// a serial container's required interface is derived from the required input interfaces
|
||||||
// of the first child and the actual output interface of the last child
|
// of the first and last children. After resolving, it is remembered in required_interface_.
|
||||||
InterfaceFlags SerialContainerPrivate::requiredInterface() const
|
InterfaceFlags SerialContainerPrivate::requiredInterface() const
|
||||||
{
|
{
|
||||||
|
if ((required_interface_ & START_IF_MASK) && (required_interface_ & END_IF_MASK))
|
||||||
|
return required_interface_;
|
||||||
|
|
||||||
if (children().empty())
|
if (children().empty())
|
||||||
return UNKNOWN;
|
return UNKNOWN;
|
||||||
return (children().front()->pimpl()->interfaceFlags() & INPUT_IF_MASK)
|
return (children().front()->pimpl()->requiredInterface() & START_IF_MASK)
|
||||||
| (children().back()->pimpl()->interfaceFlags() & OUTPUT_IF_MASK);
|
| (children().back()->pimpl()->requiredInterface() & END_IF_MASK);
|
||||||
}
|
}
|
||||||
|
|
||||||
// connect cur stage to its predecessor and successor by setting the push interface pointers
|
// connect cur stage to its predecessor and successor by setting the push interface pointers
|
||||||
@ -445,6 +458,7 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
|
|||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
impl->starts_.reset();
|
impl->starts_.reset();
|
||||||
impl->ends_.reset();
|
impl->ends_.reset();
|
||||||
|
impl->required_interface_ = UNKNOWN;
|
||||||
|
|
||||||
// recursively init all children, throws if there are no children
|
// recursively init all children, throws if there are no children
|
||||||
ContainerBase::init(robot_model);
|
ContainerBase::init(robot_model);
|
||||||
@ -485,6 +499,16 @@ void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
|
|||||||
std::placeholders::_2)));
|
std::placeholders::_2)));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// prune interface for children in range [first, last) to given direction
|
||||||
|
void SerialContainerPrivate::storeRequiredInterface(container_type::const_iterator first,
|
||||||
|
container_type::const_iterator end)
|
||||||
|
{
|
||||||
|
if (first == children().begin())
|
||||||
|
required_interface_ |= children().front()->pimpl()->interfaceFlags() & START_IF_MASK;
|
||||||
|
if (end == children().end() && !children().empty())
|
||||||
|
required_interface_ |= children().back()->pimpl()->interfaceFlags() & END_IF_MASK;
|
||||||
|
}
|
||||||
|
|
||||||
// called by parent asking for pruning of this' interface
|
// called by parent asking for pruning of this' interface
|
||||||
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
||||||
if (children().empty()) return;
|
if (children().empty()) return;
|
||||||
@ -502,9 +526,9 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
|||||||
} else { // otherwise only prune the first / last child's input / output interface
|
} else { // otherwise only prune the first / last child's input / output interface
|
||||||
StagePrivate* child_impl;
|
StagePrivate* child_impl;
|
||||||
child_impl = children().front()->pimpl();
|
child_impl = children().front()->pimpl();
|
||||||
child_impl->pruneInterface((accepted & INPUT_IF_MASK) | (child_impl->interfaceFlags() & OUTPUT_IF_MASK));
|
child_impl->pruneInterface((accepted & START_IF_MASK) | (child_impl->interfaceFlags() & END_IF_MASK));
|
||||||
child_impl = children().back()->pimpl();
|
child_impl = children().back()->pimpl();
|
||||||
child_impl->pruneInterface((accepted & OUTPUT_IF_MASK) | (child_impl->interfaceFlags() & INPUT_IF_MASK));
|
child_impl->pruneInterface((accepted & END_IF_MASK) | (child_impl->interfaceFlags() & START_IF_MASK));
|
||||||
}
|
}
|
||||||
|
|
||||||
// reset my pull interfaces, if first/last child don't pull anymore
|
// reset my pull interfaces, if first/last child don't pull anymore
|
||||||
@ -513,7 +537,7 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
|||||||
if (!children().back()->pimpl()->ends())
|
if (!children().back()->pimpl()->ends())
|
||||||
ends_.reset();
|
ends_.reset();
|
||||||
|
|
||||||
if (interfaceFlags() == InterfaceFlags())
|
if (interfaceFlags() == UNKNOWN)
|
||||||
throw InitStageException(*me(), "failed to derive propagation direction");
|
throw InitStageException(*me(), "failed to derive propagation direction");
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -522,7 +546,10 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
|||||||
void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first,
|
void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first,
|
||||||
container_type::const_iterator end)
|
container_type::const_iterator end)
|
||||||
{
|
{
|
||||||
if (first == end) return; // nothing to do in this case
|
if (first == end) {
|
||||||
|
storeRequiredInterface(first, end);
|
||||||
|
return; // nothing to do in this case
|
||||||
|
}
|
||||||
|
|
||||||
// determine accepted interface from available push interfaces
|
// determine accepted interface from available push interfaces
|
||||||
InterfaceFlags accepted;
|
InterfaceFlags accepted;
|
||||||
@ -549,7 +576,7 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
|
|||||||
} // else: for last child we cannot determine the interface yet
|
} // else: for last child we cannot determine the interface yet
|
||||||
|
|
||||||
// nothing to do if:
|
// nothing to do if:
|
||||||
// - accepted == 0: interface still unknown
|
// - accepted == UNKNOWN: interface still unknown
|
||||||
// - accepted == PROPAGATE_BOTHWAYS: no change
|
// - accepted == PROPAGATE_BOTHWAYS: no change
|
||||||
if (accepted != UNKNOWN && accepted != PROPAGATE_BOTHWAYS)
|
if (accepted != UNKNOWN && accepted != PROPAGATE_BOTHWAYS)
|
||||||
pruneInterfaces(first, end, accepted);
|
pruneInterfaces(first, end, accepted);
|
||||||
@ -563,8 +590,9 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
|
|||||||
// 1st sweep: remove push interfaces
|
// 1st sweep: remove push interfaces
|
||||||
for (auto it = first; it != end; ++it) {
|
for (auto it = first; it != end; ++it) {
|
||||||
StagePrivate* impl = (*it)->pimpl();
|
StagePrivate* impl = (*it)->pimpl();
|
||||||
// range should only contain stages with unknown required interface
|
// the required interface should be a subset of the accepted one
|
||||||
assert(impl->requiredInterface() == UNKNOWN);
|
if ((impl->requiredInterface() & accepted) != impl->requiredInterface())
|
||||||
|
throw InitStageException(*impl->me(), "Required interface not satisfied after pruning");
|
||||||
|
|
||||||
// remove push interfaces if not accepted
|
// remove push interfaces if not accepted
|
||||||
if (!(accepted & WRITES_PREV_END))
|
if (!(accepted & WRITES_PREV_END))
|
||||||
@ -578,12 +606,13 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
|
|||||||
StagePrivate* impl = (*it)->pimpl();
|
StagePrivate* impl = (*it)->pimpl();
|
||||||
impl->pruneInterface(accepted);
|
impl->pruneInterface(accepted);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
storeRequiredInterface(first, end);
|
||||||
}
|
}
|
||||||
|
|
||||||
void SerialContainerPrivate::validateConnectivity() const
|
void SerialContainerPrivate::validateConnectivity() const
|
||||||
{
|
{
|
||||||
InitStageException errors;
|
InitStageException errors;
|
||||||
boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)");
|
|
||||||
|
|
||||||
// recursively validate children
|
// recursively validate children
|
||||||
try { ContainerBasePrivate::validateConnectivity(); }
|
try { ContainerBasePrivate::validateConnectivity(); }
|
||||||
@ -593,16 +622,14 @@ void SerialContainerPrivate::validateConnectivity() const
|
|||||||
if (!children().empty()) {
|
if (!children().empty()) {
|
||||||
const StagePrivate* start = children().front()->pimpl();
|
const StagePrivate* start = children().front()->pimpl();
|
||||||
const auto my_flags = this->interfaceFlags();
|
const auto my_flags = this->interfaceFlags();
|
||||||
auto child_flags = start->interfaceFlags() & INPUT_IF_MASK;
|
auto child_flags = start->interfaceFlags() & START_IF_MASK;
|
||||||
if (child_flags != (my_flags & INPUT_IF_MASK))
|
if (child_flags != (my_flags & START_IF_MASK))
|
||||||
errors.push_back(*me(), (desc % "input" % start->name() % flowSymbol(child_flags)
|
mismatchingInterface(errors, *start, START_IF_MASK);
|
||||||
% flowSymbol(my_flags & INPUT_IF_MASK)).str());
|
|
||||||
|
|
||||||
const StagePrivate* last = children().back()->pimpl();
|
const StagePrivate* last = children().back()->pimpl();
|
||||||
child_flags = last->interfaceFlags() & OUTPUT_IF_MASK;
|
child_flags = last->interfaceFlags() & END_IF_MASK;
|
||||||
if (child_flags != (my_flags & OUTPUT_IF_MASK))
|
if (child_flags != (my_flags & END_IF_MASK))
|
||||||
errors.push_back(*me(), (desc % "output" % last->name() % flowSymbol(child_flags)
|
mismatchingInterface(errors, *last, END_IF_MASK);
|
||||||
% flowSymbol(my_flags & OUTPUT_IF_MASK)).str());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// validate connectivity of children amongst each other
|
// validate connectivity of children amongst each other
|
||||||
@ -738,14 +765,16 @@ void ParallelContainerBasePrivate::validateConnectivity() const
|
|||||||
InitStageException errors;
|
InitStageException errors;
|
||||||
InterfaceFlags my_interface = interfaceFlags();
|
InterfaceFlags my_interface = interfaceFlags();
|
||||||
InterfaceFlags children_interfaces;
|
InterfaceFlags children_interfaces;
|
||||||
boost::format desc("interface of child '%1%' (%2%) doesn't match mine (%3%)");
|
|
||||||
|
|
||||||
// check that input / output interfaces of all children are handled by my interface
|
// check that input / output interfaces of all children are handled by my interface
|
||||||
for (const auto& child : children()) {
|
for (const auto& child : children()) {
|
||||||
InterfaceFlags current = child->pimpl()->interfaceFlags();
|
InterfaceFlags current = child->pimpl()->interfaceFlags();
|
||||||
children_interfaces |= current; // compute union of all children interfaces
|
children_interfaces |= current; // compute union of all children interfaces
|
||||||
if ((current & my_interface) != current)
|
|
||||||
errors.push_back(*me(), (desc % child->name() % flowSymbol(current) % flowSymbol(my_interface)).str());
|
if ((current & my_interface & START_IF_MASK) != (current & START_IF_MASK))
|
||||||
|
mismatchingInterface(errors, *child->pimpl(), START_IF_MASK);
|
||||||
|
if ((current & my_interface & END_IF_MASK) != (current & END_IF_MASK))
|
||||||
|
mismatchingInterface(errors, *child->pimpl(), END_IF_MASK);
|
||||||
}
|
}
|
||||||
// check that there is a child matching the expected push interfaces
|
// check that there is a child matching the expected push interfaces
|
||||||
if ((my_interface & GENERATE) != (children_interfaces & GENERATE))
|
if ((my_interface & GENERATE) != (children_interfaces & GENERATE))
|
||||||
|
|||||||
@ -352,7 +352,7 @@ const char* direction(const StagePrivate& stage) {
|
|||||||
|
|
||||||
bool own_if = f & own;
|
bool own_if = f & own;
|
||||||
bool other_if = f & other;
|
bool other_if = f & other;
|
||||||
bool reverse = own & INPUT_IF_MASK;
|
bool reverse = own & START_IF_MASK;
|
||||||
if (own_if && other_if) return "<>";
|
if (own_if && other_if) return "<>";
|
||||||
if (!own_if && !other_if) return "--";
|
if (!own_if && !other_if) return "--";
|
||||||
if (other_if ^ reverse) return "->";
|
if (other_if ^ reverse) return "->";
|
||||||
@ -360,20 +360,20 @@ const char* direction(const StagePrivate& stage) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
const char* flowSymbol(InterfaceFlags f) {
|
const char* flowSymbol(InterfaceFlags f) {
|
||||||
if (f == InterfaceFlags())
|
if (f == UNKNOWN)
|
||||||
return "?"; // unknown interface
|
return "?"; // unknown interface
|
||||||
|
|
||||||
// f should have either INPUT or OUTPUT flags set (not both)
|
// f should have either INPUT or OUTPUT flags set (not both)
|
||||||
assert(static_cast<bool>(f & INPUT_IF_MASK) ^ static_cast<bool>(f & OUTPUT_IF_MASK));
|
assert(static_cast<bool>(f & START_IF_MASK) ^ static_cast<bool>(f & END_IF_MASK));
|
||||||
|
|
||||||
if (f & INPUT_IF_MASK) {
|
if (f & START_IF_MASK) {
|
||||||
if (f == READS_START) return "->";
|
if (f == READS_START) return "↓";
|
||||||
if (f == WRITES_PREV_END) return "<-";
|
if (f == WRITES_PREV_END) return "↑";
|
||||||
} else if (f & OUTPUT_IF_MASK) {
|
} else if (f & END_IF_MASK) {
|
||||||
if (f == READS_END) return "<-";
|
if (f == READS_END) return "↑";
|
||||||
if (f == WRITES_NEXT_START) return "->";
|
if (f == WRITES_NEXT_START) return "↓";
|
||||||
}
|
}
|
||||||
return "<->";
|
return "⇅";
|
||||||
}
|
}
|
||||||
|
|
||||||
std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
|
std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
|
||||||
|
|||||||
@ -304,18 +304,11 @@ TEST_F(SerialTest, init_forward) {
|
|||||||
EXPECT_INIT_SUCCESS(true, false, BW, ANY);
|
EXPECT_INIT_SUCCESS(true, false, BW, ANY);
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||||
|
|
||||||
|
// BOTH requires propagation in both directions, while FW/BW can only match one dir
|
||||||
EXPECT_INIT_SUCCESS(false, true, BOTH, FW);
|
EXPECT_INIT_FAILURE(true, true, BOTH, FW);
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
EXPECT_INIT_FAILURE(true, true, FW, BOTH);
|
||||||
|
EXPECT_INIT_FAILURE(true, true, BOTH, BW);
|
||||||
EXPECT_INIT_SUCCESS(false, true, FW, BOTH);
|
EXPECT_INIT_FAILURE(true, true, BW, BOTH);
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, false, BOTH, BW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, false, BW, BOTH);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SerialTest, init_backward) {
|
TEST_F(SerialTest, init_backward) {
|
||||||
@ -333,9 +326,6 @@ TEST_F(SerialTest, init_backward) {
|
|||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, BW, GEN);
|
EXPECT_INIT_SUCCESS(true, true, BW, GEN);
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
||||||
|
|
||||||
EXPECT_INIT_SUCCESS(true, true, BW, GEN, FW);
|
|
||||||
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(SerialTest, interface_detection) {
|
TEST_F(SerialTest, interface_detection) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user