improve error msg for mismatching container/child interfaces

This commit is contained in:
Robert Haschke 2019-02-06 10:26:22 +01:00
parent ae75c3aa7f
commit a1f7c5d1c6
4 changed files with 20 additions and 3 deletions

View File

@ -1,6 +1,7 @@
cmake_minimum_required(VERSION 2.8.12) cmake_minimum_required(VERSION 2.8.12)
project(moveit_task_constructor_core) project(moveit_task_constructor_core)
find_package(Boost REQUIRED)
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
eigen_conversions eigen_conversions
geometry_msgs geometry_msgs

View File

@ -364,4 +364,6 @@ protected:
} }
}; };
const char* flowSymbol(moveit::task_constructor::InterfaceFlags f);
} } } }

View File

@ -44,6 +44,7 @@
#include <iostream> #include <iostream>
#include <algorithm> #include <algorithm>
#include <boost/range/adaptor/reversed.hpp> #include <boost/range/adaptor/reversed.hpp>
#include <boost/format.hpp>
#include <functional> #include <functional>
using namespace std::placeholders; using namespace std::placeholders;
@ -548,16 +549,19 @@ void SerialContainer::validateConnectivity() const
{ {
auto impl = pimpl(); auto impl = pimpl();
InitStageException errors; InitStageException errors;
boost::format desc("%1% interface of '%2%' (%3%) doesn't match mine (%4%)");
// 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() & INPUT_IF_MASK) != (this->pimpl()->interfaceFlags() & INPUT_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, (desc % "input" % start->name() % flowSymbol(start->interfaceFlags())
% flowSymbol(this->pimpl()->interfaceFlags())).str());
const StagePrivate* last = impl->children().back()->pimpl(); const StagePrivate* last = impl->children().back()->pimpl();
if ((last->interfaceFlags() & OUTPUT_IF_MASK) != (this->pimpl()->interfaceFlags() & OUTPUT_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, (desc % "output" % last->name() % flowSymbol(last->interfaceFlags())
% flowSymbol(this->pimpl()->interfaceFlags())).str());
} }
// validate connectivity of children amongst each other // validate connectivity of children amongst each other
@ -734,13 +738,14 @@ void ParallelContainerBase::validateConnectivity() const
auto impl = pimpl(); auto impl = pimpl();
InterfaceFlags my_interface = impl->interfaceFlags(); InterfaceFlags my_interface = impl->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 : pimpl()->children()) { for (const auto& child : pimpl()->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) if ((current & my_interface) != current)
errors.push_back(*this, "interface of child '" + child->name() + "' doesn't match mine"); errors.push_back(*this, (desc % child->name() % flowSymbol(current) % flowSymbol(my_interface)).str());
} }
// 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))

View File

@ -349,6 +349,15 @@ const char* direction(const StagePrivate& stage) {
return "<-"; return "<-";
} }
const char* flowSymbol(moveit::task_constructor::InterfaceFlags f) {
if (f == InterfaceFlags(CONNECT)) return "";
if (f == InterfaceFlags(PROPAGATE_FORWARDS)) return "";
if (f == InterfaceFlags(PROPAGATE_BACKWARDS)) return "";
if (f == PROPAGATE_BOTHWAYS) return "";
if (f == InterfaceFlags(GENERATE)) return "";
return "?";
}
std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) { std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
// starts // starts
for (const InterfaceConstPtr& i : {impl.prevEnds(), impl.starts()}) { for (const InterfaceConstPtr& i : {impl.prevEnds(), impl.starts()}) {