mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
improve error msg for mismatching container/child interfaces
This commit is contained in:
parent
ae75c3aa7f
commit
a1f7c5d1c6
@ -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
|
||||||
|
|||||||
@ -364,4 +364,6 @@ protected:
|
|||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
const char* flowSymbol(moveit::task_constructor::InterfaceFlags f);
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -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))
|
||||||
|
|||||||
@ -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()}) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user