mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
move validateConnectivity() from ContainerBase to StagePrivate
... to allow specific stage types (PropagatingEitherWay) implementing their own validation No need for a public interface.
This commit is contained in:
parent
10c7a9cfd7
commit
905b6a3b0c
@ -64,8 +64,6 @@ public:
|
|||||||
|
|
||||||
void reset() override;
|
void reset() override;
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
/// validate connectivity of children (after init() was done)
|
|
||||||
virtual void validateConnectivity() const;
|
|
||||||
|
|
||||||
virtual bool canCompute() const = 0;
|
virtual bool canCompute() const = 0;
|
||||||
virtual void compute() = 0;
|
virtual void compute() = 0;
|
||||||
@ -89,9 +87,6 @@ public:
|
|||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
|
|
||||||
/// validate connectivity of children (after init() was done)
|
|
||||||
void validateConnectivity() const override;
|
|
||||||
|
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
void compute() override;
|
void compute() override;
|
||||||
|
|
||||||
@ -132,9 +127,6 @@ public:
|
|||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
|
|
||||||
/// validate connectivity of children (after init() was done)
|
|
||||||
void validateConnectivity() const override;
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
||||||
|
|
||||||
|
|||||||
@ -97,6 +97,8 @@ public:
|
|||||||
return const_cast<const ContainerBasePrivate*>(this)->traverseStages(const_processor, cur_depth, max_depth);
|
return const_cast<const ContainerBasePrivate*>(this)->traverseStages(const_processor, cur_depth, max_depth);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void validateConnectivity() const override;
|
||||||
|
|
||||||
// forward these methods to the public interface for containers
|
// forward these methods to the public interface for containers
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
void compute() override;
|
void compute() override;
|
||||||
@ -162,6 +164,8 @@ public:
|
|||||||
|
|
||||||
// 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;
|
||||||
|
// validate connectivity of chain
|
||||||
|
void validateConnectivity() const override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// connect cur stage to its predecessor and successor
|
// connect cur stage to its predecessor and successor
|
||||||
@ -214,6 +218,8 @@ public:
|
|||||||
// 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;
|
||||||
|
|
||||||
|
void validateConnectivity() const 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);
|
||||||
|
|||||||
@ -74,6 +74,9 @@ public:
|
|||||||
* PropagatingEitherWay uses this in restrictDirection() */
|
* PropagatingEitherWay uses this in restrictDirection() */
|
||||||
virtual void pruneInterface(InterfaceFlags accepted) {}
|
virtual void pruneInterface(InterfaceFlags accepted) {}
|
||||||
|
|
||||||
|
/// validate connectivity of children (after init() was done)
|
||||||
|
virtual void validateConnectivity() const;
|
||||||
|
|
||||||
virtual bool canCompute() const = 0;
|
virtual bool canCompute() const = 0;
|
||||||
virtual void compute() = 0;
|
virtual void compute() = 0;
|
||||||
|
|
||||||
@ -179,6 +182,8 @@ public:
|
|||||||
void initInterface(PropagatingEitherWay::Direction dir);
|
void initInterface(PropagatingEitherWay::Direction dir);
|
||||||
// prune interface to the given propagation direction
|
// prune interface to the given propagation direction
|
||||||
void pruneInterface(InterfaceFlags accepted) override;
|
void pruneInterface(InterfaceFlags accepted) override;
|
||||||
|
// validate that we can propagate in one direction at least
|
||||||
|
void validateConnectivity() const override;
|
||||||
|
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
void compute() override;
|
void compute() override;
|
||||||
|
|||||||
@ -89,6 +89,17 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void ContainerBasePrivate::validateConnectivity() const
|
||||||
|
{
|
||||||
|
InitStageException errors;
|
||||||
|
// recursively validate all children and accumulate errors
|
||||||
|
for (const auto& child : children()) {
|
||||||
|
try { child->pimpl()->validateConnectivity(); }
|
||||||
|
catch (InitStageException &e) { errors.append(e); }
|
||||||
|
}
|
||||||
|
if (errors) throw errors;
|
||||||
|
}
|
||||||
|
|
||||||
bool ContainerBasePrivate::canCompute() const
|
bool ContainerBasePrivate::canCompute() const
|
||||||
{
|
{
|
||||||
// call the method of the public interface
|
// call the method of the public interface
|
||||||
@ -239,25 +250,6 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
|
|||||||
if (errors) throw errors;
|
if (errors) throw errors;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ContainerBase::validateConnectivity() const
|
|
||||||
{
|
|
||||||
InitStageException errors;
|
|
||||||
for (const auto& child : pimpl()->children()) {
|
|
||||||
// check that child's required interface is provided
|
|
||||||
InterfaceFlags required = child->pimpl()->requiredInterface();
|
|
||||||
InterfaceFlags actual = child->pimpl()->interfaceFlags();
|
|
||||||
if ((required & actual) != required)
|
|
||||||
errors.push_back(*child, "required interface is not satisfied");
|
|
||||||
|
|
||||||
// recursively validate all children and accumulate errors
|
|
||||||
ContainerBase* child_container = dynamic_cast<ContainerBase*>(child.get());
|
|
||||||
if (!child_container) continue; // only containers provide validateConnectivity()
|
|
||||||
try { child_container->validateConnectivity(); } catch (InitStageException &e) { errors.append(e); }
|
|
||||||
}
|
|
||||||
|
|
||||||
if (errors) throw errors;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
|
std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
|
||||||
ContainerBase::StageCallback processor = [&os](const Stage& stage, int depth) -> bool {
|
ContainerBase::StageCallback processor = [&os](const Stage& stage, int depth) -> bool {
|
||||||
os << std::string(2*depth, ' ') << *stage.pimpl() << std::endl;
|
os << std::string(2*depth, ' ') << *stage.pimpl() << std::endl;
|
||||||
@ -546,42 +538,45 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void SerialContainer::validateConnectivity() const
|
void SerialContainerPrivate::validateConnectivity() const
|
||||||
{
|
{
|
||||||
auto impl = pimpl();
|
|
||||||
InitStageException errors;
|
InitStageException errors;
|
||||||
boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)");
|
boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)");
|
||||||
|
|
||||||
|
// recursively validate children
|
||||||
|
try { ContainerBasePrivate::validateConnectivity(); }
|
||||||
|
catch (InitStageException& e) { errors.append(e); }
|
||||||
|
|
||||||
// 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 (!children().empty()) {
|
||||||
const StagePrivate* start = impl->children().front()->pimpl();
|
const StagePrivate* start = children().front()->pimpl();
|
||||||
const auto my_flags = this->pimpl()->interfaceFlags();
|
const auto my_flags = this->interfaceFlags();
|
||||||
auto child_flags = start->interfaceFlags() & INPUT_IF_MASK;
|
auto child_flags = start->interfaceFlags() & INPUT_IF_MASK;
|
||||||
if (child_flags != (my_flags & INPUT_IF_MASK))
|
if (child_flags != (my_flags & INPUT_IF_MASK))
|
||||||
errors.push_back(*this, (desc % "input" % start->name() % flowSymbol(child_flags)
|
errors.push_back(*me(), (desc % "input" % start->name() % flowSymbol(child_flags)
|
||||||
% flowSymbol(my_flags & INPUT_IF_MASK)).str());
|
% flowSymbol(my_flags & INPUT_IF_MASK)).str());
|
||||||
|
|
||||||
const StagePrivate* last = impl->children().back()->pimpl();
|
const StagePrivate* last = children().back()->pimpl();
|
||||||
child_flags = last->interfaceFlags() & OUTPUT_IF_MASK;
|
child_flags = last->interfaceFlags() & OUTPUT_IF_MASK;
|
||||||
if (child_flags != (my_flags & OUTPUT_IF_MASK))
|
if (child_flags != (my_flags & OUTPUT_IF_MASK))
|
||||||
errors.push_back(*this, (desc % "output" % last->name() % flowSymbol(child_flags)
|
errors.push_back(*me(), (desc % "output" % last->name() % flowSymbol(child_flags)
|
||||||
% flowSymbol(my_flags & OUTPUT_IF_MASK)).str());
|
% flowSymbol(my_flags & OUTPUT_IF_MASK)).str());
|
||||||
}
|
}
|
||||||
|
|
||||||
// validate connectivity of children amongst each other
|
// validate connectivity of children amongst each other
|
||||||
// ContainerBase::validateConnectivity() ensures that required push interfaces are present,
|
// ContainerBasePrivate::validateConnectivity() ensures that required push interfaces are present,
|
||||||
// that is, neighbouring stages have a corresponding pull interface.
|
// that is, neighbouring stages have a corresponding pull interface.
|
||||||
// Here, it remains to check that - if a child requires a pull interface - it's indeed feeded.
|
// Here, it remains to check that - if a child has a pull interface - it's indeed feeded.
|
||||||
for (auto cur = impl->children().begin(), end = impl->children().end(); cur != end; ++cur) {
|
for (auto cur = children().begin(), end = children().end(); cur != end; ++cur) {
|
||||||
const StagePrivate* const cur_impl = **cur;
|
const StagePrivate* const cur_impl = **cur;
|
||||||
InterfaceFlags required = cur_impl->requiredInterface();
|
InterfaceFlags required = cur_impl->interfaceFlags();
|
||||||
|
|
||||||
// get iterators to prev / next stage in sequence
|
// get iterators to prev / next stage in sequence
|
||||||
auto prev = cur; --prev;
|
auto prev = cur; --prev;
|
||||||
auto next = cur; ++next;
|
auto next = cur; ++next;
|
||||||
|
|
||||||
// start pull interface fed?
|
// start pull interface fed?
|
||||||
if (cur != impl->children().begin() && // first child has not a previous one
|
if (cur != children().begin() && // first child has not a previous one
|
||||||
(required & READS_START) && !(*prev)->pimpl()->nextStarts())
|
(required & READS_START) && !(*prev)->pimpl()->nextStarts())
|
||||||
errors.push_back(**cur, "start interface is not fed");
|
errors.push_back(**cur, "start interface is not fed");
|
||||||
|
|
||||||
@ -591,9 +586,6 @@ void SerialContainer::validateConnectivity() const
|
|||||||
errors.push_back(**cur, "end interface is not fed");
|
errors.push_back(**cur, "end interface is not fed");
|
||||||
}
|
}
|
||||||
|
|
||||||
// recursively validate children
|
|
||||||
try { ContainerBase::validateConnectivity(); } catch (InitStageException& e) { errors.append(e); }
|
|
||||||
|
|
||||||
if (errors) throw errors;
|
if (errors) throw errors;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -693,6 +685,31 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void ParallelContainerBasePrivate::validateConnectivity() const
|
||||||
|
{
|
||||||
|
InitStageException errors;
|
||||||
|
InterfaceFlags my_interface = interfaceFlags();
|
||||||
|
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
|
||||||
|
for (const auto& child : children()) {
|
||||||
|
InterfaceFlags current = child->pimpl()->interfaceFlags();
|
||||||
|
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());
|
||||||
|
}
|
||||||
|
// check that there is a child matching the expected push interfaces
|
||||||
|
if ((my_interface & GENERATE) != (children_interfaces & GENERATE))
|
||||||
|
errors.push_back(*me(), "no child provides expected push interface");
|
||||||
|
|
||||||
|
// recursively validate children
|
||||||
|
try { ContainerBasePrivate::validateConnectivity(); }
|
||||||
|
catch (InitStageException& e) { errors.append(e); }
|
||||||
|
|
||||||
|
if (errors) throw errors;
|
||||||
|
}
|
||||||
|
|
||||||
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);
|
||||||
@ -736,31 +753,6 @@ void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_m
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParallelContainerBase::validateConnectivity() const
|
|
||||||
{
|
|
||||||
InitStageException errors;
|
|
||||||
auto impl = pimpl();
|
|
||||||
InterfaceFlags my_interface = impl->interfaceFlags();
|
|
||||||
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
|
|
||||||
for (const auto& child : pimpl()->children()) {
|
|
||||||
InterfaceFlags current = child->pimpl()->interfaceFlags();
|
|
||||||
children_interfaces |= current; // compute union of all children interfaces
|
|
||||||
if ((current & my_interface) != current)
|
|
||||||
errors.push_back(*this, (desc % child->name() % flowSymbol(current) % flowSymbol(my_interface)).str());
|
|
||||||
}
|
|
||||||
// check that there is a child matching the expected push interfaces
|
|
||||||
if ((my_interface & GENERATE) != (children_interfaces & GENERATE))
|
|
||||||
errors.push_back(*this, "no child provides expected push interface");
|
|
||||||
|
|
||||||
// recursively validate children
|
|
||||||
try { ContainerBase::validateConnectivity(); } catch (InitStageException& e) { errors.append(e); }
|
|
||||||
|
|
||||||
if (errors) throw errors;
|
|
||||||
}
|
|
||||||
|
|
||||||
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment)
|
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment)
|
||||||
{
|
{
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
|
|||||||
@ -83,6 +83,15 @@ InterfaceFlags StagePrivate::interfaceFlags() const
|
|||||||
return f;
|
return f;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void StagePrivate::validateConnectivity() const
|
||||||
|
{
|
||||||
|
// check that the required interface is provided
|
||||||
|
InterfaceFlags required = requiredInterface();
|
||||||
|
InterfaceFlags actual = interfaceFlags();
|
||||||
|
if ((required & actual) != required)
|
||||||
|
throw InitStageException(*me(), "required interface is not satisfied");
|
||||||
|
}
|
||||||
|
|
||||||
bool StagePrivate::storeSolution(const SolutionBasePtr& solution)
|
bool StagePrivate::storeSolution(const SolutionBasePtr& solution)
|
||||||
{
|
{
|
||||||
solution->setCreator(this);
|
solution->setCreator(this);
|
||||||
@ -427,6 +436,27 @@ void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
|
|||||||
initInterface(PropagatingEitherWay::Direction(dir));
|
initInterface(PropagatingEitherWay::Direction(dir));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void PropagatingEitherWayPrivate::validateConnectivity() const
|
||||||
|
{
|
||||||
|
InterfaceFlags required = requiredInterface();
|
||||||
|
InterfaceFlags actual = interfaceFlags();
|
||||||
|
if (actual == UNKNOWN)
|
||||||
|
throw InitStageException(*me(), "not connected in any direction");
|
||||||
|
|
||||||
|
InitStageException errors;
|
||||||
|
if ((actual & READS_START) && !(actual & WRITES_NEXT_START))
|
||||||
|
errors.push_back(*me(), "Cannot push forwards");
|
||||||
|
if ((actual & READS_END) && !(actual & WRITES_PREV_END))
|
||||||
|
errors.push_back(*me(), "Cannot push backwards");
|
||||||
|
|
||||||
|
if (required_interface_dirs_ == PropagatingEitherWay::BOTHWAY &&
|
||||||
|
(required & actual) != required)
|
||||||
|
ROS_WARN_STREAM_NAMED("PropagatingEitherWay", "Cannot propagate " <<
|
||||||
|
(actual & PROPAGATE_FORWARDS ? "backwards" : "forwards"));
|
||||||
|
|
||||||
|
if (errors) throw errors;
|
||||||
|
}
|
||||||
|
|
||||||
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
|
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
|
||||||
{
|
{
|
||||||
InterfaceFlags f;
|
InterfaceFlags f;
|
||||||
@ -434,7 +464,7 @@ InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
|
|||||||
f |= PROPAGATE_FORWARDS;
|
f |= PROPAGATE_FORWARDS;
|
||||||
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD)
|
if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD)
|
||||||
f |= PROPAGATE_BACKWARDS;
|
f |= PROPAGATE_BACKWARDS;
|
||||||
// if required_interface_dirs_ == ANYWAY, we don't require an interface
|
// if required_interface_dirs_ == AUTO, we don't require an interface
|
||||||
// but the parent container auto-derives the propagation direction
|
// but the parent container auto-derives the propagation direction
|
||||||
return f;
|
return f;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -237,7 +237,7 @@ void Task::init()
|
|||||||
// task expects its wrapped child to push to both ends, this triggers interface resolution
|
// task expects its wrapped child to push to both ends, this triggers interface resolution
|
||||||
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
|
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
|
||||||
// and *finally* validate connectivity
|
// and *finally* validate connectivity
|
||||||
stages()->validateConnectivity();
|
stages()->pimpl()->validateConnectivity();
|
||||||
|
|
||||||
// provide introspection instance to all stages
|
// provide introspection instance to all stages
|
||||||
impl->setIntrospection(introspection_.get());
|
impl->setIntrospection(introspection_.get());
|
||||||
|
|||||||
@ -134,7 +134,7 @@ protected:
|
|||||||
if (start) accepted |= WRITES_PREV_END;
|
if (start) accepted |= WRITES_PREV_END;
|
||||||
if (end) accepted |= WRITES_NEXT_START;
|
if (end) accepted |= WRITES_NEXT_START;
|
||||||
container.pimpl()->pruneInterface(accepted);
|
container.pimpl()->pruneInterface(accepted);
|
||||||
container.validateConnectivity();
|
container.pimpl()->validateConnectivity();
|
||||||
if (!expect_failure) return; // as expected
|
if (!expect_failure) return; // as expected
|
||||||
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container;
|
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container;
|
||||||
} catch (const InitStageException &e) {
|
} catch (const InitStageException &e) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user