From beffe27987a2ade3f7c446c6844388f40f05f537 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 2 Jun 2018 17:28:16 +0200 Subject: [PATCH] improve property debugging --- core/include/moveit/task_constructor/properties.h | 3 ++- core/src/container.cpp | 9 ++++++++- core/src/properties.cpp | 11 +++++++---- core/src/stage.cpp | 3 ++- 4 files changed, 19 insertions(+), 7 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 250d389b..cd5d4b14 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -128,7 +128,8 @@ public: /// get default value const boost::any& defaultValue() const { return default_; } /// serialize current value - std::string serialize() const; + std::string serialize(const boost::any& value) const; + std::string serialize() const { return serialize(value()); } /// get description text const std::string& description() const { return description_; } diff --git a/core/src/container.cpp b/core/src/container.cpp index 0b08f0c8..3f1c23b0 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -254,7 +254,14 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) // recursively init all children and accumulate errors InitStageException errors; for (auto& child : children) { - try { child->init(robot_model); } catch (InitStageException &e) { errors.append(e); } + try { child->init(robot_model); } + catch (const Property::error &e) { + std::ostringstream oss; + oss << e.what(); + pimpl()->composePropertyErrorMsg(e.name(), oss); + errors.push_back(*child, oss.str()); + } + catch (InitStageException &e) { errors.append(e); } } if (errors) throw errors; diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 53233109..1b028761 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -39,6 +39,7 @@ #include #include #include +#include namespace moveit { namespace task_constructor { @@ -83,9 +84,9 @@ void Property::reset() initialized_from_ = -1; // set to max value } -std::string Property::serialize() const { - if (!serialize_) return ""; - return serialize_(value()); +std::string Property::serialize(const boost::any& v) const { + if (!serialize_ || v.empty()) return ""; + return serialize_(v); } bool Property::initsFrom(Property::SourceFlags source) const @@ -142,7 +143,7 @@ void PropertyMap::exposeTo(PropertyMap& other, const std::set &prop void PropertyMap::exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) { const Property& p = property(name); - other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_); + other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_); } void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set &properties) @@ -218,6 +219,8 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa } catch (const Property::undefined&) { } + ROS_DEBUG_STREAM_NAMED("Properties", pair.first << ": " << p.initialized_from_ << + " -> " << source << ": " << p.serialize(value)); p.setCurrentValue(value); p.initialized_from_ = source; } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 6c320600..133dd1d0 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -222,6 +222,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& robot_model) impl->properties_.reset(); if (impl->parent()) { try { + ROS_DEBUG_STREAM_NAMED("Properties", "init '" << name() << "'"); impl->properties_.performInitFrom(PARENT, impl->parent()->properties()); } catch (const Property::error &e) { std::ostringstream oss; @@ -299,7 +300,7 @@ void StagePrivate::composePropertyErrorMsg(const std::string& property_name, std os << "declared, but undefined"; if (p.initsFrom(Stage::PARENT)) os << ", inherits from parent"; - else if (p.initsFrom(Stage::PARENT)) os << ", initializes from interface"; + if (p.initsFrom(Stage::INTERFACE)) os << ", initializes from interface"; } catch (const Property::undeclared &e) { os << "undeclared"; }