From a1f7c5d1c6c27f6f3bfb53a4cc344349f2a39ea3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 6 Feb 2019 10:26:22 +0100 Subject: [PATCH] improve error msg for mismatching container/child interfaces --- core/CMakeLists.txt | 1 + core/include/moveit/task_constructor/stage.h | 2 ++ core/src/container.cpp | 11 ++++++++--- core/src/stage.cpp | 9 +++++++++ 4 files changed, 20 insertions(+), 3 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index bc95729b..e4546e94 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 2.8.12) project(moveit_task_constructor_core) +find_package(Boost REQUIRED) find_package(catkin REQUIRED COMPONENTS eigen_conversions geometry_msgs diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 387dc18c..d606ab81 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -364,4 +364,6 @@ protected: } }; +const char* flowSymbol(moveit::task_constructor::InterfaceFlags f); + } } diff --git a/core/src/container.cpp b/core/src/container.cpp index aa9f5cc1..6e70b269 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include using namespace std::placeholders; @@ -548,16 +549,19 @@ void SerialContainer::validateConnectivity() const { auto impl = pimpl(); 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 if (!impl->children().empty()) { const StagePrivate* start = impl->children().front()->pimpl(); 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(); 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 @@ -734,13 +738,14 @@ void ParallelContainerBase::validateConnectivity() const 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, "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 if ((my_interface & GENERATE) != (children_interfaces & GENERATE)) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 9a8dd535..7d382b58 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -349,6 +349,15 @@ const char* direction(const StagePrivate& stage) { 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) { // starts for (const InterfaceConstPtr& i : {impl.prevEnds(), impl.starts()}) {