mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fixed remaining unittest
This commit is contained in:
parent
35f1540369
commit
ba75f7c835
@ -100,10 +100,6 @@ public:
|
|||||||
protected:
|
protected:
|
||||||
ContainerBasePrivate(ContainerBase *me, const std::string &name);
|
ContainerBasePrivate(ContainerBase *me, const std::string &name);
|
||||||
|
|
||||||
// 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 it or
|
// Set child's push interfaces: allow pushing if child requires it or
|
||||||
// if the interface is unknown: in this case greedily assume a push interface.
|
// if the interface is unknown: in this case greedily assume a push interface.
|
||||||
// If, during pruneInterface() later, we notice that it's not needed, prune there.
|
// If, during pruneInterface() later, we notice that it's not needed, prune there.
|
||||||
@ -172,6 +168,9 @@ class SerialContainerPrivate : public ContainerBasePrivate {
|
|||||||
public:
|
public:
|
||||||
SerialContainerPrivate(SerialContainer* me, const std::string &name);
|
SerialContainerPrivate(SerialContainer* me, const std::string &name);
|
||||||
|
|
||||||
|
// containers derive their required interface from their children
|
||||||
|
InterfaceFlags requiredInterface() const override;
|
||||||
|
|
||||||
// called by parent asking for pruning of this' interface
|
// called by parent asking for pruning of this' interface
|
||||||
void pruneInterface(InterfaceFlags accepted) override;
|
void pruneInterface(InterfaceFlags accepted) override;
|
||||||
|
|
||||||
@ -220,6 +219,12 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate {
|
|||||||
public:
|
public:
|
||||||
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string &name);
|
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string &name);
|
||||||
|
|
||||||
|
// containers derive their required interface from their children
|
||||||
|
InterfaceFlags requiredInterface() const override;
|
||||||
|
|
||||||
|
// called by parent asking for pruning of this' interface
|
||||||
|
void pruneInterface(InterfaceFlags accepted) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/// callback for new externally received states
|
/// callback for new externally received states
|
||||||
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
|
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
|
||||||
|
|||||||
@ -77,6 +77,11 @@ enum InterfaceFlag {
|
|||||||
};
|
};
|
||||||
typedef Flags<InterfaceFlag> InterfaceFlags;
|
typedef Flags<InterfaceFlag> InterfaceFlags;
|
||||||
|
|
||||||
|
// some useful constants
|
||||||
|
constexpr InterfaceFlags UNKNOWN;
|
||||||
|
constexpr InterfaceFlags INPUT_IF_MASK({READS_START, WRITES_PREV_END});
|
||||||
|
constexpr InterfaceFlags OUTPUT_IF_MASK({READS_END, WRITES_NEXT_START});
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(Interface)
|
MOVEIT_CLASS_FORWARD(Interface)
|
||||||
MOVEIT_CLASS_FORWARD(Stage)
|
MOVEIT_CLASS_FORWARD(Stage)
|
||||||
class InterfaceState;
|
class InterfaceState;
|
||||||
|
|||||||
@ -344,11 +344,20 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s
|
|||||||
: ContainerBasePrivate(me, name)
|
: ContainerBasePrivate(me, name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
// a serial container's required interface is derived from the required input interface
|
||||||
|
// of the first child and the required output interface of the last child
|
||||||
|
InterfaceFlags SerialContainerPrivate::requiredInterface() const
|
||||||
|
{
|
||||||
|
if (children().empty())
|
||||||
|
return UNKNOWN;
|
||||||
|
return (children().front()->pimpl()->requiredInterface() & INPUT_IF_MASK)
|
||||||
|
| (children().back()->pimpl()->requiredInterface() & OUTPUT_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
|
||||||
// return true if cur stage should be scheduled for a second sweep
|
// return true if cur stage should be scheduled for a second sweep
|
||||||
bool SerialContainerPrivate::connect(container_type::const_iterator cur)
|
bool SerialContainerPrivate::connect(container_type::const_iterator cur)
|
||||||
{
|
{
|
||||||
constexpr InterfaceFlags UNKNOWN;
|
|
||||||
StagePrivate* const cur_impl = **cur;
|
StagePrivate* const cur_impl = **cur;
|
||||||
InterfaceFlags required = cur_impl->requiredInterface();
|
InterfaceFlags required = cur_impl->requiredInterface();
|
||||||
|
|
||||||
@ -431,7 +440,7 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
|||||||
if (children().empty()) return;
|
if (children().empty()) return;
|
||||||
constexpr InterfaceFlags BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS});
|
constexpr InterfaceFlags BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS});
|
||||||
|
|
||||||
// I think, we only need to deal with the special case of the whole sequence to be pruned.
|
// We only need to deal with the special case of the whole sequence to be pruned.
|
||||||
if (accepted != BOTHWAYS && // will interface be restricted at all?
|
if (accepted != BOTHWAYS && // will interface be restricted at all?
|
||||||
children().front()->pimpl()->interfaceFlags() == BOTHWAYS) // still undecided?
|
children().front()->pimpl()->interfaceFlags() == BOTHWAYS) // still undecided?
|
||||||
{
|
{
|
||||||
@ -443,10 +452,8 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
|
|||||||
if (!children().back()->pimpl()->ends())
|
if (!children().back()->pimpl()->ends())
|
||||||
ends_.reset();
|
ends_.reset();
|
||||||
}
|
}
|
||||||
|
if (interfaceFlags() == UNKNOWN)
|
||||||
// If my assumption doesn't hold, we would need to prune partial sequences of stages
|
throw InitStageException(*me(), "failed to derive propagation direction");
|
||||||
// with unknown-only interfaces at the *beginning* and *end* of the whole sequence.
|
|
||||||
// Reuse code from init() in that case.
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// called by init() to prune interfaces for children in range [first, last)
|
// called by init() to prune interfaces for children in range [first, last)
|
||||||
@ -509,17 +516,14 @@ void SerialContainer::validateConnectivity() const
|
|||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
InitStageException errors;
|
InitStageException errors;
|
||||||
|
|
||||||
constexpr InterfaceFlags INP_IF_MASK({READS_START, WRITES_PREV_END});
|
|
||||||
constexpr InterfaceFlags OUT_IF_MASK({READS_END, WRITES_NEXT_START});
|
|
||||||
|
|
||||||
// check that input / output interface of first / last child matches this' resp. interface
|
// check that input / output interface of first / last child matches this' resp. interface
|
||||||
if (!impl->children().empty()) {
|
if (!impl->children().empty()) {
|
||||||
const StagePrivate* start = impl->children().front()->pimpl();
|
const StagePrivate* start = impl->children().front()->pimpl();
|
||||||
if ((start->interfaceFlags() & INP_IF_MASK) != (this->pimpl()->interfaceFlags() & INP_IF_MASK))
|
if ((start->interfaceFlags() & INPUT_IF_MASK) != (this->pimpl()->interfaceFlags() & INPUT_IF_MASK))
|
||||||
errors.push_back(*this, "input interface of '" + start->name() + "' doesn't match mine");
|
errors.push_back(*this, "input interface of '" + start->name() + "' doesn't match mine");
|
||||||
|
|
||||||
const StagePrivate* last = impl->children().back()->pimpl();
|
const StagePrivate* last = impl->children().back()->pimpl();
|
||||||
if ((last->interfaceFlags() & OUT_IF_MASK) != (this->pimpl()->interfaceFlags() & OUT_IF_MASK))
|
if ((last->interfaceFlags() & OUTPUT_IF_MASK) != (this->pimpl()->interfaceFlags() & OUTPUT_IF_MASK))
|
||||||
errors.push_back(*this, "output interface of '" + last->name() + "' doesn't match mine");
|
errors.push_back(*this, "output interface of '" + last->name() + "' doesn't match mine");
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -635,6 +639,27 @@ ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// A parallel container's required interface is derived from the required interfaces of all of its children.
|
||||||
|
// They must not conflict to each other. Otherwise an InitStageException is thrown.
|
||||||
|
InterfaceFlags ParallelContainerBasePrivate::requiredInterface() const
|
||||||
|
{
|
||||||
|
if (children().empty())
|
||||||
|
return UNKNOWN;
|
||||||
|
/* TODO: replace this with a proper check for consistency of interfaces. Allowed combinations are:
|
||||||
|
* ❘ ❘ = ❘ (connecting stages)
|
||||||
|
* ↑ ↑ = ↑ (backward propagating)
|
||||||
|
* ↓ ↓ = ↓ (forward propagating)
|
||||||
|
* ↑ ↓ = ⇅ = ⇅ ↑ = ⇅ ↓ (propagating in both directions)
|
||||||
|
* ↕ ↕ = ↕ (generating)
|
||||||
|
*/
|
||||||
|
return children().front()->pimpl()->requiredInterface();
|
||||||
|
}
|
||||||
|
|
||||||
|
void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted)
|
||||||
|
{
|
||||||
|
// TODO: forward pruning to all children with UNKNOWN required interface
|
||||||
|
}
|
||||||
|
|
||||||
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) {
|
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) {
|
||||||
for (const Stage::pointer& stage : children())
|
for (const Stage::pointer& stage : children())
|
||||||
copyState(external, stage->pimpl()->pullInterface(dir), updated);
|
copyState(external, stage->pimpl()->pullInterface(dir), updated);
|
||||||
@ -695,9 +720,6 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc
|
|||||||
|
|
||||||
void ParallelContainerBase::validateConnectivity() const
|
void ParallelContainerBase::validateConnectivity() const
|
||||||
{
|
{
|
||||||
constexpr InterfaceFlags INP_IF_MASK({READS_START, WRITES_PREV_END});
|
|
||||||
constexpr InterfaceFlags OUT_IF_MASK({READS_END, WRITES_NEXT_START});
|
|
||||||
|
|
||||||
InitStageException errors;
|
InitStageException errors;
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
InterfaceFlags my_interface = impl->interfaceFlags();
|
InterfaceFlags my_interface = impl->interfaceFlags();
|
||||||
|
|||||||
@ -177,7 +177,6 @@ void Stage::setProperty(const std::string& name, const boost::any& value) {
|
|||||||
|
|
||||||
template<InterfaceFlag own, InterfaceFlag other>
|
template<InterfaceFlag own, InterfaceFlag other>
|
||||||
const char* direction(const StagePrivate& stage) {
|
const char* direction(const StagePrivate& stage) {
|
||||||
static constexpr InterfaceFlags INPUT_IF_MASK({ READS_START | WRITES_PREV_END });
|
|
||||||
InterfaceFlags f = stage.interfaceFlags();
|
InterfaceFlags f = stage.interfaceFlags();
|
||||||
|
|
||||||
bool own_if = f & own;
|
bool own_if = f & own;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user